In [1]:
# Imports
from __future__ import division
import matplotlib.pyplot as plt
from numpy.random import multivariate_normal as normal
from dynamics import *
from simulation import *
from optimization import *
from plotting import *
# Configuration
%matplotlib
np.set_printoptions(suppress=True, precision=4)
Using matplotlib backend: MacOSX
In [3]:
x0 = normal(mu0, S0)
[x_all, z_all] = simulate_trajectory(x0, u_, N_sim)
# Plot
fig, ax = plt.subplots(figsize=(6,6))
# Trajectory
[traj_b, _] = plot_traj(ax, x_all)
# Ball observations
[obs_b] = plot_observations('ball observations', ax,
z_all[:,'x_b'], z_all[:,'y_b'])
# Catcher gaze
[arrows_c] = plot_arrows('gaze', ax, x_all[:,'x_c'],
x_all[:,'y_c'], x_all[:,'phi'])
# Appearance
ax.set_title("A simulated trajectory")
ax.grid(True)
ax.legend(loc=2, handles=[traj_b, obs_b, arrows_c])
ax.set_aspect('auto')
#fig.tight_layout()
/Library/Python/2.7/site-packages/ipykernel/__main__.py:1: DeprecationWarning: using a non-integer number instead of an integer will result in an error in the future
if __name__ == '__main__':
In [4]:
plan = create_simple_plan(m0, N_sim)
x_all = plan.prefix['X']
This is Ipopt version 3.12, running with linear solver ma57.
Number of nonzeros in equality constraint Jacobian...: 334
Number of nonzeros in inequality constraint Jacobian.: 0
Number of nonzeros in Lagrangian Hessian.............: 108
Total number of variables............................: 150
variables with only lower bounds: 0
variables with lower and upper bounds: 30
variables with only upper bounds: 0
Total number of equality constraints.................: 105
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 4.9999900e-05 1.50e+01 5.65e-04 -1.0 0.00e+00 - 0.00e+00 0.00e+00 0
Reallocating memory for MA57: lfact (3554)
1 5.1971650e+02 9.72e+00 4.67e+01 -1.0 2.50e+01 - 8.02e-03 3.57e-01h 1
2 6.2217896e+02 9.37e+00 6.61e+01 -1.0 1.61e+01 2.0 1.09e-01 4.11e-02h 1
3 6.2580242e+02 9.36e+00 1.72e+02 -1.0 1.54e+01 2.4 9.33e-02 1.43e-03h 1
4 9.8783225e+02 8.12e+00 2.40e+02 -1.0 1.54e+01 1.9 4.68e-02 1.30e-01h 1
5 1.2456528e+03 7.69e+00 4.54e+02 -1.0 3.45e+01 2.4 1.09e-03 7.28e-02h 1
6 1.4768166e+03 7.10e+00 4.92e+02 -1.0 1.24e+01 1.9 7.11e-01 7.36e-02h 1
7 3.4384030e+03 4.49e+00 1.73e+03 -1.0 1.15e+01 2.3 1.09e-01 4.59e-01h 1
8 3.6893415e+03 4.08e+00 1.56e+03 -1.0 6.22e+00 1.8 1.98e-01 9.14e-02h 1
9 5.5251857e+03 1.67e+00 1.15e+03 -1.0 5.65e+00 2.3 2.36e-01 6.20e-01h 1
iter objective inf_pr inf_du lg(mu) ||d|| lg(rg) alpha_du alpha_pr ls
10 6.3198983e+03 6.32e-01 1.10e+03 -1.0 2.15e+00 2.7 4.78e-03 6.22e-01h 1
11 6.4642424e+03 4.50e-01 1.09e+03 -1.0 8.10e-01 3.1 1.00e+00 2.88e-01h 1
12 6.7737710e+03 4.17e-02 2.56e+02 -1.0 5.77e-01 2.6 7.67e-01 1.00e+00h 1
13 6.5547052e+03 1.17e-02 4.80e+01 -1.0 3.24e-01 2.2 1.00e+00 1.00e+00f 1
14 6.4713061e+03 2.30e-03 4.63e+01 -1.0 1.17e-01 2.6 1.00e+00 1.00e+00f 1
15 6.2235396e+03 2.67e-02 5.03e+01 -1.0 3.82e-01 2.1 1.00e+00 1.00e+00f 1
16 5.9566479e+03 1.89e+00 2.05e+02 -1.0 1.61e+01 1.6 3.96e-02 1.63e-01f 2
17 5.6943924e+03 4.96e-01 3.24e+02 -1.0 9.18e-01 2.1 3.58e-01 1.00e+00f 1
18 5.6907131e+03 4.68e-01 3.08e+02 -1.0 4.08e-01 2.5 1.00e+00 5.51e-02f 1
19 5.2616921e+03 1.64e+00 2.83e+02 -1.0 2.22e+00 2.0 1.00e+00 1.00e+00f 1
iter objective inf_pr inf_du lg(mu) ||d|| lg(rg) alpha_du alpha_pr ls
20 5.0803617e+03 1.24e+00 1.74e+02 -1.0 1.11e+00 2.4 1.00e+00 1.00e+00f 1
21 5.0345342e+03 9.24e-01 9.94e+01 -1.0 8.24e-01 2.0 1.00e+00 2.60e-01f 1
22 4.3374807e+03 4.96e-01 7.52e+01 -1.0 1.67e+00 1.5 8.67e-01 1.00e+00f 1
23 2.7861472e+03 6.07e+00 1.12e+02 -1.0 9.24e+00 1.0 1.00e+00 8.40e-01f 1
24 2.7782471e+03 6.06e+00 1.12e+02 -1.0 4.83e+01 0.5 8.87e-02 1.82e-03f 1
25 2.1133880e+03 3.33e+00 2.88e+02 -1.0 4.88e+00 1.0 7.18e-02 5.81e-01f 1
26 2.1049961e+03 3.26e+00 2.81e+02 -1.0 2.17e+00 1.4 6.53e-01 2.12e-02f 1
27 2.0879608e+03 2.69e+00 2.31e+02 -1.0 2.05e+00 1.8 1.00e+00 1.74e-01f 1
28 1.7583804e+03 1.46e+00 9.22e+01 -1.0 2.28e+00 1.3 5.39e-01 1.00e+00f 1
29 1.6945348e+03 1.27e+00 2.53e+02 -1.0 2.39e+00 1.8 5.12e-02 1.00e+00f 1
iter objective inf_pr inf_du lg(mu) ||d|| lg(rg) alpha_du alpha_pr ls
30 1.6132088e+03 1.08e-01 1.57e+02 -1.0 8.82e-01 2.2 3.46e-01 1.00e+00f 1
31 1.5987760e+03 2.52e-02 4.75e+01 -1.0 1.82e-01 2.6 1.00e+00 9.04e-01f 1
32 1.5320747e+03 1.70e-02 3.12e+01 -1.0 2.38e-01 2.1 1.00e+00 1.00e+00f 1
33 1.3579934e+03 6.60e-02 3.21e+01 -1.0 7.20e-01 1.7 1.00e+00 1.00e+00f 1
34 9.8625213e+02 4.24e-02 2.94e+01 -1.0 1.94e+00 1.2 7.63e-01 1.00e+00f 1
35 8.6630143e+02 6.79e-01 4.01e+01 -1.0 1.40e+00 1.6 1.00e+00 1.00e+00f 1
36 5.4370857e+02 3.66e-02 2.06e+01 -1.0 1.53e+00 1.1 5.27e-01 1.00e+00f 1
37 4.6081301e+02 9.42e-03 2.16e+01 -1.0 6.05e-01 1.6 1.00e+00 1.00e+00f 1
38 4.3262232e+02 2.15e-04 2.08e+01 -1.0 2.17e-01 2.0 1.00e+00 1.00e+00f 1
39 3.6034020e+02 1.10e-02 1.87e+01 -1.0 5.84e-01 1.5 4.10e-01 1.00e+00f 1
iter objective inf_pr inf_du lg(mu) ||d|| lg(rg) alpha_du alpha_pr ls
40 3.3402863e+02 3.79e-03 1.71e+01 -1.0 2.00e-01 1.9 1.00e+00 1.00e+00f 1
41 2.6120961e+02 2.81e-01 1.77e+01 -1.0 8.84e-01 1.5 2.09e-01 1.00e+00f 1
42 2.2577014e+02 8.23e-03 2.43e+01 -1.0 3.20e-01 1.9 1.00e+00 1.00e+00f 1
43 1.6327025e+02 1.69e-01 1.65e+01 -1.0 7.59e-01 1.4 5.07e-01 1.00e+00f 1
44 7.9199232e+01 1.29e-01 9.78e+00 -1.0 1.40e+00 0.9 1.00e+00 1.00e+00f 1
45 4.0352848e+01 5.03e-02 3.47e+00 -1.0 1.30e+00 0.4 9.87e-01 1.00e+00f 1
46 2.8814480e+01 3.79e-01 2.24e+00 -1.0 5.17e+00 - 2.85e-01 3.55e-01f 1
47 2.2589094e+01 3.43e-01 1.23e+00 -1.0 5.43e+00 - 2.41e-01 4.48e-01f 1
48 2.0290135e+01 6.79e-01 4.39e-01 -1.0 7.79e+00 - 4.16e-01 1.00e+00f 1
49 2.0284075e+01 4.38e-02 5.13e-01 -1.7 5.48e-01 -0.0 1.00e+00 1.00e+00h 1
iter objective inf_pr inf_du lg(mu) ||d|| lg(rg) alpha_du alpha_pr ls
50 1.9786698e+01 2.46e-02 2.77e-01 -1.7 1.19e+00 -0.5 1.00e+00 4.52e-01f 1
51 1.8172957e+01 2.03e-02 6.51e-01 -1.7 3.63e+00 - 3.38e-01 5.09e-01f 1
52 1.7681362e+01 5.03e-02 1.84e-01 -1.7 2.27e+00 - 3.94e-01 1.00e+00f 1
53 1.7588007e+01 6.41e-03 1.72e-01 -2.5 2.09e-01 -0.1 1.00e+00 1.00e+00h 1
54 1.7329564e+01 1.69e-01 1.88e-01 -2.5 7.69e-01 -0.6 3.19e-01 9.64e-01f 1
55 1.6888597e+01 5.31e-01 5.39e-01 -2.5 5.22e+00 - 1.00e+00 4.50e-01f 1
56 1.6638045e+01 5.43e-01 4.26e-01 -2.5 1.83e+00 - 8.32e-01 1.00e+00h 1
57 1.6743467e+01 5.10e-02 2.37e-02 -2.5 5.35e-01 - 1.00e+00 1.00e+00h 1
58 1.6797378e+01 6.02e-04 3.88e-04 -2.5 1.08e-01 - 1.00e+00 1.00e+00h 1
59 1.6766671e+01 3.85e-06 2.45e-06 -3.8 7.49e-02 - 1.00e+00 1.00e+00h 1
iter objective inf_pr inf_du lg(mu) ||d|| lg(rg) alpha_du alpha_pr ls
60 1.6763599e+01 4.56e-07 1.61e-07 -5.7 4.57e-02 - 1.00e+00 1.00e+00h 1
61 1.6763517e+01 6.32e-08 1.97e-08 -5.7 2.28e-02 - 1.00e+00 1.00e+00h 1
62 1.6763510e+01 1.07e-08 3.32e-09 -5.7 9.64e-03 - 1.00e+00 1.00e+00h 1
63 1.6763488e+01 1.30e-09 8.08e-08 -8.6 3.28e-03 - 1.00e+00 1.00e+00h 1
Reallocating memory for MA57: lfact (3888)
64 1.6763488e+01 1.67e-11 5.18e-12 -8.6 3.81e-04 - 1.00e+00 1.00e+00h 1
65 1.6763488e+01 3.55e-15 2.21e-14 -9.0 5.93e-06 - 1.00e+00 1.00e+00h 1
Number of Iterations....: 65
(scaled) (unscaled)
Objective...............: 1.6763487587465654e+01 1.6763487587465654e+01
Dual infeasibility......: 2.2093438190040615e-14 2.2093438190040615e-14
Constraint violation....: 3.5527136788005009e-15 3.5527136788005009e-15
Complementarity.........: 9.1192976267022932e-10 9.1192976267022932e-10
Overall NLP error.......: 9.1192976267022932e-10 9.1192976267022932e-10
Number of objective function evaluations = 69
Number of objective gradient evaluations = 66
Number of equality constraint evaluations = 69
Number of inequality constraint evaluations = 0
Number of equality constraint Jacobian evaluations = 66
Number of inequality constraint Jacobian evaluations = 0
Number of Lagrangian Hessian evaluations = 65
Total CPU secs in IPOPT (w/o function evaluations) = 0.044
Total CPU secs in NLP function evaluations = 0.010
EXIT: Optimal Solution Found.
time spent in eval_f: 0.000593 s. (69 calls, 0.0085942 ms. average)
time spent in eval_grad_f: 0.000812 s. (67 calls, 0.0121194 ms. average)
time spent in eval_g: 0.000831 s. (69 calls, 0.0120435 ms. average)
time spent in eval_jac_g: 0.001202 s. (68 calls, 0.0176765 ms. average)
time spent in eval_h: 0.006144 s. (66 calls, 0.0930909 ms. average)
time spent in main loop (proc): 0.059551 s.
time spent in main loop (wall): 0 s.
time spent in callback function: 0 s.
time spent in callback preparation: 0.00035 s.
In [5]:
# Plot the optimal trajectory
fig, ax = plt.subplots(figsize=(6,6))
# Trajectory
[traj_b, _] = plot_traj(ax, x_all)
# Catcher gaze
[arrows_c] = plot_arrows('gaze', ax, x_all[:,'x_c'],
x_all[:,'y_c'], x_all[:,'phi'])
# Appearance
ax.set_title("Optimal trajectory without uncertainty")
ax.grid(True)
ax.legend(loc=2, handles=[traj_b, arrows_c])
ax.set_aspect('auto')
#fig.tight_layout()
In [6]:
# Simulate a trajectory
x0 = normal(mu0, S0)
[x_all, z_all] = simulate_trajectory(x0, u_, N_sim)
# Filter observations
b_all = filter_observations(b0, z_all, u_, N_sim)
/Library/Python/2.7/site-packages/ipykernel/__main__.py:2: DeprecationWarning: using a non-integer number instead of an integer will result in an error in the future
from ipykernel import kernelapp as app
In [7]:
# Plot the true trajectory and the filtered version
fig, ax = plt.subplots(figsize=(10,10))
# Trajectory
[traj_b, _] = plot_traj(ax, x_all)
# Filtered
[filt_mb, filt_Sb, _, _] = plot_filt_bc_mS(ax, b_all)
# Catcher gaze
[arrows_c] = plot_arrows('gaze', ax, x_all[:,'x_c'],
x_all[:,'y_c'], x_all[:,'phi'])
# Appearance
ax.set_title("Simulated trajectory and filtered observations")
ax.grid(True)
ax.legend(loc=2, handles=[traj_b, filt_mb, filt_Sb, arrows_c])
ax.set_aspect('equal')
#fig.tight_layout()
plotting.py:22: RuntimeWarning: invalid value encountered in sqrt
height = 2 * np.sqrt(s * w[0])
plotting.py:21: RuntimeWarning: invalid value encountered in sqrt
width = 2 * np.sqrt(s * w[1])
In [8]:
# Simulate an extended belief trajectory
eb_all = simulate_ebelief_trajectory(eb0, u_, N_sim)
# Simulate a trajectory
x0 = normal(mu0, S0)
[x_all, z_all] = simulate_trajectory(x0, u_, N_sim)
# Filter observations
b_all = filter_observations(b0, z_all, u_, N_sim)
/Library/Python/2.7/site-packages/ipykernel/__main__.py:5: DeprecationWarning: using a non-integer number instead of an integer will result in an error in the future
In [9]:
# Plot
fig, ax = plt.subplots(figsize=(10,10))
# Plan
[plan_mb, plan_Lb, plan_Sb, plan_SLb,
_, _, _, _, arrows_c] = plot_plan(ax, eb_all)
# Trajectory
[traj_b, _] = plot_traj(ax, x_all)
# Filtered
[filt_b, _] = plot_filt(ax, b_all)
# Appearance
ax.set_title("Uncertainty propagation, "
"sampled trajectory and filtered observations")
ax.grid(True)
ax.legend(loc=2,
handles = [plan_mb, traj_b, filt_b, plan_Lb,
plan_Sb, plan_SLb, arrows_c])
ax.set_aspect('equal')
#fig.tight_layout()
In [10]:
# Create a plan
sol = create_plan(b0, N_sim, 'pc')
# Simulate L's for plotting
eb_all = simulate_ebelief_trajectory(eb0, sol['U'], N_sim)
This is Ipopt version 3.12, running with linear solver ma97.
Number of nonzeros in equality constraint Jacobian...: 334
Number of nonzeros in inequality constraint Jacobian.: 30
Number of nonzeros in Lagrangian Hessian.............: 10302
Total number of variables............................: 150
variables with only lower bounds: 15
variables with lower and upper bounds: 30
variables with only upper bounds: 0
Total number of equality constraints.................: 105
Total number of inequality constraints...............: 15
inequality constraints with only lower bounds: 0
inequality constraints with lower and upper bounds: 0
inequality constraints with only upper bounds: 15
iter objective inf_pr inf_du lg(mu) ||d|| lg(rg) alpha_du alpha_pr ls
0 1.9340152e-01 1.50e+01 1.34e-02 -1.0 0.00e+00 - 0.00e+00 0.00e+00 0
1 1.2055720e+02 1.23e+01 4.52e+01 -1.0 2.50e+01 - 4.03e-03 1.78e-01h 1
2 1.5415887e+02 1.21e+01 7.34e+01 -1.0 2.06e+01 2.0 6.61e-02 2.23e-02h 1
3 1.6045190e+02 1.20e+01 7.54e+01 -1.0 2.01e+01 1.5 3.51e-02 4.55e-03h 1
4 1.8512422e+02 1.18e+01 3.41e+02 -1.0 2.00e+01 1.9 1.82e-01 1.54e-02h 1
5 2.5915348e+02 1.12e+01 3.37e+02 -1.0 1.97e+01 1.5 8.29e-02 4.55e-02h 1
6 3.6035530e+02 1.07e+01 4.74e+02 -1.0 1.88e+01 1.9 1.82e-01 4.80e-02h 1
7 6.4693271e+02 9.59e+00 1.17e+03 -1.0 1.79e+01 2.3 3.83e-01 1.03e-01h 1
8 7.0935355e+02 9.40e+00 1.54e+03 -1.0 1.61e+01 2.7 1.51e-01 1.98e-02h 1
9 8.1199047e+02 9.10e+00 1.37e+03 -1.0 1.57e+01 2.3 1.18e-01 3.13e-02h 1
iter objective inf_pr inf_du lg(mu) ||d|| lg(rg) alpha_du alpha_pr ls
10 1.0782249e+03 8.42e+00 1.28e+03 -1.0 1.53e+01 2.7 1.21e-03 7.45e-02h 1
11 1.2661214e+03 8.00e+00 5.44e+03 -1.0 1.41e+01 3.1 9.97e-01 5.04e-02h 1
12 2.7147715e+03 5.47e+00 1.65e+04 -1.0 1.34e+01 3.6 5.91e-02 3.16e-01h 1
13 2.7345174e+03 5.44e+00 1.69e+04 -1.0 9.17e+00 4.0 2.53e-02 5.21e-03h 1
14 2.7412130e+03 5.43e+00 1.69e+04 -1.0 9.13e+00 3.5 7.16e-05 1.79e-03h 1
15 2.7455926e+03 5.42e+00 1.70e+04 -1.0 9.11e+00 3.9 5.05e-02 1.18e-03h 1
16 3.0270294e+03 5.02e+00 1.88e+05 -1.0 9.10e+00 4.4 8.79e-01 7.39e-02h 1
17 4.6512883e+03 3.03e+00 2.20e+05 -1.0 8.43e+00 4.8 6.63e-02 4.00e-01h 1
18 7.7821706e+03 5.10e-01 8.08e+05 -1.0 5.06e+00 5.2 1.37e-01 1.00e+00h 1
19 7.7911307e+03 2.63e-03 3.26e+05 -1.0 4.14e-01 4.7 4.66e-03 1.00e+00h 1
iter objective inf_pr inf_du lg(mu) ||d|| lg(rg) alpha_du alpha_pr ls
20 7.7901749e+03 7.52e-06 3.70e+03 -1.0 5.78e-03 5.2 9.89e-01 1.00e+00f 1
21 7.7888527e+03 3.00e-07 8.05e+01 -1.0 1.70e-03 4.7 9.89e-01 1.00e+00f 1
22 7.7848421e+03 2.67e-06 8.15e+01 -1.0 5.17e-03 4.2 1.00e+00 1.00e+00f 1
23 7.7730355e+03 2.40e-05 8.02e+01 -1.0 1.53e-02 3.7 1.00e+00 1.00e+00f 1
24 7.7377470e+03 2.11e-04 7.97e+01 -1.0 4.55e-02 3.2 1.00e+00 1.00e+00f 1
25 7.6339872e+03 1.76e-03 7.84e+01 -1.0 1.34e-01 2.8 9.24e-01 1.00e+00f 1
26 7.5757496e+03 1.95e-03 7.95e+01 -1.0 4.80e-01 2.3 1.00e+00 1.82e-01f 1
27 7.4676167e+03 2.73e-03 7.54e+01 -1.0 1.45e-01 2.7 1.00e+00 1.00e+00f 1
28 7.1564005e+03 2.30e-02 7.64e+01 -1.0 4.42e-01 2.2 1.00e+00 1.00e+00f 1
29 7.0558516e+03 2.84e-03 7.66e+01 -1.0 1.66e-01 2.7 1.00e+00 1.00e+00f 1
iter objective inf_pr inf_du lg(mu) ||d|| lg(rg) alpha_du alpha_pr ls
30 6.7641164e+03 1.53e-02 6.42e+01 -1.0 4.17e-01 2.2 1.00e+00 1.00e+00f 1
31 5.6751523e+03 1.10e+00 4.64e+02 -1.0 3.04e+00 1.7 1.57e-01 1.00e+00f 1
32 5.2847730e+03 3.60e-01 1.61e+02 -1.0 4.48e+01 1.2 1.10e-01 5.94e-02f 3
33 4.5077069e+03 1.18e-01 7.51e+01 -1.0 1.57e+00 1.7 3.92e-01 1.00e+00f 1
34 4.2130238e+03 1.07e-01 6.38e+01 -1.0 5.25e-01 2.1 4.00e-02 1.00e+00f 1
35 3.4106345e+03 7.18e-02 6.34e+01 -1.0 1.57e+00 1.6 1.00e+00 1.00e+00f 1
36 3.3370337e+03 6.86e-02 6.26e+01 -1.0 3.35e+00 1.1 4.30e-01 4.43e-02f 1
37 2.7044670e+03 4.65e-02 4.97e+01 -1.0 1.38e+00 1.6 1.00e+00 1.00e+00f 1
38 1.5417348e+03 1.44e-01 3.55e+01 -1.0 2.95e+00 1.1 7.53e-01 1.00e+00f 1
39 1.1554217e+03 1.11e-01 3.05e+01 -1.0 4.40e+00 0.6 1.00e+00 2.75e-01f 1
iter objective inf_pr inf_du lg(mu) ||d|| lg(rg) alpha_du alpha_pr ls
40 1.1139331e+03 1.08e-01 3.12e+01 -1.0 5.39e+00 0.1 1.00e+00 2.60e-02f 1
41 9.2429379e+02 8.92e-02 2.64e+01 -1.0 3.44e+00 0.6 1.00e+00 1.98e-01f 1
42 4.8219656e+02 2.04e-01 1.81e+01 -1.0 4.79e+00 0.1 1.00e+00 4.01e-01f 1
43 1.6322793e+02 2.80e-01 1.33e+01 -1.0 4.25e+00 -0.4 1.00e+00 4.90e-01f 1
44 2.1950737e+01 4.72e+00 4.88e+00 -1.0 4.56e+00 -0.9 8.48e-01 6.82e-01f 1
45 1.9485222e+01 4.73e+00 4.48e+00 -1.0 4.00e+01 -1.4 3.11e-03 5.75e-02f 1
46 3.6844966e+00 5.31e+00 4.52e+00 -1.0 4.44e+00 -0.0 6.91e-01 9.86e-01f 1
47 3.8807075e+00 2.19e+00 1.27e+01 -1.0 3.61e+00 0.4 4.80e-01 5.95e-01h 1
48 4.5927303e+00 8.45e-01 1.31e+01 -1.0 1.70e+00 0.8 3.02e-01 1.00e+00h 1
49 4.1117754e+00 4.64e-01 1.27e+01 -1.0 8.36e-01 1.2 1.00e+00 1.00e+00h 1
iter objective inf_pr inf_du lg(mu) ||d|| lg(rg) alpha_du alpha_pr ls
50 3.3193316e+00 1.79e-02 4.32e+00 -1.0 3.94e-01 0.8 1.00e+00 1.00e+00h 1
51 2.2016335e+00 7.05e-03 3.33e-01 -1.0 1.69e-01 0.3 1.00e+00 1.00e+00f 1
52 1.7780105e+00 1.97e-03 8.09e-02 -1.7 1.23e-01 -0.2 1.00e+00 1.00e+00f 1
53 1.7375117e+00 1.45e-03 5.64e-02 -2.5 6.72e-02 -0.7 1.00e+00 1.00e+00h 1
54 1.3319437e+00 3.46e+00 1.44e-01 -2.5 5.53e+00 - 1.32e-01 7.66e-01f 1
55 1.2180162e+00 2.51e+00 5.25e-01 -2.5 3.90e+00 -1.1 1.00e+00 1.00e+00h 1
56 1.3521912e+00 1.23e+00 1.93e-01 -2.5 2.20e+00 -1.6 3.55e-01 1.00e+00h 1
57 1.3449950e+00 6.94e-02 4.04e-01 -2.5 2.38e+00 - 5.46e-01 1.00e+00h 1
58 1.3631750e+00 1.65e-03 1.82e-01 -2.5 1.75e-01 -0.3 1.00e+00 1.00e+00h 1
59 1.3581018e+00 4.00e-05 4.68e-02 -2.5 3.38e-02 0.1 1.00e+00 1.00e+00h 1
iter objective inf_pr inf_du lg(mu) ||d|| lg(rg) alpha_du alpha_pr ls
60 1.3454988e+00 8.58e-04 2.55e-01 -2.5 1.15e-01 -0.3 1.00e+00 1.00e+00h 1
61 1.3339260e+00 4.13e-04 3.09e-01 -2.5 2.42e-01 -0.8 1.00e+00 1.00e+00h 1
62 1.3579398e+00 4.15e-05 1.42e-01 -2.5 7.86e-01 - 1.00e+00 1.00e+00H 1
63 1.3519718e+00 6.94e-04 3.99e-02 -2.5 6.08e-02 -0.4 1.00e+00 1.00e+00h 1
64 1.3278592e+00 1.43e-03 1.96e-01 -2.5 9.91e-01 -0.9 1.00e+00 2.50e-01h 2
65 1.3200719e+00 4.97e-04 2.00e-01 -2.5 2.08e-01 -1.3 1.00e+00 1.00e+00h 1
66 1.3481873e+00 1.73e-04 2.65e-01 -2.5 3.32e-01 - 1.00e+00 1.00e+00H 1
67 1.3378408e+00 4.80e-04 8.54e-02 -2.5 5.13e-02 -0.0 1.00e+00 1.00e+00h 1
68 1.3224902e+00 2.76e-04 5.39e-01 -2.5 2.08e-01 -0.5 1.00e+00 1.00e+00h 1
69 1.3208007e+00 5.60e-05 1.42e-01 -2.5 6.85e-02 -1.0 1.00e+00 1.00e+00h 1
iter objective inf_pr inf_du lg(mu) ||d|| lg(rg) alpha_du alpha_pr ls
70 1.3189425e+00 4.67e-05 6.44e-02 -2.5 4.86e-02 -1.4 1.00e+00 1.00e+00h 1
71 1.3196990e+00 1.18e-03 1.17e-02 -2.5 1.91e-01 - 1.00e+00 1.00e+00h 1
72 1.3174245e+00 3.36e-04 5.52e-03 -3.8 1.38e-01 - 9.92e-01 1.00e+00h 1
73 1.3160055e+00 1.70e-03 1.95e-02 -3.8 1.52e-01 - 1.00e+00 1.00e+00h 1
74 1.3155010e+00 1.79e-03 5.82e-03 -3.8 7.83e-02 -1.9 1.00e+00 1.00e+00h 1
75 1.3133527e+00 1.83e-02 3.64e-02 -3.8 1.16e+00 - 5.55e-01 2.50e-01h 3
76 1.3138474e+00 2.58e-04 5.26e-02 -3.8 1.92e-01 - 1.00e+00 1.00e+00h 1
77 1.3138223e+00 3.67e-03 2.54e-02 -3.8 1.28e-01 - 1.00e+00 1.00e+00h 1
78 1.3137833e+00 1.01e-03 5.49e-03 -3.8 5.78e-02 - 1.00e+00 1.00e+00h 1
79 1.3138289e+00 1.00e-04 1.49e-03 -3.8 2.96e-02 - 1.00e+00 1.00e+00h 1
iter objective inf_pr inf_du lg(mu) ||d|| lg(rg) alpha_du alpha_pr ls
80 1.3137751e+00 7.65e-05 1.37e-03 -5.7 6.05e-02 - 1.00e+00 1.00e+00h 1
81 1.3137673e+00 3.23e-05 6.85e-04 -5.7 3.14e-02 - 1.00e+00 1.00e+00h 1
82 1.3137666e+00 6.24e-06 1.31e-04 -5.7 1.32e-02 - 1.00e+00 1.00e+00h 1
83 1.3137667e+00 4.94e-07 1.05e-05 -5.7 3.71e-03 - 1.00e+00 1.00e+00h 1
84 1.3137664e+00 1.10e-06 2.34e-05 -8.6 5.81e-03 - 1.00e+00 1.00e+00h 1
85 1.3137665e+00 5.40e-08 1.16e-06 -8.6 1.24e-03 - 1.00e+00 1.00e+00h 1
86 1.3137665e+00 1.35e-10 2.88e-09 -8.6 6.19e-05 - 1.00e+00 1.00e+00h 1
Number of Iterations....: 86
(scaled) (unscaled)
Objective...............: 1.3137664666790876e+00 1.3137664666790876e+00
Dual infeasibility......: 2.8811133444273107e-09 2.8811133444273107e-09
Constraint violation....: 1.3465850656757539e-10 1.3465850656757539e-10
Complementarity.........: 2.5385612494982231e-09 2.5385612494982231e-09
Overall NLP error.......: 2.8811133444273107e-09 2.8811133444273107e-09
Number of objective function evaluations = 101
Number of objective gradient evaluations = 87
Number of equality constraint evaluations = 101
Number of inequality constraint evaluations = 101
Number of equality constraint Jacobian evaluations = 87
Number of inequality constraint Jacobian evaluations = 87
Number of Lagrangian Hessian evaluations = 86
Total CPU secs in IPOPT (w/o function evaluations) = 0.332
Total CPU secs in NLP function evaluations = 3.025
EXIT: Optimal Solution Found.
time spent in eval_f: 0.004498 s. (101 calls, 0.0445347 ms. average)
time spent in eval_grad_f: 0.011614 s. (88 calls, 0.131977 ms. average)
time spent in eval_g: 0.005692 s. (101 calls, 0.0563564 ms. average)
time spent in eval_jac_g: 0.005213 s. (89 calls, 0.058573 ms. average)
time spent in eval_h: 3.00285 s. (87 calls, 34.5155 ms. average)
time spent in main loop (proc): 3.67655 s.
time spent in main loop (wall): 0 s.
time spent in callback function: 0 s.
time spent in callback preparation: 0.000799 s.
In [11]:
# Sample several trajectories with optimal controls
[X_all, B_all] = sim_traj(b0, sol['U'], N_sim)
In [12]:
# Plot plan
fig, ax = plt.subplots(figsize=(10,10))
# Plan
[plan_mb, plan_Lb, plan_Sb, plan_SLb,
_, _, _, _, arrows_c] = plot_plan(ax, eb_all)
# Appearance
ax.set_title("Planned trajectory")
ax.grid(True)
ax.legend(loc=2, handles = [plan_mb, plan_Lb, plan_Sb, plan_SLb, arrows_c])
ax.set_aspect('equal')
#.tight_layout()
In [ ]:
# Plot plan and sampled trajectories
fig, ax = plt.subplots(1,2,figsize=(20,8))
# True trajectories
plot_plan(ax[0], eb_all)
plot_traj_beam(ax[0], X_all)
# Appearance
ax[0].set_title("True trajectories")
ax[0].grid(True)
ax[0].set_aspect('equal')
# Belief trajectories
plot_plan(ax[1], eb_all)
plot_filt_beam(ax[1], B_all)
# Appearance
ax[1].set_title("Filtered trajectories")
ax[1].grid(True)
ax[1].set_aspect('equal')
#fig.tight_layout()
In [13]:
# Simulate
[X_all, B_all, EB_all] = mpc_loop(eb0, N_sim, N_ctrl, N_delay, 'pc')
This is Ipopt version 3.12, running with linear solver ma97.
Number of nonzeros in equality constraint Jacobian...: 265
Number of nonzeros in inequality constraint Jacobian.: 24
Number of nonzeros in Lagrangian Hessian.............: 6447
Total number of variables............................: 120
variables with only lower bounds: 12
variables with lower and upper bounds: 24
variables with only upper bounds: 0
Total number of equality constraints.................: 84
Total number of inequality constraints...............: 12
inequality constraints with only lower bounds: 0
inequality constraints with lower and upper bounds: 0
inequality constraints with only upper bounds: 12
iter objective inf_pr inf_du lg(mu) ||d|| lg(rg) alpha_du alpha_pr ls
0 1.9434556e-01 1.50e+01 1.34e-02 -1.0 0.00e+00 - 0.00e+00 0.00e+00 0
1 1.9815736e+02 1.17e+01 5.88e+01 -1.0 2.50e+01 - 3.96e-03 2.24e-01h 1
2 2.4359718e+02 1.14e+01 8.73e+01 -1.0 1.94e+01 2.0 1.15e-01 2.57e-02h 1
3 2.6520141e+02 1.12e+01 9.32e+01 -1.0 1.89e+01 1.5 2.41e-02 1.53e-02h 1
4 2.7567058e+02 1.11e+01 3.30e+02 -1.0 1.86e+01 1.9 2.46e-01 5.68e-03h 1
5 5.0371550e+02 9.73e+00 3.04e+02 -1.0 1.85e+01 1.5 1.66e-01 1.19e-01h 1
6 9.0159702e+02 8.30e+00 2.94e+02 -1.0 1.63e+01 1.9 1.24e-01 1.45e-01h 1
7 9.3582970e+02 8.20e+00 3.24e+02 -1.0 1.39e+01 2.3 9.78e-02 1.14e-02h 1
8 1.1122651e+03 7.71e+00 3.62e+02 -1.0 1.38e+01 1.8 2.85e-04 5.95e-02h 1
9 1.5272484e+03 6.77e+00 6.15e+02 -1.0 1.30e+01 2.3 8.27e-01 1.22e-01h 1
iter objective inf_pr inf_du lg(mu) ||d|| lg(rg) alpha_du alpha_pr ls
10 2.6190511e+03 4.86e+00 2.04e+03 -1.0 1.14e+01 2.7 1.89e-01 2.82e-01h 1
11 3.7240423e+03 3.30e+00 1.82e+03 -1.0 8.18e+00 2.2 3.88e-02 3.20e-01h 1
12 3.7568362e+03 3.25e+00 1.80e+03 -1.0 5.56e+00 1.7 4.61e-01 1.54e-02h 1
13 6.4982646e+03 5.53e-02 8.09e+02 -1.0 5.47e+00 2.2 4.08e-03 1.00e+00h 1
14 6.3381876e+03 3.57e-03 9.68e+01 -1.0 2.44e-01 2.6 9.15e-01 9.74e-01f 1
15 5.8781515e+03 4.56e-02 9.54e+01 -1.0 7.25e-01 2.1 4.69e-01 1.00e+00f 1
16 5.7101153e+03 3.93e-03 1.13e+02 -1.0 3.22e-01 2.5 1.00e+00 1.00e+00f 1
17 5.2622368e+03 2.39e-02 7.96e+01 -1.0 6.80e-01 2.1 7.43e-01 1.00e+00f 1
18 4.1252284e+03 2.26e-01 7.49e+01 -1.0 1.92e+00 1.6 1.00e+00 1.00e+00f 1
19 2.2534006e+03 1.57e+00 5.06e+01 -1.0 3.90e+00 1.1 7.48e-01 1.00e+00f 1
iter objective inf_pr inf_du lg(mu) ||d|| lg(rg) alpha_du alpha_pr ls
20 1.8800058e+03 1.24e+00 4.57e+01 -1.0 5.59e+00 0.6 4.89e-01 1.72e-01f 1
21 1.8122704e+03 1.21e+00 4.48e+01 -1.0 7.14e+00 0.2 4.88e-01 2.56e-02f 1
22 1.1893881e+03 9.38e-01 3.59e+01 -1.0 8.64e+00 -0.3 1.00e+00 2.17e-01f 1
23 5.9169073e+02 6.40e-01 2.51e+01 -1.0 7.47e+00 -0.8 7.80e-01 3.12e-01f 1
24 1.2615830e+02 7.09e-01 1.14e+01 -1.0 5.50e+00 -1.3 6.94e-01 5.53e-01f 1
25 2.5158422e+01 3.10e+00 4.95e+00 -1.0 4.23e+00 - 6.36e-01 5.64e-01f 1
26 1.2518474e+01 7.39e+00 3.40e+00 -1.0 1.55e+01 - 9.39e-02 3.13e-01f 1
27 7.4187985e+00 5.29e+00 2.49e+00 -1.0 8.22e+00 -1.7 3.64e-01 2.69e-01f 1
28 3.4481444e+00 3.22e+00 1.44e+00 -1.0 9.21e+00 - 4.45e-01 4.22e-01f 1
29 1.6261950e+00 4.23e+00 6.09e-01 -1.0 4.35e+00 -1.3 2.57e-01 6.52e-01f 1
iter objective inf_pr inf_du lg(mu) ||d|| lg(rg) alpha_du alpha_pr ls
30 1.3177262e+00 1.98e+00 3.49e-01 -1.0 1.63e+00 -0.9 1.00e+00 1.00e+00f 1
31 1.6315048e+00 3.83e-01 3.97e-01 -1.0 4.24e+00 - 6.74e-01 1.00e+00h 1
32 1.5479530e+00 1.05e-01 9.54e-02 -1.7 1.25e+00 - 9.39e-01 1.00e+00h 1
33 1.5530613e+00 9.74e-03 6.88e-01 -2.5 3.03e-01 -1.4 1.00e+00 1.00e+00h 1
34 1.5453098e+00 5.54e-04 6.44e-02 -2.5 8.36e-02 -0.9 1.00e+00 1.00e+00h 1
35 1.4899110e+00 8.31e-02 1.06e-01 -2.5 1.50e+00 - 1.00e+00 1.00e+00h 1
36 1.5150538e+00 4.08e-02 4.01e-02 -2.5 7.25e-01 - 1.00e+00 1.00e+00h 1
37 1.5157287e+00 1.12e-03 1.57e-02 -2.5 6.19e-02 -1.4 1.00e+00 1.00e+00h 1
38 1.5133376e+00 3.79e-03 2.03e-02 -3.8 1.09e-01 -1.9 9.73e-01 1.00e+00h 1
39 1.5097254e+00 2.83e-02 2.55e-02 -3.8 1.51e+00 - 1.00e+00 2.17e-01h 3
iter objective inf_pr inf_du lg(mu) ||d|| lg(rg) alpha_du alpha_pr ls
40 1.5108893e+00 1.27e-03 3.72e-02 -3.8 1.66e-01 - 1.00e+00 1.00e+00h 1
41 1.5108601e+00 1.13e-03 4.73e-03 -3.8 6.32e-02 - 1.00e+00 1.00e+00h 1
42 1.5109414e+00 1.93e-04 1.07e-03 -3.8 2.58e-02 - 1.00e+00 1.00e+00h 1
43 1.5108990e+00 6.25e-06 1.99e-04 -5.7 4.71e-02 - 9.99e-01 1.00e+00h 1
44 1.5108814e+00 7.02e-06 1.78e-04 -5.7 2.53e-02 - 1.00e+00 1.00e+00h 1
45 1.5108768e+00 1.30e-06 3.35e-05 -5.7 1.06e-02 - 1.00e+00 1.00e+00h 1
46 1.5108758e+00 1.10e-07 2.82e-06 -5.7 3.06e-03 - 1.00e+00 1.00e+00h 1
47 1.5108743e+00 2.33e-07 5.97e-06 -8.6 4.70e-03 - 1.00e+00 1.00e+00h 1
48 1.5108741e+00 1.32e-08 3.41e-07 -8.6 1.06e-03 - 1.00e+00 1.00e+00h 1
49 1.5108740e+00 4.18e-11 1.08e-09 -8.6 5.99e-05 - 1.00e+00 1.00e+00h 1
Number of Iterations....: 49
(scaled) (unscaled)
Objective...............: 1.5108740387838580e+00 1.5108740387838580e+00
Dual infeasibility......: 1.0801168362495339e-09 1.0801168362495339e-09
Constraint violation....: 4.1818992713160696e-11 4.1818992713160696e-11
Complementarity.........: 2.5550105040903337e-09 2.5550105040903337e-09
Overall NLP error.......: 2.5550105040903337e-09 2.5550105040903337e-09
Number of objective function evaluations = 54
Number of objective gradient evaluations = 50
Number of equality constraint evaluations = 54
Number of inequality constraint evaluations = 54
Number of equality constraint Jacobian evaluations = 50
Number of inequality constraint Jacobian evaluations = 50
Number of Lagrangian Hessian evaluations = 49
Total CPU secs in IPOPT (w/o function evaluations) = 0.135
Total CPU secs in NLP function evaluations = 1.153
EXIT: Optimal Solution Found.
time spent in eval_f: 0.002124 s. (54 calls, 0.0393333 ms. average)
time spent in eval_grad_f: 0.006153 s. (51 calls, 0.120647 ms. average)
time spent in eval_g: 0.002658 s. (54 calls, 0.0492222 ms. average)
time spent in eval_jac_g: 0.002772 s. (52 calls, 0.0533077 ms. average)
time spent in eval_h: 1.14244 s. (50 calls, 22.8488 ms. average)
time spent in main loop (proc): 1.45397 s.
time spent in main loop (wall): 0 s.
time spent in callback function: 0 s.
time spent in callback preparation: 0.000649 s.
This is Ipopt version 3.12, running with linear solver ma97.
Number of nonzeros in equality constraint Jacobian...: 219
Number of nonzeros in inequality constraint Jacobian.: 20
Number of nonzeros in Lagrangian Hessian.............: 4377
Total number of variables............................: 100
variables with only lower bounds: 10
variables with lower and upper bounds: 20
variables with only upper bounds: 0
Total number of equality constraints.................: 70
Total number of inequality constraints...............: 10
inequality constraints with only lower bounds: 0
inequality constraints with lower and upper bounds: 0
inequality constraints with only upper bounds: 10
iter objective inf_pr inf_du lg(mu) ||d|| lg(rg) alpha_du alpha_pr ls
0 1.9495944e-01 1.61e+01 1.30e-02 -1.0 0.00e+00 - 0.00e+00 0.00e+00 0
1 3.1740858e+02 1.07e+01 7.41e+01 -1.0 2.54e+01 - 4.80e-03 3.38e-01h 1
2 3.4831255e+02 1.05e+01 8.25e+01 -1.0 1.68e+01 2.0 2.73e-01 1.70e-02h 1
3 4.8230833e+02 9.87e+00 3.19e+02 -1.0 1.65e+01 2.4 4.81e-01 6.41e-02h 1
4 5.5073207e+02 9.50e+00 3.55e+02 -1.0 1.55e+01 1.9 1.31e-01 3.75e-02h 1
5 6.8909162e+02 8.86e+00 5.61e+02 -1.0 1.49e+01 2.4 3.01e-01 6.66e-02h 1
6 7.6461443e+02 8.56e+00 1.42e+03 -1.0 1.39e+01 2.8 4.09e-01 3.39e-02h 1
7 1.1783081e+03 7.14e+00 1.31e+03 -1.0 1.34e+01 2.3 4.75e-02 1.65e-01h 1
8 1.1984824e+03 7.08e+00 1.21e+03 -1.0 1.12e+01 2.7 4.76e-01 7.67e-03h 1
9 3.2840301e+03 3.05e+00 2.60e+03 -1.0 1.11e+01 2.3 2.11e-01 5.78e-01h 1
iter objective inf_pr inf_du lg(mu) ||d|| lg(rg) alpha_du alpha_pr ls
10 3.3355203e+03 2.96e+00 2.53e+03 -1.0 4.69e+00 1.8 4.31e-01 2.89e-02h 1
11 3.4494120e+03 2.79e+00 2.38e+03 -1.0 4.55e+00 2.2 2.77e-02 5.98e-02h 1
12 5.1014147e+03 6.67e-01 1.17e+03 -1.0 4.28e+00 1.7 6.74e-02 1.00e+00h 1
13 4.6748835e+03 2.63e-01 1.26e+02 -1.0 1.16e+00 2.2 9.51e-01 1.00e+00f 1
14 3.8962681e+03 9.15e-01 8.15e+01 -1.0 1.84e+00 1.7 9.53e-02 1.00e+00f 1
15 3.6074068e+03 2.69e-01 1.19e+02 -1.0 1.01e+00 2.1 6.22e-01 1.00e+00f 1
16 3.1034561e+03 7.90e-02 5.25e+01 -1.0 1.22e+00 1.6 2.96e-01 8.70e-01f 1
17 2.8731153e+03 2.28e-02 6.09e+01 -1.0 5.21e-01 2.1 1.00e+00 1.00e+00f 1
18 2.3027687e+03 8.25e-02 4.94e+01 -1.0 1.27e+00 1.6 1.00e+00 1.00e+00f 1
19 2.2336912e+03 2.89e+00 6.92e+01 -1.0 7.98e+01 1.1 1.97e-02 3.35e-02f 1
iter objective inf_pr inf_du lg(mu) ||d|| lg(rg) alpha_du alpha_pr ls
20 2.1387616e+03 2.73e+00 6.53e+01 -1.0 2.26e+01 0.6 9.51e-04 5.45e-02f 1
21 2.0420187e+03 1.87e+00 9.52e+01 -1.0 3.01e+00 2.0 3.58e-02 3.01e-01f 1
22 2.0368411e+03 1.80e+00 1.16e+02 -1.0 1.37e+00 2.4 1.00e+00 3.50e-02f 1
23 2.0038647e+03 1.57e+00 9.71e+01 -1.0 2.23e+00 1.9 4.22e-01 1.01e-01f 1
24 1.9791721e+03 1.33e+00 1.52e+02 -1.0 1.53e+00 2.3 1.00e+00 2.01e-01f 1
25 1.9585735e+03 1.20e+00 4.91e+02 -1.0 1.26e+00 2.8 1.00e+00 1.00e+00f 1
26 1.8890642e+03 1.55e-01 1.87e+02 -1.0 7.76e-01 2.3 3.97e-01 1.00e+00f 1
27 1.6658221e+03 1.52e-02 4.74e+01 -1.0 6.12e-01 1.8 6.90e-01 1.00e+00f 1
28 1.4509385e+03 2.76e-02 3.73e+01 -1.0 1.57e+00 1.3 2.10e-01 3.91e-01f 1
29 1.4471502e+03 2.71e-02 3.72e+01 -1.0 5.73e-01 1.8 1.00e+00 1.97e-02f 1
iter objective inf_pr inf_du lg(mu) ||d|| lg(rg) alpha_du alpha_pr ls
30 1.0136468e+03 1.82e-01 2.69e+01 -1.0 1.40e+00 1.3 3.82e-01 1.00e+00f 1
31 6.9775959e+02 5.10e-01 2.30e+01 -1.0 2.82e+00 0.8 1.00e+00 4.48e-01f 1
32 5.6454859e+02 2.23e-01 2.02e+01 -1.0 1.06e+00 1.2 5.32e-01 5.72e-01f 1
33 4.6470032e+02 1.71e-01 1.78e+01 -1.0 1.94e+00 0.8 1.00e+00 2.61e-01f 1
34 2.1502148e+02 1.46e-01 1.15e+01 -1.0 3.06e+00 0.3 4.54e-01 5.23e-01f 1
35 7.8225780e+01 2.26e-01 1.01e+01 -1.0 2.89e+00 -0.2 1.00e+00 5.00e-01f 1
36 1.4353442e+01 1.70e-01 2.03e+00 -1.0 1.20e+00 0.2 1.00e+00 1.00e+00f 1
37 1.9734756e+00 4.23e-02 3.91e-01 -1.0 6.85e-01 -0.2 1.00e+00 1.00e+00f 1
38 1.4526935e+00 3.55e-02 2.92e-01 -1.7 4.66e-01 - 7.73e-01 1.00e+00f 1
39 1.3469605e+00 8.35e-01 1.11e-01 -1.7 3.77e+00 - 7.70e-01 1.00e+00f 1
iter objective inf_pr inf_du lg(mu) ||d|| lg(rg) alpha_du alpha_pr ls
40 1.3292363e+00 1.59e-01 6.44e-02 -1.7 4.59e-01 -0.7 1.00e+00 1.00e+00h 1
41 1.3198372e+00 5.38e-02 4.64e-02 -2.5 5.62e-01 - 8.77e-01 1.00e+00h 1
42 1.3025967e+00 4.31e-01 2.63e-01 -2.5 1.47e+00 - 8.17e-01 7.90e-01h 1
43 1.3054740e+00 7.98e-03 1.43e-01 -2.5 7.59e-01 - 1.00e+00 1.00e+00h 1
44 1.3074538e+00 2.26e-03 8.31e-03 -2.5 5.33e-01 - 1.00e+00 1.00e+00h 1
45 1.3037892e+00 4.54e-03 4.96e-02 -3.8 2.89e-01 - 9.99e-01 1.00e+00h 1
46 1.3032623e+00 1.82e-04 1.71e-02 -3.8 1.13e-01 - 1.00e+00 1.00e+00h 1
47 1.3030789e+00 1.28e-04 2.05e-03 -3.8 7.53e-02 - 1.00e+00 1.00e+00h 1
48 1.3030532e+00 1.03e-06 5.40e-05 -3.8 1.64e-02 - 1.00e+00 1.00e+00h 1
49 1.3029805e+00 3.87e-06 1.05e-04 -5.7 4.47e-02 - 9.96e-01 1.00e+00h 1
iter objective inf_pr inf_du lg(mu) ||d|| lg(rg) alpha_du alpha_pr ls
50 1.3029599e+00 3.93e-06 9.29e-05 -5.7 2.31e-02 - 1.00e+00 1.00e+00h 1
51 1.3029544e+00 5.86e-07 1.42e-05 -5.7 8.72e-03 - 1.00e+00 1.00e+00h 1
52 1.3029520e+00 1.45e-07 3.53e-06 -8.6 4.62e-03 - 9.99e-01 1.00e+00h 1
53 1.3029516e+00 3.53e-09 8.53e-08 -8.6 6.74e-04 - 1.00e+00 1.00e+00h 1
54 1.3029516e+00 1.68e-12 4.09e-11 -8.6 1.48e-05 - 1.00e+00 1.00e+00h 1
Number of Iterations....: 54
(scaled) (unscaled)
Objective...............: 1.3029516238493455e+00 1.3029516238493455e+00
Dual infeasibility......: 4.0851148852549812e-11 4.0851148852549812e-11
Constraint violation....: 1.6839862837514374e-12 1.6839862837514374e-12
Complementarity.........: 2.5093503968484323e-09 2.5093503968484323e-09
Overall NLP error.......: 2.5093503968484323e-09 2.5093503968484323e-09
Number of objective function evaluations = 55
Number of objective gradient evaluations = 55
Number of equality constraint evaluations = 55
Number of inequality constraint evaluations = 55
Number of equality constraint Jacobian evaluations = 55
Number of inequality constraint Jacobian evaluations = 55
Number of Lagrangian Hessian evaluations = 54
Total CPU secs in IPOPT (w/o function evaluations) = 0.105
Total CPU secs in NLP function evaluations = 0.906
EXIT: Optimal Solution Found.
time spent in eval_f: 0.001691 s. (55 calls, 0.0307455 ms. average)
time spent in eval_grad_f: 0.005048 s. (56 calls, 0.0901429 ms. average)
time spent in eval_g: 0.002313 s. (55 calls, 0.0420545 ms. average)
time spent in eval_jac_g: 0.002273 s. (57 calls, 0.0398772 ms. average)
time spent in eval_h: 0.899815 s. (55 calls, 16.3603 ms. average)
time spent in main loop (proc): 1.07835 s.
time spent in main loop (wall): 0 s.
time spent in callback function: 0 s.
time spent in callback preparation: 0.000441 s.
This is Ipopt version 3.12, running with linear solver ma97.
Number of nonzeros in equality constraint Jacobian...: 173
Number of nonzeros in inequality constraint Jacobian.: 16
Number of nonzeros in Lagrangian Hessian.............: 2707
Total number of variables............................: 80
variables with only lower bounds: 8
variables with lower and upper bounds: 16
variables with only upper bounds: 0
Total number of equality constraints.................: 56
Total number of inequality constraints...............: 8
inequality constraints with only lower bounds: 0
inequality constraints with lower and upper bounds: 0
inequality constraints with only upper bounds: 8
iter objective inf_pr inf_du lg(mu) ||d|| lg(rg) alpha_du alpha_pr ls
0 1.9053759e-01 1.78e+01 1.16e-02 -1.0 0.00e+00 - 0.00e+00 0.00e+00 0
1 5.9827544e+02 8.58e+00 9.47e+01 -1.0 2.64e+01 - 5.66e-03 5.21e-01h 1
2 6.2873957e+02 8.44e+00 9.32e+01 -1.0 1.26e+01 2.0 3.79e-01 1.62e-02h 1
3 9.5948075e+02 7.30e+00 5.26e+02 -1.0 1.24e+01 2.4 5.29e-01 1.52e-01h 1
4 1.0497262e+03 6.88e+00 5.46e+02 -1.0 1.06e+01 1.9 6.25e-02 5.69e-02h 1
5 1.4052769e+03 5.48e+00 9.12e+02 -1.0 9.95e+00 2.4 2.38e-01 2.03e-01h 1
6 1.4128310e+03 5.45e+00 9.30e+02 -1.0 7.93e+00 2.8 1.69e-01 4.48e-03h 1
7 2.4981613e+03 2.54e+00 1.33e+03 -1.0 7.90e+00 2.3 6.00e-01 5.47e-01h 1
8 3.6935778e+03 1.14e-01 7.88e+02 -1.0 3.57e+00 1.8 2.08e-01 1.00e+00h 1
9 3.6670240e+03 1.21e-02 2.38e+02 -1.0 2.49e-01 2.3 7.41e-01 1.00e+00f 1
iter objective inf_pr inf_du lg(mu) ||d|| lg(rg) alpha_du alpha_pr ls
10 3.6224130e+03 8.88e-04 7.76e+01 -1.0 9.33e-02 2.7 5.82e-01 1.00e+00f 1
11 3.4916272e+03 1.23e-02 5.45e+01 -1.0 3.27e-01 2.2 4.76e-01 1.00e+00f 1
12 3.4319576e+03 2.05e-03 5.87e+01 -1.0 1.32e-01 2.6 1.00e+00 1.00e+00f 1
13 3.2599419e+03 2.87e-02 6.38e+01 -1.0 4.31e-01 2.2 4.77e-01 1.00e+00f 1
14 3.1852312e+03 3.88e-03 5.89e+01 -1.0 1.49e-01 2.6 1.00e+00 1.00e+00f 1
15 2.9830943e+03 3.38e-02 6.38e+01 -1.0 4.85e-01 2.1 1.00e+00 1.00e+00f 1
16 2.2963637e+03 7.26e-01 1.02e+02 -1.0 1.69e+00 1.6 2.71e-01 1.00e+00f 1
17 1.3579316e+03 1.01e+00 1.06e+02 -1.0 2.62e+00 1.2 7.32e-01 1.00e+00f 1
18 1.1125357e+03 6.32e-01 4.02e+01 -1.0 1.03e+00 1.6 1.00e+00 1.00e+00f 1
19 9.0859027e+02 5.46e+00 6.41e+01 -1.0 2.29e+01 1.1 9.50e-02 1.86e-01f 1
iter objective inf_pr inf_du lg(mu) ||d|| lg(rg) alpha_du alpha_pr ls
20 8.3422779e+02 1.47e+00 2.72e+02 -1.0 5.70e+00 1.5 1.00e+00 1.00e+00f 1
21 8.4556992e+02 4.84e-01 6.56e+02 -1.0 8.87e-01 2.9 9.06e-01 1.00e+00h 1
22 8.5829107e+02 5.41e-03 1.32e+02 -1.0 5.37e-01 2.4 1.00e+00 1.00e+00h 1
23 8.4823902e+02 3.97e-05 2.83e+01 -1.0 4.30e-02 2.8 1.00e+00 1.00e+00f 1
24 8.1643265e+02 3.99e-04 2.91e+01 -1.0 1.33e-01 2.3 1.00e+00 1.00e+00f 1
25 7.3044816e+02 2.38e-03 2.63e+01 -1.0 3.60e-01 1.9 1.00e+00 1.00e+00f 1
26 5.8875193e+02 3.04e-02 2.29e+01 -1.0 9.11e-01 1.4 1.00e+00 7.03e-01f 1
27 5.8534888e+02 2.88e-02 2.28e+01 -1.0 3.08e-01 1.8 1.00e+00 5.25e-02f 1
28 4.3136331e+02 1.11e-01 1.74e+01 -1.0 8.06e-01 1.3 1.00e+00 1.00e+00f 1
29 1.8413234e+02 6.17e-01 1.40e+01 -1.0 1.81e+00 0.9 1.00e+00 1.00e+00f 1
iter objective inf_pr inf_du lg(mu) ||d|| lg(rg) alpha_du alpha_pr ls
30 1.6876789e+02 5.73e-01 1.30e+01 -1.0 2.09e+00 0.4 1.00e+00 7.16e-02f 1
31 7.8500962e+01 1.87e-01 6.71e+00 -1.0 1.05e+00 0.8 1.00e+00 1.00e+00f 1
32 4.7379365e+01 2.46e-01 6.54e+00 -1.0 1.45e+00 0.3 1.00e+00 3.58e-01f 1
33 2.8655184e+00 1.51e+00 4.02e+00 -1.0 2.20e+00 -0.1 1.00e+00 1.00e+00f 1
34 1.2967807e+00 1.91e+00 1.68e+00 -1.0 1.64e+00 0.3 1.00e+00 1.00e+00f 1
35 1.3578439e+00 1.15e+00 7.83e+00 -1.0 1.68e+00 0.7 1.00e+00 5.00e-01h 2
36 1.4236278e+00 6.06e-01 3.82e+00 -1.0 1.52e+00 0.2 1.00e+00 1.00e+00h 1
37 1.3713064e+00 4.82e-02 6.34e-01 -1.0 7.41e-01 -0.2 1.00e+00 1.00e+00h 1
38 1.2885223e+00 1.41e-03 1.39e-01 -1.7 3.87e-01 - 1.00e+00 1.00e+00h 1
39 1.2681430e+00 1.37e-01 3.13e-01 -2.5 7.59e-01 - 9.96e-01 1.00e+00h 1
iter objective inf_pr inf_du lg(mu) ||d|| lg(rg) alpha_du alpha_pr ls
40 1.2456422e+00 1.77e-01 2.31e-01 -2.5 1.15e+00 - 4.46e-01 1.00e+00h 1
41 1.2641346e+00 9.26e-03 2.68e-01 -2.5 3.02e-01 -0.7 1.00e+00 1.00e+00h 1
42 1.2610400e+00 5.35e-03 1.44e-02 -2.5 2.65e-01 - 1.00e+00 1.00e+00h 1
43 1.2575114e+00 9.41e-03 1.50e-02 -3.8 2.40e-01 - 9.62e-01 1.00e+00h 1
44 1.2582462e+00 7.71e-04 8.10e-03 -3.8 8.36e-02 - 1.00e+00 1.00e+00h 1
45 1.2582344e+00 1.37e-04 8.18e-04 -3.8 2.56e-02 - 1.00e+00 1.00e+00h 1
46 1.2581092e+00 9.83e-06 7.78e-05 -5.7 3.37e-02 - 9.99e-01 1.00e+00h 1
47 1.2580972e+00 3.17e-07 1.41e-06 -5.7 8.26e-03 - 1.00e+00 1.00e+00h 1
48 1.2580953e+00 3.37e-09 2.89e-08 -8.6 9.27e-04 - 1.00e+00 1.00e+00h 1
49 1.2580953e+00 1.39e-13 8.25e-13 -8.6 4.92e-06 - 1.00e+00 1.00e+00h 1
Number of Iterations....: 49
(scaled) (unscaled)
Objective...............: 1.2580952924360171e+00 1.2580952924360171e+00
Dual infeasibility......: 8.2549245217222733e-13 8.2549245217222733e-13
Constraint violation....: 1.3855583347321954e-13 1.3855583347321954e-13
Complementarity.........: 2.5060900508055916e-09 2.5060900508055916e-09
Overall NLP error.......: 2.5060900508055916e-09 2.5060900508055916e-09
Number of objective function evaluations = 54
Number of objective gradient evaluations = 50
Number of equality constraint evaluations = 54
Number of inequality constraint evaluations = 54
Number of equality constraint Jacobian evaluations = 50
Number of inequality constraint Jacobian evaluations = 50
Number of Lagrangian Hessian evaluations = 49
Total CPU secs in IPOPT (w/o function evaluations) = 0.091
Total CPU secs in NLP function evaluations = 0.520
EXIT: Optimal Solution Found.
time spent in eval_f: 0.00159 s. (54 calls, 0.0294444 ms. average)
time spent in eval_grad_f: 0.004155 s. (51 calls, 0.0814706 ms. average)
time spent in eval_g: 0.002109 s. (54 calls, 0.0390556 ms. average)
time spent in eval_jac_g: 0.001891 s. (52 calls, 0.0363654 ms. average)
time spent in eval_h: 0.5112 s. (50 calls, 10.224 ms. average)
time spent in main loop (proc): 0.799209 s.
time spent in main loop (wall): 0 s.
time spent in callback function: 0 s.
time spent in callback preparation: 0.000444 s.
This is Ipopt version 3.12, running with linear solver ma97.
Number of nonzeros in equality constraint Jacobian...: 127
Number of nonzeros in inequality constraint Jacobian.: 12
Number of nonzeros in Lagrangian Hessian.............: 1437
Total number of variables............................: 60
variables with only lower bounds: 6
variables with lower and upper bounds: 12
variables with only upper bounds: 0
Total number of equality constraints.................: 42
Total number of inequality constraints...............: 6
inequality constraints with only lower bounds: 0
inequality constraints with lower and upper bounds: 0
inequality constraints with only upper bounds: 6
iter objective inf_pr inf_du lg(mu) ||d|| lg(rg) alpha_du alpha_pr ls
0 1.8007225e-01 1.97e+01 9.41e-03 -1.0 0.00e+00 - 0.00e+00 0.00e+00 0
1 9.2619590e+02 5.05e-01 1.11e+02 -1.0 2.73e+01 - 1.02e-02 1.00e+00h 1
2 1.0268333e+03 1.34e-01 1.30e+04 -1.0 1.30e+00 4.0 5.79e-01 1.00e+00h 1
3 1.0572311e+03 9.09e-03 5.70e+03 -1.0 2.81e-01 4.4 9.47e-01 1.00e+00h 1
4 1.0589158e+03 1.58e-04 3.80e+02 -1.0 4.27e-02 3.9 1.00e+00 1.00e+00h 1
5 1.0566603e+03 2.02e-05 2.80e+01 -1.0 9.36e-03 3.5 1.00e+00 1.00e+00f 1
6 1.0498183e+03 2.00e-04 2.88e+01 -1.0 2.92e-02 3.0 1.00e+00 1.00e+00f 1
7 1.0284728e+03 2.43e-03 3.12e+01 -1.0 9.80e-02 2.5 1.00e+00 1.00e+00f 1
8 9.5619199e+02 3.96e-02 3.47e+01 -1.0 3.62e-01 2.0 1.00e+00 1.00e+00f 1
9 7.5258959e+02 1.46e-01 3.29e+01 -1.0 8.99e-01 1.6 1.00e+00 1.00e+00f 1
iter objective inf_pr inf_du lg(mu) ||d|| lg(rg) alpha_du alpha_pr ls
10 4.2851756e+02 4.76e-02 2.20e+01 -1.0 1.81e+00 1.1 1.00e+00 1.00e+00f 1
11 1.1500742e+02 2.92e-02 9.20e+00 -1.0 2.30e+00 0.6 8.62e-01 1.00e+00f 1
12 9.2890677e+00 1.73e-02 2.54e+00 -1.0 1.82e+00 0.1 1.00e+00 9.89e-01f 1
13 8.9319906e-01 3.10e-03 2.01e-01 -1.0 1.51e+00 - 1.00e+00 1.00e+00f 1
14 8.9086571e-01 3.47e-03 1.41e-01 -1.7 1.07e-01 - 1.00e+00 1.00e+00h 1
15 8.9058585e-01 8.93e-05 1.80e-02 -2.5 1.60e-02 -0.3 1.00e+00 1.00e+00h 1
16 8.8865768e-01 1.27e-03 2.67e-02 -3.8 5.82e-02 -0.8 1.00e+00 1.00e+00h 1
17 8.8773700e-01 2.85e-04 1.37e-02 -3.8 2.66e-02 -0.4 1.00e+00 1.00e+00h 1
18 8.7953131e-01 1.23e-02 3.05e-01 -3.8 2.11e-01 -0.9 1.00e+00 1.00e+00h 1
19 8.4465109e-01 1.47e-02 2.49e-01 -3.8 2.76e-01 -0.4 1.00e+00 1.00e+00h 1
iter objective inf_pr inf_du lg(mu) ||d|| lg(rg) alpha_du alpha_pr ls
20 8.3819019e-01 6.57e-04 8.76e-02 -3.8 9.21e-02 -0.0 1.00e+00 1.00e+00h 1
21 8.2708878e-01 1.73e-03 3.01e-01 -3.8 2.34e-01 -0.5 1.00e+00 2.50e-01h 3
22 8.4254030e-01 6.07e-05 9.06e-01 -3.8 1.27e-01 - 1.00e+00 1.00e+00H 1
23 8.2692723e-01 1.84e-02 1.88e-01 -3.8 3.62e-01 -1.0 1.00e+00 5.00e-01f 2
24 8.2086042e-01 1.45e-02 9.60e-02 -3.8 4.19e-01 -1.5 1.00e+00 2.50e-01h 3
25 8.1978971e-01 1.28e-02 8.99e-02 -3.8 7.43e-01 - 1.00e+00 1.25e-01h 4
26 8.2526403e-01 9.00e-04 3.96e-02 -3.8 3.92e-01 - 1.00e+00 1.00e+00H 1
27 9.6410579e-01 1.57e-05 2.38e+00 -3.8 1.72e+00 - 2.41e-01 1.00e+00H 1
28 9.1892065e-01 8.62e-05 1.43e+00 -3.8 2.97e-01 -1.0 1.00e+00 1.00e+00F 1
29 9.0705037e-01 1.99e-05 1.11e+00 -3.8 9.73e-02 -0.6 1.00e+00 1.00e+00F 1
iter objective inf_pr inf_du lg(mu) ||d|| lg(rg) alpha_du alpha_pr ls
30 9.0645743e-01 6.89e-05 1.08e+00 -3.8 7.83e-01 -1.1 1.00e+00 1.56e-02h 7
31 9.0104977e-01 3.05e-05 2.23e-01 -3.8 7.57e-02 -0.7 1.00e+00 1.00e+00H 1
32 8.9707640e-01 7.92e-03 7.47e-02 -3.8 1.51e-01 -1.1 1.00e+00 1.00e+00h 1
33 8.9426683e-01 2.63e-03 7.22e-02 -3.8 1.08e-01 -1.6 1.00e+00 1.00e+00h 1
34 8.9213785e-01 1.89e-03 1.29e-01 -3.8 2.53e-01 -2.1 1.00e+00 1.00e+00h 1
35 8.9100925e-01 2.78e-03 9.93e-02 -3.8 1.52e-01 -1.7 1.00e+00 1.00e+00h 1
36 8.9032836e-01 4.34e-04 6.41e-03 -3.8 7.96e-02 -1.2 1.00e+00 1.00e+00h 1
37 8.8982144e-01 1.51e-04 6.40e-03 -3.8 3.79e-02 -0.8 1.00e+00 1.00e+00h 1
38 8.8119886e-01 7.39e-03 1.78e-01 -3.8 2.72e-01 -1.3 1.00e+00 1.00e+00h 1
39 8.2807051e-01 1.64e-03 5.97e-01 -3.8 5.94e-01 -0.9 1.00e+00 1.00e+00h 1
iter objective inf_pr inf_du lg(mu) ||d|| lg(rg) alpha_du alpha_pr ls
40 8.2645669e-01 2.85e-04 3.37e-01 -3.8 1.39e-01 -1.3 1.00e+00 1.00e+00h 1
41 8.2169029e-01 4.19e-04 6.65e-02 -3.8 6.32e-02 -0.9 1.00e+00 1.00e+00h 1
42 8.2131292e-01 3.08e-04 1.85e-03 -3.8 1.12e-01 - 1.00e+00 1.00e+00h 1
43 8.2134057e-01 9.21e-06 2.25e-05 -3.8 6.40e-02 - 1.00e+00 1.00e+00h 1
44 8.2134229e-01 3.86e-10 1.28e-08 -3.8 1.70e-03 - 1.00e+00 1.00e+00h 1
45 8.2134171e-01 1.89e-07 4.51e-06 -5.7 6.39e-03 - 1.00e+00 1.00e+00h 1
46 8.2134172e-01 1.01e-10 2.07e-08 -8.6 5.35e-04 - 1.00e+00 1.00e+00h 1
47 8.2134172e-01 1.78e-15 8.15e-14 -8.6 5.68e-07 - 1.00e+00 1.00e+00h 1
Number of Iterations....: 47
(scaled) (unscaled)
Objective...............: 8.2134172207722178e-01 8.2134172207722178e-01
Dual infeasibility......: 8.1523685205033382e-14 8.1523685205033382e-14
Constraint violation....: 1.7763568394002505e-15 1.7763568394002505e-15
Complementarity.........: 2.5059065610354838e-09 2.5059065610354838e-09
Overall NLP error.......: 2.5059065610354838e-09 2.5059065610354838e-09
Number of objective function evaluations = 89
Number of objective gradient evaluations = 48
Number of equality constraint evaluations = 89
Number of inequality constraint evaluations = 89
Number of equality constraint Jacobian evaluations = 48
Number of inequality constraint Jacobian evaluations = 48
Number of Lagrangian Hessian evaluations = 47
Total CPU secs in IPOPT (w/o function evaluations) = 0.071
Total CPU secs in NLP function evaluations = 0.304
EXIT: Optimal Solution Found.
time spent in eval_f: 0.001845 s. (89 calls, 0.0207303 ms. average)
time spent in eval_grad_f: 0.002997 s. (49 calls, 0.0611633 ms. average)
time spent in eval_g: 0.002474 s. (89 calls, 0.0277978 ms. average)
time spent in eval_jac_g: 0.001355 s. (50 calls, 0.0271 ms. average)
time spent in eval_h: 0.296114 s. (48 calls, 6.16904 ms. average)
time spent in main loop (proc): 0.547056 s.
time spent in main loop (wall): 0 s.
time spent in callback function: 0 s.
time spent in callback preparation: 0.000396 s.
This is Ipopt version 3.12, running with linear solver ma97.
Number of nonzeros in equality constraint Jacobian...: 81
Number of nonzeros in inequality constraint Jacobian.: 8
Number of nonzeros in Lagrangian Hessian.............: 567
Total number of variables............................: 40
variables with only lower bounds: 4
variables with lower and upper bounds: 8
variables with only upper bounds: 0
Total number of equality constraints.................: 28
Total number of inequality constraints...............: 4
inequality constraints with only lower bounds: 0
inequality constraints with lower and upper bounds: 0
inequality constraints with only upper bounds: 4
iter objective inf_pr inf_du lg(mu) ||d|| lg(rg) alpha_du alpha_pr ls
0 1.6731505e-01 2.20e+01 8.69e-03 -1.0 0.00e+00 - 0.00e+00 0.00e+00 0
1 4.0949726e+02 2.07e-01 7.69e+01 -1.0 2.69e+01 - 1.31e-02 1.00e+00h 1
2 3.7890521e+02 1.03e-02 6.30e+01 -1.0 6.30e-01 2.0 9.48e-01 1.00e+00f 1
3 2.3246872e+02 1.67e+00 8.29e+01 -1.0 3.38e+00 1.5 2.54e-01 9.20e-01f 1
4 3.1128240e+02 7.15e-01 2.23e+02 -1.0 2.72e+00 1.9 8.04e-02 1.00e+00h 1
5 3.1873835e+02 4.57e-02 1.99e+02 -1.0 6.39e-01 2.4 1.00e+00 1.00e+00h 1
6 3.2733981e+02 3.76e-04 8.23e+01 -1.0 1.30e-01 2.8 1.00e+00 1.00e+00h 1
7 3.1835337e+02 2.81e-04 1.62e+01 -1.0 7.71e-02 2.3 1.00e+00 1.00e+00f 1
8 2.9288806e+02 2.51e-03 1.54e+01 -1.0 2.20e-01 1.8 1.00e+00 1.00e+00f 1
9 2.2306953e+02 3.47e-01 1.84e+01 -1.0 9.37e-01 1.4 7.12e-01 1.00e+00f 1
iter objective inf_pr inf_du lg(mu) ||d|| lg(rg) alpha_du alpha_pr ls
10 1.8430006e+02 4.04e-03 2.27e+01 -1.0 3.64e-01 1.8 1.00e+00 1.00e+00f 1
11 1.2850364e+02 7.52e-02 1.41e+01 -1.0 5.32e-01 1.3 1.00e+00 1.00e+00f 1
12 5.2557064e+01 2.44e-01 9.06e+00 -1.0 1.20e+00 0.8 1.00e+00 1.00e+00f 1
13 1.0510404e+01 2.62e-02 2.81e+00 -1.0 1.22e+00 0.4 1.00e+00 1.00e+00f 1
14 6.4845590e-01 5.09e-03 2.50e-01 -1.0 1.19e+00 - 1.00e+00 1.00e+00f 1
15 6.4515468e-01 2.61e-03 1.66e-01 -1.7 2.28e-01 - 1.00e+00 1.00e+00h 1
16 6.4516371e-01 1.61e-05 7.97e-03 -2.5 5.15e-03 -0.1 1.00e+00 1.00e+00h 1
17 6.4495848e-01 7.50e-05 4.75e-03 -3.8 1.41e-02 -0.6 1.00e+00 1.00e+00h 1
18 6.4445684e-01 4.65e-04 1.04e-02 -3.8 3.52e-02 -1.1 1.00e+00 1.00e+00h 1
19 6.4033601e-01 8.80e-03 7.26e-02 -3.8 1.52e-01 -1.5 1.00e+00 1.00e+00h 1
iter objective inf_pr inf_du lg(mu) ||d|| lg(rg) alpha_du alpha_pr ls
20 6.2814916e-01 1.89e-02 3.79e-01 -3.8 2.78e+00 -1.1 1.00e+00 1.07e-01h 3
21 6.2204356e-01 1.24e-03 1.11e-01 -3.8 4.90e-02 0.2 1.00e+00 1.00e+00h 1
22 6.0233761e-01 2.87e-04 1.13e-01 -3.8 1.24e-01 -0.3 1.00e+00 1.00e+00h 1
23 5.8860809e-01 6.19e-04 4.26e-01 -3.8 2.77e-01 -0.7 1.00e+00 1.00e+00f 1
24 5.8330945e-01 1.65e-01 8.32e-02 -3.8 1.74e+01 - 4.19e-02 4.23e-02h 2
25 4.3518284e-01 1.62e+00 5.70e-01 -3.8 1.01e+01 - 2.04e-01 3.82e-01h 1
26 5.0934308e-01 1.45e+00 1.85e+00 -3.8 1.36e+00 0.6 1.00e+00 1.17e-01h 4
27 5.0539543e-01 1.33e+00 1.44e+00 -3.8 1.21e+01 - 8.24e-02 9.69e-02h 1
28 5.4191325e-01 6.82e-01 8.19e-01 -3.8 4.22e+00 - 9.76e-01 5.00e-01h 2
29 5.7043646e-01 3.44e-01 4.16e-01 -3.8 1.75e+00 - 5.56e-01 5.00e-01h 2
iter objective inf_pr inf_du lg(mu) ||d|| lg(rg) alpha_du alpha_pr ls
30 5.7844873e-01 1.72e-01 2.11e-01 -3.8 9.93e-01 - 5.62e-01 5.00e-01h 2
31 5.7988622e-01 4.98e-02 2.96e-02 -3.8 6.23e-01 - 8.73e-01 1.00e+00h 1
32 5.7114659e-01 3.68e-02 7.35e-03 -3.8 4.77e-01 - 1.00e+00 1.00e+00h 1
33 5.7745928e-01 3.25e-04 1.53e-02 -3.8 2.33e-01 - 1.00e+00 1.00e+00h 1
34 5.7744500e-01 4.59e-06 5.00e-03 -3.8 1.24e-02 - 1.00e+00 1.00e+00h 1
35 5.7743674e-01 1.93e-06 9.77e-04 -3.8 3.26e-03 - 1.00e+00 1.00e+00h 1
36 5.7741164e-01 7.47e-07 1.53e-04 -5.7 3.50e-02 - 9.81e-01 1.00e+00h 1
37 5.7741011e-01 1.16e-07 9.36e-06 -5.7 1.15e-02 - 1.00e+00 1.00e+00h 1
38 5.7741008e-01 1.10e-09 6.28e-08 -5.7 1.02e-03 - 1.00e+00 1.00e+00h 1
39 5.7741008e-01 5.67e-10 2.56e-08 -8.6 8.66e-04 - 1.00e+00 1.00e+00h 1
iter objective inf_pr inf_du lg(mu) ||d|| lg(rg) alpha_du alpha_pr ls
40 5.7741008e-01 5.68e-14 2.24e-12 -8.6 7.25e-06 - 1.00e+00 1.00e+00h 1
Number of Iterations....: 40
(scaled) (unscaled)
Objective...............: 5.7741007704104963e-01 5.7741007704104963e-01
Dual infeasibility......: 2.2412141587047074e-12 2.2412141587047074e-12
Constraint violation....: 5.6843418860808015e-14 5.6843418860808015e-14
Complementarity.........: 2.5067120126622258e-09 2.5067120126622258e-09
Overall NLP error.......: 2.5067120126622258e-09 2.5067120126622258e-09
Number of objective function evaluations = 53
Number of objective gradient evaluations = 41
Number of equality constraint evaluations = 53
Number of inequality constraint evaluations = 53
Number of equality constraint Jacobian evaluations = 41
Number of inequality constraint Jacobian evaluations = 41
Number of Lagrangian Hessian evaluations = 40
Total CPU secs in IPOPT (w/o function evaluations) = 0.036
Total CPU secs in NLP function evaluations = 0.109
EXIT: Optimal Solution Found.
time spent in eval_f: 0.000769 s. (53 calls, 0.0145094 ms. average)
time spent in eval_grad_f: 0.001702 s. (42 calls, 0.0405238 ms. average)
time spent in eval_g: 0.000976 s. (53 calls, 0.0184151 ms. average)
time spent in eval_jac_g: 0.000808 s. (43 calls, 0.0187907 ms. average)
time spent in eval_h: 0.105024 s. (41 calls, 2.56156 ms. average)
time spent in main loop (proc): 0.156976 s.
time spent in main loop (wall): 0 s.
time spent in callback function: 0 s.
time spent in callback preparation: 0.000286 s.
This is Ipopt version 3.12, running with linear solver ma97.
Number of nonzeros in equality constraint Jacobian...: 35
Number of nonzeros in inequality constraint Jacobian.: 4
Number of nonzeros in Lagrangian Hessian.............: 97
Total number of variables............................: 20
variables with only lower bounds: 2
variables with lower and upper bounds: 4
variables with only upper bounds: 0
Total number of equality constraints.................: 14
Total number of inequality constraints...............: 2
inequality constraints with only lower bounds: 0
inequality constraints with lower and upper bounds: 0
inequality constraints with only upper bounds: 2
iter objective inf_pr inf_du lg(mu) ||d|| lg(rg) alpha_du alpha_pr ls
0 2.2232202e-01 2.52e+01 1.98e-02 -1.0 0.00e+00 - 0.00e+00 0.00e+00 0
1 1.8458139e+02 2.62e-01 1.24e+02 -1.0 2.70e+01 - 1.43e-02 1.00e+00h 1
2 1.7637470e+02 5.67e-03 2.67e+01 -1.0 2.67e-01 2.0 9.61e-01 1.00e+00f 1
3 1.5299513e+02 5.42e-02 1.43e+01 -1.0 4.28e-01 1.5 1.00e+00 1.00e+00f 1
4 1.1285374e+02 2.16e-02 1.10e+01 -1.0 9.90e-01 1.0 1.00e+00 1.00e+00f 1
5 4.8251675e+01 5.56e-02 6.68e+00 -1.0 1.80e+00 0.6 1.00e+00 1.00e+00f 1
6 3.1832021e+01 8.22e-02 5.42e+00 -1.0 1.62e+01 - 2.40e-01 1.89e-01f 1
7 3.4873577e+00 1.11e-01 4.29e+00 -1.0 6.84e+00 - 4.87e-01 6.92e-01f 1
8 2.5192056e+00 8.81e-02 3.46e+00 -1.0 5.45e+00 - 1.00e+00 2.00e-01f 1
9 9.7821077e-01 1.99e-02 1.23e+00 -1.0 1.25e+00 - 1.00e+00 6.79e-01f 1
iter objective inf_pr inf_du lg(mu) ||d|| lg(rg) alpha_du alpha_pr ls
10 8.5673365e-01 6.01e-04 3.24e-02 -1.0 1.75e-01 - 1.00e+00 1.00e+00f 1
11 7.2353937e-01 1.95e-04 1.40e-02 -2.5 1.17e-01 - 9.61e-01 1.00e+00f 1
12 6.8426910e-01 5.04e-05 3.07e-03 -2.5 8.93e-02 - 1.00e+00 1.00e+00h 1
13 6.7187049e-01 6.46e-06 4.63e-04 -3.8 4.54e-02 - 1.00e+00 1.00e+00h 1
14 6.7075873e-01 7.32e-08 2.73e-05 -5.7 3.36e-02 - 1.00e+00 1.00e+00h 1
15 6.7075270e-01 4.43e-12 4.48e-08 -5.7 1.69e-03 - 1.00e+00 1.00e+00h 1
16 6.7074717e-01 9.81e-13 1.69e-10 -8.6 6.67e-05 - 1.00e+00 1.00e+00h 1
Number of Iterations....: 16
(scaled) (unscaled)
Objective...............: 6.7074717283315954e-01 6.7074717283315954e-01
Dual infeasibility......: 1.6891305021360381e-10 1.6891305021360381e-10
Constraint violation....: 9.8054897534893826e-13 9.8054897534893826e-13
Complementarity.........: 2.6509688059849003e-09 2.6509688059849003e-09
Overall NLP error.......: 2.6509688059849003e-09 2.6509688059849003e-09
Number of objective function evaluations = 17
Number of objective gradient evaluations = 17
Number of equality constraint evaluations = 17
Number of inequality constraint evaluations = 17
Number of equality constraint Jacobian evaluations = 17
Number of inequality constraint Jacobian evaluations = 17
Number of Lagrangian Hessian evaluations = 16
Total CPU secs in IPOPT (w/o function evaluations) = 0.012
Total CPU secs in NLP function evaluations = 0.013
EXIT: Optimal Solution Found.
time spent in eval_f: 0.000159 s. (17 calls, 0.00935294 ms. average)
time spent in eval_grad_f: 0.000412 s. (18 calls, 0.0228889 ms. average)
time spent in eval_g: 0.000277 s. (17 calls, 0.0162941 ms. average)
time spent in eval_jac_g: 0.000211 s. (19 calls, 0.0111053 ms. average)
time spent in eval_h: 0.011482 s. (17 calls, 0.675412 ms. average)
time spent in main loop (proc): 0.058516 s.
time spent in main loop (wall): 0 s.
time spent in callback function: 0 s.
time spent in callback preparation: 0.000119 s.
In [14]:
# Plot
mpc_plot(X_all, B_all, EB_all, N_sim, N_ctrl, N_delay)
In [ ]:
In [ ]:
In [ ]:
In [ ]:
t = ca.SX.sym('t')
x = ca.SX.sym('x')
u = ca.SX.sym('u')
dae = ca.SXFunction('Dynamics', ca.daeIn(x=x, p=u, t=t), ca.daeOut(ode=u))
integrator = ca.Integrator('Integrator', 'cvodes', dae)
integrator({'x0': x, 'p': u})['xf']
In [ ]:
In [ ]:
t = ca.SX.sym('t')
# Create f
[x_b,y_b,vx_b,vy_b,x_c,y_c,phi] = state[...]
[v,w,psi] = control[...]
# Define right-hand side
rhs = cat.struct_SX(state)
rhs['x_b'] = vx_b
rhs['y_b'] = vy_b
rhs['vx_b'] = 0
rhs['vy_b'] = 0
rhs['x_c'] = v * ca.cos(phi)
rhs['y_c'] = v * ca.sin(phi)
rhs['phi'] = w
# Continuous dynamics
dae = ca.SXFunction('Continuous dynamics', ca.daeIn(x=state, p=control, t=t), ca.daeOut(ode=rhs))
In [ ]:
# Integrator
opts = {
'tf' : dt
}
integrator = ca.Integrator('Integrator', 'cvodes', dae, opts)
In [ ]:
# Get integration result
state_next = integrator({'x0': state, 'p': control})['xf']
In [ ]:
F = ca.SXFunction('Discrete dynamics', [state,control], [state_next])
In [ ]:
In [ ]:
Content source: b4be1/ball_catcher
Similar notebooks: