.. DO NOT EDIT. .. THIS FILE WAS AUTOMATICALLY GENERATED BY SPHINX-GALLERY. .. TO MAKE CHANGES, EDIT THE SOURCE PYTHON FILE: .. "examples/orbital_1.py" .. LINE NUMBERS ARE GIVEN BELOW. .. only:: html .. note:: :class: sphx-glr-download-link-note :ref:`Go to the end ` to download the full example code. .. rst-class:: sphx-glr-example-title .. _sphx_glr_examples_orbital_1.py: Linear Covariance Analysis ========================== .. GENERATED FROM PYTHON SOURCE LINES 7-14 This example is a simple implementation of linear covariance analysis [#geller2006]_ 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. .. GENERATED FROM PYTHON SOURCE LINES 14-29 .. code-block:: Python 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) .. GENERATED FROM PYTHON SOURCE LINES 30-32 The core of the analysis is an ODE system that represents the true state the follows the CW equations. .. GENERATED FROM PYTHON SOURCE LINES 32-68 .. code-block:: Python 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 .. GENERATED FROM PYTHON SOURCE LINES 69-71 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. .. GENERATED FROM PYTHON SOURCE LINES 71-133 .. code-block:: Python 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) .. GENERATED FROM PYTHON SOURCE LINES 134-135 A Terminating event at the end of the maneuver .. GENERATED FROM PYTHON SOURCE LINES 135-142 .. code-block:: Python class Terminate(LinCovCW.Event): terminate = True at_time = MajorBurn.tem .. GENERATED FROM PYTHON SOURCE LINES 143-144 Trajectory analysis to simulate the system .. GENERATED FROM PYTHON SOURCE LINES 144-159 .. code-block:: Python 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)) .. GENERATED FROM PYTHON SOURCE LINES 160-161 Simulate .. GENERATED FROM PYTHON SOURCE LINES 161-201 .. code-block:: Python 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) .. rst-class:: sphx-glr-script-out .. code-block:: none [11.03987383] .. GENERATED FROM PYTHON SOURCE LINES 202-262 .. code-block:: Python 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 # """ .. image-sg:: /examples/images/sphx_glr_orbital_1_001.png :alt: orbital 1 :srcset: /examples/images/sphx_glr_orbital_1_001.png :class: sphx-glr-single-img .. rst-class:: sphx-glr-script-out .. code-block:: none .. GENERATED FROM PYTHON SOURCE LINES 263-300 .. code-block:: Python 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 # """ .. rst-class:: sphx-glr-script-out .. code-block:: none 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] .. GENERATED FROM PYTHON SOURCE LINES 301-308 .. code-block:: Python 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) .. rst-class:: sphx-glr-script-out .. code-block:: none [93543.09121237] [42980.59515077] [84.09660115] [2839.87927261] time: 3.5427948549999826 .. GENERATED FROM PYTHON SOURCE LINES 309-312 .. code-block:: Python plot_traj(hohmann_sim) .. image-sg:: /examples/images/sphx_glr_orbital_1_002.png :alt: orbital 1 :srcset: /examples/images/sphx_glr_orbital_1_002.png :class: sphx-glr-single-img .. rst-class:: sphx-glr-script-out .. code-block:: none .. GENERATED FROM PYTHON SOURCE LINES 314-340 .. code-block:: Python 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) .. rst-class:: sphx-glr-script-out .. code-block:: none 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. .. GENERATED FROM PYTHON SOURCE LINES 341-345 .. code-block:: Python tot_delta_v_sim = Sim(**sim_kwargs, tig=total_delta_v.tig, tem=total_delta_v.tem) plot_traj(tot_delta_v_sim) .. image-sg:: /examples/images/sphx_glr_orbital_1_003.png :alt: orbital 1 :srcset: /examples/images/sphx_glr_orbital_1_003.png :class: sphx-glr-single-img .. rst-class:: sphx-glr-script-out .. code-block:: none .. GENERATED FROM PYTHON SOURCE LINES 346-349 .. code-block:: Python total_delta_v_constrained = TotalDeltaV(pos_disp_max=10.0) .. rst-class:: sphx-glr-script-out .. code-block:: none 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. .. GENERATED FROM PYTHON SOURCE LINES 350-356 .. code-block:: Python 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) .. image-sg:: /examples/images/sphx_glr_orbital_1_004.png :alt: orbital 1 :srcset: /examples/images/sphx_glr_orbital_1_004.png :class: sphx-glr-single-img .. rst-class:: sphx-glr-script-out .. code-block:: none .. GENERATED FROM PYTHON SOURCE LINES 360-379 .. code-block:: Python 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() .. rst-class:: sphx-glr-script-out .. code-block:: none 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] .. GENERATED FROM PYTHON SOURCE LINES 380-385 .. rubric:: References .. [#geller2006] Geller, D. K., "Linear Covariance Techniques for Orbital Rendezvous Analysis and Autonomous Onboard Mission Planning," Journal of Guidance, Control, and Dynamics, Vol. 29, No. 6, 2006, pp. 1404–1414. https://doi.org/10.2514/1.19447 .. rst-class:: sphx-glr-timing **Total running time of the script:** (0 minutes 13.273 seconds) .. _sphx_glr_download_examples_orbital_1.py: .. only:: html .. container:: sphx-glr-footer sphx-glr-footer-example .. container:: sphx-glr-download sphx-glr-download-jupyter :download:`Download Jupyter notebook: orbital_1.ipynb ` .. container:: sphx-glr-download sphx-glr-download-python :download:`Download Python source code: orbital_1.py ` .. container:: sphx-glr-download sphx-glr-download-zip :download:`Download zipped: orbital_1.zip ` .. only:: html .. rst-class:: sphx-glr-signature `Gallery generated by Sphinx-Gallery `_