# Time optimal control problem¶

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.

## Problem setup¶

In [1]:
%matplotlib inline

In [2]:
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

In [3]:
p.dot = v
v.dot = -10*(p-u)-v*sqrt(sum_square(v)+1)+w


Set up the path constraints

In [4]:
# Specify some parameters for circular obstacles
#
circles = [  (vertcat([2,2]),      1),
(vertcat([0.5,-2]), 1.5),
]

# List of path constraints
h = []
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:

In [5]:
from scipy.stats import norm
gamma = norm.ppf(0.99)
print gamma

2.32634787404


## Solve the OCP problem¶

In [6]:
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).
******************************************************************************

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]

Out[6]:
2.7537471526254755

## Plotting of results¶

In [7]:
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)