Time optimal control problem

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]]))


    

In [3]:

    

p.dot = v
v.dot = -10*(p-u)-v*sqrt(sum_square(v)+1)+w


    

In [4]:

    

# 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
  )


    

In [5]:

    

from scipy.stats import norm
gamma = norm.ppf(0.99)
print gamma


    

    




2.32634787404

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)
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]






    
Out[6]:





2.7537471526254755

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)
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')


    

In [ ]: