Double inverted pendulum#

In this Jupyter Notebook we illustrate the example DIP. This example illustrates how to use DAE models in do-mpc.

Open an interactive online Jupyter Notebook with this content on Binder:

Binder

The example consists of the three modules template_model.py, which describes the system model, template_mpc.py, which defines the settings for the control and template_simulator.py, which sets the parameters for the simulator. The modules are used in main.py for the closed-loop execution of the controller.

We start by importing basic modules and do-mpc.

[1]:
import numpy as np
import sys
from casadi import *

# Add do_mpc to path. This is not necessary if it was installed via pip
import os
rel_do_mpc_path = os.path.join('..','..','..')
sys.path.append(rel_do_mpc_path)

# Import do_mpc package:
import do_mpc

Model#

In the following we will present the configuration, setup and connection between these blocks, starting with the model.

In this example we consider the double pendulum on a card as depicted below:

cba60e32c82242d8ae6ca120ee8ea1a2

The system is described in terms of its horizontal position \(x\) and the two angles \(\theta\), where \(\theta_1 = \theta_2 = 0\) denotes the upright position.

We will formulate a continuous dynamic model for this system and start by initiating a do-mpc Model instance:

[2]:
model_type = 'continuous' # either 'discrete' or 'continuous'
model = do_mpc.model.Model(model_type)

Parameters#

The model is configured with the following (certain) parameters:

[3]:
m0 = 0.6  # kg, mass of the cart
m1 = 0.2  # kg, mass of the first rod
m2 = 0.2  # kg, mass of the second rod
L1 = 0.5  # m,  length of the first rod
L2 = 0.5  # m,  length of the second rod

g = 9.80665 # m/s^2, Gravity

We furthermore introduce the following derived parameters to conveniently formulate the model equations below.

[4]:
l1 = L1/2 # m,
l2 = L2/2 # m,
J1 = (m1 * l1**2) / 3   # Inertia
J2 = (m2 * l2**2) / 3   # Inertia

h1 = m0 + m1 + m2
h2 = m1*l1 + m2*L1
h3 = m2*l2
h4 = m1*l1**2 + m2*L1**2 + J1
h5 = m2*l2*L1
h6 = m2*l2**2 + J2
h7 = (m1*l1 + m2*L1) * g
h8 = m2*l2*g

Euler-Lagrangian equations#

The dynamics of the double pendulum can be derived from the Euler-Lagrangian equations, which yield:

\begin{align} h_1\ddot{x}+h_2\ddot{\theta}_1\cos(\theta_1)+h_3\ddot{\theta}_2\cos(\theta_2) &= (h_2\dot{\theta}_1^{2}\sin(\theta_1) + h_3\dot{\theta}_2^{2}\sin(\theta_2) + u)\\ h_2\cos(\theta_1)\ddot{x} + h_4\ddot{\theta}_1 + h_5\cos(\theta_1-\theta_2)\ddot{\theta}_2 &= (h_7\sin(\theta_1) - h_5\dot{\theta}_2^{2}\sin(\theta_1-\theta_2))\\ h_3\cos(\theta_2)\ddot{x} + h_5\cos(\theta_1-\theta_2)\ddot{\theta}_1 + h_6\ddot{\theta}_2 &= (h_5\dot{\theta}_1^{2}\sin(\theta_1-\theta_2) + h_8\sin(\theta_2)) \end{align}

we introduce the states

\[x=[x,\theta_1, \theta_2, \dot{x}, \dot{\theta}_1, \dot{\theta}_2]^T\]

and input:

\[u = f\]

which is the horizontal force applied to the cart.

[5]:
pos = model.set_variable('_x',  'pos')
theta = model.set_variable('_x',  'theta', (2,1))
dpos = model.set_variable('_x',  'dpos')
dtheta = model.set_variable('_x',  'dtheta', (2,1))

u = model.set_variable('_u',  'force')

At this point we have two options. The typical approach would be to rewrite the system as:

\[M(x) \dot x = A(x) x + B u\]

where it can be shown that

\[\det(M) > 0, \forall x\]

such that we can obtain the ODE:

\[\dot x = M(x)^{-1}(A(x)x + B u)\]

do-mpc fully supports this option but it requires some nasty reformulations of the above equations and yields a very complex expression for the ODE.

Instead we suggest …

Differential algebraic equation (DAE)#

We introduce new algebraic states

\[z=[\ddot{x}, \ddot{\theta}_1, \ddot{\theta}_2]^T\]
[6]:
ddpos = model.set_variable('_z', 'ddpos')
ddtheta = model.set_variable('_z', 'ddtheta', (2,1))

which makes it very convenient to formulate the ODE in terms of \(x,u,z\):

\[\dot{x} = [\dot{x}, \dot{\theta}_1, \dot{\theta}_2, \ddot{x}, \ddot{\theta}_1, \ddot{\theta}_2]^T\]
[7]:
model.set_rhs('pos', dpos)
model.set_rhs('theta', dtheta)
model.set_rhs('dpos', ddpos)
model.set_rhs('dtheta', ddtheta)

The only remaining challenge is to implement the relationship between \(x,u\) and \(z\), in the form of:

\[g(x,u,z)=0\]

with do-mpc this is easily achieved:

[8]:
euler_lagrange = vertcat(
        # 1
        h1*ddpos+h2*ddtheta[0]*cos(theta[0])+h3*ddtheta[1]*cos(theta[1])
        - (h2*dtheta[0]**2*sin(theta[0]) + h3*dtheta[1]**2*sin(theta[1]) + u),
        # 2
        h2*cos(theta[0])*ddpos + h4*ddtheta[0] + h5*cos(theta[0]-theta[1])*ddtheta[1]
        - (h7*sin(theta[0]) - h5*dtheta[1]**2*sin(theta[0]-theta[1])),
        # 3
        h3*cos(theta[1])*ddpos + h5*cos(theta[0]-theta[1])*ddtheta[0] + h6*ddtheta[1]
        - (h5*dtheta[0]**2*sin(theta[0]-theta[1]) + h8*sin(theta[1]))
    )

model.set_alg('euler_lagrange', euler_lagrange)

with just a few lines of code we have defined the dynamics of the double pendulum!

Energy equations#

The next step is to introduce new “auxiliary” expressions to do-mpc for the kinetic and potential energy of the system. This is required in this example, because we will need these expressions for the formulation of the MPC controller.

Introducing these expressions has the additional advantage that do-mpc saves and exports the calculated values, which is great for monitoring and debugging.

For the kinetic energy, we have:

\[\begin{split}\begin{aligned} E_{\text{kin,cart}} &= \frac{1}{2} m \dot{x}^{2}\\ E_{\text{kin}, p_1} &= \frac{1}{2} m_1 ( (\dot{x} + l_1 \dot{\theta}_1 \cos(\theta_1))^{2} + (l_1 \dot{\theta}_1 \sin(\theta_1))^{2}) + \frac{1}{2} J_1 \dot{\theta}_1^{2}\\ E_{\text{kin}, p_2} &= \frac{1}{2} m_2 ( (\dot{x} + L_1 \dot{\theta}_1 \cos(\theta_1) + l_2 \dot{\theta}_2 \cos(\theta_2))^{2} \\ &+ (L_1 \dot{\theta}_1 \sin(\theta_1) + l_2 \dot{\theta}_2 \sin(\theta_2))^2) + \frac{1}{2} J_2 \dot{\theta}_1^{2} \end{aligned}\end{split}\]

and for the potential energy:

\[E_{\text{pot}} = m_1 g l_1 \cos(\theta_1) + m_2 g (L_1 \cos(\theta_1) + l_2 \cos(\theta_2))\]

It only remains to formulate the expressions and set them to the model:

[9]:
E_kin_cart = 1 / 2 * m0 * dpos**2
E_kin_p1 = 1 / 2 * m1 * (
    (dpos + l1 * dtheta[0] * cos(theta[0]))**2 +
    (l1 * dtheta[0] * sin(theta[0]))**2) + 1 / 2 * J1 * dtheta[0]**2
E_kin_p2 = 1 / 2 * m2 * (
    (dpos + L1 * dtheta[0] * cos(theta[0]) + l2 * dtheta[1] * cos(theta[1]))**2 +
    (L1 * dtheta[0] * sin(theta[0]) + l2 * dtheta[1] * sin(theta[1]))**
    2) + 1 / 2 * J2 * dtheta[0]**2

E_kin = E_kin_cart + E_kin_p1 + E_kin_p2

E_pot = m1 * g * l1 * cos(
theta[0]) + m2 * g * (L1 * cos(theta[0]) +
                            l2 * cos(theta[1]))

model.set_expression('E_kin', E_kin)
model.set_expression('E_pot', E_pot)
[9]:
SX(((0.490333*cos(theta_0))+(1.96133*((0.5*cos(theta_0))+(0.25*cos(theta_1))))))

Finally, the model setup is completed:

[10]:
# Build the model
model.setup()

Controller#

Next, the controller is configured. First, an instance of the MPC class is generated with the prediction model defined above:

[11]:
mpc = do_mpc.controller.MPC(model)

We choose the prediction horizon n_horizon=100, set the robust horizon n_robust = 0. The time step t_step is set to \(0.04s\) and parameters of the applied discretization scheme orthogonal collocation are as seen below:

[12]:
setup_mpc = {
    'n_horizon': 100,
    'n_robust': 0,
    'open_loop': 0,
    't_step': 0.04,
    'state_discretization': 'collocation',
    'collocation_type': 'radau',
    'collocation_deg': 3,
    'collocation_ni': 1,
    'store_full_solution': True,
    # Use MA27 linear solver in ipopt for faster calculations:
    'nlpsol_opts': {'ipopt.linear_solver': 'mumps'}
}
mpc.set_param(**setup_mpc)

Objective#

The control objective is to errect the double pendulum and to stabilize it in the up-up position. It is not straight-forward to formulate an objective which yields this result. Classical set-point tracking, e.g. with the set-point:

\[\theta_s = [0,0,0]\]

and a quadratic cost:

\[J = \sum_{k=0}^N (\theta-\theta_s)^2\]

is known to work very poorly. Clearly, the problem results from the fact that \(\theta_s = 2\pi n,\ n\in\mathbb{Z}\) is also a valid solution.

Instead we will use an energy-based formulation for the objective. If we think of energy in terms of potential and kinetic energy it is clear that we want to maximize the potential energy (up-up position) and minimize the kinetic energy (stabilization).

Since we have already introduced the expressions for the potential and kinetic energy in the model, we can now simply reuse these expresssions for the fomulation of the objective function, as shown below:

[13]:
mterm = model.aux['E_kin'] - model.aux['E_pot'] # terminal cost
lterm = model.aux['E_kin'] - model.aux['E_pot'] # stage cost

mpc.set_objective(mterm=mterm, lterm=lterm)
# Input force is implicitly restricted through the objective.
mpc.set_rterm(force=0.1)

Constraints#

In the next step, the constraints of the control problem are set. In this case, there is only an upper and lower bounds for the input.

[14]:
mpc.bounds['lower','_u','force'] = -4
mpc.bounds['upper','_u','force'] = 4

We can now setup the controller.

[15]:
mpc.setup()

Estimator#

We assume, that all states can be directly measured (state-feedback):

[16]:
estimator = do_mpc.estimator.StateFeedback(model)

Simulator#

To create a simulator in order to run the MPC in a closed-loop, we create an instance of the do-mpc simulator which is based on the same model:

[29]:
simulator = do_mpc.simulator.Simulator(model)

For the simulation, we use the time step t_step as for the optimizer:

[30]:
params_simulator = {
    # Note: cvode doesn't support DAE systems.
    'integration_tool': 'idas',
    'abstol': 1e-8,
    'reltol': 1e-8,
    't_step': 0.04
}

simulator.set_param(**params_simulator)
[31]:
simulator.setup()

Closed-loop simulation#

For the simulation of the MPC configured for the DIP, we inspect the file main.py. We define the initial state of the system and set for all parts of the closed-loop configuration:

[32]:
simulator.x0['theta'] = 0.99*np.pi

x0 = simulator.x0.cat.full()

mpc.x0 = x0
estimator.x0 = x0

mpc.set_initial_guess()

Note that mpc.set_initial_guess() is very important in this example. Also note that we didn’t set the initial state at exactly \(\pi\) which results in unfavorable numerical issues (it will work however).

Prepare visualization#

For the visualization of the control performance, we first import matplotlib and change some basic settings:

[33]:
import matplotlib.pyplot as plt
plt.ion()
from matplotlib import rcParams
rcParams['text.usetex'] = False
rcParams['axes.grid'] = True
rcParams['lines.linewidth'] = 2.0
rcParams['axes.labelsize'] = 'xx-large'
rcParams['xtick.labelsize'] = 'xx-large'
rcParams['ytick.labelsize'] = 'xx-large'

We use the plotting capabilities, which are included in do-mpc. The mpc_graphics contain information like the current estimated state and the predicted trajectory of the states and inputs based on the solution of the control problem. The sim_graphics contain the information about the simulated evaluation of the system.

[34]:
mpc_graphics = do_mpc.graphics.Graphics(mpc.data)

For the example of the DIP we create a new function which takes as input the states (at a given time \(k\)) and returns the x and y positions of the two bars (the arms of the pendulum).

[35]:
def pendulum_bars(x):
    x = x.flatten()
    # Get the x,y coordinates of the two bars for the given state x.
    line_1_x = np.array([
        x[0],
        x[0]+L1*np.sin(x[1])
    ])

    line_1_y = np.array([
        0,
        L1*np.cos(x[1])
    ])

    line_2_x = np.array([
        line_1_x[1],
        line_1_x[1] + L2*np.sin(x[2])
    ])

    line_2_y = np.array([
        line_1_y[1],
        line_1_y[1] + L2*np.cos(x[2])
    ])

    line_1 = np.stack((line_1_x, line_1_y))
    line_2 = np.stack((line_2_x, line_2_y))

    return line_1, line_2

We then setup a graphic:

[36]:
%%capture

fig = plt.figure(figsize=(16,9))

ax1 = plt.subplot2grid((4, 2), (0, 0), rowspan=4)
ax2 = plt.subplot2grid((4, 2), (0, 1))
ax3 = plt.subplot2grid((4, 2), (1, 1))
ax4 = plt.subplot2grid((4, 2), (2, 1))
ax5 = plt.subplot2grid((4, 2), (3, 1))

ax2.set_ylabel('$E_{kin}$ [J]')
ax3.set_ylabel('$E_{pot}$ [J]')
ax4.set_ylabel('Angle  [rad]')
ax5.set_ylabel('Input force [N]')

# Axis on the right.
for ax in [ax2, ax3, ax4, ax5]:
    ax.yaxis.set_label_position("right")
    ax.yaxis.tick_right()
    if ax != ax5:
        ax.xaxis.set_ticklabels([])

ax5.set_xlabel('time [s]')

mpc_graphics.add_line(var_type='_aux', var_name='E_kin', axis=ax2)
mpc_graphics.add_line(var_type='_aux', var_name='E_pot', axis=ax3)
mpc_graphics.add_line(var_type='_x', var_name='theta', axis=ax4)
mpc_graphics.add_line(var_type='_u', var_name='force', axis=ax5)

ax1.axhline(0,color='black')

bar1 = ax1.plot([],[], '-o', linewidth=5, markersize=10)
bar2 = ax1.plot([],[], '-o', linewidth=5, markersize=10)

ax1.set_xlim(-1.8,1.8)
ax1.set_ylim(-1.2,1.2)
ax1.set_axis_off()

fig.align_ylabels()
fig.tight_layout()

Run open-loop#

Before we test the closed loop case, lets plot one open-loop prediction to check how the resulting graphic looks like.

[25]:
u0 = mpc.make_step(x0)

******************************************************************************
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.3, running with linear solver mumps.
NOTE: Other linear solvers might be more efficient (see Ipopt documentation).

Number of nonzeros in equality constraint Jacobian...:    19406
Number of nonzeros in inequality constraint Jacobian.:        0
Number of nonzeros in Lagrangian Hessian.............:     6814

Total number of variables............................:     4330
                     variables with only lower bounds:        0
                variables with lower and upper bounds:      100
                     variables with only upper bounds:        0
Total number of equality constraints.................:     4206
Total number of inequality constraints...............:        0
        inequality constraints with only lower bounds:        0
   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 1.9799658e+002 4.62e-002 1.00e-003  -1.0 0.00e+000    -  0.00e+000 0.00e+000   0
   1 1.9809206e+002 7.33e-005 3.25e-004  -1.0 1.53e+000  -4.0 1.00e+000 1.00e+000h  1
   2 1.9808344e+002 8.59e-005 1.22e-004  -2.5 4.56e-001  -3.6 1.00e+000 1.00e+000h  1
   3 1.9753270e+002 2.78e+000 5.70e-001  -3.8 2.32e+001  -4.1 6.69e-001 1.00e+000f  1
   4 1.9586898e+002 2.82e-001 3.12e-001  -3.8 1.14e+001  -3.6 3.78e-001 1.00e+000h  1
   5 1.9529674e+002 1.38e+000 1.17e+000  -3.8 5.85e+001  -4.1 4.40e-001 4.20e-001H  1
   6 1.9266815e+002 2.02e-001 3.44e-001  -3.8 9.02e+000  -3.7 1.63e-002 1.00e+000h  1
   7 1.9230231e+002 1.47e-001 2.58e-001  -3.8 2.10e+001  -4.2 9.32e-001 2.44e-001h  1
   8 1.9168746e+002 3.42e-002 2.53e-002  -3.8 6.96e+000  -3.7 1.00e+000 1.00e+000h  1
   9 1.9143721e+002 3.64e-002 1.08e-001  -3.8 2.48e+001  -4.2 1.00e+000 1.82e-001h  1
iter    objective    inf_pr   inf_du lg(mu)  ||d||  lg(rg) alpha_du alpha_pr  ls
  10 1.9079370e+002 3.46e-001 7.82e-001  -3.8 4.32e+002  -4.7 4.15e-001 6.65e-002f  1
  11 1.8908306e+002 4.01e-001 4.09e-001  -3.8 4.52e+001  -4.3 1.00e+000 8.14e-001h  1
  12 1.8756861e+002 2.58e-001 1.53e-001  -3.8 1.60e+001  -3.8 9.86e-001 1.00e+000h  1
  13 1.8672963e+002 1.14e-001 1.17e-001  -3.8 9.14e+000  -3.4 1.00e+000 7.85e-001h  1
  14 1.8599444e+002 2.54e-001 2.29e-001  -3.8 6.29e+001  -3.9 1.00e+000 1.66e-001f  1
  15 1.8497680e+002 2.03e-001 2.37e-001  -3.8 1.23e+001  -3.5 4.49e-001 1.00e+000h  1
  16 1.8431902e+002 1.32e+000 8.76e-001  -3.8 2.62e+002  -3.9 4.75e-001 1.14e-001f  1
  17 1.8419241e+002 1.26e+000 8.40e-001  -3.8 1.56e+002  -4.4 4.12e-001 4.26e-002h  1
  18 1.8355642e+002 1.89e-001 3.51e-001  -3.8 1.42e+001  -4.0 4.32e-001 1.00e+000h  1
  19 1.8340528e+002 8.42e-002 1.34e-001  -3.8 1.80e+001  -3.6 3.85e-001 6.91e-001h  1
iter    objective    inf_pr   inf_du lg(mu)  ||d||  lg(rg) alpha_du alpha_pr  ls
  20 1.8278290e+002 1.71e-001 8.75e-002  -3.8 1.52e+001  -4.0 5.64e-001 1.00e+000h  1
  21 1.8137877e+002 1.19e+000 1.93e-001  -3.8 4.27e+001  -4.5 7.99e-001 1.00e+000h  1
  22 1.8054394e+002 9.66e-001 2.76e-001  -3.8 2.48e+002  -5.0 1.00e+000 1.90e-001h  1
  23 1.7935141e+002 6.67e-001 6.46e-001  -3.8 7.50e+001  -4.6 1.00e+000 1.00e+000h  1
  24 1.7911409e+002 2.49e-002 6.09e-002  -3.8 2.17e+001  -4.1 8.34e-001 1.00e+000h  1
  25 1.7901217e+002 2.49e-002 7.69e-002  -3.8 1.01e+002  -5.1 1.00e+000 7.60e-002h  1
  26 1.7841964e+002 3.14e-001 2.48e-001  -3.8 2.56e+002  -5.6 1.00e+000 2.07e-001f  1
  27 1.7779311e+002 4.29e-001 2.39e-001  -3.8 2.35e+002  -5.1 6.42e-001 1.87e-001h  1
  28 1.7641493e+002 3.35e+000 5.22e+000  -3.8 2.10e+002  -4.7 1.00e+000 8.47e-001h  1
  29 1.7645866e+002 2.41e+000 3.83e+000  -3.8 5.03e+001  -4.3 3.56e-003 3.01e-001h  1
iter    objective    inf_pr   inf_du lg(mu)  ||d||  lg(rg) alpha_du alpha_pr  ls
  30 1.7678347e+002 4.10e-001 2.53e+000  -3.8 5.37e+001  -3.9 6.38e-001 1.00e+000h  1
  31 1.7626872e+002 1.41e-001 1.57e-001  -3.8 3.71e+001  -4.3 1.00e+000 1.00e+000h  1
  32 1.7595595e+002 1.68e-001 1.65e-001  -3.8 1.13e+002  -4.8 4.39e-001 2.67e-001h  1
  33 1.7542093e+002 1.98e-001 9.70e-002  -3.8 5.13e+001  -4.4 1.00e+000 1.00e+000h  1
  34 1.7511185e+002 6.43e-002 1.12e-001  -3.8 3.50e+001  -4.0 1.00e+000 1.00e+000h  1
  35 1.7415559e+002 6.50e-001 1.10e+000  -3.8 1.31e+002  -4.4 8.92e-001 7.73e-001f  1
  36 1.7380288e+002 3.13e-002 1.50e-001  -3.8 3.09e+001  -4.0 7.01e-001 1.00e+000h  1
  37 1.7304848e+002 9.75e-002 3.54e-001  -3.8 5.16e+001  -4.5 8.25e-001 1.00e+000h  1
  38 1.7285605e+002 9.13e-002 3.20e-001  -3.8 1.54e+002  -5.0 2.54e-001 8.40e-002h  1
  39 1.7252950e+002 9.73e-002 3.00e-001  -3.8 3.42e+002  -5.4 2.53e-001 7.95e-002h  1
iter    objective    inf_pr   inf_du lg(mu)  ||d||  lg(rg) alpha_du alpha_pr  ls
  40 1.7200551e+002 1.49e-001 2.55e-001  -3.8 1.47e+002  -5.0 3.78e-001 3.66e-001h  1
  41 1.7157999e+002 1.24e-001 3.12e-001  -3.8 5.67e+001  -4.6 1.00e+000 1.00e+000h  1
  42 1.7119524e+002 3.75e-001 1.99e-001  -3.8 2.21e+002  -5.1 5.87e-001 3.09e-001h  1
  43 1.7097272e+002 3.44e-001 1.89e-001  -3.8 1.65e+002  -4.6 1.00e+000 3.35e-001h  1
  44 1.7078865e+002 8.53e-002 2.85e-001  -3.8 5.44e+001  -4.2 1.00e+000 1.00e+000h  1
  45 1.7024868e+002 4.88e-001 1.27e+000  -3.8 2.24e+002  -4.7 3.93e-001 4.54e-001h  1
  46 1.7001929e+002 1.26e-001 1.96e-001  -3.8 6.59e+001  -4.3 1.00e+000 7.89e-001h  1
  47 1.6968674e+002 1.12e-001 4.08e-001  -3.8 6.88e+001  -4.8 1.00e+000 1.00e+000h  1
  48 1.6906303e+002 4.92e-001 1.04e+000  -3.8 1.39e+002  -5.2 1.00e+000 1.00e+000h  1
  49 1.6886981e+002 1.19e-001 1.54e-001  -3.8 7.22e+001  -4.8 1.00e+000 1.00e+000h  1
iter    objective    inf_pr   inf_du lg(mu)  ||d||  lg(rg) alpha_du alpha_pr  ls
  50 1.6869616e+002 1.07e+000 8.69e-001  -3.8 4.15e+002  -5.3 1.69e-001 1.82e-001h  1
  51 1.6829813e+002 6.56e-001 5.55e-001  -3.8 1.59e+002  -4.9 1.56e-002 3.75e-001h  1
  52 1.6813212e+002 1.78e-001 4.90e-001  -3.8 6.84e+001  -4.4 1.00e+000 1.00e+000h  1
  53 1.6790310e+002 2.83e-001 8.01e-001  -3.8 1.70e+003  -4.9 1.88e-002 2.72e-002h  1
  54 1.6725101e+002 9.59e-001 8.27e-001  -3.8 9.57e+001  -4.5 7.48e-001 1.00e+000h  1
  55 1.6704769e+002 1.91e-001 1.78e-001  -3.8 7.40e+001  -5.0 1.00e+000 1.00e+000h  1
  56 1.6657356e+002 9.58e-001 5.43e-001  -3.8 1.29e+002  -5.4 1.00e+000 6.39e-001h  1
  57 1.6606471e+002 6.17e-001 4.89e-001  -3.8 9.69e+001  -5.0 1.00e+000 1.00e+000h  1
  58 1.6464272e+002 5.07e+000 7.50e+000  -3.8 1.62e+003  -5.5 4.56e-002 1.58e-001f  1
  59 1.6626159e+002 1.68e+000 2.52e+000  -3.8 9.05e+001  -5.1 1.00e+000 8.45e-001h  1
