Is there some memory in the solver?

User Questions
1

Hi!
I have created acados solver for mpc problem in Matlab. When I call the solver first time (with initial conditions X) it solves the problem as expected. However, when i try to solve the problem with different initial conditions (Y) the solver solves the problem in a way similar to the first initial condition even though the results are not expected in this case. To be confident, I have created the solver again (without any change in its structure) and call it with the second initial conditions (Y) and in this case the results were as expected. Therefore, I wonder if there is some memory in the algorithm?

In another case when I call the solver firstly with initial conditions W, the solver works fine. When I test it with conditions Z, works fine also but when i repeat the test with the conditions W it gives unexpexted results. The photos for this case where the blue line represents the results, red the reference and circle are some obstacles.

The next picture with shows the expected results
1
The next picture shows other conditions with the same solver
2
The conditions in the next picture are the same as in the first picture but the results were unexpected
3

2

@FreyJo Could you explain how i can solve this issue? Thank in advance.

3

Hey @Muhammad

What do you mean with initial condition?
Did you just set x0?

The initialization of an optimization solver is very important.
The solver can diverge or converge to different solutions.

In Matlab, you can use

ocp.set('init_x', *);
ocp.set('init_u', *);
ocp.set('init_lam', *);
ocp.set('init_pi', *);

to initialize the OCP solver.
Does that solve your issue?

1 Like
4

Thanks @FreyJo for reply,

What do you mean with initial condition?
Did you just set x0?

I mean the initial position of the mobile robot (x , y , theta and v) which is setted as sym_x vector in the model. In other words, these are the values from which the solver should start. I have setted them using constr_x0. However, in the following there is how i use the solver where:
t_ocp the solver
x0 the initial position of the robot
xt the target position which is used in mayer term
obstacles_parameters these are some parameters which i used to describe the obstacles.
guess is the reference trajectory and used for y_ref
where N = 15

function [xt_traj , ut_traj , time , guess] = call_solver(t_ocp , x0 , xt , obstacles_parameters )

tic

num_obst = 3;

xt(5:num_obst+4) = 0;

guess = zeros(15,6+num_obst);

guess(:,1) = linspace(x0(1),xt(1),15);

guess(:,2) = linspace(x0(2),xt(2),15);

guess(:,3) = atan2(xt(2)-x0(2),xt(1)-x0(1))*ones(15,1);

guess(:,4) = sqrt((xt(1)-0)^2 + (xt(2)-0)^2 )/6;

W_x = diag([1e2 , 1e2 , 5e2 , 10]);

W_u = diag([0,1e-1]);

W_obst = 6e2 * eye(num_obst);

W = blkdiag(W_x, W_u , W_obst);

t_ocp.set('constr_x0', x0(1:4));

yref = guess;

for i=1:14

t_ocp.set('cost_y_ref', yref(i,:), i);

end

t_ocp.set('cost_y_ref_e', xt, 15);

t_ocp.set('p',obstacles_parameters);

t_ocp.set('cost_W',W);

t_ocp.solve();

xt_traj = t_ocp.get('x');

ut_traj = t_ocp.get('u');

end

As it is shown, I set x0 using constr_x0.

The initialization of an optimization solver is very important.
The solver can diverge or converge to different solutions.

In Matlab, you can use
ocp.set('init_x', *);
ocp.set('init_u', *);
ocp.set('init_lam', *);
ocp.set('init_pi', *);

Actually I didn’t set any of these. As I have seen in test_template_pendulum_exact_hess, I have to set init_x as matrix (model.nx by N+1). Does this mean that I have to set initial value at each point N along the trajectory? Should these initial values be as same as the reference points yref? Also In the documentation of acados ( problem_formulation_ocp_mex.pdf) I didn’t see refer to initi_x or init_u. How should I set them?

5

Hi @Muhammad ,
until now your OCP knows only the starting condition/constraint (constr_x0). With the init_* variables you initialize your actual optimization variables for the solver.
The file getting_started/minimal_example_ocp.m shows you how to init them. In your case, maybe the variable guess would be a first starting point.

init_x is your start trajectory of the solver (init variable). init_u is the initial trajectory of your controls. They should be at least bring a feasible result when calculating your ODE.

Bye

1 Like
6

Thanks @mss for reply!
I have setted the initial trajectory for the state and control as following!

function [xt_traj , ut_traj , time , guess] = call_solver(t_ocp , x0 , xt , obstacles_parameters , plot_flag)
tic
num_obst = size(obstacles_parameters,1)/5;
xt(5:num_obst+4) = 0;

guess = zeros(15,6+num_obst);
guess(:,1) = linspace(x0(1),xt(1),15);
guess(:,2) = linspace(x0(2),xt(2),15);
guess(:,3) = atan2(xt(2)-x0(2),xt(1)-x0(1))*ones(15,1);
guess(:,4) = sqrt((xt(1)-0)^2 + (xt(2)-0)^2 )/6;
xt_init = vertcat(guess(: , 1:4) , guess(end , 1:4));

W_x = diag([ 2e2 ,  2e2 , 2e2 , 1e-1]);
W_u = diag([1e-1,1e-1]);
W_obst = 8e2 * eye(num_obst);
W = blkdiag(W_x, W_u , W_obst);


init_u = zeros (2 , 15);
t_ocp.set('constr_x0', x0(1:4));
t_ocp.set('init_x', xt_init);
t_ocp.set('init_u', init_u);

yref = guess;
for i=1:14
    t_ocp.set('cost_y_ref', yref(i,:), i);
end
t_ocp.set('cost_y_ref_e', xt, 15);
t_ocp.set('p',obstacles_parameters);
t_ocp.set('cost_W',W);

t_ocp.solve();
xt_traj = t_ocp.get('x');
ut_traj = t_ocp.get('u');

end

However, the results are sometimes not well, the following picture shows one test . Please not that the blue line is the result trajectory from the solver, red dashed line is the reference trajectory and i have used it to set both init_x and y_ref. Also, the circles are some obstacle which the blue line should not cross them. Actually, i don’t know how the init_x can help if i set them using the red dashed line which is passed through some not feasible solution (because of the obstacle)

test

I don’t know why the resulted blue trajectory is given in this way.

7

Hey @FreyJo and @mss
Unfortunately, I didn’t figure out any effect for init_x and init_y in my implementation. The initial state is taken as constr_x0 in the solver. In the following, I paste my Matlab implementation and explaine the case.
The next script describe the dynamic model

function model = bicycle_model()

import casadi.*

% system dimensions
nx = 4;
nu = 2;
L = 0.65;
% named symbolic variables
x = SX.sym('x');         
y = SX.sym('y');         
theta = SX.sym('theta'); 
v = SX.sym('v');         
a = SX.sym('a');  
delta = SX.sym('delta');

% (unnamed) symbolic variables
sym_x = vertcat(x, y, theta , v);
sym_xdot = SX.sym('xdot', nx, 1);
sym_u = vertcat(a, delta);
expr_h = sym_u;
% dynamics
expr_f_expl = vertcat(cos(theta)*v, ...
                      sin(theta)*v, ...
                      tan(delta)*v/L, ...
                      a);
expr_f_impl = expr_f_expl - sym_xdot;            

%% cost - nonlinear least sqares
cost_expr_y = vertcat(sym_x , sym_u);
cost_expr_y_e = vertcat(sym_x);

%% weights on the model
W_x = diag([5e2 , 5e2 , 1e3 , 10]);
W_u = diag([0,1e-1]);
W = blkdiag(W_x, W_u);
W_e = diag([5e4 , 5e4 , 1e3 , 10]);
%% model structure
model.nx = nx;
model.nu = nu;
model.sym_x = sym_x;
model.sym_xdot = sym_xdot;
model.sym_u = sym_u;
model.expr_h = expr_h;
model.expr_f_expl = expr_f_expl;
model.expr_f_impl = expr_f_impl;
model.cost_expr_y = cost_expr_y;
model.cost_expr_y_e = cost_expr_y_e;
model.W = W;
model.W_e = W_e;
end

next script is to create the solver

%% Code for creating acados solver for using optimization problem as trajectory planning 
clear all
close all
% reference trajectory 
path = zeros(15,6);
g = linspace(0,4,15);
path(:,1)=g';
path(:,2)=g';
path(:,3)=0.78;
path(:,4)=0.2;
% initial position
x0=[0,0,1.7,0];
% target position
xt = [path(end,1) path(end,2) path(end,3) 0];
% solver parameters
N = 15;
T = 6;
% solver model
model = bicycle_model();
model_name = 'bicycle';
% solver model class
ocp_model = acados_ocp_model();
ocp_model.set('name', model_name);
ocp_model.set('T', T);
ocp_model.set('sym_x', model.sym_x);
ocp_model.set('sym_u', model.sym_u);
ocp_model.set('sym_xdot', model.sym_xdot);
% cost : nonlinear-least squares
ocp_model.set('cost_type', 'nonlinear_ls');
ocp_model.set('cost_type_e', 'nonlinear_ls');
ocp_model.set('cost_expr_y', model.cost_expr_y);
ocp_model.set('cost_expr_y_e', model.cost_expr_y_e);

% weights
ocp_model.set('cost_W', model.W);
ocp_model.set('cost_W_e', model.W_e);

% dynamics
ocp_model.set('dyn_type', 'explicit');
ocp_model.set('dyn_expr_f', model.expr_f_expl);

% constraints
ocp_model.set('constr_type', 'auto');
ocp_model.set('constr_expr_h', model.expr_h);
U_max = [.6,0.2];
ocp_model.set('constr_lh', -U_max); % lower bound on h
ocp_model.set('constr_uh', U_max);  % upper bound on h
ocp_model.set('constr_x0', x0);

