I have written an optimization problem in ACADO, and it works as expected. Here is the program: where and
x0 = [4.99098,13.5041,-0.481149,-0.0124761
5.1939,13.37,-0.3891,0.0136
5.3879,13.2902,-0.3957,-0.1045
5.5818,13.2059,-0.4298,-0.2737
5.7758,13.1097,-0.4952,-0.4026
6.1313,12.8784,-0.658,-0.4009
6.2929,12.7443,-0.7241,-0.3075
6.4545,12.5939,-0.7699,-0.17
6.7434,12.3066,-0.7854,0
7.0303,12.0197,-0.7854,0
7.3152,11.7348,-0.7854,0
7.602,11.4439,-0.8175,-0.2784
7.7434,11.2832,-0.8852,-0.4152
7.9758,10.9506,-1.0325,-0.3547
8.1677,10.586,-1.1298,-0.1944
8.2586,10.3853,-1.1591,-0.1207
8.4167,10.0096,-1.1772,0.0458
8.5029,9.8076,-1.1534,0.1866
8.6797,9.4439,-1.0842,0.1742
8.8826,9.0803,-1.0491,0.0151
8.9868,8.8985,-1.0544,-0.0744
9.1828,8.5348,-1.1063,-0.2042
9.358,8.151,-1.1731,-0.1031
9.4433,7.9449,-1.1822,-0.0824
9.5265,7.7348,-1.2094,-0.1909
9.6011,7.5247,-1.251,-0.2359
9.6655,7.3146,-1.2953,-0.2257
9.7205,7.1045,-1.3323,-0.1678
9.7688,6.8944,-1.3548,-0.0747
9.8141,6.6843,-1.3585,0.0344];
x0 = x0';
x_init = [4.99098,13.5041,-0.481149,-0.0124761];
x_final = [9.8141,6.6843,-1.3585,0.0344];
x_r = size(x0);
N = x_r(2)-1;
BEGIN_ACADO;
acadoSet('problemname', 'ocp_car');
DifferentialState x y th phi;
Control v w;
f = acado.DifferentialEquation();
f.add(dot(x) == cos(th)*v);
f.add(dot(y) == sin(th)*v);
f.add(dot(th) == tan(phi)*v/0.68);
f.add(dot(phi) == w);
ocp = acado.OCP(0.0, 5, N);
ocp.minimizeLagrangeTerm(v*v+w*w);
ocp.subjectTo( f );
ocp.subjectTo( 'AT_START', x == x_init(1));
ocp.subjectTo( 'AT_START', y == x_init(2));
ocp.subjectTo( 'AT_START', th == x_init(3));
ocp.subjectTo( 'AT_START', phi == x_init(4));
ocp.subjectTo( 'AT_END', x == x_final(1));
ocp.subjectTo( 'AT_END', y == x_final(2));
ocp.subjectTo( 'AT_END', th == x_final(3));
ocp.subjectTo( 'AT_END', phi == x_final(4));
algo = acado.OptimizationAlgorithm(ocp);
algo.set('INTEGRATOR_TOLERANCE', 1e-5 );
algo.set('DISCRETIZATION_TYPE', 'MULTIPLE_SHOOTING');
END_ACADO;
out = ocp_car_RUN();
figure;
hold on
scatter(out.STATES(:,2), out.STATES(:,3), 'r');
scatter(x0(1,:), x0(2,:), 'b');
Now I want to formulate the same program CasADi, here is my attempt:
opti = casadi.Opti();
X = opti.variable(4, N+1);
U = opti.variable(2, N);
obj = U(1,:)*U(1,:)' + U(2,:)*U(2,:)';
opti.minimize(obj(1,1));
f = @(x,u) [u(1)*cos(x(3)); u(1)*sin(x(3)); u(1)*tan(x(4))/0.68; u(2)];
dt = 0.670;
for k=1:N
k1 = f(X(:,k), U(:,k));
k2 = f(X(:,k) + dt/2*k1, U(:,k));
k3 = f(X(:,k) + dt/2*k2, U(:,k));
k4 = f(X(:,k) + dt*k3, U(:,k));
x_next = X(:,k) + dt/6*(k1+2*k2+2*k3+k4);
opti.subject_to(X(:,k+1)==x_next);
end
opti.subject_to(X(:,1) == x_init');
opti.subject_to(X(:,end) == x_final');
opti.set_initial(X, x0);
opti.solver('ipopt');
sol = opti.solve();
hold on;
solved_val=sol.value(X)';
scatter(x0(1,:), x0(2,:), 'b');
scatter(solved_val(1,:), solved_val(2,:), 'r');
I am not sure the way I formulate in CasADi is correct. If someone points out where I made some mistakes, would appreciate it.
Below is a solution in Python Gekko that can also run in Matlab: How to call GEKKO correctly from Matlab
There is additional code to create a plot of the states and a comparison to the
x0
values. Thex0
values are different than the same solution fromACADO
andGekko
that share the same solution.This doesn't answer the specific question on finding the problem with the CasADi code, but it hopefully gives another point of reference.