Not getting the expected answer?

Question

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.

Answer 1

Below is a solution in Python Gekko that can also run in Matlab: How to call GEKKO correctly from Matlab

import numpy as np
from gekko import GEKKO
m = GEKKO(remote=False)

n  = 30
z0 = [4.99098,13.5041,-0.481149,-0.0124761]
zf = [9.8141,6.6843,-1.3585,0.0344]

u = m.Array(m.MV,2)
v,w = u; v.STATUS=1; w.STATUS=1

z = m.Array(m.Var,4)
x,y,th,phi = z
for i,zi in enumerate(z):
    zi.value = z0[i]

m.Equation(x.dt() == m.cos(th)*v)
m.Equation(y.dt() == m.sin(th)*v)
m.Equation(0.68*th.dt() == m.tan(phi)*v)
m.Equation(phi.dt() == w)
m.Minimize(v**2 + w**2)

f = np.zeros(n); f[-1]=1e3
final = m.Param(f)
m.Minimize(final*(x-zf[0])**2)
m.Minimize(final*(y-zf[1])**2)
m.Minimize(final*(th-zf[2])**2)
m.Minimize(final*(phi-zf[3])**2)

m.time = np.linspace(0,5,n)
m.options.IMODE=6; m.options.NODES=3; m.options.SOLVER=1
m.solve()

There is additional code to create a plot of the states and a comparison to the x0 values. The x0 values are different than the same solution from ACADO and Gekko that share the same solution.

# For comparison
zs = np.array([[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]])

import matplotlib.pyplot as plt
plt.figure(1)
plt.subplot(2,2,1)
plt.plot(m.time,x,label='x')
plt.legend()
plt.subplot(2,2,2)
plt.plot(m.time,y,label='y')
plt.legend()
plt.subplot(2,2,3)
plt.plot(m.time,th,label='th')
plt.legend()
plt.subplot(2,2,4)
plt.plot(m.time,phi,label='phi')
plt.legend()

plt.figure(2)
plt.plot(x,y,'ro',label='Gekko')
plt.plot(zs[:,0],zs[:,1],'bo',label='x0')
plt.xlabel('x'); plt.ylabel('y')
plt.show()

This doesn't answer the specific question on finding the problem with the CasADi code, but it hopefully gives another point of reference.