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)
# Disturbance
w = dist(2,cov=50*DMatrix([[1,0],[0,1]]))
Specify the system dynamics as ODE
p.dot = v
v.dot = -10*(p-u)-v*sqrt(sum_square(v)+1)+w
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(
Prob(
norm_2(p-center) >= radius # Nominally, don't hit the obstacles
) <= 0.99 # Make sure we wont hit it with probability 0.99
)
Probability 0.99 implies that we need to back-off from the path constraint with 2.3 standard deviations:
from scipy.stats import norm
gamma = norm.ppf(0.99)
print gamma
2.32634787404
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) Warning. Slicot plugin not found. You may see degraded performance ****************************************************************************** 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.: 10860 Number of nonzeros in Lagrangian Hessian.............: 0 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.68e-01 0.0 0.00e+00 - 0.00e+00 0.00e+00 0 1 2.1657687e+00 1.84e+00 2.47e+01 -0.1 7.36e+00 - 4.63e-02 3.59e-01f 1 2 2.3648752e+00 1.18e+00 6.63e+01 -0.1 9.94e+00 - 8.54e-01 3.47e-01h 1 3 3.2887282e+00 1.30e+00 1.01e+01 -0.8 6.50e+00 - 7.88e-01 1.00e+00h 1 4 3.3248734e+00 3.16e-02 3.17e-01 -2.5 1.37e+00 - 9.65e-01 1.00e+00h 1 5 3.1257760e+00 3.00e-01 1.87e-01 -3.4 3.35e+00 - 9.81e-01 1.00e+00h 1 6 3.0194001e+00 2.61e-01 1.92e-01 -2.6 3.36e+00 - 7.15e-01 9.38e-01h 1 7 3.0306765e+00 5.00e-01 2.53e-01 -2.4 1.10e+01 - 4.68e-01 4.82e-01h 2 8 2.9124163e+00 1.36e-01 2.60e-01 -2.6 3.93e+00 - 1.00e+00 1.00e+00h 1 9 2.8618526e+00 3.35e-01 2.75e-02 -2.3 3.34e+00 - 9.86e-01 1.00e+00h 1 iter objective inf_pr inf_du lg(mu) ||d|| lg(rg) alpha_du alpha_pr ls 10 2.7931785e+00 7.34e-02 5.62e-02 -2.7 2.33e+00 - 1.00e+00 9.95e-01h 1 11 2.7850355e+00 1.56e-02 3.92e-02 -3.3 1.03e+00 - 1.00e+00 9.98e-01h 1 12 2.7750510e+00 1.78e-02 6.74e-02 -3.3 2.62e+00 - 1.00e+00 9.45e-01H 1 13 2.7793391e+00 7.78e-02 6.32e-02 -2.7 1.19e+01 - 8.08e-01 2.50e-01h 3 14 2.7712775e+00 2.07e-02 3.78e-02 -3.0 1.33e+00 - 1.00e+00 1.00e+00h 1 15 2.7607517e+00 5.45e-03 2.65e-02 -3.4 5.39e-01 - 1.00e+00 9.77e-01h 1 16 2.7601729e+00 1.10e-02 3.60e-02 -3.7 2.69e+00 - 1.00e+00 2.28e-01h 3 17 2.7558985e+00 5.93e-03 2.09e-02 -4.1 7.08e-01 - 1.00e+00 9.97e-01h 1 18 2.7539639e+00 1.94e-03 2.90e-03 -4.7 4.65e-01 - 1.00e+00 9.97e-01h 1 19 2.7538477e+00 1.28e-03 7.15e-03 -5.4 2.48e-01 - 1.00e+00 1.00e+00h 1 iter objective inf_pr inf_du lg(mu) ||d|| lg(rg) alpha_du alpha_pr ls 20 2.7538463e+00 1.55e-04 5.83e-03 -6.0 1.05e-01 - 1.00e+00 1.00e+00h 1 21 2.7537947e+00 3.44e-05 1.29e-02 -6.5 2.87e-01 - 9.97e-01 1.00e+00H 1 22 2.7545118e+00 3.74e-05 8.18e-03 -5.1 4.53e-01 - 1.00e+00 1.00e+00H 1 23 2.7537486e+00 1.50e-03 3.18e-03 -6.3 4.24e-01 - 1.00e+00 8.89e-01h 1 24 2.7537472e+00 2.63e-05 6.98e-04 -7.0 4.85e-02 - 1.00e+00 1.00e+00h 1 25 2.7537481e+00 2.91e-06 3.44e-04 -8.4 1.58e-02 - 1.00e+00 1.00e+00h 1 26 2.7537479e+00 3.46e-07 5.96e-04 -9.1 3.90e-02 - 1.00e+00 1.00e+00H 1 27 2.7537473e+00 6.65e-06 1.02e-04 -10.6 2.00e-02 - 1.00e+00 1.00e+00h 1 28 2.7537472e+00 7.53e-07 1.01e-04 -11.0 1.08e-02 - 1.00e+00 1.00e+00h 1 29 2.7537472e+00 1.23e-07 1.92e-05 -11.0 3.27e-03 - 1.00e+00 1.00e+00h 1 iter objective inf_pr inf_du lg(mu) ||d|| lg(rg) alpha_du alpha_pr ls 30 2.7537472e+00 1.61e-08 3.08e-07 -11.0 1.04e-03 - 1.00e+00 1.00e+00h 1 31 2.7537472e+00 2.38e-09 1.39e-07 -11.0 4.14e-04 - 1.00e+00 1.00e+00h 1 32 2.7537472e+00 3.18e-10 3.83e-08 -11.0 1.54e-04 - 1.00e+00 1.00e+00h 1 33 2.7537472e+00 7.64e-12 8.19e-09 -11.0 3.27e-05 - 1.00e+00 1.00e+00h 1 Number of Iterations....: 33 (scaled) (unscaled) Objective...............: 2.7537471526254755e+00 2.7537471526254755e+00 Dual infeasibility......: 8.1866526092932990e-09 8.1866526092932990e-09 Constraint violation....: 7.6401107662604772e-12 7.6401107662604772e-12 Complementarity.........: 9.9999999999999994e-12 9.9999999999999994e-12 Overall NLP error.......: 8.1866526092932990e-09 8.1866526092932990e-09 Number of objective function evaluations = 53 Number of objective gradient evaluations = 34 Number of equality constraint evaluations = 53 Number of inequality constraint evaluations = 53 Number of equality constraint Jacobian evaluations = 34 Number of inequality constraint Jacobian evaluations = 34 Number of Lagrangian Hessian evaluations = 0 Total CPU secs in IPOPT (w/o function evaluations) = 0.536 Total CPU secs in NLP function evaluations = 12.804 EXIT: Optimal Solution Found. proc wall num mean mean time time evals proc time wall time eval_f 0.395 [s] 0.395 [s] 53 7.45 [ms] 7.44 [ms] eval_grad_f 0.267 [s] 0.267 [s] 35 7.64 [ms] 7.63 [ms] eval_g 0.393 [s] 0.393 [s] 53 7.42 [ms] 7.42 [ms] eval_jac_g 12.081 [s] 12.074 [s] 36 335.58 [ms] 335.39 [ms] main loop 13.349 [s] 13.339 [s]
2.7537471526254755
figure(figsize=(12,12))
# 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')
# Plot covariances
circle = array([[sin(x),cos(x)] for x in linspace(0,2*pi,100)]).T
for pcov, pn in zip(cov(p),value(p)):
w,v = numpy.linalg.eig(pcov)
W = mul(v,diag(sqrt(w)))
e = gamma*mul(W,circle)
plot(e[0,:].T + pn[0],e[1,:].T + pn[1],'r')