Note
Go to the end to download the full example code.
Linear Covariance Analysis¶
This example is a simple implementation of linear covariance analysis [1] to evaluate closed-loop guidance, navigation, and control performance by propagating navigation errors and trajectory dispersions through a single simulation pass rather than simulating many realizations as in a Monte Carlo analysis.
We’ll use the Clohessy-Wiltshire equations to simulate the relative motion between a target vehicle in circular orbit and a chaser vehicle.
import casadi as ca
import numpy as np
import condor as co
from condor.backend import operators as ops
I6 = ops.eye(6)
Z6 = ops.zeros((6, 6))
W = ops.concat([I6, Z6], axis=0)
I3 = ops.eye(3)
Z3 = ops.zeros((3, 3))
V = ops.concat([Z3, I3, ops.zeros((6, 3))], axis=0)
The core of the analysis is an ODE system that represents the true state the follows the CW equations.
class LinCovCW(co.ODESystem):
omega = parameter()
scal_w = parameter(shape=6)
x = state(shape=6)
C = state(shape=(12, 12), symmetric=True)
initial[x] = parameter(shape=x.shape, name="initial_x")
initial[C] = parameter(shape=C.shape, name="initial_C")
# [0, 0, 0, 1, 0, 0],
# [0, 0, 0, 0, 1, 0],
# [0, 0, 0, 0, 0, 1],
# [0, 0, 0, 0, 0, 2*omega],
# [0, -omega**2, 0, 0, 0, 0],
# [0, 0, 3*omega**2, -2*omega, 0, 0]
Acw = ops.zeros((6, 6))
Acw[:3, 3:] = ops.eye(3)
Acw[3, 5] = 2 * omega
Acw[4, 1] = -(omega**2)
Acw[5, 2] = 3 * omega**2
Acw[5, 3] = -2 * omega
Scal_w = ops.diag(scal_w)
Cov_prop_offset = W @ Scal_w @ W.T
Fcal = ops.zeros((12, 12))
Fcal[:6, :6] = Acw
Fcal[6:, 6:] = Acw
dot[x] = Acw @ x
dot[C] = Fcal @ C + C @ Fcal.T + Cov_prop_offset
A discrete targeting maneuver can be implemented as an event on the ODE system that instantaneously updates the relative velocity as well as the augmented covariance.
class MajorBurn(LinCovCW.Event):
#: Target position
rd = parameter(shape=3)
#: Time ignition
tig = parameter()
#: Time end maneuver
tem = parameter()
scal_v = parameter(shape=3)
Delta_v_mag = state()
Delta_v_disp = state()
at_time = tig
td = tem - tig
stm = ops.zeros((6, 6))
stm[0, 0] = 1
stm[0, 2] = 6 * omega * td - 6 * ops.sin(omega * td)
stm[0, 3] = -3 * td + 4 * ops.sin(omega * td) / omega
stm[0, 5] = 2 * (1 - ops.cos(omega * td)) / omega
stm[1, 1] = ops.cos(omega * td)
stm[1, 4] = ops.sin(omega * td) / omega
stm[2, 2] = 4 - 3 * ops.cos(omega * td)
stm[2, 3] = 2 * (ops.cos(omega * td) - 1) / omega
stm[2, 5] = ops.sin(omega * td) / omega
stm[3, 2] = 6 * omega * (1 - ops.cos(omega * td))
stm[3, 3] = 4 * ops.cos(omega * td) - 3
stm[3, 5] = 2 * ops.sin(omega * td)
stm[4, 1] = -omega * ops.sin(omega * td)
stm[4, 4] = ops.cos(omega * td)
stm[5, 2] = 3 * omega * ops.sin(omega * td)
stm[5, 3] = -2 * ops.sin(omega * td)
stm[5, 5] = ops.cos(omega * td)
T_pp = stm[:3, :3]
T_pv = stm[:3, 3:]
T_pv_inv = ca.solve(T_pv, ops.eye(3))
Delta_v = (T_pv_inv @ rd - T_pv_inv @ T_pp @ x[:3, 0]) - x[3:, 0]
update[Delta_v_mag] = Delta_v_mag + ca.norm_2(Delta_v)
update[x] = x + ops.concat([ops.zeros((3, 1)), Delta_v])
DG = ca.vertcat(ops.zeros((3, 6)), ca.horzcat(-(T_pv_inv @ T_pp), -I3))
Dcal = ca.vertcat(
ca.horzcat(I6, DG),
ca.horzcat(Z6, I6 + DG),
)
Scal_v = ops.diag(scal_v)
update[C] = Dcal @ C @ Dcal.T + V @ Scal_v @ V.T
Mc = DG @ ops.concat([Z6, I6], axis=1)
sigma_Dv__2 = ca.trace(Mc @ C @ Mc.T)
update[Delta_v_disp] = Delta_v_disp + ca.sqrt(sigma_Dv__2)
A Terminating event at the end of the maneuver
class Terminate(LinCovCW.Event):
terminate = True
at_time = MajorBurn.tem
Trajectory analysis to simulate the system
class Sim(LinCovCW.TrajectoryAnalysis):
# TODO: add final burn Delta v (assume final relative v is 0, can get magnitude and
# dispersion)
tot_Delta_v_mag = trajectory_output(Delta_v_mag)
tot_Delta_v_disp = trajectory_output(Delta_v_disp)
# tf = parameter()
Mr = ca.horzcat(I3, ca.MX(3, 9))
sigma_r__2 = ca.trace(Mr @ C @ Mr.T)
final_pos_disp = trajectory_output(ca.sqrt(sigma_r__2))
Simulate
def make_C0(sigma_p, sigma_v, rho, sigma_p_nav=None, sigma_v_nav=None, rho_nav=None):
D0 = np.zeros((6, 6))
D0[:3, :3] = np.eye(3) * (sigma_p**2)
D0[2, 3] = rho * sigma_p * sigma_v
D0[3, 2] = rho * sigma_p * sigma_v
D0[3:, 3:] = np.eye(3) * (sigma_v**2)
sigma_p_nav = sigma_p_nav or sigma_p
sigma_v_nav = sigma_v_nav or sigma_v
rho_nav = rho_nav or rho
P0 = np.zeros((6, 6))
P0[:3, :3] = np.eye(3) * (sigma_p_nav**2)
P0[2, 3] = rho_nav * sigma_p_nav * sigma_v_nav
P0[3, 2] = rho_nav * sigma_p_nav * sigma_v_nav
P0[3:, 3:] = np.eye(3) * (sigma_v_nav**2)
C = np.concat(
(np.concat((D0, D0), axis=1), np.concat((D0, D0 + P0), axis=1)), axis=0
)
return C
initial_C = make_C0(100 / 3, 0.11 / 3, 0.9, 10 / 3, 0.011 / 3, 0.9)
sim_kwargs = dict(
omega=0.00114,
scal_w=[0.0] * 3 + [4.8e-10] * 3,
scal_v=[2.5e-7] * 3,
initial_x=[-2000.0, 0.0, 1000.0, 1.71, 0.0, 0.0],
initial_C=make_C0(100 / 3, 0.11 / 3, 0.9, 10 / 3, 0.011 / 3, 0.9),
rd=[500.0, 0.0, 0.0],
)
out = Sim(**sim_kwargs, tig=156.7, tem=156.7 + 15 * 60)
print(out.final_pos_disp)
[11.03987383]
import matplotlib.pyplot as plt
from matplotlib.patches import Ellipse
def plot_traj(sim, x_idx=0, y_idx=2):
"""create relative motion plot in the vertical plane"""
fig, ax = plt.subplots(constrained_layout=True)
ax.invert_xaxis()
ax.set_xlabel("V-bar (m)")
ax.invert_yaxis()
ax.set_ylabel("R-bar (m)")
ax.grid(True)
ax.set_aspect("equal")
plt.xlim(1000, -2500)
plt.ylim(1500, -500)
fig.set_size_inches(7.4, 4.2)
sim_color = "C0"
plt.plot([0.0], [0.0], "ok")
xs = sim.x[x_idx].squeeze()
ys = sim.x[y_idx].squeeze()
ax.plot(xs, ys, "o-", color=sim_color)
ax.plot(xs[0], ys[0], "og")
n_std = 3
for pos_vel, cov in zip(sim.x.T.squeeze(), sim.C.T):
pos = pos_vel[[x_idx, y_idx]]
C = cov[[x_idx, y_idx]][:, [x_idx, y_idx]]
lambda_, v = np.linalg.eig(C)
lambda_ = np.sqrt(lambda_)
ellipse = Ellipse(
pos,
width=lambda_[0] * n_std * 2,
height=lambda_[1] * n_std * 2,
angle=np.degrees(np.arctan2(*v[:, 0][::-1])),
edgecolor=sim_color,
facecolor="none",
)
ax.add_patch(ellipse)
return ax
plot_traj(out)
# TODO
# DV_idx = Sim.trajectory_output.flat_index(Sim.tot_Delta_v_mag)
# tig_idx = Sim.parameter.flat_index(Sim.tig)
# tem_idx = Sim.parameter.flat_index(Sim.tem)
# init_jac = Sim.implementation.callback.jac_callback(Sim.implementation.callback.p, [])
# print("init grad wrt tig", init_jac[DV_idx, tig_idx])
# print("init grad wrt tem", init_jac[DV_idx, tem_idx])
# """
# init grad wrt tig 0.0209833
# init grad wrt tem -0.0260249
# """
<Axes: xlabel='V-bar (m)', ylabel='R-bar (m)'>
class Hohmann(co.OptimizationProblem):
tig = variable(initializer=200.0)
tem = variable(initializer=500.0)
sim = Sim(tig=tig, tem=tem, **sim_kwargs)
objective = sim.tot_Delta_v_mag
constraint(tem > tig + 30)
constraint(tig > 0.1)
class Options:
# TODO
# __implementation__ = co.implementations.ScipyTrustConstr
exact_hessian = False
from time import perf_counter
hoh_start = perf_counter()
hohmann = Hohmann()
hoh_stop = perf_counter()
print(hohmann._stats)
print((hohmann.tem - hohmann.tig) * hohmann.sim.omega * 180 / np.pi)
# TODO
# opt_jac = Sim.implementation.callback.jac_callback(Sim.implementation.callback.p, [])
# print("opt grad wrt tig", opt_jac[DV_idx, tig_idx])
# print("opt grad wrt tem", opt_jac[DV_idx, tem_idx])
# """
# opt grad wrt tig -4.48258e-09
# opt grad wrt tem -1.47125e-09
# """
This is Ipopt version 3.14.11, running with linear solver MUMPS 5.4.1.
Number of nonzeros in equality constraint Jacobian...: 0
Number of nonzeros in inequality constraint Jacobian.: 3
Number of nonzeros in Lagrangian Hessian.............: 0
Total number of variables............................: 2
variables with only lower bounds: 0
variables with lower and upper bounds: 0
variables with only upper bounds: 0
Total number of equality constraints.................: 0
Total number of inequality constraints...............: 2
inequality constraints with only lower bounds: 2
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 6.4936391e+00 0.00e+00 2.60e-02 0.0 0.00e+00 - 0.00e+00 0.00e+00 0
1 6.4925217e+00 0.00e+00 1.46e-02 -6.6 4.70e-02 - 9.90e-01 1.00e+00f 1
2 4.1522436e+00 0.00e+00 6.51e-03 -2.8 1.47e+02 - 9.84e-01 1.00e+00f 1
3 3.1748709e+00 0.00e+00 4.05e-03 -4.8 1.15e+02 - 1.00e+00 1.00e+00f 1
4 2.2263549e+00 0.00e+00 2.21e-03 -5.7 1.86e+02 - 1.00e+00 1.00e+00f 1
5 1.5484505e+00 0.00e+00 1.38e-03 -6.5 2.22e+02 - 1.00e+00 1.00e+00f 1
6 1.1046691e+00 0.00e+00 9.48e-04 -7.0 2.93e+02 - 1.00e+00 7.15e-01f 1
7 6.1851591e-01 0.00e+00 4.91e-04 -6.0 3.99e+02 - 4.14e-03 1.00e+00f 1
8 3.8428264e-01 0.00e+00 2.04e-04 -6.6 4.13e+02 - 1.00e+00 1.00e+00f 1
9 3.2279045e-01 0.00e+00 7.42e-05 -6.6 2.92e+02 - 1.00e+00 1.00e+00f 1
iter objective inf_pr inf_du lg(mu) ||d|| lg(rg) alpha_du alpha_pr ls
10 2.9923153e-01 0.00e+00 4.12e-05 -7.6 2.44e+02 - 1.00e+00 1.00e+00f 1
11 2.8667982e-01 0.00e+00 1.61e-05 -11.0 3.09e+02 - 1.00e+00 1.00e+00f 1
12 2.8516474e-01 0.00e+00 4.61e-06 -8.4 9.54e+01 - 1.00e+00 1.00e+00f 1
13 2.8500540e-01 0.00e+00 7.76e-07 -9.4 3.64e+01 - 1.00e+00 1.00e+00f 1
14 2.8500011e-01 0.00e+00 1.72e-07 -11.0 8.69e+00 - 1.00e+00 1.00e+00f 1
15 2.8500003e-01 0.00e+00 9.59e-08 -11.0 7.39e-01 - 1.00e+00 1.00e+00f 1
16 2.8500000e-01 0.00e+00 5.46e-11 -11.0 2.81e-01 - 1.00e+00 1.00e+00h 1
Number of Iterations....: 16
(scaled) (unscaled)
Objective...............: 2.8500000000002523e-01 2.8500000000002523e-01
Dual infeasibility......: 5.4600501196373501e-11 5.4600501196373501e-11
Constraint violation....: 0.0000000000000000e+00 0.0000000000000000e+00
Variable bound violation: 0.0000000000000000e+00 0.0000000000000000e+00
Complementarity.........: 9.9999999167391752e-12 9.9999999167391752e-12
Overall NLP error.......: 5.4600501196373501e-11 5.4600501196373501e-11
Number of objective function evaluations = 17
Number of objective gradient evaluations = 17
Number of equality constraint evaluations = 0
Number of inequality constraint evaluations = 17
Number of equality constraint Jacobian evaluations = 0
Number of inequality constraint Jacobian evaluations = 17
Number of Lagrangian Hessian evaluations = 0
Total seconds in IPOPT = 3.535
EXIT: Optimal Solution Found.
{'iter_count': 16, 'iterations': {'alpha_du': [0.0, 0.9898285295685858, 0.9842402293560555, 1.0, 1.0, 1.0, 1.0, 0.004140027452292917, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0], 'alpha_pr': [0.0, 1.0, 1.0, 1.0, 1.0, 1.0, 0.7149778955287908, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0], 'd_norm': [0.0, 0.04700771427236512, 147.0343393040177, 115.44642648212835, 186.41902166571953, 221.9326066411802, 292.9552687947239, 399.49156752841947, 413.03084327256175, 291.56535502963817, 244.33777326276436, 308.7385371689279, 95.41495081615221, 36.41536802920983, 8.690722845600636, 0.7389692851209383, 0.28148098240986763], 'inf_du': [0.026024879059342105, 0.014605956528173522, 0.006505450906661856, 0.0040476207149814, 0.00220898995347599, 0.0013759448721080239, 0.0009482486871725277, 0.0004911213979005004, 0.00020364975153507164, 7.418601739019045e-05, 4.1238876684231294e-05, 1.6056230256091602e-05, 4.606922850215797e-06, 7.761787402831366e-07, 1.719536451262994e-07, 9.587813839220988e-08, 5.46005011963735e-11], 'inf_pr': [0.0, 0.0, 5.684341886080802e-14, 1.1368683772161603e-13, 1.1368683772161603e-13, 0.0, 0.0, 2.2737367544323206e-13, 2.2737367544323206e-13, 4.547473508864641e-13, 4.547473508864641e-13, 0.0, 4.547473508864641e-13, 4.547473508864641e-13, 0.0, 4.547473508864641e-13, 4.547473508864641e-13], 'mu': [1.0, 2.3495000001000001e-07, 0.0014545402508683457, 1.5658669865842725e-05, 2.093876447410522e-06, 3.1758246357022486e-07, 9.878583835498034e-08, 1.0564209597778857e-06, 2.5408339995710536e-07, 2.3511341809476054e-07, 2.6468653562476205e-08, 1e-11, 3.773980300584271e-09, 3.898525625933994e-10, 1e-11, 1e-11, 1e-11], 'obj': [6.493639138134979, 6.492521733862819, 4.1522436029850045, 3.174870894093014, 2.2263548730449343, 1.548450524063878, 1.1046690573928997, 0.6185159065573307, 0.38428263659915207, 0.3227904451096955, 0.2992315271794575, 0.2866798222756138, 0.28516473560253824, 0.28500540412278463, 0.2850001073857912, 0.2850000346414181, 0.28500000000002523], 'regularization_size': [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]}, 'n_call_callback_fun': 0, 'n_call_nlp_f': 17, 'n_call_nlp_g': 17, 'n_call_nlp_grad': 0, 'n_call_nlp_grad_f': 18, 'n_call_nlp_jac_g': 18, 'return_status': 'Solve_Succeeded', 'success': True, 't_proc_callback_fun': 0.0, 't_proc_nlp_f': 1.58444, 't_proc_nlp_g': 0.00020200000000000003, 't_proc_nlp_grad': 0.0, 't_proc_nlp_grad_f': 8.218545, 't_proc_nlp_jac_g': 0.000152, 't_wall_callback_fun': 0.0, 't_wall_nlp_f': 0.545384529, 't_wall_nlp_g': 0.000121345, 't_wall_nlp_grad': 0.0, 't_wall_nlp_grad_f': 2.957861413, 't_wall_nlp_jac_g': 0.000146705, 'unified_return_status': 'SOLVER_RET_SUCCESS'}
[179.99997662]
hohmann_sim = Sim(**sim_kwargs, tig=hohmann.tig, tem=hohmann.tem)
print(hohmann_sim.tot_Delta_v_disp)
print(hohmann_sim.final_pos_disp)
print(hohmann.tig, hohmann.tem)
print("time:", hoh_stop - hoh_start)
[93543.09121237]
[42980.59515077]
[84.09660115] [2839.87927261]
time: 3.5427948549999826
plot_traj(hohmann_sim)
<Axes: xlabel='V-bar (m)', ylabel='R-bar (m)'>
class TotalDeltaV(co.OptimizationProblem):
tig = variable(initializer=200.0)
tem = variable(initializer=500.0)
constraint(tem - tig, lower_bound=30.0)
constraint(tig, lower_bound=0.0)
sim = Sim(tig=tig, tem=tem, **sim_kwargs)
# TODO: adding a parameter and constraint to existing problem SHOULD be done by
# inheritance... I suppose the originally Hohmann model could easily be written to
# include more parameters to solve all permutations of this problem... weights for
# each output, upper bounds for each output (and combinations?)
# what about including a default for a paremter at a model level? no, just make a
# dict like unbounded_kwargs to fill in with a large number/inf
pos_disp_max = parameter()
constraint(sim.final_pos_disp - pos_disp_max, upper_bound=0.0)
objective = sim.tot_Delta_v_mag + 3 * sim.tot_Delta_v_disp
class Options:
exact_hessian = False
total_delta_v = TotalDeltaV(pos_disp_max=1000)
This is Ipopt version 3.14.11, running with linear solver MUMPS 5.4.1.
Number of nonzeros in equality constraint Jacobian...: 0
Number of nonzeros in inequality constraint Jacobian.: 5
Number of nonzeros in Lagrangian Hessian.............: 0
Total number of variables............................: 2
variables with only lower bounds: 0
variables with lower and upper bounds: 0
variables with only upper bounds: 0
Total number of equality constraints.................: 0
Total number of inequality constraints...............: 3
inequality constraints with only lower bounds: 2
inequality constraints with lower and upper bounds: 0
inequality constraints with only upper bounds: 1
iter objective inf_pr inf_du lg(mu) ||d|| lg(rg) alpha_du alpha_pr ls
0 7.1721180e+00 0.00e+00 2.79e-02 0.0 0.00e+00 - 0.00e+00 0.00e+00 0
1 7.1707977e+00 0.00e+00 1.58e-02 -6.3 5.12e-02 - 9.90e-01 1.00e+00f 1
2 4.6314943e+00 0.00e+00 1.28e-02 -2.8 1.47e+02 - 9.99e-01 1.00e+00f 1
3 3.5748124e+00 0.00e+00 8.14e-03 -4.9 1.16e+02 - 9.99e-01 1.00e+00f 1
4 2.5573172e+00 0.00e+00 4.64e-03 -6.0 1.88e+02 - 1.00e+00 1.00e+00f 1
5 2.1303875e+00 0.00e+00 3.47e-03 -2.6 2.23e+02 - 1.00e+00 5.34e-01f 1
6 1.3896162e+00 0.00e+00 1.87e-03 -3.9 2.90e+02 - 7.65e-01 1.00e+00f 1
7 9.2343203e-01 0.00e+00 9.86e-04 -9.6 3.38e+02 - 5.86e-01 1.00e+00h 1
8 6.5880088e-01 0.00e+00 3.97e-04 -4.1 4.10e+02 - 1.00e+00 1.00e+00h 1
9 5.9556262e-01 0.00e+00 9.33e-05 -5.0 2.73e+02 - 9.70e-01 1.00e+00h 1
iter objective inf_pr inf_du lg(mu) ||d|| lg(rg) alpha_du alpha_pr ls
10 5.9214434e-01 0.00e+00 2.49e-05 -7.1 8.68e+01 - 1.00e+00 1.00e+00h 1
11 5.9201333e-01 0.00e+00 2.46e-06 -6.8 1.30e+01 - 9.99e-01 1.00e+00h 1
12 5.9200809e-01 0.00e+00 8.58e-07 -11.0 2.16e+00 - 1.00e+00 1.00e+00h 1
13 5.9200750e-01 0.00e+00 4.37e-09 -11.0 1.05e+00 - 1.00e+00 1.00e+00h 1
Number of Iterations....: 13
(scaled) (unscaled)
Objective...............: 5.9200749529483088e-01 5.9200749529483088e-01
Dual infeasibility......: 4.3674765216242987e-09 4.3674765216242987e-09
Constraint violation....: 0.0000000000000000e+00 0.0000000000000000e+00
Variable bound violation: 0.0000000000000000e+00 0.0000000000000000e+00
Complementarity.........: 1.0003863375195823e-11 1.0003863375195823e-11
Overall NLP error.......: 4.3674765216242987e-09 4.3674765216242987e-09
Number of objective function evaluations = 14
Number of objective gradient evaluations = 14
Number of equality constraint evaluations = 0
Number of inequality constraint evaluations = 14
Number of equality constraint Jacobian evaluations = 0
Number of inequality constraint Jacobian evaluations = 14
Number of Lagrangian Hessian evaluations = 0
Total seconds in IPOPT = 5.091
EXIT: Optimal Solution Found.
tot_delta_v_sim = Sim(**sim_kwargs, tig=total_delta_v.tig, tem=total_delta_v.tem)
plot_traj(tot_delta_v_sim)
<Axes: xlabel='V-bar (m)', ylabel='R-bar (m)'>
total_delta_v_constrained = TotalDeltaV(pos_disp_max=10.0)
This is Ipopt version 3.14.11, running with linear solver MUMPS 5.4.1.
Number of nonzeros in equality constraint Jacobian...: 0
Number of nonzeros in inequality constraint Jacobian.: 5
Number of nonzeros in Lagrangian Hessian.............: 0
Total number of variables............................: 2
variables with only lower bounds: 0
variables with lower and upper bounds: 0
variables with only upper bounds: 0
Total number of equality constraints.................: 0
Total number of inequality constraints...............: 3
inequality constraints with only lower bounds: 2
inequality constraints with lower and upper bounds: 0
inequality constraints with only upper bounds: 1
iter objective inf_pr inf_du lg(mu) ||d|| lg(rg) alpha_du alpha_pr ls
0 5.9200750e-01 2.16e+01 1.00e-03 0.0 0.00e+00 - 0.00e+00 0.00e+00 0
1 5.9200750e-01 2.16e+01 2.14e+00 -6.1 2.16e+01 - 9.90e-01 4.58e-04h 1
2 1.2228222e+00 3.62e+00 7.92e-04 -1.2 9.87e+02 - 1.00e+00 1.00e+00h 1
3 1.7561268e+00 4.26e-01 1.00e-03 -2.5 2.71e+02 - 1.00e+00 1.00e+00h 1
4 1.8678722e+00 0.00e+00 1.40e-04 -2.5 4.38e+01 - 1.00e+00 9.99e-01h 1
5 1.8655032e+00 9.04e-05 2.26e-04 -3.7 8.88e-01 - 1.00e+00 9.83e-01h 1
6 1.8653259e+00 0.00e+00 4.41e-07 -9.8 1.48e-01 - 1.00e+00 1.00e+00h 1
7 1.8653256e+00 1.77e-07 6.53e-11 -11.0 2.08e-04 - 1.00e+00 9.99e-01h 1
8 1.8653257e+00 3.71e-09 5.16e-12 -11.0 1.83e-05 - 1.00e+00 1.00e+00h 1
Number of Iterations....: 8
(scaled) (unscaled)
Objective...............: 1.8653256701834278e+00 1.8653256701834278e+00
Dual infeasibility......: 5.1613787029058944e-12 5.1613787029058944e-12
Constraint violation....: 3.7053692839117500e-09 3.7053692839117500e-09
Variable bound violation: 0.0000000000000000e+00 0.0000000000000000e+00
Complementarity.........: 1.0007858846440985e-11 1.0007858846440985e-11
Overall NLP error.......: 3.7053692839117500e-09 3.7053692839117500e-09
Number of objective function evaluations = 9
Number of objective gradient evaluations = 9
Number of equality constraint evaluations = 0
Number of inequality constraint evaluations = 9
Number of equality constraint Jacobian evaluations = 0
Number of inequality constraint Jacobian evaluations = 9
Number of Lagrangian Hessian evaluations = 0
Total seconds in IPOPT = 3.344
EXIT: Optimal Solution Found.
tot_delta_v_constrained_sim = Sim(
**sim_kwargs, tig=total_delta_v_constrained.tig, tem=total_delta_v_constrained.tem
)
plot_traj(tot_delta_v_constrained_sim)
<Axes: xlabel='V-bar (m)', ylabel='R-bar (m)'>
print("\n" * 2, "unconstrained Delta v")
print(total_delta_v._stats)
print((total_delta_v.tem - total_delta_v.tig) * total_delta_v.sim.omega * 180 / np.pi)
print(tot_delta_v_sim.final_pos_disp)
print("\n" * 2, "constrained Delta v")
print(total_delta_v_constrained._stats)
print(
(total_delta_v_constrained.tem - total_delta_v_constrained.tig)
* total_delta_v_constrained.sim.omega
* 180
/ np.pi
)
print(tot_delta_v_constrained_sim.final_pos_disp)
plt.show()
unconstrained Delta v
{'iter_count': 13, 'iterations': {'alpha_du': [0.0, 0.9898141549346541, 0.9989278174798525, 0.9990960213085416, 1.0, 1.0, 0.7648596964406856, 0.5862474784081099, 1.0, 0.9697500890827727, 1.0, 0.9992803183413812, 0.999730792435811, 1.0], 'alpha_pr': [0.0, 1.0, 1.0, 1.0, 1.0, 0.5339668186920494, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0], 'd_norm': [0.0, 0.05118226403964429, 147.31055370906319, 115.9030963785144, 187.7307422406854, 222.85632685767274, 289.9825004267278, 338.1023341474204, 410.30625790258506, 272.9966005936474, 86.7514026881369, 13.026263618110319, 2.1579846814235597, 1.0512703158552346], 'inf_du': [0.02790119311111404, 0.015815194559341193, 0.012774754882013146, 0.008137855209943404, 0.004635016915627543, 0.00346838177267059, 0.001865019991076233, 0.0009862330287145002, 0.0003968806797413072, 9.334584411958396e-05, 2.4910941790117898e-05, 2.460104947922069e-06, 8.583286196174579e-07, 4.367476521624299e-09], 'inf_pr': [0.0, 4.360686034488026e-09, 0.03827253453391677, 0.028469932938151032, 0.0916000178693821, 0.09143869225079015, 0.504690568855608, 0.654219996834172, 0.816749143628158, 0.26204794649061114, 0.014750287883089186, 7.302732888092578e-05, 1.0747301985247759e-05, 2.520303382880229e-06], 'mu': [1.0, 4.877296677568146e-07, 0.0014972410914916795, 1.3109105651092832e-05, 9.418225541706091e-07, 0.0024587055752338387, 0.00012123606688100062, 2.4710280014104476e-10, 8.115076356398707e-05, 1.005939225662401e-05, 7.416463910006809e-08, 1.7263629772068547e-07, 1e-11, 1e-11], 'obj': [7.17211796170742, 7.170797695051909, 4.631494345636032, 3.57481241849663, 2.5573171734478692, 2.13038750389673, 1.3896162137529395, 0.9234320342547346, 0.6588008797889437, 0.5955626221096694, 0.5921443437405733, 0.5920133330973244, 0.5920080922567432, 0.5920074952948309], 'regularization_size': [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]}, 'n_call_callback_fun': 0, 'n_call_nlp_f': 14, 'n_call_nlp_g': 14, 'n_call_nlp_grad': 0, 'n_call_nlp_grad_f': 15, 'n_call_nlp_jac_g': 15, 'return_status': 'Solve_Succeeded', 'success': True, 't_proc_callback_fun': 0.0, 't_proc_nlp_f': 0.0019140000000000003, 't_proc_nlp_g': 1.4388120000000002, 't_proc_nlp_grad': 0.0, 't_proc_nlp_grad_f': 6.550837999999999, 't_proc_nlp_jac_g': 7.561986999999999, 't_wall_callback_fun': 0.0, 't_wall_nlp_f': 0.0014354359999999998, 't_wall_nlp_g': 0.427427727, 't_wall_nlp_grad': 0.0, 't_wall_nlp_grad_f': 2.282310342, 't_wall_nlp_jac_g': 2.357267798, 'unified_return_status': 'SOLVER_RET_SUCCESS'}
[141.41273551]
[31.59576929]
constrained Delta v
{'iter_count': 8, 'iterations': {'alpha_du': [0.0, 0.9900018897466567, 1.0, 1.0, 1.0, 1.0, 0.9996268012042545, 1.0, 1.0], 'alpha_pr': [0.0, 0.0004582329793298304, 1.0, 1.0, 0.9994936327994016, 0.9830114834329401, 1.0, 0.9991980560682624, 1.0], 'd_norm': [0.0, 21.60473044624338, 987.2174903416433, 270.604063877841, 43.784350825985435, 0.88782206324506, 0.1475963083607423, 0.0002075639437303516, 1.8250048221303496e-05], 'inf_du': [0.001, 2.137965788523227, 0.0007916274745348199, 0.0010007425567115336, 0.00014046987840786538, 0.00022595345100623332, 4.4122064773543905e-07, 6.530280179453674e-11, 5.161378702905894e-12], 'inf_pr': [21.605769282246474, 21.595868806222196, 3.6514809624529843, 0.42851534272920383, 0.01171158463192223, 0.00020366313871320388, 1.2360041218611366e-07, 1.7664496531441501e-07, 3.739342611887318e-09], 'mu': [1.0, 7.416902494414024e-07, 0.07074735738619545, 0.003465881807236856, 0.003538173208019528, 0.00020039387938700507, 1.44963112998512e-10, 1e-11, 1.000785830098696e-11], 'obj': [0.5920074952948309, 0.5920074952949033, 1.222822205246997, 1.756126803238427, 1.8678722024772674, 1.8655031920393472, 1.8653259329125786, 1.865325618137415, 1.8653256701834278], 'regularization_size': [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]}, 'n_call_callback_fun': 0, 'n_call_nlp_f': 9, 'n_call_nlp_g': 9, 'n_call_nlp_grad': 0, 'n_call_nlp_grad_f': 10, 'n_call_nlp_jac_g': 10, 'return_status': 'Solve_Succeeded', 'success': True, 't_proc_callback_fun': 0.0, 't_proc_nlp_f': 0.003979, 't_proc_nlp_g': 0.8663569999999999, 't_proc_nlp_grad': 0.0, 't_proc_nlp_grad_f': 4.531675000000001, 't_proc_nlp_jac_g': 4.800182, 't_wall_callback_fun': 0.0, 't_wall_nlp_f': 0.000981068, 't_wall_nlp_g': 0.237523443, 't_wall_nlp_grad': 0.0, 't_wall_nlp_grad_f': 1.530496932, 't_wall_nlp_jac_g': 1.5610772720000001, 'unified_return_status': 'SOLVER_RET_SUCCESS'}
[62.33473703]
[10.00000001]
References
Total running time of the script: (0 minutes 13.273 seconds)