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
sys.path.append('../../../')

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

9e093896f00d47ba88d2ecbce513734a

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{align} 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{align}

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:

[14]:
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.

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

We can now setup the controller.

[16]:
mpc.setup()

Estimator

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

[17]:
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:

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

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

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

simulator.set_param(**params_simulator)
[20]:
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:

[21]:
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:

[22]:
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.

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

[24]:
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:

[25]:
%%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.

[26]:
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+02 4.62e-02 1.00e-03  -1.0 0.00e+00    -  0.00e+00 0.00e+00   0
   1  1.9809206e+02 7.33e-05 3.25e-04  -1.0 1.53e+00  -4.0 1.00e+00 1.00e+00h  1
   2  1.9808344e+02 8.59e-05 1.22e-04  -2.5 4.56e-01  -3.6 1.00e+00 1.00e+00h  1
   3  1.9753270e+02 2.78e+00 5.70e-01  -3.8 2.32e+01  -4.1 6.69e-01 1.00e+00f  1
   4  1.9586898e+02 2.82e-01 3.12e-01  -3.8 1.14e+01  -3.6 3.78e-01 1.00e+00h  1
   5  1.9529675e+02 1.38e+00 1.17e+00  -3.8 5.85e+01  -4.1 4.40e-01 4.20e-01H  1
   6  1.9266817e+02 2.02e-01 3.44e-01  -3.8 9.02e+00  -3.7 1.63e-02 1.00e+00h  1
   7  1.9230233e+02 1.47e-01 2.58e-01  -3.8 2.10e+01  -4.2 9.32e-01 2.44e-01h  1
   8  1.9168748e+02 3.42e-02 2.53e-02  -3.8 6.96e+00  -3.7 1.00e+00 1.00e+00h  1
   9  1.9143723e+02 3.64e-02 1.08e-01  -3.8 2.48e+01  -4.2 1.00e+00 1.82e-01h  1
iter    objective    inf_pr   inf_du lg(mu)  ||d||  lg(rg) alpha_du alpha_pr  ls
  10  1.9079375e+02 3.46e-01 7.82e-01  -3.8 4.32e+02  -4.7 4.15e-01 6.65e-02f  1
  11  1.8908314e+02 4.01e-01 4.09e-01  -3.8 4.52e+01  -4.3 1.00e+00 8.14e-01h  1
  12  1.8756874e+02 2.58e-01 1.53e-01  -3.8 1.60e+01  -3.8 9.86e-01 1.00e+00h  1
  13  1.8672974e+02 1.14e-01 1.17e-01  -3.8 9.14e+00  -3.4 1.00e+00 7.85e-01h  1
  14  1.8599478e+02 2.54e-01 2.29e-01  -3.8 6.29e+01  -3.9 1.00e+00 1.66e-01f  1
  15  1.8497700e+02 2.04e-01 2.37e-01  -3.8 1.23e+01  -3.5 4.49e-01 1.00e+00h  1
  16  1.8432030e+02 1.32e+00 8.74e-01  -3.8 2.65e+02  -3.9 4.70e-01 1.13e-01f  1
  17  1.8419900e+02 1.26e+00 8.39e-01  -3.8 1.56e+02  -4.4 4.11e-01 4.08e-02h  1
  18  1.8356256e+02 1.91e-01 3.51e-01  -3.8 1.42e+01  -4.0 4.33e-01 1.00e+00h  1
  19  1.8341217e+02 8.79e-02 1.36e-01  -3.8 1.80e+01  -3.6 3.87e-01 6.81e-01h  1
iter    objective    inf_pr   inf_du lg(mu)  ||d||  lg(rg) alpha_du alpha_pr  ls
  20  1.8278812e+02 1.76e-01 8.50e-02  -3.8 1.52e+01  -4.0 5.64e-01 1.00e+00h  1
  21  1.8138293e+02 1.20e+00 1.92e-01  -3.8 4.27e+01  -4.5 7.99e-01 1.00e+00h  1
  22  1.8054638e+02 9.70e-01 2.77e-01  -3.8 2.47e+02  -5.0 1.00e+00 1.90e-01h  1
  23  1.7935640e+02 6.65e-01 6.44e-01  -3.8 7.49e+01  -4.6 1.00e+00 1.00e+00h  1
  24  1.7911838e+02 2.49e-02 6.02e-02  -3.8 2.17e+01  -4.1 8.37e-01 1.00e+00h  1
  25  1.7853437e+02 1.68e-01 8.35e-02  -3.8 3.72e+01  -4.6 1.00e+00 1.00e+00h  1
  26  1.7811649e+02 2.22e-01 1.28e-01  -3.8 1.79e+02  -5.1 2.68e-01 1.72e-01h  1
  27  1.7728888e+02 7.04e-01 7.44e-01  -3.8 8.19e+01  -4.7 1.00e+00 1.00e+00h  1
  28  1.7707269e+02 2.61e-02 7.21e-02  -3.8 2.92e+01  -4.2 7.54e-01 1.00e+00h  1
  29  1.7675347e+02 7.15e-02 7.34e-02  -3.8 5.09e+01  -4.7 1.00e+00 5.45e-01h  1
iter    objective    inf_pr   inf_du lg(mu)  ||d||  lg(rg) alpha_du alpha_pr  ls
  30  1.7656004e+02 8.96e-02 1.00e-01  -3.8 3.03e+02  -5.2 6.98e-02 6.52e-02h  1
  31  1.7563497e+02 5.41e-01 5.17e-01  -3.8 9.26e+01  -4.8 1.00e+00 1.00e+00h  1
  32  1.7525047e+02 3.61e-01 2.32e-01  -3.8 9.44e+01  -4.3 1.00e+00 4.12e-01h  1
  33  1.7495933e+02 3.41e-02 1.10e-01  -3.8 3.39e+01  -3.9 1.00e+00 1.00e+00h  1
  34  1.7432319e+02 2.26e-01 2.67e-01  -3.8 9.19e+01  -4.4 1.00e+00 6.21e-01h  1
  35  1.7390163e+02 3.10e-02 2.47e-01  -3.8 2.92e+01  -4.0 7.87e-01 1.00e+00h  1
  36  1.7313607e+02 1.12e-01 2.71e-01  -3.8 5.51e+01  -4.4 9.59e-01 1.00e+00h  1
  37  1.7299030e+02 1.06e-01 2.56e-01  -3.8 1.38e+02  -4.9 2.75e-01 6.47e-02h  1
  38  1.7267489e+02 1.08e-01 2.52e-01  -3.8 3.03e+02  -5.4 2.44e-01 7.04e-02h  1
  39  1.7196110e+02 1.73e-01 2.89e-01  -3.8 1.32e+02  -5.0 2.15e-01 4.92e-01h  1
iter    objective    inf_pr   inf_du lg(mu)  ||d||  lg(rg) alpha_du alpha_pr  ls
  40  1.7170067e+02 8.75e-02 1.86e-01  -3.8 5.09e+01  -4.5 1.00e+00 6.14e-01h  1
  41  1.7115391e+02 5.65e-01 4.01e-01  -3.8 1.83e+02  -5.0 5.43e-01 5.38e-01h  1
  42  1.7100405e+02 4.56e-01 2.74e-01  -3.8 1.60e+02  -4.6 6.76e-01 2.38e-01h  1
  43  1.7087350e+02 5.18e-02 2.11e-01  -3.8 4.92e+01  -4.2 1.00e+00 1.00e+00h  1
  44  1.7030581e+02 5.44e-01 9.89e-01  -3.8 1.17e+02  -4.6 7.19e-01 9.99e-01h  1
  45  1.7005616e+02 8.90e-02 2.77e-01  -3.8 6.20e+01  -4.2 1.00e+00 9.02e-01h  1
  46  1.6976868e+02 9.06e-02 3.76e-01  -3.8 5.71e+01  -4.7 1.00e+00 1.00e+00h  1
  47  1.6949702e+02 1.31e-01 4.04e-01  -3.8 1.38e+02  -5.2 1.00e+00 4.11e-01h  1
  48  1.6825821e+02 3.02e+00 3.39e+00  -3.8 2.78e+02  -5.7 1.00e+00 1.00e+00h  1
  49  1.6835630e+02 2.53e+00 2.93e+00  -3.8 1.17e+02  -4.3 1.00e+00 1.68e-01h  1
iter    objective    inf_pr   inf_du lg(mu)  ||d||  lg(rg) alpha_du alpha_pr  ls
  50  1.6831590e+02 1.03e-01 4.54e-01  -3.8 1.47e+02  -4.8 1.00e+00 1.00e+00h  1
  51  1.6813870e+02 1.20e-01 5.22e-01  -3.8 2.67e+02  -5.3 9.77e-01 1.42e-01h  1
  52  1.6683410e+02 3.09e+00 7.38e+00  -3.8 2.59e+02  -4.9 1.00e+00 1.00e+00f  1
  53  1.6767930e+02 8.18e-01 4.75e+00  -3.8 1.01e+02  -4.4 1.00e+00 1.00e+00h  1
  54  1.6658953e+02 2.92e-01 7.99e-01  -3.8 1.41e+02  -4.9 3.94e-01 1.00e+00h  1
  55  1.6611363e+02 5.81e-01 7.49e-01  -3.8 1.67e+02  -5.4 6.53e-01 7.96e-01h  1
  56  1.6568816e+02 4.98e-01 3.06e-01  -3.8 1.11e+02  -5.0 6.60e-01 1.00e+00h  1
  57  1.6563425e+02 4.04e-01 2.37e-01  -3.8 8.71e+01  -4.5 1.00e+00 2.05e-01h  1
  58  1.6509801e+02 9.33e-01 1.38e+00  -3.8 3.55e+02  -5.0 9.47e-01 3.49e-01h  1
  59  1.6331635e+02 6.14e+00 2.03e+01  -3.8 6.58e+02  -4.6 7.55e-01 4.61e-01h  1
iter    objective    inf_pr   inf_du lg(mu)  ||d||  lg(rg) alpha_du alpha_pr  ls
  60  1.6332304e+02 6.11e+00 2.02e+01  -3.8 1.62e+02  -5.1 7.54e-02 4.62e-03h  1
  61  1.6433321e+02 3.00e+00 1.10e+01  -3.8 8.37e+01  -4.6 1.00e+00 6.15e-01h  1
  62  1.6491421e+02 9.96e-01 5.14e+00  -3.8 1.20e+02  -5.1 1.92e-01 1.00e+00h  1
  63  1.6190117e+02 1.05e+01 1.07e+01  -3.8 2.91e+03  -5.6 7.46e-02 1.38e-01f  1
  64  1.6192596e+02 1.04e+01 1.06e+01  -3.8 1.23e+02  -5.2 3.81e-01 1.32e-02h  1
  65  1.6425769e+02 1.91e+00 1.36e+00  -3.8 9.76e+01  -4.7 2.60e-01 1.00e+00h  1
  66  1.6293148e+02 7.12e-01 1.33e+00  -3.8 2.08e+02  -5.2 3.63e-01 5.55e-01h  1
  67  1.6206843e+02 7.95e-01 1.90e+00  -3.8 1.86e+02  -4.8 5.50e-01 1.00e+00h  1
  68  1.6177105e+02 4.34e-01 1.27e+00  -3.8 9.70e+01  -4.4 1.00e+00 6.91e-01h  1
  69  1.6040557e+02 2.79e+00 3.27e+00  -3.8 6.57e+02  -4.8 2.32e-01 2.08e-01f  1
iter    objective    inf_pr   inf_du lg(mu)  ||d||  lg(rg) alpha_du alpha_pr  ls
  70  1.6003795e+02 2.21e+00 2.40e+00  -3.8 4.01e+02  -5.3 6.37e-01 2.03e-01h  1
  71  1.6019840e+02 1.06e-01 1.24e+00  -3.8 8.54e+01  -4.9 1.00e+00 1.00e+00h  1
  72  1.5957075e+02 5.90e-01 7.94e-01  -3.8 4.21e+02  -5.4 2.99e-01 2.24e-01h  1
  73  1.5928692e+02 6.84e-01 7.55e-01  -3.8 1.58e+03  -4.9 1.02e-01 3.01e-02h  1
  74  1.5861499e+02 7.20e-01 2.35e+00  -3.8 1.85e+02  -4.5 1.00e+00 6.65e-01h  1
  75  1.5856253e+02 3.99e-01 1.24e+00  -3.8 4.73e+01  -4.1 1.00e+00 4.42e-01h  1
  76  1.5799508e+02 2.85e-01 6.18e-01  -3.8 8.43e+01  -4.6 1.00e+00 1.00e+00h  1
  77  1.5797097e+02 6.26e-03 2.33e-02  -3.8 2.28e+01  -4.1 1.00e+00 1.00e+00h  1
  78  1.5778892e+02 2.41e-02 2.28e-02  -3.8 9.83e+01  -5.1 2.84e-01 2.34e-01h  1
  79  1.5634981e+02 2.67e+00 1.45e+00  -3.8 2.04e+02  -5.6 6.16e-02 1.00e+00f  1
iter    objective    inf_pr   inf_du lg(mu)  ||d||  lg(rg) alpha_du alpha_pr  ls
  80  1.5646425e+02 2.08e-01 9.77e-01  -3.8 9.08e+01  -4.2 4.87e-01 1.00e+00h  1
  81  1.5628636e+02 1.53e-01 7.60e-01  -3.8 4.83e+01  -4.7 5.04e-01 2.71e-01h  1
  82  1.5524790e+02 6.66e-01 7.25e-01  -3.8 1.16e+02  -5.2 2.51e-01 8.23e-01h  1
  83  1.5485177e+02 6.30e-01 7.14e-01  -3.8 2.70e+02  -4.8 1.79e-01 1.06e-01h  1
  84  1.5379176e+02 1.35e+00 7.63e-01  -3.8 3.27e+03  -5.2 1.09e-02 2.80e-02f  1
  85  1.5361813e+02 8.28e-01 4.67e-01  -3.8 8.92e+01  -4.8 2.64e-01 3.83e-01h  1
  86  1.5232805e+02 1.28e+00 7.57e-01  -3.8 2.97e+02  -5.3 3.97e-01 4.44e-01h  1
  87  1.5040335e+02 3.35e+00 4.37e+00  -3.8 7.97e+02  -4.9 2.93e-02 2.42e-01h  1
  88  1.5094804e+02 1.92e+00 2.68e+00  -3.8 4.92e+01  -4.4 1.00e+00 4.59e-01h  1
  89  1.4949280e+02 1.27e+00 2.15e+00  -3.8 1.94e+02  -4.9 2.40e-01 9.64e-01h  1
iter    objective    inf_pr   inf_du lg(mu)  ||d||  lg(rg) alpha_du alpha_pr  ls
  90  1.4956815e+02 1.09e-01 3.28e-01  -3.8 6.06e+01  -4.5 9.43e-01 1.00e+00h  1
  91  1.4953537e+02 1.08e-01 3.24e-01  -3.8 2.05e+02  -5.0 9.33e-01 1.39e-02h  1
  92  1.4890139e+02 3.67e-01 4.89e-01  -3.8 6.13e+01  -4.5 5.63e-01 1.00e+00h  1
  93  1.4853303e+02 1.16e-01 2.57e-01  -3.8 4.13e+01  -4.1 1.00e+00 9.25e-01h  1
  94  1.4831508e+02 1.30e-01 2.35e-01  -3.8 8.21e+01  -4.6 5.57e-01 2.15e-01h  1
  95  1.4726021e+02 1.07e+00 7.33e-01  -3.8 2.51e+02  -5.1 2.49e-01 3.51e-01f  1
  96  1.4654416e+02 1.12e+00 9.25e-01  -3.8 1.17e+03  -5.6 3.47e-02 5.26e-02h  1
  97  1.4657503e+02 3.58e-01 3.25e-01  -3.8 3.14e+01  -4.2 1.00e+00 6.96e-01h  1
  98  1.4601623e+02 2.47e-01 2.11e-01  -3.8 6.58e+01  -4.7 9.09e-01 7.88e-01h  1
  99  1.4567054e+02 1.32e-01 1.96e-01  -3.8 4.59e+01  -4.3 5.09e-01 9.14e-01h  1
iter    objective    inf_pr   inf_du lg(mu)  ||d||  lg(rg) alpha_du alpha_pr  ls
 100  1.4488250e+02 6.12e-01 1.18e+00  -3.8 2.40e+02  -4.8 4.77e-01 3.43e-01f  1
 101  1.4463381e+02 3.51e-01 6.04e-01  -3.8 5.98e+01  -4.3 4.49e-01 4.32e-01h  1
 102  1.4363782e+02 5.96e-01 7.79e-01  -3.8 1.13e+02  -4.8 1.00e+00 8.25e-01h  1
 103  1.4307603e+02 5.59e-01 7.76e-01  -3.8 3.10e+02  -5.3 7.46e-02 1.57e-01h  1
 104  1.4182938e+02 1.26e+00 1.10e+00  -3.8 1.40e+02  -4.9 9.61e-01 7.94e-01h  1
 105  1.4154081e+02 9.89e-01 9.83e-01  -3.8 1.36e+02  -4.4 1.15e-01 2.45e-01h  1
 106  1.4152654e+02 6.10e-01 6.10e-01  -3.8 2.62e+01  -4.0 1.00e+00 3.82e-01h  1
 107  1.4088934e+02 4.59e-01 3.36e-01  -3.8 6.03e+01  -4.5 3.97e-01 1.00e+00h  1
 108  1.4038010e+02 3.71e-01 3.44e-01  -3.8 1.17e+02  -5.0 5.58e-01 3.97e-01h  1
 109  1.3959093e+02 5.25e-01 6.51e-01  -3.8 3.78e+02  -5.4 1.84e-01 1.75e-01h  1
iter    objective    inf_pr   inf_du lg(mu)  ||d||  lg(rg) alpha_du alpha_pr  ls
 110  1.3926145e+02 5.16e-01 6.25e-01  -3.8 3.36e+02  -5.0 6.16e-02 7.07e-02h  1
 111  1.3894595e+02 3.63e-01 3.82e-01  -3.8 9.13e+01  -4.6 1.00e+00 3.29e-01h  1
 112  1.3874127e+02 6.35e-02 1.29e-01  -3.8 3.72e+01  -4.2 1.00e+00 1.00e+00h  1
 113  1.3837559e+02 1.45e-01 3.03e-01  -3.8 1.29e+02  -4.6 2.60e-01 2.69e-01h  1
 114  1.3787255e+02 2.03e-01 3.13e-01  -3.8 4.38e+01  -4.2 5.40e-01 1.00e+00h  1
 115  1.3768399e+02 1.91e-01 2.62e-01  -3.8 9.58e+01  -4.7 4.32e-01 1.55e-01h  1
 116  1.3717559e+02 2.89e-01 2.68e-01  -3.8 2.38e+02  -5.2 1.68e-01 1.67e-01f  1
 117  1.3639192e+02 4.81e-01 5.56e-01  -3.8 8.70e+02  -5.6 4.84e-02 6.97e-02f  1
 118  1.3490462e+02 1.54e+00 1.58e+00  -3.8 1.35e+03  -5.2 5.93e-02 8.68e-02f  1
 119  1.3490818e+02 1.40e+00 1.44e+00  -3.8 3.72e+01  -3.9 7.39e-01 9.14e-02h  1
iter    objective    inf_pr   inf_du lg(mu)  ||d||  lg(rg) alpha_du alpha_pr  ls
 120  1.3454765e+02 7.88e-01 6.30e-01  -3.8 1.07e+02  -4.4 1.00e+00 4.45e-01h  1
 121  1.3382891e+02 8.02e-01 1.05e+00  -3.8 4.07e+02  -4.8 2.62e-02 1.37e-01h  1
 122  1.3320144e+02 8.37e-01 1.06e+00  -3.8 7.34e+03  -5.3 1.93e-02 6.24e-03f  1
 123  1.3250550e+02 5.02e-01 4.48e-01  -3.8 1.34e+02  -4.9 1.00e+00 4.90e-01h  1
 124  1.3248374e+02 4.59e-01 4.09e-01  -3.8 4.23e+01  -4.5 1.00e+00 8.59e-02h  1
 125  1.3200480e+02 4.21e-01 3.63e-01  -3.8 1.87e+02  -4.9 5.69e-01 2.05e-01h  1
 126  1.3169215e+02 2.48e-01 1.67e-01  -3.8 7.07e+01  -4.5 1.00e+00 4.82e-01h  1
 127  1.3151459e+02 5.34e-02 1.28e-01  -3.8 3.19e+01  -4.1 1.00e+00 1.00e+00h  1
 128  1.3083326e+02 3.20e-01 7.78e-01  -3.8 1.21e+02  -4.6 6.13e-01 4.90e-01h  1
 129  1.3062369e+02 1.98e-01 4.53e-01  -3.8 4.02e+01  -4.1 1.00e+00 4.12e-01h  1
iter    objective    inf_pr   inf_du lg(mu)  ||d||  lg(rg) alpha_du alpha_pr  ls
 130  1.2943314e+02 8.04e-01 1.02e+00  -3.8 8.98e+01  -4.6 4.49e-01 1.00e+00h  1
 131  1.2864832e+02 6.08e-01 8.79e-01  -3.8 1.84e+02  -5.1 1.54e-01 3.35e-01h  1
 132  1.2843346e+02 5.04e-01 7.19e-01  -3.8 1.03e+02  -4.7 6.63e-01 1.74e-01h  1
 133  1.2826253e+02 1.27e-01 1.11e-01  -3.8 3.64e+01  -4.2 8.82e-01 7.38e-01h  1
 134  1.2767639e+02 2.47e-01 3.86e-01  -3.8 1.38e+02  -4.7 1.00e+00 3.44e-01h  1
 135  1.2689859e+02 4.00e-01 1.13e+00  -3.8 7.25e+01  -4.3 1.00e+00 1.00e+00h  1
 136  1.2678366e+02 3.92e-01 1.09e+00  -3.8 2.20e+02  -4.8 1.52e-02 3.89e-02h  1
 137  1.2579812e+02 1.11e+00 1.58e+00  -3.8 3.03e+04  -5.2 1.68e-03 2.70e-03f  1
 138  1.2552581e+02 8.71e-01 1.20e+00  -3.8 1.06e+02  -4.8 1.00e+00 2.15e-01h  1
 139  1.2531576e+02 5.98e-02 1.36e-01  -3.8 3.50e+01  -4.4 7.73e-01 1.00e+00h  1
iter    objective    inf_pr   inf_du lg(mu)  ||d||  lg(rg) alpha_du alpha_pr  ls
 140  1.2448485e+02 3.68e-01 3.42e-01  -3.8 1.03e+02  -4.9 4.33e-01 6.11e-01h  1
 141  1.2365017e+02 4.10e-01 8.69e-01  -3.8 8.53e+01  -4.4 1.00e+00 8.14e-01h  1
 142  1.2339145e+02 1.73e-01 3.92e-01  -3.8 3.24e+01  -4.0 8.73e-01 6.28e-01h  1
 143  1.2299968e+02 1.65e-01 3.48e-01  -3.8 6.84e+01  -4.5 9.54e-01 3.50e-01h  1
 144  1.2189571e+02 6.78e-01 7.76e-01  -3.8 1.99e+02  -5.0 2.89e-01 3.78e-01f  1
 145  1.2148963e+02 6.73e-01 8.10e-01  -3.8 6.68e+02  -5.5 1.07e-01 4.38e-02h  1
 146  1.2039463e+02 7.14e-01 9.70e-01  -3.8 2.19e+02  -5.0 1.03e-01 3.81e-01h  1
 147  1.1894946e+02 1.39e+00 2.93e+00  -3.8 1.59e+02  -4.6 1.00e+00 8.37e-01h  1
 148  1.1886773e+02 8.30e-01 1.70e+00  -3.8 5.06e+01  -4.2 3.16e-01 3.99e-01h  1
 149  1.1769785e+02 7.95e-01 1.04e+00  -3.8 1.14e+02  -4.7 3.80e-01 8.73e-01h  1
iter    objective    inf_pr   inf_du lg(mu)  ||d||  lg(rg) alpha_du alpha_pr  ls
 150  1.1692299e+02 6.21e-01 9.49e-01  -3.8 1.88e+02  -5.1 4.14e-01 3.16e-01h  1
 151  1.1628021e+02 3.93e-01 3.40e-01  -3.8 9.29e+01  -4.7 5.26e-01 5.93e-01h  1
 152  1.1596667e+02 1.71e-01 3.11e-01  -3.8 5.82e+01  -4.3 1.00e+00 7.21e-01h  1
 153  1.1555936e+02 2.58e-01 5.55e-01  -3.8 3.54e+02  -4.8 2.06e-01 1.02e-01h  1
 154  1.1476575e+02 4.95e-01 6.02e-01  -3.8 5.98e+01  -4.3 5.20e-01 9.93e-01h  1
 155  1.1463494e+02 4.57e-01 5.55e-01  -3.8 1.64e+02  -4.8 1.48e-01 7.91e-02h  1
 156  1.1424206e+02 4.40e-01 5.21e-01  -3.8 3.11e+02  -5.3 1.09e-01 1.00e-01h  1
 157  1.1396062e+02 4.49e-01 5.25e-01  -3.8 2.17e+04  -5.8 1.38e-03 1.32e-03f  1
 158  1.1361445e+02 4.58e-01 5.16e-01  -3.8 1.60e+03  -5.3 1.42e-02 2.14e-02f  1
 159  1.1291241e+02 3.93e-01 3.59e-01  -3.8 1.57e+02  -4.9 2.82e-01 3.35e-01h  1
iter    objective    inf_pr   inf_du lg(mu)  ||d||  lg(rg) alpha_du alpha_pr  ls
 160  1.1246879e+02 2.21e-01 1.64e-01  -3.8 6.27e+01  -4.5 1.00e+00 6.15e-01f  1
 161  1.1119022e+02 1.18e+00 2.12e+00  -3.8 1.51e+03  -5.0 3.55e-02 6.60e-02f  1
 162  1.1097107e+02 1.03e+00 1.85e+00  -3.8 1.18e+02  -4.5 6.44e-03 1.33e-01h  1
 163  1.1062571e+02 1.03e+00 1.84e+00  -3.8 1.30e+03  -5.0 3.38e-02 1.61e-02h  1
 164  1.1049302e+02 7.93e-01 1.39e+00  -3.8 6.88e+01  -4.6 2.11e-01 2.32e-01h  1
 165  1.1039501e+02 7.77e-01 1.36e+00  -3.8 3.54e+02  -5.1 3.57e-02 2.09e-02h  1
 166  1.1019492e+02 5.15e-01 8.52e-01  -3.8 6.60e+01  -4.6 3.30e-01 3.37e-01h  1
 167  1.1001758e+02 4.84e-01 7.85e-01  -3.8 2.16e+02  -5.1 1.28e-01 6.54e-02h  1
 168  1.0892822e+02 1.02e+00 1.04e+00  -3.8 5.55e+03  -5.6 1.06e-02 1.68e-02f  1
 169  1.0880315e+02 2.96e-01 2.21e-01  -3.8 4.12e+01  -4.3 1.00e+00 7.13e-01h  1
iter    objective    inf_pr   inf_du lg(mu)  ||d||  lg(rg) alpha_du alpha_pr  ls
 170  1.0781742e+02 8.09e-01 2.02e+00  -3.8 9.29e+01  -4.7 5.53e-01 1.00e+00h  1
 171  1.0770287e+02 5.68e-01 1.42e+00  -3.8 4.38e+01  -4.3 7.05e-01 2.98e-01h  1
 172  1.0733575e+02 4.23e-01 1.02e+00  -3.8 9.29e+01  -4.8 2.96e-01 3.11e-01h  1
 173  1.0672927e+02 4.68e-01 7.80e-01  -3.8 2.71e+02  -5.3 1.60e-01 1.87e-01h  1
 174  1.0643176e+02 3.85e-01 5.25e-01  -3.8 1.32e+02  -4.8 4.35e-01 2.45e-01h  1
 175  1.0574448e+02 6.53e-01 7.64e-01  -3.8 9.20e+02  -5.3 7.55e-02 6.55e-02f  1
 176  1.0506841e+02 6.97e-01 7.23e-01  -3.8 2.14e+02  -4.9 3.64e-01 2.81e-01h  1
 177  1.0501340e+02 4.67e-01 5.44e-01  -3.8 5.16e+01  -4.5 1.96e-01 3.26e-01h  1
 178  1.0485871e+02 4.34e-01 5.71e-01  -3.8 2.19e+02  -4.9 5.23e-01 6.34e-02h  1
 179  1.0479127e+02 2.99e-01 4.53e-01  -3.8 4.78e+01  -4.5 1.00e+00 2.96e-01h  1
iter    objective    inf_pr   inf_du lg(mu)  ||d||  lg(rg) alpha_du alpha_pr  ls
 180  1.0472605e+02 2.87e-01 4.47e-01  -3.8 1.79e+02  -5.0 3.82e-01 3.25e-02h  1
 181  1.0397384e+02 6.94e-01 1.91e+00  -3.8 7.16e+04  -5.5 1.21e-03 9.75e-04f  1
 182  1.0388792e+02 4.95e-01 1.33e+00  -3.8 5.41e+01  -4.6 1.00e+00 2.87e-01f  1
 183  1.0374072e+02 4.69e-01 1.22e+00  -3.8 1.87e+02  -5.1 3.55e-01 6.97e-02h  1
 184  1.0370252e+02 3.81e-01 9.67e-01  -3.8 4.33e+01  -4.7 1.00e+00 1.97e-01h  1
 185  1.0347757e+02 3.51e-01 7.32e-01  -3.8 1.40e+02  -5.1 2.17e-02 1.75e-01h  1
 186  1.0312877e+02 4.04e-01 7.46e-01  -3.8 4.90e+02  -5.6 2.86e-01 8.16e-02h  1
 187  1.0277559e+02 5.10e-01 1.05e+00  -3.8 6.69e+02  -5.2 4.66e-02 7.02e-02h  1
 188  1.0267076e+02 4.38e-01 9.98e-01  -3.8 9.61e+01  -4.8 6.44e-01 1.59e-01h  1
 189  1.0265925e+02 4.30e-01 9.80e-01  -3.8 9.79e+01  -4.8 1.17e-01 1.89e-02h  1
iter    objective    inf_pr   inf_du lg(mu)  ||d||  lg(rg) alpha_du alpha_pr  ls
 190  1.0250405e+02 3.27e-01 1.05e+00  -3.8 1.17e+02  -4.9 9.79e-01 2.56e-01h  1
 191  1.0262844e+02 1.57e-01 5.15e-01  -3.8 2.27e+01  -4.4 1.00e+00 5.11e-01h  1
 192  1.0257558e+02 1.28e-01 3.75e-01  -3.8 5.07e+01  -4.9 1.00e+00 4.13e-01h  1
 193  1.0242797e+02 3.78e-01 1.61e+00  -3.8 8.23e+01  -5.4 5.38e-01 7.00e-01h  1
 194  1.0238268e+02 3.65e-01 1.56e+00  -3.8 1.54e+02  -5.9 5.51e-02 4.30e-02h  1
 195  1.0232307e+02 3.29e-01 1.40e+00  -3.8 1.04e+02  -6.4 1.00e+00 1.18e-01h  1
 196  1.0227044e+02 2.99e-01 1.26e+00  -3.8 1.17e+02  -6.8 2.42e-01 1.22e-01h  1
 197  1.0223323e+02 2.99e-01 1.25e+00  -3.8 6.70e+02  -6.4 7.21e-02 1.45e-02h  1
 198  1.0223389e+02 2.98e-01 1.25e+00  -3.8 9.18e+00  -5.1 9.94e-01 3.51e-03h  1
 199  1.0247820e+02 2.82e-01 1.15e+00  -3.8 1.47e+02  -4.7 1.21e-01 7.35e-02h  1
iter    objective    inf_pr   inf_du lg(mu)  ||d||  lg(rg) alpha_du alpha_pr  ls
 200  1.0252776e+02 9.44e-03 5.36e-02  -3.8 9.09e+00  -4.2 1.00e+00 9.84e-01h  1
 201  1.0247606e+02 3.07e-03 2.00e-02  -3.8 2.58e+00  -4.7 1.00e+00 1.00e+00h  1
 202  1.0245546e+02 3.88e-03 1.35e-02  -3.8 5.83e+00  -5.2 8.88e-01 1.00e+00h  1
 203  1.0244869e+02 8.45e-03 4.28e-02  -3.8 9.14e+00  -5.7 1.00e+00 1.00e+00h  1
 204  1.0244901e+02 4.38e-03 1.86e-02  -3.8 6.99e+00  -5.2 1.00e+00 1.00e+00h  1
 205  1.0244856e+02 4.37e-03 1.80e-01  -3.8 5.82e+01  -4.8 2.17e-01 1.35e-02h  7
 206  1.0245247e+02 3.21e-03 1.01e-01  -3.8 4.59e+00  -4.4 1.00e+00 2.50e-01h  3
 207  1.0243578e+02 2.68e-04 2.87e-03  -3.8 1.20e+00  -4.0 1.00e+00 1.00e+00h  1
 208  1.0243534e+02 4.39e-03 5.71e-02  -3.8 9.09e+00  -4.4 8.53e-01 5.00e-01h  2
 209  1.0240901e+02 2.63e-03 2.17e-02  -3.8 3.56e+00  -4.0 1.00e+00 1.00e+00h  1
iter    objective    inf_pr   inf_du lg(mu)  ||d||  lg(rg) alpha_du alpha_pr  ls
 210  1.0239110e+02 9.14e-03 7.21e-02  -3.8 5.37e+01  -4.5 4.81e-02 9.83e-02h  4
 211  1.0231924e+02 5.95e-03 8.47e-02  -3.8 6.88e+00  -4.1 4.24e-01 1.00e+00h  1
 212  1.0222353e+02 3.62e-02 2.32e-01  -3.8 2.28e+01  -4.5 7.09e-02 1.00e+00h  1
 213  1.0217568e+02 3.68e-02 1.71e-01  -3.8 7.26e+01  -5.0 5.37e-01 1.19e-01h  1
 214  1.0207708e+02 2.31e-01 5.60e-01  -3.8 1.34e+03  -5.5 3.61e-04 3.30e-02f  1
 215  1.0174246e+02 7.39e-01 1.37e+00  -3.8 1.41e+02  -5.1 3.95e-01 5.53e-01h  1
 216  1.0137982e+02 4.49e-01 7.90e-01  -3.8 1.03e+02  -5.5 5.36e-01 5.09e-01h  1
 217  1.0136098e+02 4.26e-01 7.48e-01  -3.8 1.07e+02  -6.0 2.89e-01 5.59e-02h  1
 218  1.0129866e+02 3.66e-01 5.82e-01  -3.8 1.17e+02  -6.5 4.65e-01 2.82e-01h  1
 219  1.0135890e+02 1.87e-01 4.62e-01  -3.8 5.79e+01  -7.0 5.19e-01 6.46e-01h  1
iter    objective    inf_pr   inf_du lg(mu)  ||d||  lg(rg) alpha_du alpha_pr  ls
 220  1.0138908e+02 1.77e-01 4.85e-01  -3.8 6.76e+01  -7.4 5.66e-01 4.03e-01h  1
 221  1.0149101e+02 1.92e-02 3.98e-02  -3.8 1.43e+01  -7.9 1.00e+00 1.00e+00h  1
 222  1.0150628e+02 8.17e-03 1.90e-02  -3.8 8.62e+00  -8.4 1.00e+00 1.00e+00h  1
 223  1.0150236e+02 1.34e-05 5.03e-05  -3.8 5.07e-01  -8.9 1.00e+00 1.00e+00h  1
 224  1.0149787e+02 1.93e-04 4.01e-04  -5.7 1.25e+00  -9.4 9.88e-01 9.96e-01h  1
 225  1.0149774e+02 4.36e-06 9.54e-06  -5.7 1.80e-01  -9.8 1.00e+00 1.00e+00h  1
 226  1.0149768e+02 2.97e-07 5.92e-07  -8.6 4.85e-02 -10.3 9.99e-01 1.00e+00h  1
 227  1.0149768e+02 1.27e-10 2.47e-10  -8.6 9.97e-04 -10.8 1.00e+00 1.00e+00h  1

Number of Iterations....: 227

                                   (scaled)                 (unscaled)
Objective...............:   1.0149767957695215e+02    1.0149767957695215e+02
Dual infeasibility......:   2.4670754328326439e-10    2.4670754328326439e-10
Constraint violation....:   1.2653078584889954e-10    1.2653078584889954e-10
Complementarity.........:   2.5715300299692318e-09    2.5715300299692318e-09
Overall NLP error.......:   2.5715300299692318e-09    2.5715300299692318e-09


Number of objective function evaluations             = 248
Number of objective gradient evaluations             = 228
Number of equality constraint evaluations            = 248
Number of inequality constraint evaluations          = 0
Number of equality constraint Jacobian evaluations   = 228
Number of inequality constraint Jacobian evaluations = 0
Number of Lagrangian Hessian evaluations             = 227
Total CPU secs in IPOPT (w/o function evaluations)   =      3.739
Total CPU secs in NLP function evaluations           =      0.239

EXIT: Optimal Solution Found.
           S  :   t_proc      (avg)   t_wall      (avg)    n_eval
       nlp_f  |   6.86ms ( 27.66us)   6.94ms ( 28.00us)       248
       nlp_g  |  41.08ms (165.66us)  41.15ms (165.92us)       248
    nlp_grad  | 531.00us (531.00us) 531.00us (531.00us)         1
  nlp_grad_f  |  14.41ms ( 62.92us)  14.44ms ( 63.07us)       229
  nlp_hess_l  |  89.71ms (395.21us)  89.86ms (395.87us)       227
   nlp_jac_g  |  75.52ms (329.77us)  75.66ms (330.41us)       229
       total  |   4.11 s (  4.11 s)   4.11 s (  4.11 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:

[27]:
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
[27]:
../_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):

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

[31]:
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.