% ocp options class
ocp_opts = acados_ocp_opts();
ocp_opts.set('param_scheme_N', N);
ocp_opts.set('nlp_solver', 'sqp');
ocp_opts.set('sim_method', 'erk');
ocp_opts.set('nlp_solver_max_iter', 50);
ocp_opts.set('nlp_solver_tol_stat', 100);
ocp_opts.set('nlp_solver_tol_eq', 1e-2);
ocp_opts.set('qp_solver', 'partial_condensing_hpipm');
ocp_opts.set('sim_method_num_stages', 4);
ocp_opts.set('sim_method_num_steps', 3);
ocp_opts.set('qp_solver_cond_N', 5);

% create ocp solver
ocp = acados_ocp(ocp_model, ocp_opts);

for k=0:N-1
   yref = path(k+1,:)';
   ocp.set('cost_y_ref', yref, k);
end
ocp.set('cost_y_ref_e', yref(1:4) , N);

size_path = size(path);
ocp.set('constr_x0', x0);
init_x = [path(:,1:4) ; path(end,1:4)]';
init_u = [zeros(2,N)]';
ocp.set('init_x', init_x);
ocp.set('init_u', init_u);

% solve
ocp.solve();
xt_traj = ocp.get('x');
ut_traj = ocp.get('u');
plot(xt_traj(1,:) , xt_traj(2,:))

When run these script, the initial state is taken as const_x0 and not as the first elements of init_x, where constr_x0 = x0 = [0 , 0 , 1.7 , 0] where 1.7 rad = 96.9 deg is the orientation angle of the robot. While

init_x =

  Columns 1 through 12

         0    0.2857    0.5714    0.8571    1.1429    1.4286    1.7143    2.0000    2.2857    2.5714    2.8571    3.1429
         0    0.2857    0.5714    0.8571    1.1429    1.4286    1.7143    2.0000    2.2857    2.5714    2.8571    3.1429
    0.7800    0.7800    0.7800    0.7800    0.7800    0.7800    0.7800    0.7800    0.7800    0.7800    0.7800    0.7800
    0.2000    0.2000    0.2000    0.2000    0.2000    0.2000    0.2000    0.2000    0.2000    0.2000    0.2000    0.2000

  Columns 13 through 16

    3.4286    3.7143    4.0000    4.0000
    3.4286    3.7143    4.0000    4.0000
    0.7800    0.7800    0.7800    0.7800
    0.2000    0.2000    0.2000    0.2000

it is clearly that the initial value of the orientation is 0.78 rad = 45 deg. The result is shown in the following figure

test

I have removed all obstacle implementation to see the effect of init_x and init_u.

8

Hi,
I don’t know exactly what you expect from your last plot. When running your optimization you should see some differences in the solver output and how many iterations he needs. Try to compare an init with all values at zero vs. an init with your reference trajectory.

Actually, i don’t know how the init_x can help if i set them using the red dashed line which is passed through some not feasible solution (because of the obstacle)

You might want to give your best knowledge up to now to the solver to increase efficiency or solver convergence. E.g. if you run the optimization multiple times (like in MPC) you can use your last solution as a starting point. (I don’t know if acados uses the previous solution automatically as init_* if not otherwise stated.)

Run your code in the very first post again and set your init_* values each time to the same values (e.g. zeros/reference traj./…) . It might give you a hint, whether the different solutions are based on initialization or another reason.

9

Hey @mss !

I don’t know exactly what you expect from your last plot.

I expect the initial orientation of the resulted trajectory is 0.75 rad = 45 deg since it has been setted in init_x.
How to set the initial position of the solution? Is it using constr_x0 or using init_x.

You might want to give your best knowledge up to now to the solver to increase efficiency or solver convergence.

Actually, I use the straight line from the initial position of the robot to the target position as a reference trajectory.

My goal to understand what is the reason of the behavior of the resulted trajectory is the next plot. Why does the trajectory choose to turn to right and not to left.

Is it because the initial states or something another?

if you run the optimization multiple times (like in MPC) you can use your last solution as a starting point.

I have to indicate that the resulted trajectory is for only one solution ocp problem. I mean that I run the optimization only one time to get the optimal trajectory along the horizon and no need to use the control action.

10

Hi @Muhammad

This is constr_x0. init_x is only the starting point for the solver from where he tries to find a solution.

Probably because your cost is lower on this way. Some questions to come closer:

  • Do you have enough iterations for your solver that it can converge?
  • Is the accuracy high/low enough that the solution is feasible?
  • Do you get the same behavior if you have everything rotated by 45° (your ref is on the x-axis)?
    • Might the influence of u in your cost function try to reduce the used energy?
    • Other approaches: almost no weight on u or penalizing delta u ==> your actual task needs no control weighting, but not sure if weight == 0 works properly.

That you will find out, if you solve the problem once by init_x = zeros( ), init_u = zeros( ) [all of the below mentioned]

and then by solving it with e.g. your reference values for init_x. Create the solver each time new, for the case of other internal states.

2 Likes
11

@mss and @FreyJo Thanks for you!
It was init_x as you have said. I think its role is the initial solution or as you have said:

init_x is only the starting point for the solver from where he tries to find a solution.