# Path Planning for a Simple Car



The following equations describe a simple model of a car

\begin{align}
\frac{dx}{dt} & = u \cos(\theta) \\
\frac{dy}{dt} & = u \sin(\theta) \\
\frac{d\theta}{dt} & = \frac{u}{L}\tan(\phi) \\
\end{align}

where $x$ and $y$ denote the position of the center of the rear axle, $\theta$ is the angle of the car axis to the horizontal, and $phi$ is the angle of the front steering wheels to the car axis. The length $L$ is the distance from the center of the rear axle to the center of the front axle.

The velocity $u$ is controlled by acceleration of the car, the position of the wheels is controlled by the rate limited steering input $v$.

\begin{align}
\frac{du}{dt} & = a \\
\frac{d\phi}{dt} & = v
\end{align}

The state of the car is determined by the value of the five state variables $x$, $y$, $\theta$, $u$, and $\phi$.

The path planning problem is to find find values of the functions $a(t)$ and $v(t)$ on a time interval $0 \leq t \leq t_f$ to steer the car from an initial condition $\left[x(0), y(0), \theta(0), u(0), \phi(0)\right]$ to a specified final condition $\left[x(t_f), y(t_f), \theta(t_f), u(t_f), \phi(t_f)\right]$ that minimizes an objective function:

\begin{align}
J  = \min \int_0^{t_f} \left( \phi(t)^2 + \alpha a(t)^2 + \beta v(t)^2\right)\,dt
\end{align}

## Pyomo Model


```python
from pyomo.environ import *
from pyomo.dae import *

L = 2
tf = 50

# create a model object
m = ConcreteModel()

# define the independent variable
m.time = ContinuousSet(bounds=(0, tf))

# define control inputs
m.a = Var(m.time)
m.v = Var(m.time, domain=Reals, bounds=(-0.1,0.1))

# define the dependent variables
m.x = Var(m.time)
m.y = Var(m.time)
m.t = Var(m.time)
m.u = Var(m.time)
m.p = Var(m.time, domain=Reals, bounds=(-0.5,0.5))

m.dxdt = DerivativeVar(m.x)
m.dydt = DerivativeVar(m.y)
m.dtdt = DerivativeVar(m.t)
m.dudt = DerivativeVar(m.u)
m.dpdt = DerivativeVar(m.p)

# define the differential equation as a constraint
m.ode_x = Constraint(m.time, rule=lambda m, time: m.dxdt[time] == m.u[time]*cos(m.t[time]))
m.ode_y = Constraint(m.time, rule=lambda m, time: m.dydt[time] == m.u[time]*sin(m.t[time]))
m.ode_t = Constraint(m.time, rule=lambda m, time: m.dtdt[time] == m.u[time]*tan(m.p[time])/L)
m.ode_u = Constraint(m.time, rule=lambda m, time: m.dudt[time] == m.a[time])
m.ode_p = Constraint(m.time, rule=lambda m, time: m.dpdt[time] == m.v[time])

# path constraints
m.path_x1 = Constraint(m.time, rule=lambda m, time: m.x[time] >= 0)
m.path_y1 = Constraint(m.time, rule=lambda m, time: m.y[time] >= 0)

# initial conditions
m.ic = ConstraintList()
m.ic.add(m.x[0]==0)
m.ic.add(m.y[0]==0)
m.ic.add(m.t[0]==0)
m.ic.add(m.u[0]==0)
m.ic.add(m.p[0]==0)

# final conditions
m.fc = ConstraintList()
m.fc.add(m.x[tf]==0)
m.fc.add(m.y[tf]==20)
m.fc.add(m.t[tf]==0)
m.fc.add(m.u[tf]==0)
m.fc.add(m.p[tf]==0)

# define the optimization objective
m.integral = Integral(m.time, wrt=m.time, 
                      rule=lambda m, time: 0.2*m.p[time]**2 + m.a[time]**2 + m.v[time]**2)
m.obj = Objective(expr=m.integral)

# transform and solve
TransformationFactory('dae.collocation').apply_to(m, wrt=m.time, nfe=3, ncp=12, method='BACKWARD')
solver = SolverFactory('ipopt')
solver.options['linear_solver'] = 'MA57'
solver.solve(m, tee=True).write()
```

    Ipopt 3.12.10: linear_solver=MA57
    
    
    ******************************************************************************
    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.12.10, running with linear solver ma57.
    
    Number of nonzeros in equality constraint Jacobian...:     3011
    Number of nonzeros in inequality constraint Jacobian.:       74
    Number of nonzeros in Lagrangian Hessian.............:      222
    
    Total number of variables............................:      444
                         variables with only lower bounds:        0
                    variables with lower and upper bounds:       74
                         variables with only upper bounds:        0
    Total number of equality constraints.................:      375
    Total number of inequality constraints...............:       74
            inequality constraints with only lower bounds:       74
       inequality constraints with lower and upper bounds:        0
            inequality constraints with only upper bounds:        0
    
    iter    objective    inf_pr   inf_du lg(mu)  ||d||  lg(rg) alpha_du alpha_pr  ls
       0  0.0000000e+00 2.00e+01 1.00e+00  -1.0 0.00e+00    -  0.00e+00 0.00e+00   0
       1  2.3944631e-01 3.14e-01 1.70e+06  -1.0 1.99e+01    -  5.00e-04 9.90e-01f  1
       2  2.3944740e-01 3.14e-01 1.70e+06  -1.0 1.81e+04   6.0 5.47e-06 1.07e-08h 10
       3  2.3946389e-01 3.14e-01 1.70e+06  -1.0 1.81e+04   5.5 5.48e-06 4.27e-08h  8
       4  2.3957001e-01 3.14e-01 1.70e+06  -1.0 1.79e+04   5.0 5.85e-06 8.56e-08h  7
       5r 2.3957001e-01 3.14e-01 9.99e+02  -0.5 0.00e+00   5.5 0.00e+00 3.39e-07R  5
       6r 2.4107197e-01 3.17e-01 9.97e+02  -0.5 2.28e+02    -  8.24e-03 2.27e-03f  1
       7r 2.4810995e-01 1.85e+00 9.88e+02  -0.5 1.80e+02    -  2.63e-03 1.07e-02f  1
       8r 2.5615278e-01 2.64e+00 9.75e+02  -0.5 6.43e+01    -  1.94e-02 1.22e-02f  1
       9r 2.7422847e-01 2.86e+00 9.47e+02  -0.5 3.06e+01    -  5.74e-02 2.79e-02f  1
    iter    objective    inf_pr   inf_du lg(mu)  ||d||  lg(rg) alpha_du alpha_pr  ls
      10r 3.1764375e-01 2.85e+00 8.82e+02  -0.5 5.08e+00    -  6.87e-01 6.94e-02f  1
      11r 7.5452012e-01 3.00e+00 2.41e+02  -0.5 2.41e+00    -  1.00e+00 7.27e-01f  1
      12r 7.5887866e-01 3.00e+00 7.50e+00  -0.5 3.50e-02   2.0 9.65e-01 1.00e+00f  1
      13r 7.8728499e-01 3.03e+00 7.17e+00  -1.2 5.74e-02   1.5 8.15e-01 9.05e-01f  1
      14r 8.2720582e-01 3.03e+00 3.13e+00  -1.2 3.71e-02   1.9 1.00e+00 1.00e+00f  1
      15r 8.4724902e-01 3.04e+00 3.40e+00  -1.9 1.46e-02   2.4 1.00e+00 1.00e+00f  1
      16r 8.6660180e-01 3.04e+00 5.53e+01  -1.9 5.27e-02   1.9 5.21e-01 3.21e-01f  1
      17r 8.8510940e-01 3.04e+00 3.59e+00  -1.9 1.71e-02   2.3 1.00e+00 9.62e-01f  1
      18r 9.5689371e-01 3.05e+00 2.05e+01  -1.9 7.73e-02   1.8 6.49e-01 8.48e-01f  1
      19r 9.5993600e-01 3.05e+00 2.60e+01  -1.9 2.77e+00   1.4 1.52e-02 1.80e-03f  1
    iter    objective    inf_pr   inf_du lg(mu)  ||d||  lg(rg) alpha_du alpha_pr  ls
      20r 1.0046251e+00 3.05e+00 8.19e+01  -1.9 1.01e-01   1.8 1.00e+00 6.15e-01f  1
      21r 1.0194045e+00 3.05e+00 1.51e+02  -1.9 1.57e+00   1.3 8.01e-02 1.17e-02f  1
      22r 1.0825627e+00 3.05e+00 6.07e+01  -1.9 1.39e-01   1.7 1.00e+00 5.08e-01f  1
      23r 1.0865506e+00 3.05e+00 7.72e+01  -1.9 4.31e-02   2.2 6.24e-01 1.51e-01f  1
      24r 1.2649305e+00 3.05e+00 1.47e+01  -1.9 1.64e-01   1.7 8.31e-01 9.76e-01f  1
      25r 1.3861553e+00 3.05e+00 9.98e+00  -1.9 7.99e-02   2.1 8.26e-01 7.84e-01f  1
      26r 1.5217519e+00 3.05e+00 1.16e+02  -1.9 8.00e-01   1.6 3.38e-01 6.48e-02f  1
      27r 1.6713449e+00 3.05e+00 5.94e+01  -1.9 1.16e-01   2.1 1.00e+00 4.23e-01f  1
      28r 1.7126657e+00 3.05e+00 2.46e+01  -1.9 4.26e-02   2.5 1.00e+00 3.74e-01f  1
      29r 1.7720112e+00 3.05e+00 9.02e+01  -1.9 1.52e-01   2.0 8.73e-01 1.52e-01f  1
    iter    objective    inf_pr   inf_du lg(mu)  ||d||  lg(rg) alpha_du alpha_pr  ls
      30r 1.8910752e+00 3.05e+00 1.31e+01  -1.9 4.73e-02   2.4 1.00e+00 1.00e+00f  1
      31r 2.0314643e+00 3.05e+00 9.71e+01  -1.9 1.83e-01   2.0 1.00e+00 2.71e-01f  1
      32r 2.0624158e+00 3.05e+00 6.35e+01  -1.9 5.83e-02   2.4 3.18e-01 1.78e-01f  1
      33r 2.2521504e+00 3.05e+00 6.49e+01  -1.9 2.45e-01   1.9 4.47e-01 2.33e-01f  1
      34r 2.3947446e+00 3.05e+00 2.93e+01  -1.9 7.60e-02   2.3 4.77e-01 5.87e-01f  1
      35r 2.7675424e+00 3.05e+00 3.57e+01  -1.9 3.16e-01   1.9 3.76e-01 3.22e-01f  1
      36r 2.9279502e+00 3.05e+00 5.97e+01  -1.9 1.11e-01   2.3 1.00e+00 3.64e-01f  1
      37r 3.0498793e+00 3.05e+00 4.15e+01  -1.9 3.99e-02   2.7 1.00e+00 7.57e-01f  1
      38r 3.1611925e+00 3.05e+00 1.88e+02  -1.9 1.49e-01   2.2 1.00e+00 1.89e-01f  1
      39r 3.3722800e+00 3.05e+00 2.43e+01  -1.9 5.27e-02   2.7 1.00e+00 1.00e+00f  1
    iter    objective    inf_pr   inf_du lg(mu)  ||d||  lg(rg) alpha_du alpha_pr  ls
      40r 3.5376710e+00 3.05e+00 1.45e+02  -1.9 1.90e-01   2.2 1.00e+00 2.10e-01f  1
      41r 3.8133908e+00 3.05e+00 2.69e+01  -1.9 6.57e-02   2.6 1.00e+00 1.00e+00f  1
      42r 4.0978198e+00 3.05e+00 1.88e+02  -1.9 2.42e-01   2.1 1.00e+00 2.66e-01f  1
      43r 4.4143536e+00 3.05e+00 3.00e+01  -1.9 8.29e-02   2.6 1.00e+00 8.49e-01f  1
      44r 4.7007309e+00 3.05e+00 1.13e+02  -1.9 3.08e-01   2.1 5.35e-01 1.98e-01f  1
      45r 4.7912801e+00 3.05e+00 5.89e+01  -1.9 1.03e-01   2.5 1.00e+00 1.86e-01f  1
      46r 5.7813782e+00 3.04e+00 5.17e+01  -1.9 3.64e-01   2.0 2.80e-01 5.40e-01f  1
      47r 6.2653737e+00 3.04e+00 5.56e+01  -1.9 1.30e-01   2.5 1.00e+00 6.86e-01f  1
      48r 6.5241911e+00 3.04e+00 4.84e+01  -1.9 5.65e-01   2.0 8.64e-02 9.21e-02f  1
      49r 6.7481247e+00 3.04e+00 7.05e+01  -1.9 1.75e-01   2.4 6.29e-01 2.60e-01f  1
    iter    objective    inf_pr   inf_du lg(mu)  ||d||  lg(rg) alpha_du alpha_pr  ls
      50r 6.7778153e+00 3.04e+00 1.02e+02  -1.9 7.54e-01   1.9 6.51e-02 9.25e-03f  1
      51r 7.1899470e+00 3.04e+00 6.95e+01  -1.9 2.07e-01   2.4 5.23e-01 4.07e-01f  1
      52r 7.5401885e+00 3.03e+00 4.56e+01  -1.9 7.55e-02   2.8 1.00e+00 9.06e-01f  1
      53r 7.9588459e+00 3.03e+00 4.92e+01  -1.9 2.84e-01   2.3 2.18e-01 3.06e-01f  1
      54r 8.4684734e+00 3.02e+00 5.42e+01  -1.9 1.00e-01   2.7 8.20e-01 1.00e+00f  1
      55r 9.6075613e+00 3.00e+00 3.31e+02  -1.9 4.94e-01   2.3 9.88e-02 4.96e-01f  1
      56r 1.0033493e+01 2.99e+00 7.01e+02  -1.9 2.45e-01   2.7 3.06e-02 2.87e-01f  1
      57r 1.0529050e+01 2.98e+00 4.93e+02  -1.9 9.44e-02   3.1 1.75e-01 1.00e+00f  1
      58r 1.0786193e+01 2.98e+00 4.87e+02  -1.9 4.38e+00   2.6 7.48e-03 2.01e-02f  1
      59r 1.1137189e+01 2.98e+00 2.98e+02  -1.9 2.29e-01   3.1 7.58e-01 3.69e-01f  1
    iter    objective    inf_pr   inf_du lg(mu)  ||d||  lg(rg) alpha_du alpha_pr  ls
      60r 1.1140408e+01 2.98e+00 2.98e+02  -1.9 8.56e+00   2.6 1.05e-03 1.08e-04f  1
      61r 1.1470039e+01 2.97e+00 1.71e+02  -1.9 2.32e-01   3.0 4.84e-01 4.24e-01f  1
      62r 1.1993402e+01 2.96e+00 2.19e+02  -1.9 2.18e+00   2.5 7.17e-02 1.04e-01f  1
      63r 1.2000689e+01 2.96e+00 2.19e+02  -1.9 3.48e-01   3.0 2.67e-01 6.34e-03f  1
      64r 1.2001008e+01 2.96e+00 2.18e+02  -1.9 7.71e-01   2.5 4.62e-03 1.10e-02f  1
      65r 1.2173444e+01 2.96e+00 3.33e+02  -1.9 1.66e-01   2.9 4.35e-02 3.40e-01f  1
      66r 1.2176191e+01 2.96e+00 3.07e+02  -1.9 6.44e-02   3.3 6.17e-01 1.17e-02f  1
      67r 1.2230847e+01 2.95e+00 1.58e+02  -1.9 2.09e-01   2.9 1.04e-02 1.26e-01f  1
      68r 1.2291329e+01 2.95e+00 3.73e+02  -1.9 7.26e-02   3.3 1.77e-01 2.93e-01f  1
      69r 1.2299721e+01 2.95e+00 3.65e+02  -1.9 2.68e-01   2.8 2.26e-02 1.89e-02f  1
    iter    objective    inf_pr   inf_du lg(mu)  ||d||  lg(rg) alpha_du alpha_pr  ls
      70r 1.2519464e+01 2.94e+00 1.64e+02  -1.9 1.10e-01   3.2 1.00e+00 7.79e-01f  1
      71r 1.2521387e+01 2.94e+00 1.30e+02  -1.9 3.09e-01   2.7 2.73e-01 2.78e-03f  1
      72r 1.2667769e+01 2.93e+00 1.05e+02  -1.9 6.94e-01   2.3 1.06e-01 4.70e-02f  1
      73r 1.2683551e+01 2.93e+00 4.21e+02  -1.9 1.54e-01   2.7 1.00e+00 1.64e-02f  1
      74r 1.2895987e+01 2.92e+00 3.86e+02  -1.9 5.76e-01   2.2 1.40e-01 8.69e-02f  1
      75r 1.3442459e+01 2.90e+00 1.98e+02  -1.9 1.79e-01   2.6 1.00e+00 5.10e-01f  1
      76r 1.3461504e+01 2.90e+00 2.03e+02  -1.9 7.41e-01   2.2 9.19e-02 8.76e-03f  1
      77r 1.3674754e+01 2.88e+00 1.56e+02  -1.9 2.19e-01   2.6 9.17e-02 2.18e-01f  1
      78r 1.3968591e+01 2.85e+00 1.31e+02  -1.9 9.20e-01   2.1 5.39e-02 1.17e-01f  1
      79r 1.3997769e+01 2.85e+00 1.60e+02  -1.9 2.56e-01   2.5 9.27e-01 2.50e-02f  1
    iter    objective    inf_pr   inf_du lg(mu)  ||d||  lg(rg) alpha_du alpha_pr  ls
      80r 1.4283667e+01 2.82e+00 1.55e+02  -1.9 9.87e-01   2.1 2.22e-01 1.16e-01f  1
      81r 1.5287094e+01 2.75e+00 9.37e+01  -1.9 3.07e-01   2.5 1.00e+00 7.66e-01f  1
      82r 1.5424386e+01 2.73e+00 9.59e+01  -1.9 1.33e+00   2.0 1.25e-01 5.07e-02f  1
      83r 1.5589962e+01 2.72e+00 1.53e+02  -1.9 3.95e-01   2.4 9.10e-01 1.17e-01f  1
      84r 1.5798251e+01 2.68e+00 2.07e+02  -1.9 1.53e+00   2.0 1.69e-01 8.31e-02f  1
      85r 1.5864673e+01 2.68e+00 2.89e+02  -1.9 4.58e-01   2.4 3.02e-01 4.39e-02f  1
      86r 1.5871499e+01 2.67e+00 5.33e+02  -1.9 1.43e-01   2.8 5.58e-01 1.47e-02f  1
      87r 1.6332858e+01 2.63e+00 1.51e+02  -1.9 4.27e-01   2.3 1.04e-02 3.64e-01f  1
      88r 1.6764829e+01 2.59e+00 2.27e+02  -1.9 1.71e-01   2.8 1.03e-01 7.70e-01f  1
      89r 1.6858590e+01 2.59e+00 1.78e+02  -1.9 5.58e-01   2.3 1.45e-01 5.50e-02f  1
    iter    objective    inf_pr   inf_du lg(mu)  ||d||  lg(rg) alpha_du alpha_pr  ls
      90r 1.6885729e+01 2.58e+00 1.00e+02  -1.9 1.97e-01   2.7 6.93e-01 4.30e-02f  1
      91r 1.7028327e+01 2.57e+00 1.00e+02  -1.9 5.90e-01   2.2 1.86e-02 7.64e-02f  1
      92r 1.7220396e+01 2.55e+00 3.54e+02  -1.9 2.09e-01   2.7 6.66e-01 2.98e-01f  1
      93r 1.7880555e+01 2.49e+00 1.64e+02  -1.9 6.74e-01   2.2 1.25e-01 3.01e-01f  1
      94r 1.8011846e+01 2.48e+00 1.48e+02  -1.9 2.51e-01   2.6 2.44e-01 1.62e-01f  1
      95r 1.8026497e+01 2.48e+00 1.46e+02  -1.9 8.08e-01   2.1 2.52e-02 8.12e-03f  1
      96r 1.8061637e+01 2.48e+00 1.39e+02  -1.9 2.88e-01   2.6 1.80e-01 4.53e-02f  1
      97r 1.8229229e+01 2.43e+00 1.56e+02  -1.9 8.68e-01   2.1 1.85e-02 1.93e-01f  1
      98r 1.8382445e+01 2.43e+00 1.28e+02  -1.9 3.36e-01   2.5 1.41e-01 1.90e-01f  1
      99r 1.8383071e+01 2.43e+00 1.20e+02  -1.9 1.10e+00   2.0 2.75e-02 2.99e-03f  1
    iter    objective    inf_pr   inf_du lg(mu)  ||d||  lg(rg) alpha_du alpha_pr  ls
     100r 1.8417316e+01 2.43e+00 1.12e+02  -1.9 3.68e-01   2.5 3.67e-01 8.44e-02f  1
     101r 1.8385445e+01 2.43e+00 1.06e+02  -1.9 1.43e+00   2.0 2.20e-02 7.74e-02f  1
     102r 1.8543858e+01 2.43e+00 1.08e+02  -1.9 4.29e-01   2.4 3.40e-01 4.57e-01f  1
     103r 1.8594229e+01 2.43e+00 1.08e+02  -1.9 1.63e-01   2.8 7.43e-01 2.12e-01f  1
     104r 1.8789845e+01 2.43e+00 1.11e+02  -1.9 5.15e-01   2.4 1.11e-01 2.85e-01f  1
     105r 1.8790260e+01 2.43e+00 1.11e+02  -1.9 3.64e+00   1.9 1.65e-02 2.38e-04f  1
     106r 1.9058189e+01 2.43e+00 1.13e+02  -1.9 5.80e-01   2.3 6.58e-01 3.24e-01f  1
     107r 1.9114799e+01 2.43e+00 1.13e+02  -1.9 2.07e+00   1.8 2.30e-03 2.10e-02f  1
     108r 1.9467546e+01 2.43e+00 1.16e+02  -1.9 6.76e-01   2.3 2.57e-01 3.47e-01f  1
     109r 1.9729927e+01 2.43e+00 1.18e+02  -1.9 2.39e+00   1.8 1.91e-02 6.68e-02f  1
    iter    objective    inf_pr   inf_du lg(mu)  ||d||  lg(rg) alpha_du alpha_pr  ls
     110r 1.9738007e+01 2.43e+00 1.72e+02  -1.9 7.88e-01   2.2 2.21e-01 7.23e-03f  1
     111r 2.0188345e+01 2.43e+00 1.26e+02  -1.9 2.85e+00   1.7 1.90e-02 9.58e-02f  1
     112r 2.0278748e+01 2.43e+00 2.19e+02  -1.9 9.22e-01   2.2 2.79e-01 6.90e-02f  1
     113r 2.0332047e+01 2.43e+00 3.07e+02  -1.9 3.31e-01   2.6 6.07e-01 1.24e-01f  1
     114r 2.1064536e+01 2.43e+00 1.28e+02  -1.9 1.05e+00   2.1 1.09e-01 5.45e-01f  1
     115r 2.1076998e+01 2.43e+00 1.28e+02  -1.9 3.94e-01   2.5 5.70e-01 2.26e-02f  1
     116r 2.1091284e+01 2.42e+00 1.30e+02  -1.9 1.65e+00   2.0 1.04e-03 3.18e-02f  1
     117r 2.1645070e+01 2.41e+00 1.38e+02  -1.9 4.61e-01   2.5 1.00e+00 1.00e+00f  1
     118r 2.1853550e+01 2.41e+00 1.40e+02  -1.9 1.76e-01   2.9 8.97e-01 9.89e-01f  1
     119r 2.1929536e+01 2.41e+00 1.45e+02  -1.9 5.99e-01   2.4 3.30e-01 2.67e-01f  1
    iter    objective    inf_pr   inf_du lg(mu)  ||d||  lg(rg) alpha_du alpha_pr  ls
     120r 2.2011057e+01 2.40e+00 1.48e+02  -1.9 2.14e-01   2.9 1.72e-01 4.65e-01f  1
     121r 2.1995724e+01 2.40e+00 1.51e+02  -1.9 1.93e+00   2.4 6.89e-02 6.04e-03f  1
     122r 2.2043377e+01 2.40e+00 1.53e+02  -1.9 2.52e-01   2.8 6.61e-02 3.93e-01f  1
     123r 2.2045208e+01 2.40e+00 1.53e+02  -1.9 9.07e-02   3.2 1.00e+00 2.21e-02f  1
     124r 2.2012821e+01 2.39e+00 1.58e+02  -1.9 3.28e-01   2.7 5.25e-02 4.98e-01f  1
     125r 2.2013085e+01 2.39e+00 1.58e+02  -1.9 1.36e-01   3.2 4.72e-01 2.09e-02f  1
     126r 2.1936777e+01 2.39e+00 1.60e+02  -1.9 8.82e-01   2.7 3.52e-03 5.88e-02f  1
     127r 2.1936191e+01 2.39e+00 1.60e+02  -1.9 1.72e-01   3.1 2.62e-01 9.70e-03f  1
     128r 2.1769055e+01 2.38e+00 1.83e+02  -1.9 1.12e+00   2.6 4.35e-02 8.89e-02f  1
     129r 2.1768437e+01 2.38e+00 3.63e+02  -1.9 2.28e-01   3.1 1.00e+00 5.39e-03f  1
    iter    objective    inf_pr   inf_du lg(mu)  ||d||  lg(rg) alpha_du alpha_pr  ls
     130r 2.1769609e+01 2.38e+00 5.52e+02  -1.9 4.39e-01   2.6 3.35e-01 2.45e-02f  1
     131r 2.1639326e+01 2.38e+00 3.53e+02  -1.9 1.81e+00   2.1 7.56e-04 4.90e-02f  1
     132r 2.1628215e+01 2.38e+00 3.16e+02  -1.9 6.10e-01   2.5 5.51e-02 6.88e-02f  1
     133r 2.1631226e+01 2.38e+00 2.53e+02  -1.9 2.16e-01   3.0 1.45e-01 1.66e-01f  1
     134r 2.1571901e+01 2.37e+00 3.55e+02  -1.9 9.18e-01   2.5 6.82e-03 2.59e-01f  1
     135r 2.1576184e+01 2.37e+00 3.02e+02  -1.9 3.00e-01   2.9 1.46e-01 1.41e-01f  1
     136r 2.1573400e+01 2.37e+00 3.00e+02  -1.9 1.48e+00   2.4 2.63e-03 1.93e-03f  1
     137r 2.1500100e+01 2.36e+00 2.50e+02  -1.9 4.00e-01   2.9 2.33e-01 3.00e-01f  1
     138r 2.1195455e+01 2.35e+00 4.21e+02  -1.9 2.09e+00   2.4 1.33e-03 1.43e-01f  1
     139r 2.1194076e+01 2.35e+00 3.32e+02  -1.9 5.72e-01   2.8 2.93e-01 5.22e-03f  1
    iter    objective    inf_pr   inf_du lg(mu)  ||d||  lg(rg) alpha_du alpha_pr  ls
     140r 2.1197582e+01 2.35e+00 4.00e+02  -1.9 1.67e-01   2.3 4.45e-01 5.32e-03f  1
     141r 2.1242747e+01 2.34e+00 3.91e+02  -1.9 8.96e-01   1.9 4.27e-02 2.95e-02f  1
     142r 2.1435316e+01 2.33e+00 4.52e+02  -1.9 2.19e-01   2.3 8.79e-01 2.58e-01f  1
     143r 2.1440677e+01 2.32e+00 4.55e+02  -1.9 1.75e+00   1.8 1.87e-02 3.71e-03f  1
     144r 2.1715426e+01 2.29e+00 2.66e+02  -1.9 2.28e-01   2.2 2.22e-02 3.85e-01f  1
     145r 2.1830875e+01 2.26e+00 2.59e+02  -1.9 1.97e+01   1.8 7.43e-04 4.88e-03f  1
     146r 2.1838825e+01 2.26e+00 2.75e+02  -1.9 2.89e-01   2.2 3.42e-01 7.72e-03f  1
     147r 2.1947904e+01 2.25e+00 2.47e+02  -1.9 1.06e-01   2.6 8.79e-01 2.88e-01f  1
     148r 2.2105525e+01 2.23e+00 2.17e+02  -1.9 3.98e-01   2.1 1.32e-01 1.26e-01f  1
     149r 2.2134956e+01 2.22e+00 3.02e+02  -1.9 1.38e-01   2.6 1.00e+00 6.83e-02f  1
    iter    objective    inf_pr   inf_du lg(mu)  ||d||  lg(rg) alpha_du alpha_pr  ls
     150r 2.2334035e+01 2.19e+00 2.63e+02  -1.9 5.78e-01   2.1 5.09e-02 1.56e-01f  1
     151r 2.2337543e+01 2.19e+00 2.12e+02  -1.9 1.73e-01   2.5 1.00e+00 7.30e-03f  1
     152r 2.2581253e+01 2.16e+00 1.59e+02  -1.9 4.31e-01   2.0 1.46e-01 2.07e-01f  1
     153r 2.2752927e+01 2.14e+00 2.64e+02  -1.9 1.51e-01   2.5 6.54e-01 3.63e-01f  1
     154r 2.3205495e+01 2.07e+00 6.95e+01  -1.9 6.05e-01   2.0 1.20e-02 3.08e-01f  1
     155r 2.3219945e+01 2.06e+00 1.07e+02  -1.9 2.19e-01   2.4 2.28e-01 2.48e-02f  1
     156r 2.3421671e+01 2.01e+00 1.07e+02  -1.9 1.25e+00   1.9 1.49e-03 1.04e-01f  1
     157r 2.4199799e+01 1.88e+00 1.69e+02  -1.9 3.38e-01   2.4 3.44e-01 1.00e+00f  1
     158r 2.4241509e+01 1.86e+00 1.66e+02  -1.9 5.67e+00   1.9 8.51e-03 8.24e-03f  1
     159r 2.4773601e+01 1.73e+00 2.25e+02  -1.9 5.21e-01   2.3 1.14e-01 5.55e-01f  1
    iter    objective    inf_pr   inf_du lg(mu)  ||d||  lg(rg) alpha_du alpha_pr  ls
     160r 2.5145959e+01 1.65e+00 1.05e+02  -1.9 1.97e-01   2.7 7.04e-01 9.49e-01f  1
     161r 2.6211664e+01 1.35e+00 2.30e+02  -1.9 7.19e-01   2.3 1.31e-01 8.62e-01f  1
     162r 2.6745008e+01 1.20e+00 1.33e+02  -1.9 2.79e-01   2.7 1.00e+00 1.00e+00f  1
     163r 2.7198606e+01 1.05e+00 1.36e+02  -1.9 9.05e-01   2.2 1.48e-01 2.98e-01f  1
     164r 2.7277269e+01 1.03e+00 1.37e+02  -1.9 3.28e-01   2.6 4.12e-01 1.28e-01f  1
     165r 2.7284835e+01 1.02e+00 1.37e+02  -1.9 1.00e+00   2.2 2.08e-03 6.43e-03f  1
     166r 2.7465903e+01 9.38e-01 1.35e+02  -1.9 3.43e-01   2.6 1.33e-01 2.72e-01f  1
     167r 2.7294620e+01 8.67e-01 1.36e+02  -1.9 6.01e+00   2.1 2.75e-03 1.69e-02f  1
     168r 2.7300145e+01 8.63e-01 1.36e+02  -1.9 4.07e-01   2.5 3.26e-01 7.96e-03f  1
     169r 2.7294898e+01 8.56e-01 1.36e+02  -1.9 2.21e+00   2.0 3.49e-02 3.48e-03f  1
    iter    objective    inf_pr   inf_du lg(mu)  ||d||  lg(rg) alpha_du alpha_pr  ls
     170r 2.7415792e+01 7.80e-01 1.35e+02  -1.9 4.60e-01   2.5 8.50e-03 1.65e-01f  1
     171r 2.7490404e+01 7.42e-01 1.34e+02  -1.9 1.64e-01   2.9 1.00e+00 2.30e-01f  1
     172r 2.8076067e+01 4.16e-01 1.35e+02  -1.9 5.61e-01   2.4 9.86e-02 5.81e-01f  1
     173r 2.8084890e+01 4.12e-01 1.33e+02  -1.9 2.09e-01   2.8 2.16e-01 1.83e-02f  1
     174r 2.8999650e+01 3.32e-01 3.31e+02  -1.9 6.79e-01   2.4 1.63e-02 6.01e-01f  1
     175r 2.9009728e+01 3.31e-01 1.47e+02  -1.9 2.74e-01   2.8 5.05e-01 1.49e-02f  1
     176r 2.9017103e+01 3.31e-01 1.90e+02  -1.9 6.56e-01   2.3 4.24e-01 8.21e-03f  1
     177r 2.9103410e+01 3.36e-01 1.69e+02  -1.9 2.30e+00   1.8 2.40e-03 3.36e-02f  1
     178r 2.9111063e+01 3.36e-01 2.93e+02  -1.9 6.15e-01   2.3 4.12e-01 8.44e-03f  1
     179r 2.9119692e+01 3.35e-01 3.47e+02  -1.9 7.83e-01   1.8 2.74e-01 4.51e-03f  1
    iter    objective    inf_pr   inf_du lg(mu)  ||d||  lg(rg) alpha_du alpha_pr  ls
     180r 2.9248334e+01 3.24e-01 3.25e+02  -1.9 1.42e-01   2.2 4.17e-01 2.39e-01f  1
     181r 2.9339205e+01 3.17e-01 3.84e+02  -1.9 5.76e-01   1.7 5.00e-01 5.19e-02f  1
     182r 2.9857064e+01 2.77e-01 1.18e+02  -1.9 1.63e-01   2.2 1.00e+00 8.12e-01f  1
     183  2.9536957e+01 2.61e-01 1.25e+01  -1.0 7.73e+00    -  4.93e-02 5.70e-02f  1
     184  2.4784538e+01 2.26e-01 4.15e+01  -1.0 2.34e+00    -  1.47e-01 1.35e-01f  1
     185  2.0960902e+01 1.90e-01 4.48e+02  -1.0 1.85e+00    -  2.51e-01 1.57e-01f  1
     186  1.3880219e+01 1.21e-01 1.57e+02  -1.0 2.38e+00    -  2.80e-01 3.64e-01f  1
     187  9.3255736e+00 1.15e-01 1.85e+03  -1.0 4.18e+00    -  1.21e-01 3.32e-01f  1
     188  5.4145707e+00 1.25e-01 2.89e+03  -1.0 2.88e+00    -  3.93e-01 5.61e-01f  1
     189  3.6426624e+00 1.57e-01 5.62e+03  -1.0 2.27e+00    -  4.21e-01 5.78e-01f  1
    iter    objective    inf_pr   inf_du lg(mu)  ||d||  lg(rg) alpha_du alpha_pr  ls
     190  3.2307772e+00 2.30e-01 1.81e+04  -1.0 4.68e+00    -  1.63e-01 3.86e-01f  1
     191  3.0902394e+00 1.55e-01 6.71e+04  -1.0 3.25e+00    -  1.54e-01 7.36e-01f  1
     192  2.9397007e+00 5.23e-02 6.59e+04  -1.0 1.93e+00    -  6.44e-01 7.73e-01f  1
     193  3.1497024e+00 1.01e-01 7.47e+04  -1.0 1.40e+01    -  2.72e-01 2.91e-01f  1
     194  3.2405928e+00 9.50e-02 6.41e+04  -1.0 1.11e+01    -  4.10e-01 3.50e-01f  1
     195  3.2427972e+00 7.10e-02 7.61e+04  -1.0 8.06e-02   5.0 2.00e-01 2.53e-01h  1
     196  3.2428487e+00 7.06e-02 2.10e+06  -1.0 5.91e-02   4.5 8.02e-01 5.40e-03h  1
     197  3.2658106e+00 2.12e-03 6.37e+02  -1.0 5.90e-02   4.0 1.00e+00 1.00e+00f  1
     198  3.2662774e+00 6.77e-05 9.64e+00  -1.7 2.63e-03   3.6 1.00e+00 1.00e+00h  1
     199  3.2652460e+00 9.40e-07 3.90e+02  -3.8 2.51e-03   3.1 7.65e-01 1.00e+00h  1
    iter    objective    inf_pr   inf_du lg(mu)  ||d||  lg(rg) alpha_du alpha_pr  ls
     200  3.2648669e+00 2.32e-07 2.05e-01  -3.8 5.03e-04   2.6 1.00e+00 1.00e+00h  1
     201  2.7252592e+00 2.18e-02 1.86e-01  -5.7 1.85e+01    -  8.77e-02 9.30e-02f  1
     202  2.7120246e+00 2.52e-04 1.71e+02  -5.7 2.86e-02   2.1 1.52e-03 1.00e+00h  1
     203  2.7088596e+00 6.73e-07 1.19e+00  -5.7 1.80e-03   1.7 9.62e-01 1.00e+00h  1
     204  2.6993179e+00 1.51e-05 2.87e+00  -5.7 1.36e+01    -  2.12e-01 2.47e-03f  1
     205  2.0334704e+00 4.74e-02 6.03e+00  -5.7 1.15e+01    -  5.86e-01 2.66e-01f  1
     206  2.0275664e+00 4.41e-02 8.21e+00  -5.7 7.04e-02   1.2 4.65e-01 6.94e-02h  1
     207  1.9552605e+00 1.45e-03 1.55e+00  -5.7 6.50e-02   0.7 7.20e-01 1.00e+00h  1
     208  1.9274463e+00 6.67e-05 8.03e+00  -5.7 2.84e-02   0.2 4.68e-01 1.00e+00h  1
     209  1.8800274e+00 1.69e-04 3.81e-02  -5.7 6.84e-02  -0.3 1.00e+00 1.00e+00f  1
    iter    objective    inf_pr   inf_du lg(mu)  ||d||  lg(rg) alpha_du alpha_pr  ls
     210  1.8387412e+00 2.63e-04 6.38e+00  -5.7 1.64e-01  -0.7 1.00e+00 5.32e-01f  1
     211  1.7047944e+00 2.69e-03 2.57e-02  -5.7 4.15e-01  -1.2 1.00e+00 1.00e+00f  1
     212  1.6560050e+00 2.28e-03 1.02e+01  -5.7 7.99e-01  -1.7 1.00e+00 2.63e-01h  1
     213  1.5754300e+00 2.42e-03 1.05e+01  -5.7 1.26e+00  -2.2 1.00e+00 3.05e-01f  1
     214  1.4871266e+00 6.78e-03 9.78e+00  -5.7 1.71e+00  -2.6 1.00e+00 2.82e-01f  1
     215  1.3593439e+00 5.60e-02 4.49e+00  -5.7 2.71e+00  -3.1 2.07e-01 4.98e-01f  1
     216  1.2724754e+00 3.92e-02 3.32e+00  -5.7 4.01e+00  -3.6 2.95e-02 5.44e-01h  1
     217  1.2646625e+00 3.79e-02 3.75e+00  -5.7 2.56e+00  -3.2 1.51e-01 4.12e-02h  1
     218  1.2563356e+00 6.29e-02 4.29e+00  -5.7 1.53e+01  -3.6 8.89e-02 7.62e-02h  1
     219  1.2513169e+00 6.23e-02 3.90e+00  -5.7 8.93e+00  -4.1 6.59e-03 1.42e-02h  1
    iter    objective    inf_pr   inf_du lg(mu)  ||d||  lg(rg) alpha_du alpha_pr  ls
     220  1.2388335e+00 7.15e-02 9.71e+00  -5.7 4.11e+00  -3.7 3.03e-02 2.78e-01f  2
     221  1.2156177e+00 7.01e-02 9.49e+00  -5.7 4.78e+00  -3.3 2.76e-02 9.33e-02h  1
     222  1.1844158e+00 2.97e-02 2.32e+01  -5.7 1.21e+00  -3.7 4.42e-02 6.25e-01f  1
     223  1.1778575e+00 2.97e-02 2.36e+01  -5.7 1.48e+01  -3.3 5.04e-06 7.88e-03f  1
     224  1.1737085e+00 2.96e-02 2.51e+01  -5.7 2.43e+01  -2.0 2.40e-03 5.12e-03f  3
     225  1.1322622e+00 2.09e-02 1.64e+01  -5.7 9.96e-01  -2.5 3.11e-01 8.12e-01h  1
     226  1.1191471e+00 1.64e-02 2.38e+01  -5.7 8.53e-01  -2.9 1.00e+00 2.07e-01h  1
     227  1.1048699e+00 2.00e-02 1.59e+01  -5.7 2.87e+00  -3.4 6.32e-01 2.78e-01h  1
     228  1.1008121e+00 2.02e-02 1.59e+01  -5.7 2.32e+01  -3.9 9.67e-02 1.27e-02h  1
     229  9.9183899e-01 1.50e-01 1.77e+01  -5.7 3.35e+00  -3.5 2.09e-02 1.00e+00H  1
    iter    objective    inf_pr   inf_du lg(mu)  ||d||  lg(rg) alpha_du alpha_pr  ls
     230  1.0014766e+00 1.21e-01 1.01e+01  -5.7 2.50e-01  -1.2 9.49e-01 1.92e-01h  1
     231  1.0018598e+00 1.17e-01 9.65e+00  -5.7 1.66e+00    -  6.76e-04 3.23e-02h  1
     232  1.0530709e+00 8.01e-03 1.61e+00  -5.7 4.10e-01  -1.7 6.52e-01 1.00e+00h  1
     233  1.0409443e+00 4.83e-03 8.42e-03  -5.7 3.26e-01  -2.2 1.00e+00 1.00e+00h  1
     234  1.0296045e+00 6.00e-03 6.47e+00  -5.7 5.00e-01  -2.7 5.23e-01 1.00e+00h  1
     235  1.0284511e+00 5.78e-03 1.19e+01  -5.7 1.40e+00  -3.2 9.47e-01 4.23e-02h  1
     236  9.9103227e-01 2.85e-02 1.41e+01  -5.7 3.75e+00    -  7.16e-02 1.00e+00F  1
     237  9.8654556e-01 2.34e-02 1.36e+01  -5.7 2.49e+00  -2.7 1.08e-01 1.79e-01h  1
     238  9.8775477e-01 1.07e-02 2.07e+01  -5.7 1.32e+00    -  2.36e-01 1.00e+00h  1
     239  9.8390913e-01 1.25e-02 1.86e+01  -5.7 4.41e+00    -  1.05e-01 1.10e-01h  3
    iter    objective    inf_pr   inf_du lg(mu)  ||d||  lg(rg) alpha_du alpha_pr  ls
     240  9.7112372e-01 2.10e-03 9.88e+00  -5.7 5.46e-01  -3.2 6.93e-01 1.00e+00h  1
     241  9.6632776e-01 2.86e-03 6.17e-04  -5.7 7.36e-01  -3.7 1.00e+00 1.00e+00h  1
     242  9.6330343e-01 1.13e-03 2.92e-04  -5.7 5.23e-01  -4.2 1.00e+00 1.00e+00h  1
     243  9.6243929e-01 6.99e-05 2.09e-05  -5.7 1.46e-01  -4.6 1.00e+00 1.00e+00h  1
     244  9.6241181e-01 6.93e-07 1.77e-07  -5.7 1.53e-02  -5.1 1.00e+00 1.00e+00h  1
     245  9.6240889e-01 4.24e-08 7.97e+00  -8.6 1.91e-03  -5.6 8.82e-01 9.94e-01h  1
     246  9.6240843e-01 5.00e-09 1.18e-08  -8.6 3.01e-05  -5.2 1.00e+00 1.00e+00f  1
    
    Number of Iterations....: 246
    
                                       (scaled)                 (unscaled)
    Objective...............:   9.6240842970768548e-01    9.6240842970768548e-01
    Dual infeasibility......:   1.1835675377269705e-08    1.1835675377269705e-08
    Constraint violation....:   5.0000000828016719e-09    5.0000000828016719e-09
    Complementarity.........:   1.4841224854874622e-08    1.4841224854874622e-08
    Overall NLP error.......:   5.0000000828016719e-09    1.4841224854874622e-08
    
    
    Number of objective function evaluations             = 290
    Number of objective gradient evaluations             = 71
    Number of equality constraint evaluations            = 290
    Number of inequality constraint evaluations          = 290
    Number of equality constraint Jacobian evaluations   = 248
    Number of inequality constraint Jacobian evaluations = 248
    Number of Lagrangian Hessian evaluations             = 246
    Total CPU secs in IPOPT (w/o function evaluations)   =      0.417
    Total CPU secs in NLP function evaluations           =      0.022
    
    EXIT: Optimal Solution Found.
    # ==========================================================
    # = Solver Results                                         =
    # ==========================================================
    # ----------------------------------------------------------
    #   Problem Information
    # ----------------------------------------------------------
    Problem: 
    - Lower bound: -inf
      Upper bound: inf
      Number of objectives: 1
      Number of constraints: 449
      Number of variables: 444
      Sense: unknown
    # ----------------------------------------------------------
    #   Solver Information
    # ----------------------------------------------------------
    Solver: 
    - Status: ok
      Message: Ipopt 3.12.10\x3a Optimal Solution Found
      Termination condition: optimal
      Id: 0
      Error rc: 0
      Time: 0.5478520393371582
    # ----------------------------------------------------------
    #   Solution Information
    # ----------------------------------------------------------
    Solution: 
    - number of solutions: 0
      number of solutions displayed: 0


## Accessing Solution Data


```python
# access the results
time = [time for time in m.time]

a = [m.a[time]() for time in m.time]
v = [m.v[time]() for time in m.time]

x = [m.x[time]() for time in m.time]
y = [m.y[time]() for time in m.time]
t = [m.t[time]() for time in m.time]
u = [m.u[time]() for time in m.time]
p = [m.p[time]() for time in m.time]
```

## Visualizing Car Path


```python
% matplotlib inline
import numpy as np
import matplotlib.pyplot as plt

scl=0.3

def draw_car(x=0, y=0, theta=0, phi=0):
    R = np.array([[np.cos(theta), -np.sin(theta)], [np.sin(theta), np.cos(theta)]])
    car = np.array([[0.2, 0.5], [-0.2, 0.5], [0, 0.5], [0, -0.5],
                    [0.2, -0.5], [-0.2, -0.5], [0, -0.5], [0, 0], [L, 0], [L, 0.5],
                    [L + 0.2*np.cos(phi), 0.5 + 0.2*np.sin(phi)],
                    [L - 0.2*np.cos(phi), 0.5 - 0.2*np.sin(phi)], [L, 0.5],[L, -0.5],
                    [L + 0.2*np.cos(phi), -0.5 + 0.2*np.sin(phi)],
                    [L - 0.2*np.cos(phi), -0.5 - 0.2*np.sin(phi)]])
    carz= scl*R.dot(car.T)
    plt.plot(x + carz[0], y + carz[1], 'k', lw=2)
    plt.plot(x, y, 'k.', ms=10)
    
plt.figure(figsize=(10,10))
for xs,ys,ts,ps in zip(x,y,t,p):   
    draw_car(xs, ys, ts, scl*ps)
plt.plot(x, y, 'r--', lw=0.8)
plt.axis('square')
```


```python
plt.figure(figsize=(10,8))
plt.subplot(311)
plt.plot(time, a, time, v)
plt.legend(['Acceleration','Steering Input'])

plt.subplot(312)
plt.plot(time, p, time, t)
plt.legend(['Wheel Position','Car Direction'])

plt.subplot(313)
plt.plot(time, u)
plt.legend(['Velocity'])
```


```python

```


```python

```

945dd85885a4137b6353d2652d34159778144601
ipynb
Jupyter Notebook
notebooks/dae/Path_Planning_for_a_Simple_Car.ipynb
gschivley/ND-Pyomo-Cookbook
7bddccd14eb3044e9d3e7c46999e1c9c29541e16
Apache-2.0
notebooks/dae/Path_Planning_for_a_Simple_Car.ipynb
gschivley/ND-Pyomo-Cookbook
7bddccd14eb3044e9d3e7c46999e1c9c29541e16
Apache-2.0
notebooks/dae/Path_Planning_for_a_Simple_Car.ipynb
gschivley/ND-Pyomo-Cookbook
7bddccd14eb3044e9d3e7c46999e1c9c29541e16
Apache-2.0
2018-10-14T16:40:40.000Z
2021-07-20T12:02:29.000Z
Qwen/Qwen-72B
1. YES
2. YES