iter    objective    inf_pr   inf_du lg(mu)  ||d||  lg(rg) alpha_du alpha_pr  ls
  60 1.6458662e+002 1.33e+000 3.83e+000  -3.8 2.07e+002  -4.6 5.73e-001 6.34e-001h  1
  61 1.6393555e+002 5.06e-001 1.56e+000  -3.8 1.98e+002  -5.1 1.00e+000 1.00e+000h  1
  62 1.6335159e+002 1.02e+000 1.14e+000  -3.8 2.38e+002  -5.6 1.00e+000 6.25e-001h  1
  63 1.6158405e+002 6.86e+000 8.40e+000  -3.8 2.86e+002  -5.2 1.81e-001 1.00e+000h  1
  64 1.6215958e+002 5.45e+000 6.73e+000  -3.8 9.45e+001  -4.7 1.00e+000 2.15e-001h  1
  65 1.6273466e+002 3.79e+000 4.83e+000  -3.8 9.35e+001  -4.3 1.00e+000 3.27e-001h  1
  66 1.6183355e+002 1.80e+000 2.33e+000  -3.8 1.71e+002  -4.8 7.42e-002 5.37e-001h  1
  67 1.6091649e+002 6.21e-001 1.95e+000  -3.8 1.90e+002  -5.3 2.54e-001 1.00e+000h  1
  68 1.5984939e+002 8.56e+000 5.36e+000  -3.8 5.89e+002  -5.7 3.10e-001 6.94e-001h  1
  69 1.5879235e+002 5.00e+000 2.61e+000  -3.8 3.62e+002  -5.3 3.31e-001 3.85e-001h  1
iter    objective    inf_pr   inf_du lg(mu)  ||d||  lg(rg) alpha_du alpha_pr  ls
  70 1.5878528e+002 1.98e+000 9.23e-001  -3.8 1.22e+002  -4.9 4.57e-001 6.28e-001h  1
  71 1.5737834e+002 1.80e+000 1.53e+000  -3.8 2.68e+002  -5.4 2.19e-001 6.64e-001h  1
  72 1.5708649e+002 1.64e+000 1.44e+000  -3.8 5.33e+002  -4.9 1.50e-002 8.87e-002h  1
  73 1.5683289e+002 5.99e-001 1.52e+000  -3.8 1.38e+002  -4.5 2.34e-001 1.00e+000f  1
  74 1.5669989e+002 3.72e-001 8.79e-001  -3.8 4.34e+001  -4.1 7.25e-001 3.93e-001h  1
  75 1.5604987e+002 5.37e-001 6.23e-001  -3.8 8.31e+001  -4.6 6.88e-001 1.00e+000h  1
  76 1.5588161e+002 4.59e-001 5.45e-001  -3.8 1.32e+002  -5.0 2.47e-001 1.82e-001h  1
  77 1.5563047e+002 4.74e-001 5.73e-001  -3.8 4.92e+002  -5.5 1.16e-001 7.24e-002h  1
  78 1.5539016e+002 4.58e-001 5.75e-001  -3.8 3.16e+002  -5.1 6.28e-002 1.13e-001h  1
  79 1.5521279e+002 3.46e-001 4.57e-001  -3.8 9.52e+001  -4.7 1.00e+000 2.73e-001h  1
iter    objective    inf_pr   inf_du lg(mu)  ||d||  lg(rg) alpha_du alpha_pr  ls
  80 1.5505242e+002 8.15e-002 1.32e-001  -3.8 3.74e+001  -4.2 1.00e+000 1.00e+000h  1
  81 1.5493811e+002 1.01e-001 1.31e-001  -3.8 2.12e+002  -4.7 2.12e-001 7.92e-002h  1
  82 1.5465089e+002 1.74e-001 2.32e-001  -3.8 5.54e+001  -4.3 1.00e+000 7.47e-001h  1
  83 1.5456570e+002 1.77e-001 2.33e-001  -3.8 2.83e+002  -4.8 1.04e-001 3.27e-002h  1
  84 1.5440037e+002 1.44e-001 1.66e-001  -3.8 4.80e+001  -4.3 4.72e-001 3.36e-001h  1
  85 1.5407611e+002 2.49e-001 1.65e-001  -3.8 1.44e+002  -4.8 2.12e-001 2.59e-001h  1
  86 1.5371105e+002 4.04e-001 3.14e-001  -3.8 3.83e+002  -5.3 1.31e-001 1.26e-001h  1
  87 1.5361938e+002 3.69e-001 2.90e-001  -3.8 1.17e+002  -4.9 5.48e-001 1.03e-001h  1
  88 1.5316240e+002 5.61e-001 5.09e-001  -3.8 9.94e+002  -5.3 7.88e-002 6.80e-002f  1
  89 1.5309233e+002 1.53e-001 1.12e-001  -3.8 2.79e+001  -4.0 7.90e-001 7.26e-001h  1
iter    objective    inf_pr   inf_du lg(mu)  ||d||  lg(rg) alpha_du alpha_pr  ls
  90 1.5299821e+002 1.21e-001 8.12e-002  -3.8 5.49e+001  -4.5 1.00e+000 2.44e-001h  1
  91 1.5272112e+002 2.39e-001 2.69e-001  -3.8 2.84e+002  -5.0 4.16e-001 1.41e-001h  1
  92 1.5248948e+002 3.02e-001 3.58e-001  -3.8 1.23e+003  -5.0 1.24e-001 2.23e-002h  1
  93 1.5208923e+002 2.92e-001 1.91e-001  -3.8 7.93e+001  -4.6 1.00e+000 5.35e-001h  1
  94 1.5165593e+002 4.43e-001 3.89e-001  -3.8 1.99e+002  -5.1 3.68e-001 2.49e-001h  1
  95 1.5154571e+002 3.47e-001 3.06e-001  -3.8 5.88e+001  -4.6 1.00e+000 2.29e-001h  1
  96 1.5097015e+002 6.25e-001 5.01e-001  -3.8 2.31e+002  -5.1 4.66e-001 3.02e-001h  1
  97 1.5076115e+002 5.87e-001 4.83e-001  -3.8 1.89e+002  -4.7 4.59e-001 1.31e-001h  1
  98 1.5059995e+002 1.75e-001 2.03e-001  -3.8 3.56e+001  -4.3 1.00e+000 7.24e-001h  1
  99 1.5026190e+002 1.89e-001 1.84e-001  -3.8 7.00e+001  -4.8 8.53e-001 4.10e-001h  1
iter    objective    inf_pr   inf_du lg(mu)  ||d||  lg(rg) alpha_du alpha_pr  ls
 100 1.4889151e+002 4.19e+000 2.64e+000  -3.8 1.02e+003  -5.2 1.64e-001 2.34e-001f  1
 101 1.4886649e+002 4.06e+000 2.57e+000  -3.8 1.05e+002  -4.8 1.00e+000 2.96e-002h  1
 102 1.4893088e+002 2.49e+000 1.77e+000  -3.8 8.55e+001  -4.4 1.00e+000 4.10e-001h  1
 103 1.4854574e+002 1.52e+000 1.09e+000  -3.8 9.81e+001  -4.9 7.78e-001 3.86e-001h  1
 104 1.4839437e+002 7.61e-001 5.56e-001  -3.8 4.35e+001  -4.4 1.00e+000 5.00e-001h  1
 105 1.4812349e+002 6.49e-001 4.73e-001  -3.8 1.79e+002  -4.9 7.38e-001 1.50e-001h  1
 106 1.4779392e+002 5.70e-001 4.12e-001  -3.8 2.16e+002  -5.0 1.63e-001 1.26e-001h  1
 107 1.4739548e+002 2.05e-001 2.93e-001  -3.8 5.17e+001  -4.5 1.00e+000 8.06e-001h  1
 108 1.4721843e+002 2.01e-001 2.78e-001  -3.8 1.22e+002  -5.0 3.97e-001 1.27e-001h  1
 109 1.4708638e+002 2.11e-001 2.89e-001  -3.8 7.01e+002  -5.5 1.16e-001 1.71e-002h  1
iter    objective    inf_pr   inf_du lg(mu)  ||d||  lg(rg) alpha_du alpha_pr  ls
 110 1.4683471e+002 2.09e-001 3.33e-001  -3.8 1.24e+002  -5.1 1.00e+000 1.73e-001h  1
 111 1.4661437e+002 1.32e-001 1.83e-001  -3.8 4.93e+001  -4.6 1.00e+000 4.66e-001h  1
 112 1.4625878e+002 3.44e-001 6.18e-001  -3.8 5.97e+002  -5.1 1.10e-001 7.02e-002h  1
 113 1.4605316e+002 3.23e-001 5.71e-001  -3.8 1.34e+002  -4.7 5.12e-001 1.25e-001h  1
 114 1.4587093e+002 6.48e-002 1.26e-001  -3.8 2.55e+001  -4.3 1.00e+000 8.23e-001h  1
 115 1.4568491e+002 6.21e-002 1.21e-001  -3.8 4.04e+001  -4.7 1.00e+000 3.96e-001h  1
 116 1.4545966e+002 9.02e-002 1.44e-001  -3.8 1.03e+002  -5.2 3.53e-001 2.06e-001h  1
 117 1.4507278e+002 2.02e-001 4.40e-001  -3.8 3.25e+002  -5.7 1.72e-001 1.33e-001h  1
 118 1.4481436e+002 2.14e-001 4.96e-001  -3.8 1.66e+002  -5.3 1.87e-001 1.33e-001h  1
 119 1.4453267e+002 2.14e-001 4.22e-001  -3.8 8.65e+001  -4.8 1.00e+000 3.70e-001h  1
iter    objective    inf_pr   inf_du lg(mu)  ||d||  lg(rg) alpha_du alpha_pr  ls
 120 1.4448491e+002 1.79e-001 3.62e-001  -3.8 3.21e+001  -4.4 1.00e+000 1.71e-001h  1
 121 1.4413536e+002 3.32e-001 1.27e+000  -3.8 1.55e+002  -4.9 7.11e-001 2.79e-001h  1
 122 1.4400210e+002 1.99e-001 7.27e-001  -3.8 3.19e+001  -4.5 1.00e+000 4.03e-001h  1
 123 1.4387560e+002 1.80e-001 6.44e-001  -3.8 8.81e+001  -4.9 7.87e-001 1.16e-001h  1
 124 1.4359493e+002 1.76e-001 5.64e-001  -3.8 3.24e+002  -5.4 1.15e-001 9.49e-002h  1
 125 1.4282377e+002 1.25e+000 1.47e+000  -3.8 1.20e+002  -5.0 1.00e+000 1.00e+000h  1
 126 1.4240341e+002 4.57e-001 6.60e-001  -3.8 7.68e+001  -4.6 5.65e-001 6.41e-001h  1
 127 1.4135154e+002 2.36e+000 1.06e+000  -3.8 3.83e+002  -5.0 3.24e-001 4.13e-001h  1
 128 1.4123980e+002 1.94e+000 8.81e-001  -3.8 6.70e+001  -4.6 6.77e-001 1.77e-001h  1
 129 1.4119646e+002 1.92e+000 8.71e-001  -3.8 2.44e+002  -5.1 3.24e-001 1.17e-002h  1
iter    objective    inf_pr   inf_du lg(mu)  ||d||  lg(rg) alpha_du alpha_pr  ls
 130 1.4108190e+002 1.50e+000 6.79e-001  -3.8 7.45e+001  -4.7 1.00e+000 2.18e-001h  1
 131 1.4028488e+002 8.63e-001 1.55e+000  -3.8 1.95e+002  -5.1 4.36e-001 5.52e-001h  1
 132 1.3989449e+002 2.64e-001 8.00e-001  -3.8 5.86e+001  -4.7 3.48e-001 6.92e-001h  1
 133 1.3967474e+002 2.71e-001 8.57e-001  -3.8 2.11e+002  -5.2 3.55e-001 1.08e-001h  1
 134 1.3950765e+002 2.77e-001 8.95e-001  -3.8 4.32e+002  -5.2 5.27e-002 3.64e-002h  1
 135 1.3937031e+002 2.85e-001 9.27e-001  -3.8 5.39e+003  -5.3 9.99e-003 2.47e-003f  1
 136 1.3914186e+002 3.29e-001 1.03e+000  -3.8 7.31e+001  -4.9 1.00e+000 7.18e-001h  1
 137 1.3899514e+002 8.55e-002 2.01e-001  -3.8 2.77e+001  -4.4 1.00e+000 1.00e+000h  1
 138 1.3894414e+002 8.28e-002 1.98e-001  -3.8 7.17e+001  -4.9 2.73e-001 6.98e-002h  1
 139 1.3885984e+002 9.19e-002 3.08e-001  -3.8 3.16e+002  -5.4 2.05e-001 2.81e-002h  1
iter    objective    inf_pr   inf_du lg(mu)  ||d||  lg(rg) alpha_du alpha_pr  ls
 140 1.3830738e+002 7.17e-001 1.02e+000  -3.8 1.00e+003  -5.9 1.44e-001 1.05e-001f  1
 141 1.3787017e+002 1.21e-001 1.82e-001  -3.8 5.50e+001  -4.5 1.00e+000 1.00e+000h  1
 142 1.3770434e+002 1.57e-001 1.51e-001  -3.8 1.26e+002  -5.0 1.43e-001 2.50e-001h  1
 143 1.3675757e+002 1.01e+002 2.68e+001  -3.8 1.13e+003  -5.5 1.59e-001 1.00e+000f  1
 144 1.3625684e+002 8.57e+001 2.27e+001  -3.8 4.12e+002  -5.1 1.14e-002 1.56e-001h  1
 145 1.3622971e+002 8.45e+001 2.24e+001  -3.8 2.91e+002  -5.6 4.90e-001 1.40e-002h  1
 146 1.3598151e+002 4.31e+001 1.26e+001  -3.8 3.25e+002  -5.1 2.16e-001 5.22e-001h  1
 147 1.3583076e+002 2.97e+001 8.95e+000  -3.8 3.83e+002  -5.6 3.95e-001 3.30e-001h  1
 148 1.3193233e+002 3.22e+001 1.41e+001  -3.8 9.31e+002  -6.1 3.09e-003 1.00e+000h  1
 149 1.2948992e+002 4.62e+001 9.21e+000  -3.8 1.94e+003  -6.6 5.40e-002 5.00e-001h  2
iter    objective    inf_pr   inf_du lg(mu)  ||d||  lg(rg) alpha_du alpha_pr  ls
 150 1.2730554e+002 1.20e+001 1.33e+001  -3.8 1.21e+003  -6.1 1.67e-001 5.00e-001h  2
 151 1.2850471e+002 1.51e+000 1.24e+000  -3.8 2.14e+002  -5.7 1.86e-001 1.00e+000h  1
 152 1.2686679e+002 1.35e+001 6.38e+000  -3.8 4.49e+002  -5.3 1.06e-001 1.00e+000h  1
 153 1.2717669e+002 2.09e+000 2.02e+000  -3.8 1.93e+002  -5.8 1.60e-001 9.23e-001h  1
 154 1.2594944e+002 1.84e+000 9.12e-001  -3.8 6.81e+002  -6.2 1.14e-001 3.18e-001h  1
 155 1.2272685e+002 9.62e+001 1.44e+001  -3.8 3.13e+003  -6.7 5.25e-002 3.42e-001f  2
 156 1.2255668e+002 7.99e+001 1.24e+001  -3.8 3.41e+003  -7.2 6.26e-002 1.38e-001h  1
 157 1.2235994e+002 5.58e+001 8.59e+000  -3.8 5.00e+002  -5.0 2.00e-001 3.11e-001h  1
 158 1.2241173e+002 4.54e+001 6.92e+000  -3.8 3.95e+002  -4.5 1.19e-001 1.91e-001h  1
 159 1.2241971e+002 3.86e+001 5.87e+000  -3.8 4.02e+002  -5.0 2.35e-001 1.53e-001h  1
iter    objective    inf_pr   inf_du lg(mu)  ||d||  lg(rg) alpha_du alpha_pr  ls
 160 1.1987220e+002 3.73e+001 1.12e+001  -3.8 1.77e+003  -5.5 7.81e-002 4.16e-001h  1
 161 1.2012337e+002 3.11e+001 9.28e+000  -3.8 2.91e+002  -6.0 1.96e-001 1.70e-001h  1
 162 1.2012923e+002 2.29e+001 6.95e+000  -3.8 4.44e+002  -6.4 8.16e-002 2.70e-001h  1
 163 1.1925205e+002 9.49e+000 4.42e+000  -3.8 5.85e+002  -6.9 2.30e-001 5.12e-001h  1
 164 1.1843605e+002 8.11e+000 4.02e+000  -3.8 2.00e+003  -5.6 1.05e-002 9.63e-002h  1
 165 1.1828840e+002 7.20e+000 3.59e+000  -3.8 3.13e+002  -5.2 2.41e-001 1.11e-001h  1
 166 1.1687591e+002 4.23e+000 2.58e+000  -3.8 8.21e+002  -5.6 6.72e-002 3.66e-001h  1
 167 1.1695091e+002 3.65e+000 2.23e+000  -3.8 1.55e+002  -5.2 7.70e-001 1.37e-001h  1
 168 1.1403130e+002 1.16e+002 3.35e+001  -3.8 1.19e+003  -5.7 6.03e-002 1.00e+000h  1
 169 1.1396433e+002 1.02e+002 2.96e+001  -3.8 5.11e+002  -5.3 3.76e-001 1.21e-001h  1
