Optimal control problem 1ΒΆ
Matthew Kelly: An Introduction to Trajectory Optimization: How to Do Your Own Direct Collocation
import the packages
serial multiple shooting (loop)
parallel multiple shooting (map)
[5]:
import simulatetraj as a
import casadi as cs
import matplotlib.pyplot as plt
loop=True
map=True
refer 2.1 in paper for the optimal control problem.
Integration object for each shooting interval created. m denotes number of inverals in a shooting interval.
[6]:
m=1
sym_dict=a.get_symbols(n_x=2,n_u=1,n_p=0,N=m)
f=cs.vertcat(sym_dict['x'][1],sym_dict['u'])
_=a.scale_ode(sym_dict=sym_dict,f=f)
_=a.integrator(sym_dict=sym_dict,options=None)
transcribe and solve using Opti()
traditional for loop
[7]:
if loop:
nlp=cs.Opti()
N=200
grid=cs.linspace(0,1,N+1)
X=nlp.variable(sym_dict['n_x'],N+1)
U=nlp.variable(sym_dict['n_u'],N)
obj=cs.sumsqr(U)*(grid[1]-grid[0])
nlp.minimize(obj)
x0=cs.DM([0,0])
xf=cs.DM([1,0])
nlp.subject_to(X[:,0]-x0==0)
for i in range(N):
a.simulate(sym_dict=sym_dict,t0=grid[i],tf=grid[i+1],x0=X[:,i],u=cs.repmat(U[:,i],1,m))
nlp.subject_to(sym_dict['x_res'][:,-1]-X[:,i+1]==0)
nlp.subject_to(X[:,-1]-xf==0)
nlp.solver('ipopt')
nlp.set_initial(X[0,:],cs.np.linspace(0,1,N+1))
nlp.set_initial(X[1,:],cs.np.linspace(0,1,N+1))
nlp.set_initial(U,cs.np.linspace(0,1,N))
sol=nlp.solve()
obj_val=sol.value(nlp.f)
print(f'Objective value: Paper - 12 | code - {obj_val}')
plt.figure()
plt.plot(grid.full(),sol.value(X[0,:]),label='x_1')
plt.plot(grid.full(),sol.value(X[1,:]),label='x_2')
plt.legend()
plt.grid(True)
plt.show()
plt.figure()
plt.step(grid.full(),cs.np.hstack((cs.np.nan,sol.value(U))),label='u')
plt.legend()
plt.grid(True)
plt.show()
else:
pass
This is Ipopt version 3.14.11, running with linear solver MUMPS 5.4.1.
Number of nonzeros in equality constraint Jacobian...: 1404
Number of nonzeros in inequality constraint Jacobian.: 0
Number of nonzeros in Lagrangian Hessian.............: 200
Total number of variables............................: 602
variables with only lower bounds: 0
variables with lower and upper bounds: 0
variables with only upper bounds: 0
Total number of equality constraints.................: 404
Total number of inequality constraints...............: 0
inequality constraints with only lower bounds: 0
inequality constraints with lower and upper bounds: 0
inequality constraints with only upper bounds: 0
iter objective inf_pr inf_du lg(mu) ||d|| lg(rg) alpha_du alpha_pr ls
0 3.3417085e-01 1.00e+00 2.97e-10 -1.0 0.00e+00 - 0.00e+00 0.00e+00 0
1 1.2000205e+01 3.96e-08 8.08e-08 -1.7 6.97e+00 - 1.00e+00 1.00e+00h 1
2 1.2000301e+01 1.23e-13 2.19e-13 -8.6 2.62e-05 - 1.00e+00 1.00e+00h 1
Number of Iterations....: 2
(scaled) (unscaled)
Objective...............: 1.2000300799006434e+01 1.2000300799006434e+01
Dual infeasibility......: 2.1883536649447421e-13 2.1883536649447421e-13
Constraint violation....: 1.2290168882600483e-13 1.2290168882600483e-13
Variable bound violation: 0.0000000000000000e+00 0.0000000000000000e+00
Complementarity.........: 0.0000000000000000e+00 0.0000000000000000e+00
Overall NLP error.......: 2.1883536649447421e-13 2.1883536649447421e-13
Number of objective function evaluations = 3
Number of objective gradient evaluations = 3
Number of equality constraint evaluations = 3
Number of inequality constraint evaluations = 0
Number of equality constraint Jacobian evaluations = 3
Number of inequality constraint Jacobian evaluations = 0
Number of Lagrangian Hessian evaluations = 2
Total seconds in IPOPT = 0.139
EXIT: Optimal Solution Found.
solver : t_proc (avg) t_wall (avg) n_eval
nlp_f | 0 ( 0) 6.00us ( 2.00us) 3
nlp_g | 26.00ms ( 8.67ms) 12.47ms ( 4.16ms) 3
nlp_grad_f | 0 ( 0) 28.00us ( 7.00us) 4
nlp_hess_l | 42.00ms ( 21.00ms) 43.92ms ( 21.96ms) 2
nlp_jac_g | 72.00ms ( 18.00ms) 73.38ms ( 18.34ms) 4
total | 140.00ms (140.00ms) 139.35ms (139.35ms) 1
Objective value: Paper - 12 | code - 12.000300799006434
same NLP but uses vectorization/maps
little faster
[8]:
if map:
nlp=cs.Opti()
N=200
grid=cs.linspace(0,1,N+1).T
X=nlp.variable(sym_dict['n_x'],N+1)
U=nlp.variable(sym_dict['n_u'],N)
obj=cs.sumsqr(U)*(grid[1]-grid[0])
int_map=sym_dict['x_n'].map(N,'thread',2)
defect=X[:,1:]-int_map(x0=X[:,0:-1],u=U,p=cs.vertcat(grid[:,0:-1],grid[:,1:]))['xf']
x0=cs.DM([0,0])
xf=cs.DM([1,0])
nlp.minimize(obj)
nlp.subject_to(X[:,0]-x0==0)
nlp.subject_to(defect==0)
nlp.subject_to(X[:,-1]-xf==0)
nlp.solver('ipopt')
nlp.set_initial(X[0,:],cs.np.linspace(0,1,N+1))
nlp.set_initial(X[1,:],cs.np.linspace(0,1,N+1))
nlp.set_initial(U,cs.np.linspace(0,1,N))
sol=nlp.solve()
obj_val=sol.value(nlp.f)
print(f'Objective value: Paper - 12 | code - {obj_val}')
plt.figure()
plt.plot(grid.full().flatten(),sol.value(X[0,:]).flatten(),label='x_1')
plt.plot(grid.full().flatten(),sol.value(X[1,:]).flatten(),label='x_2')
plt.legend()
plt.grid(True)
plt.show()
plt.figure()
plt.step(grid.full().flatten(),cs.np.hstack((cs.np.nan,sol.value(U))).flatten(),label='u')
plt.legend()
plt.grid(True)
plt.show()
else:
pass
This is Ipopt version 3.14.11, running with linear solver MUMPS 5.4.1.
Number of nonzeros in equality constraint Jacobian...: 1404
Number of nonzeros in inequality constraint Jacobian.: 0
Number of nonzeros in Lagrangian Hessian.............: 200
Total number of variables............................: 602
variables with only lower bounds: 0
variables with lower and upper bounds: 0
variables with only upper bounds: 0
Total number of equality constraints.................: 404
Total number of inequality constraints...............: 0
inequality constraints with only lower bounds: 0
inequality constraints with lower and upper bounds: 0
inequality constraints with only upper bounds: 0
iter objective inf_pr inf_du lg(mu) ||d|| lg(rg) alpha_du alpha_pr ls
0 3.3417085e-01 1.00e+00 3.20e-10 -1.0 0.00e+00 - 0.00e+00 0.00e+00 0
1 1.2000205e+01 3.93e-08 8.93e-08 -1.7 6.97e+00 - 1.00e+00 1.00e+00h 1
2 1.2000301e+01 1.34e-13 2.65e-13 -8.6 2.64e-05 - 1.00e+00 1.00e+00h 1
Number of Iterations....: 2
(scaled) (unscaled)
Objective...............: 1.2000300799005419e+01 1.2000300799005419e+01
Dual infeasibility......: 2.6486018239735287e-13 2.6486018239735287e-13
Constraint violation....: 1.3411494137471891e-13 1.3411494137471891e-13
Variable bound violation: 0.0000000000000000e+00 0.0000000000000000e+00
Complementarity.........: 0.0000000000000000e+00 0.0000000000000000e+00
Overall NLP error.......: 2.6486018239735287e-13 2.6486018239735287e-13
Number of objective function evaluations = 3
Number of objective gradient evaluations = 3
Number of equality constraint evaluations = 3
Number of inequality constraint evaluations = 0
Number of equality constraint Jacobian evaluations = 3
Number of inequality constraint Jacobian evaluations = 0
Number of Lagrangian Hessian evaluations = 2
Total seconds in IPOPT = 0.099
EXIT: Optimal Solution Found.
solver : t_proc (avg) t_wall (avg) n_eval
nlp_f | 0 ( 0) 13.00us ( 4.33us) 3
nlp_g | 20.00ms ( 6.67ms) 9.71ms ( 3.24ms) 3
nlp_grad_f | 0 ( 0) 32.00us ( 8.00us) 4
nlp_hess_l | 15.00ms ( 7.50ms) 25.76ms ( 12.88ms) 2
nlp_jac_g | 46.00ms ( 11.50ms) 51.46ms ( 12.87ms) 4
total | 100.00ms (100.00ms) 99.65ms ( 99.65ms) 1
Objective value: Paper - 12 | code - 12.00030079900542
