In this example we will set-up a simple 4-state system, and find a time-optimal trajectory that avoids some obstacles in the world.
%matplotlib inline
from casadi import *
from pylab import *
from optoy import *
# Time
t = time()
# An optimization variable for the end time
T = var(lb=0,init=4)
# States: position and velocity
p = state(2,init=vertcat([3*sin(2*pi/T.init*t),3*cos(2*pi/T.init*t)]))
v = state(2)
# Control
u = control(2)
Specify the system dynamics as ODE
p.dot = v
v.dot = -10*(p-u)-v*sqrt(sum_square(v)+1)
Set up the path constraints
# Specify some parameters for circular obstacles
# ( position, radius)
#
circles = [ (vertcat([2,2]), 1),
(vertcat([0.5,-2]), 1.5),
]
# List of path constraints
h = []
for center, radius in circles:
h.append(
norm_2(p-center) >= radius # Don't hit the obstacles
)
ocp(T,h+[p.start[0]==0],regularize=[0.1*u/sqrt(2)],N=30,T=T,verbose=True,periodic=True,integration_intervals=2)
(1, 1) ****************************************************************************** This program contains Ipopt, a library for large-scale nonlinear optimization. Ipopt is released as open source code under the Eclipse Public License (EPL). For more information visit http://projects.coin-or.org/Ipopt ****************************************************************************** This is Ipopt version 3.11.9, running with linear solver mumps. NOTE: Other linear solvers might be more efficient (see Ipopt documentation). Number of nonzeros in equality constraint Jacobian...: 969 Number of nonzeros in inequality constraint Jacobian.: 120 Number of nonzeros in Lagrangian Hessian.............: 811 Total number of variables............................: 185 variables with only lower bounds: 1 variables with lower and upper bounds: 0 variables with only upper bounds: 0 Total number of equality constraints.................: 125 Total number of inequality constraints...............: 60 inequality constraints with only lower bounds: 0 inequality constraints with lower and upper bounds: 0 inequality constraints with only upper bounds: 60 iter objective inf_pr inf_du lg(mu) ||d|| lg(rg) alpha_du alpha_pr ls 0 4.0000000e+00 3.28e+00 6.13e-01 -1.0 0.00e+00 - 0.00e+00 0.00e+00 0 1 3.9721767e+00 3.22e+00 5.57e+01 -1.0 5.29e+00 0.0 9.30e-01 1.91e-02h 1 2 4.5743296e+00 1.71e+00 4.11e+01 -1.0 6.38e+00 -0.5 3.17e-01 1.00e+00h 1 3 4.5160620e+00 4.78e-02 2.89e+01 -1.0 1.29e+00 -0.1 5.57e-01 1.00e+00h 1 4 4.5387073e+00 1.18e-02 6.84e+00 -1.0 5.09e-01 -0.5 8.05e-01 1.00e+00f 1 5 4.5008564e+00 2.37e-02 2.60e+00 -1.0 6.10e-01 -1.0 4.87e-01 1.00e+00f 1 6 4.3904478e+00 4.20e-02 8.12e-01 -1.0 8.20e-01 -1.5 6.32e-01 1.00e+00f 1 7 3.8063326e+00 7.62e-02 8.91e-02 -1.7 1.02e+00 -2.0 1.00e+00 1.00e+00h 1 8 3.1482676e+00 1.44e-01 2.28e-02 -2.5 1.53e+00 -2.4 1.00e+00 8.52e-01h 1 9 2.7625842e+00 2.69e-01 1.38e-02 -2.5 3.64e+00 -2.9 9.47e-01 8.03e-01h 1 iter objective inf_pr inf_du lg(mu) ||d|| lg(rg) alpha_du alpha_pr ls 10 2.6035215e+00 4.97e-01 2.95e-02 -2.5 5.41e+00 -3.4 5.34e-01 6.97e-01h 1 11 2.5788066e+00 5.84e-01 4.58e-02 -2.5 7.11e+01 - 4.13e-02 3.11e-02h 1 12 2.5527467e+00 2.96e-01 3.33e-02 -2.5 3.74e+00 -3.9 7.11e-01 1.00e+00h 1 13 2.5597431e+00 9.62e-02 1.67e-02 -2.5 2.31e+00 - 1.00e+00 1.00e+00h 1 14 2.5598411e+00 1.97e-03 7.88e-04 -2.5 4.93e-01 - 1.00e+00 1.00e+00h 1 15 2.5334465e+00 4.00e-02 1.75e-02 -3.8 2.38e+00 - 9.22e-01 5.69e-01h 1 16 2.5176035e+00 3.65e-02 1.16e-02 -3.8 1.31e+00 - 1.00e+00 7.97e-01h 1 17 2.5117268e+00 9.60e-03 1.70e-03 -3.8 6.50e-01 - 1.00e+00 9.45e-01h 1 18 2.5107429e+00 3.37e-04 7.40e-05 -3.8 1.79e-01 - 1.00e+00 1.00e+00h 1 19 2.5090323e+00 6.61e-04 1.69e-03 -5.7 2.13e-01 - 9.95e-01 8.63e-01h 1 iter objective inf_pr inf_du lg(mu) ||d|| lg(rg) alpha_du alpha_pr ls 20 2.5087117e+00 5.08e-05 1.15e-04 -5.7 5.52e-02 - 1.00e+00 9.83e-01h 1 21 2.5086992e+00 5.20e-07 5.68e-07 -5.7 1.21e-02 - 1.00e+00 1.00e+00f 1 22 2.5086749e+00 1.34e-07 3.10e-06 -8.6 4.57e-03 - 1.00e+00 9.94e-01h 1 23 2.5086747e+00 7.71e-11 6.55e-11 -8.6 1.65e-04 - 1.00e+00 1.00e+00h 1 Number of Iterations....: 23 (scaled) (unscaled) Objective...............: 2.5086746746469544e+00 2.5086746746469544e+00 Dual infeasibility......: 6.5457194679976140e-11 6.5457194679976140e-11 Constraint violation....: 7.7104544971007272e-11 7.7104544971007272e-11 Complementarity.........: 2.5613845271550734e-09 2.5613845271550734e-09 Overall NLP error.......: 2.5613845271550734e-09 2.5613845271550734e-09 Number of objective function evaluations = 24 Number of objective gradient evaluations = 24 Number of equality constraint evaluations = 24 Number of inequality constraint evaluations = 24 Number of equality constraint Jacobian evaluations = 24 Number of inequality constraint Jacobian evaluations = 24 Number of Lagrangian Hessian evaluations = 23 Total CPU secs in IPOPT (w/o function evaluations) = 0.064 Total CPU secs in NLP function evaluations = 0.472 EXIT: Optimal Solution Found. proc wall num mean mean time time evals proc time wall time eval_f 0.013 [s] 0.013 [s] 24 0.53 [ms] 0.53 [ms] eval_grad_f 0.016 [s] 0.016 [s] 25 0.65 [ms] 0.65 [ms] eval_g 0.015 [s] 0.015 [s] 24 0.61 [ms] 0.61 [ms] eval_jac_g 0.133 [s] 0.133 [s] 26 5.10 [ms] 5.10 [ms] eval_h 0.324 [s] 0.324 [s] 24 13.50 [ms] 13.49 [ms] main loop 0.545 [s] 0.543 [s]
2.5086746746469544
# Plot the nominal trajectory
plot(value(p[0]),value(p[1]),'o-')
xlabel('x position')
ylabel('y position')
title('time-optimal trajectory')
axis('equal')
# Plot the obstacles
theta = linspace(0,2*pi,1000)
for center, radius in circles:
fill(radius*cos(theta) + center[0],radius*sin(theta) + center[1],'r')