iter    objective    inf_pr   inf_du lg(mu)  ||d||  lg(rg) alpha_du alpha_pr  ls
 170 1.1441372e+002 7.54e+001 2.24e+001  -3.8 4.44e+002  -4.8 2.41e-001 2.68e-001h  1
 171 1.1458239e+002 7.13e+001 2.12e+001  -3.8 3.55e+002  -5.3 1.68e-001 5.43e-002h  1
 172 1.1469604e+002 6.88e+001 2.05e+001  -3.8 3.49e+002  -4.9 1.00e+000 3.55e-002h  1
 173 1.1545675e+002 5.43e+001 1.62e+001  -3.8 3.66e+002  -5.4 1.49e-001 2.16e-001h  1
 174 1.1545941e+002 4.66e+001 1.40e+001  -3.8 5.94e+002  -5.8 1.97e-001 1.45e-001h  1
 175 1.1251362e+002 3.14e+001 2.07e+001  -3.8 1.46e+003  -6.3 4.09e-003 5.00e-001h  2
 176 1.0909732e+002 5.24e+001 2.17e+001  -3.8 1.12e+005  -5.9 6.44e-004 9.01e-003f  2
 177 1.0972329e+002 4.39e+001 1.82e+001  -3.8 9.19e+002  -6.4 4.38e-001 1.63e-001h  1
 178 1.0795690e+002 2.82e+001 1.82e+001  -3.8 1.83e+003  -5.9 3.80e-004 1.08e-001H  1
 179 1.0772544e+002 1.76e+001 1.27e+001  -3.8 2.57e+002  -6.4 1.15e-002 3.40e-001h  1
iter    objective    inf_pr   inf_du lg(mu)  ||d||  lg(rg) alpha_du alpha_pr  ls
 180 1.0645927e+002 1.12e+001 1.56e+001  -3.8 1.49e+003  -6.9 6.03e-002 3.56e-001h  1
 181 1.0665505e+002 9.96e+000 1.40e+001  -3.8 7.46e+002  -4.7 1.39e-001 9.82e-002h  1
 182 1.0693419e+002 9.31e+000 1.31e+001  -3.8 2.69e+002  -4.2 1.70e-001 6.48e-002h  1
 183 1.0714176e+002 8.25e+000 1.16e+001  -3.8 5.27e+002  -4.7 1.58e-001 1.09e-001h  1
 184 1.0721437e+002 7.36e+000 1.03e+001  -3.8 3.12e+002  -5.2 1.81e-001 1.08e-001h  1
 185 1.0717031e+002 5.61e+000 7.40e+000  -3.8 3.71e+002  -5.7 2.81e-001 2.47e-001h  1
 186 1.0735766e+002 3.75e+000 4.35e+000  -3.8 2.52e+002  -6.2 1.28e-001 3.71e-001h  1
 187 1.0757405e+002 1.20e+000 1.26e+000  -3.8 2.31e+002  -6.6 2.60e-001 6.51e-001h  1
 188 1.0736537e+002 9.85e-001 9.75e-001  -3.8 3.01e+002  -5.3 2.22e-001 2.26e-001h  1
 189 1.0724804e+002 7.30e-001 6.01e-001  -3.8 1.92e+002  -4.9 3.92e-001 3.98e-001h  1
iter    objective    inf_pr   inf_du lg(mu)  ||d||  lg(rg) alpha_du alpha_pr  ls
 190 1.0725223e+002 5.78e-001 4.76e-001  -3.8 6.48e+001  -4.4 6.14e-001 2.08e-001h  1
 191 1.0646175e+002 4.95e+000 3.32e+000  -3.8 2.29e+002  -4.9 2.10e-001 8.23e-001h  1
 192 1.0636671e+002 4.35e+000 2.92e+000  -3.8 2.25e+002  -5.4 7.82e-001 1.19e-001h  1
 193 1.0642119e+002 3.46e+000 2.36e+000  -3.8 9.29e+001  -5.0 1.00e+000 2.06e-001h  1
 194 1.0599766e+002 2.14e+000 1.32e+000  -3.8 2.99e+002  -5.5 4.28e-001 3.50e-001h  1
 195 1.0590057e+002 6.50e-001 3.91e-001  -3.8 1.11e+002  -5.0 1.00e+000 6.50e-001h  1
 196 1.0587632e+002 2.05e-001 2.85e-001  -3.8 6.32e+001  -4.6 9.49e-001 1.00e+000h  1
 197 1.0530230e+002 2.17e+000 2.34e+000  -3.8 9.23e+002  -5.1 4.48e-002 1.59e-001f  1
 198 1.0521833e+002 1.49e+000 1.61e+000  -3.8 6.20e+001  -4.7 6.11e-001 3.11e-001h  1
 199 1.0508252e+002 1.35e+000 1.43e+000  -3.8 1.89e+002  -5.1 7.04e-002 1.07e-001h  1
iter    objective    inf_pr   inf_du lg(mu)  ||d||  lg(rg) alpha_du alpha_pr  ls
 200 1.0469443e+002 1.32e+000 1.15e+000  -3.8 4.66e+002  -5.6 1.15e-001 1.34e-001h  1
 201 1.0439211e+002 1.33e+000 1.02e+000  -3.8 9.86e+002  -6.1 1.01e-001 4.85e-002h  1
 202 1.0435698e+002 9.98e-001 7.73e-001  -3.8 5.72e+001  -4.8 1.75e-001 2.46e-001h  1
 203 1.0436089e+002 5.06e-001 4.06e-001  -3.8 1.84e+001  -4.3 1.00e+000 4.95e-001h  1
 204 1.0435698e+002 4.47e-003 2.40e-002  -3.8 9.03e+000  -3.9 8.17e-001 1.00e+000h  1
 205 1.0431862e+002 2.52e-002 9.41e-002  -3.8 6.28e+001  -4.4 1.65e-001 1.90e-001h  1
 206 1.0418205e+002 2.21e-002 1.15e-001  -3.8 1.37e+001  -4.0 1.12e-001 4.36e-001h  1
 207 1.0395873e+002 9.32e-003 1.28e-001  -3.8 6.14e+000  -3.5 2.80e-001 8.52e-001f  1
 208 1.0369948e+002 3.04e-002 2.73e-001  -3.8 4.13e+002  -4.0 6.36e-003 1.91e-002f  1
 209 1.0369484e+002 3.04e-002 1.77e-001  -3.8 7.62e+002  -4.5 1.06e-002 9.30e-005h  1
iter    objective    inf_pr   inf_du lg(mu)  ||d||  lg(rg) alpha_du alpha_pr  ls
 210 1.0339571e+002 3.11e-002 2.00e-001  -3.8 3.44e+001  -4.1 1.09e-001 1.22e-001f  1
 211 1.0317165e+002 2.85e-002 1.88e-001  -3.8 6.84e+001  -4.5 1.85e-002 8.92e-002h  1
 212 1.0309052e+002 2.67e-002 1.76e-001  -3.8 2.18e+001  -4.1 9.94e-002 6.59e-002h  1
 213 1.0288887e+002 2.72e-002 1.55e-001  -3.8 8.52e+001  -4.6 6.12e-002 9.10e-002h  1
 214 1.0285420e+002 2.61e-002 1.49e-001  -3.8 1.87e+001  -4.2 1.86e-001 4.04e-002h  1
 215 1.0274139e+002 2.50e-002 1.33e-001  -3.8 7.05e+001  -4.6 1.81e-001 8.06e-002h  1
 216 1.0251850e+002 8.17e-002 3.09e-001  -3.8 1.06e+003  -5.1 4.35e-002 2.27e-002f  1
 217 1.0227868e+002 7.78e-002 2.76e-001  -3.8 5.95e+001  -4.7 9.01e-002 1.96e-001h  1
 218 1.0201351e+002 1.13e-001 3.18e-001  -3.8 2.20e+002  -5.2 7.94e-001 1.43e-001h  1
 219 1.0191683e+002 9.28e-002 2.56e-001  -3.8 5.83e+001  -4.7 3.19e-001 1.85e-001h  1
iter    objective    inf_pr   inf_du lg(mu)  ||d||  lg(rg) alpha_du alpha_pr  ls
 220 1.0145399e+002 7.30e-001 4.70e-001  -3.8 2.79e+002  -5.2 5.67e-001 2.68e-001h  1
 221 1.0063872e+002 2.16e+000 3.07e+000  -3.8 2.50e+002  -4.8 1.00e+000 7.02e-001h  1
 222 1.0044644e+002 5.45e-002 2.13e-001  -3.8 6.24e+001  -4.4 1.00e+000 9.99e-001h  1
 223 1.0031700e+002 5.19e-002 1.56e-001  -3.8 6.24e+001  -4.8 1.00e+000 3.28e-001h  1
 224 9.9718832e+001 5.81e-001 9.56e-001  -3.8 1.17e+002  -5.3 1.00e+000 8.73e-001f  1
 225 9.9108535e+001 1.34e+000 1.31e+000  -3.8 3.83e+002  -5.8 3.36e-001 2.94e-001h  1
 226 9.8093059e+001 4.22e+000 2.81e+000  -3.8 3.09e+002  -5.4 4.56e-001 6.12e-001h  1
 227 9.8315991e+001 2.78e+000 1.82e+000  -3.8 8.62e+001  -4.0 1.00e+000 3.52e-001h  1
 228 9.8022717e+001 3.60e-001 3.72e-001  -3.8 7.60e+001  -4.5 5.34e-001 1.00e+000h  1
 229 9.7355332e+001 1.16e+000 1.01e+000  -3.8 1.39e+002  -5.0 2.48e-001 7.02e-001h  1
iter    objective    inf_pr   inf_du lg(mu)  ||d||  lg(rg) alpha_du alpha_pr  ls
 230 9.6087508e+001 3.68e+000 3.53e+000  -3.8 2.27e+002  -5.5 6.75e-001 1.00e+000h  1
 231 9.6034280e+001 3.44e+000 3.30e+000  -3.8 2.32e+002  -5.0 1.02e-001 6.41e-002h  1
 232 9.6190352e+001 1.27e+000 1.17e+000  -3.8 9.33e+001  -4.6 1.00e+000 6.19e-001h  1
 233 9.5830048e+001 9.12e-001 1.43e+000  -3.8 1.87e+002  -5.1 2.71e-001 5.98e-001h  1
 234 9.5240415e+001 1.28e+000 1.83e+000  -3.8 1.86e+002  -4.7 5.90e-001 4.91e-001h  1
 235 9.4612201e+001 1.33e+000 1.15e+000  -3.8 3.51e+002  -5.1 1.64e-001 2.37e-001h  1
 236 9.3452047e+001 2.76e+000 3.30e+000  -3.8 4.02e+002  -5.6 2.60e-001 4.60e-001h  1
 237 9.2596866e+001 3.10e+000 2.09e+000  -3.8 1.12e+003  -5.2 1.57e-002 1.11e-001h  1
 238 9.1618590e+001 2.88e+000 3.07e+000  -3.8 1.31e+003  -4.8 9.43e-002 9.90e-002h  1
 239 9.1466051e+001 2.59e+000 2.73e+000  -3.8 3.42e+002  -5.2 2.99e-001 9.62e-002h  1
iter    objective    inf_pr   inf_du lg(mu)  ||d||  lg(rg) alpha_du alpha_pr  ls
 240 9.0795018e+001 2.28e+000 1.95e+000  -3.8 1.04e+003  -5.7 2.70e-001 9.61e-002h  1
 241 9.0825122e+001 2.06e+000 1.74e+000  -3.8 1.95e+002  -5.3 1.00e+000 9.43e-002h  1
 242 8.9756097e+001 3.36e+000 2.19e+000  -3.8 2.68e+003  -5.8 4.66e-002 8.14e-002h  1
 243 9.0270793e+001 2.39e+000 1.53e+000  -3.8 8.18e+001  -4.4 7.66e-001 2.97e-001h  1
 244 9.0096434e+001 2.18e+000 1.50e+000  -3.8 3.32e+002  -4.9 3.68e-001 8.43e-002h  1
 245 9.1013646e+001 8.15e-002 3.05e-001  -3.8 5.71e+001  -4.5 1.00e+000 9.51e-001h  1
 246 9.0564951e+001 4.99e-001 1.02e+000  -3.8 9.32e+001  -5.0 8.13e-001 7.68e-001h  1
 247 9.0340773e+001 4.40e-001 9.91e-001  -3.8 1.66e+002  -5.5 2.26e-001 1.99e-001h  1
 248 9.0275084e+001 4.24e-001 9.62e-001  -3.8 2.36e+002  -5.9 2.78e-001 4.32e-002h  1
 249 8.9963644e+001 4.81e-001 1.14e+000  -3.8 3.39e+002  -6.4 1.43e-001 1.54e-001h  1
iter    objective    inf_pr   inf_du lg(mu)  ||d||  lg(rg) alpha_du alpha_pr  ls
 250 8.9476331e+001 1.21e+000 1.45e+000  -3.8 1.24e+003  -6.9 9.72e-002 6.88e-002h  1
 251 8.9407240e+001 5.51e-001 1.58e+000  -3.8 9.15e+001  -4.7 1.00e+000 9.99e-001h  1
 252 8.9378692e+001 9.17e-002 1.53e-001  -3.8 2.69e+001  -4.2 1.00e+000 9.06e-001h  1
 253 8.9074534e+001 3.32e-001 4.21e-001  -3.8 5.33e+001  -4.7 1.00e+000 8.30e-001h  1
 254 8.8956757e+001 3.17e-001 4.23e-001  -3.8 1.53e+002  -5.2 8.86e-001 9.31e-002h  1
 255 8.7941468e+001 1.70e+000 5.09e+000  -3.8 2.73e+002  -5.7 3.62e-001 5.49e-001h  1
 256 8.7620316e+001 1.38e+000 3.62e+000  -3.8 1.78e+002  -5.2 6.63e-001 2.41e-001h  1
 257 8.7670392e+001 8.85e-001 2.17e+000  -3.8 8.62e+001  -4.8 1.00e+000 3.60e-001h  1
 258 8.8061495e+001 3.25e-002 1.95e-001  -3.8 3.08e+001  -4.4 1.00e+000 1.00e+000h  1
 259 8.7997534e+001 5.03e-002 1.70e-001  -3.8 6.68e+001  -4.9 2.49e-001 2.65e-001h  1
iter    objective    inf_pr   inf_du lg(mu)  ||d||  lg(rg) alpha_du alpha_pr  ls
 260 8.7889911e+001 1.48e-001 5.11e-001  -3.8 2.83e+003  -5.3 1.45e-002 1.07e-002f  1
 261 8.7850877e+001 1.51e-001 7.86e-001  -3.8 2.78e+002  -4.9 6.42e-001 2.44e-002h  1
 262 8.7681871e+001 1.56e-001 3.11e-001  -3.8 3.57e+001  -4.5 1.00e+000 7.64e-001h  1
 263 8.7428995e+001 2.80e-001 5.35e-001  -3.8 8.32e+001  -5.0 6.90e-001 4.93e-001h  1
 264 8.7291842e+001 6.23e-002 1.89e-001  -3.8 2.81e+001  -4.5 1.00e+000 1.00e+000h  1
 265 8.7284489e+001 1.04e-002 2.83e-002  -3.8 1.03e+001  -4.1 1.00e+000 1.00e+000h  1
 266 8.7260376e+001 1.83e-003 5.26e-003  -3.8 4.00e+000  -3.7 1.00e+000 1.00e+000h  1
 267 8.7194400e+001 1.09e-001 2.23e-001  -3.8 1.98e+001  -4.2 1.00e+000 8.50e-001h  1
 268 8.6981128e+001 3.61e-002 4.92e-002  -3.8 7.84e+000  -3.7 1.00e+000 1.00e+000h  1
 269 8.6799113e+001 8.82e-002 2.08e-001  -3.8 2.07e+001  -4.2 1.00e+000 1.00e+000h  1
iter    objective    inf_pr   inf_du lg(mu)  ||d||  lg(rg) alpha_du alpha_pr  ls
 270 8.6689873e+001 5.36e-002 1.72e-001  -3.8 2.90e+001  -4.7 1.00e+000 4.52e-001h  1
 271 8.6550756e+001 1.09e-001 2.84e-001  -3.8 5.70e+001  -5.2 1.00e+000 4.06e-001h  1
 272 8.6472602e+001 1.29e-001 3.38e-001  -3.8 2.59e+002  -5.6 2.18e-001 5.32e-002h  1
 273 8.6348190e+001 2.05e-001 5.95e-001  -3.8 1.94e+002  -5.2 4.10e-001 1.44e-001h  1
 274 8.6151228e+001 2.21e-001 8.33e-001  -3.8 7.90e+001  -4.8 8.82e-001 4.08e-001h  1
 275 8.5934213e+001 8.36e-002 5.60e-001  -3.8 3.06e+001  -4.4 1.00e+000 1.00e+000h  1
 276 8.5700421e+001 1.67e-001 9.23e-001  -3.8 1.03e+002  -4.8 1.19e-001 2.49e-001h  1
 277 8.5101673e+001 5.97e-001 1.93e+000  -3.8 3.60e+002  -5.3 5.85e-002 1.73e-001f  1
 278 8.4745154e+001 5.31e-001 1.61e+000  -3.8 1.72e+002  -5.8 4.91e-001 1.67e-001h  1
 279 8.4294451e+001 5.09e-001 2.10e+000  -3.8 2.92e+002  -6.3 1.85e-001 1.60e-001h  1
iter    objective    inf_pr   inf_du lg(mu)  ||d||  lg(rg) alpha_du alpha_pr  ls
 280 8.3895819e+001 6.33e-001 2.27e+000  -3.8 3.31e+002  -5.8 3.99e-001 1.51e-001h  1
 281 8.3902083e+001 6.29e-001 2.26e+000  -3.8 2.38e+001  -4.5 1.00e+000 7.68e-003h  1
 282 8.3870136e+001 4.72e-001 1.49e+000  -3.8 1.13e+002  -5.0 1.00e+000 3.22e-001h  1
 283 8.4183246e+001 2.43e-001 7.66e-001  -3.8 3.69e+001  -4.6 1.00e+000 4.81e-001h  1
 284 8.4608001e+001 3.44e-002 1.11e-001  -3.8 9.09e+000  -4.1 1.00e+000 8.51e-001h  1
 285 8.4615601e+001 3.01e-002 2.86e-001  -3.8 2.41e+001  -4.6 1.00e+000 7.30e-001h  1
 286 8.4588308e+001 9.39e-003 9.90e-002  -3.8 1.02e+001  -4.2 1.00e+000 1.00e+000h  1
 287 8.4457666e+001 1.23e-001 1.27e+000  -3.8 4.57e+001  -4.7 1.00e+000 7.63e-001h  1
 288 8.4330631e+001 1.26e-001 1.25e+000  -3.8 1.19e+002  -5.1 2.43e-001 1.07e-001h  1
 289 8.4156090e+001 1.61e-001 1.46e+000  -3.8 5.44e+002  -5.6 3.12e-001 3.89e-002h  1
iter    objective    inf_pr   inf_du lg(mu)  ||d||  lg(rg) alpha_du alpha_pr  ls
 290 8.4034261e+001 1.61e-001 1.39e+000  -3.8 1.97e+002  -6.1 1.25e-001 7.73e-002h  1
 291 8.3848975e+001 1.77e-001 1.04e+000  -3.8 9.33e+001  -6.6 1.00e+000 3.81e-001h  1
 292 8.3898784e+001 1.13e-001 8.75e-001  -3.8 3.73e+001  -7.1 1.00e+000 1.00e+000h  1
 293 8.4074886e+001 3.43e-002 2.70e-001  -3.8 1.85e+001  -7.5 1.00e+000 1.00e+000h  1
 294 8.4022386e+001 2.77e-003 1.59e-002  -3.8 5.56e+000  -8.0 1.00e+000 1.00e+000h  1
 295 8.4025598e+001 7.08e-007 8.11e-006  -3.8 1.47e-001  -8.5 1.00e+000 1.00e+000h  1
 296 8.4017779e+001 2.89e-005 2.29e-004  -5.7 5.26e-001  -9.0 9.97e-001 1.00e+000f  1
 297 8.4017734e+001 4.90e-009 3.97e-008  -5.7 6.49e-003  -9.4 1.00e+000 1.00e+000h  1
 298 8.4017636e+001 4.70e-009 3.72e-008  -8.6 6.69e-003  -9.9 1.00e+000 1.00e+000h  1
 299 8.4017636e+001 1.43e-014 5.68e-014  -8.6 1.23e-006 -10.4 1.00e+000 1.00e+000h  1

Number of Iterations....: 299

                                   (scaled)                 (unscaled)
Objective...............:  8.4017636352189939e+001   8.4017636352189939e+001
Dual infeasibility......:  5.6843418860808015e-014   5.6843418860808015e-014
Constraint violation....:  1.4311902357677653e-014   1.4311902357677653e-014
Complementarity.........:  2.5059048518614286e-009   2.5059048518614286e-009
Overall NLP error.......:  2.5059048518614286e-009   2.5059048518614286e-009


Number of objective function evaluations             = 315
Number of objective gradient evaluations             = 300
Number of equality constraint evaluations            = 315
Number of inequality constraint evaluations          = 0
Number of equality constraint Jacobian evaluations   = 300
Number of inequality constraint Jacobian evaluations = 0
Number of Lagrangian Hessian evaluations             = 299
Total CPU secs in IPOPT (w/o function evaluations)   =     13.914
Total CPU secs in NLP function evaluations           =      2.054

EXIT: Optimal Solution Found.
           S  :   t_proc      (avg)   t_wall      (avg)    n_eval
       nlp_f  |  75.00ms (238.10us)  75.11ms (238.45us)       315
       nlp_g  | 354.00ms (  1.12ms) 354.22ms (  1.12ms)       315
    nlp_grad  |        0 (       0)        0 (       0)         1
  nlp_grad_f  |  89.00ms (295.68us)  91.04ms (302.46us)       301
  nlp_hess_l  | 816.00ms (  2.73ms) 813.18ms (  2.72ms)       299
   nlp_jac_g  | 691.00ms (  2.30ms) 691.45ms (  2.30ms)       301
       total  |  15.98 s ( 15.98 s)  15.98 s ( 15.98 s)         1

The first optimization will take rather long (4 seconds) but in the end we get:

EXIT: Optimal Solution Found.

which tells us that we found an optimal solution. Note that follow-up optimizations take around 100 ms due to warmstarting.

We can visualize the open-loop prediction with:

[37]:
line1, line2 = pendulum_bars(x0)
bar1[0].set_data(line1[0],line1[1])
bar2[0].set_data(line2[0],line2[1])
mpc_graphics.plot_predictions()
mpc_graphics.reset_axes()

fig
[37]:
../_images/example_gallery_DIP_57_0.png

The open-loop prediction looks perfectly fine! We see that within the horizon the potential energy settles on a plateau greater than zero, while the kinetic energy becomes zero. This indicates our desired up-up position. Both angles seem to reach \(2\pi\).

Run closed-loop#

The closed-loop system is now simulated for 100 steps (and the ouput of the optimizer is suppressed):

[38]:
%%capture
# Quickly reset the history of the MPC data object.
mpc.reset_history()

n_steps = 100
for k in range(n_steps):
    u0 = mpc.make_step(x0)
    y_next = simulator.make_step(u0)
    x0 = estimator.make_step(y_next)

Results#

The next cell converts the results of the closed-loop MPC simulation into a gif (might take a few minutes):

[39]:
from matplotlib.animation import FuncAnimation, FFMpegWriter, ImageMagickWriter

# The function describing the gif:
x_arr = mpc.data['_x']
def update(t_ind):
    line1, line2 = pendulum_bars(x_arr[t_ind])
    bar1[0].set_data(line1[0],line1[1])
    bar2[0].set_data(line2[0],line2[1])
    mpc_graphics.plot_results(t_ind)
    mpc_graphics.plot_predictions(t_ind)
    mpc_graphics.reset_axes()


anim = FuncAnimation(fig, update, frames=n_steps, repeat=False)
gif_writer = ImageMagickWriter(fps=20)
anim.save('anim_dip.gif', writer=gif_writer)

The result is shown below, where solid lines are the recorded trajectories and dashed lines are the predictions of the scenarios:

animdip

Controller with obstacle avoidance#

To make the example even more interesting it is possible to add obstacles and include a set-point tracking task, where the pendulum must be errect at a desired location.

Please note that the animation below now shows the pendulum position (of the cart) as well as the desired setpoint instead of the angles.

animdipobs

The code to create this animation is included in the do-mpc example files and is just an extension of the example shown above.

The necessary changes include the detection of obstacle intersection and an adapted objective function that includes the set-point tracking for the position.