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

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