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)


    

In [3]:

    

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


    

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(
     norm_2(p-center) >= radius  # Don't hit the obstacles
  )


    

In [5]:

    

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]






    
Out[5]:





2.5086746746469544

In [6]:

    

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


    

In [ ]: