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