Updating optimal control package for Windows installation

sempy_optimal_control/sempy/optimal_control/GL_V/spacecraft_trajectories/LEO2NRHO/HALO2NRHO.py

-import numpy as np
-import math
-import cppad_py
-import matplotlib.pyplot as plt
-import pickle
-
-from sempy.core.init.primary import Primary
-from sempy.core.init.cr3bp import Cr3bp
-from sempy.core.orbits.nrho import NRHO
-from sempy.core.orbits.halo import Halo
-
-from sempy.core.crtbp import jacobi
-from sempy.optimal_control.src.spacecraft import Spacecraft
-
-from sempy.optimal_control.spacecraft_trajectories.mission import Mission
-from sempy.optimal_control.src.transcription import Transcription
-from sempy.optimal_control.src.solver import IPOPT
-from sempy.optimal_control.src.optimization import Optimization
-
-from sempy.optimal_control.spacecraft_trajectories.LEO2NRHO.earth2manifold_guess import Earth2NRHOFixedPositionGuess, Earth2NRHOFixedVelocityGuess
-
-import sempy.core.init.constants as cst 
-
-
-
-class HALO2NRHO(Mission):
-
-    def __init__(self, spacecraft_prm, halo_prm, nrho_prm, n_nodes):
-        """ `HALO2NRHO` class implements a low-thrust transfer between a Halo and a NRHO orbits
-            using trajectory stacking method to generate the initial guess.
-            
-            cf. Robert Pritchett, Kathleen Howell, and Daniel Grebow. “Low-Thrust Transfer Design Based 
-            on Collocation Techniques: Applications in the Restricted Three-Body Problem”. 
-
-            Parameters
-            ----------
-            spacecraft_prm : dict
-                Spacecraft parameters dictionnary
-            halo_prm : dict
-                Halo orbit parameters namely : library point (l1 or l2), family (southern or northern) and
-                either Orbit extension or Jacobi constant
-            nrho_prm : dict
-                NRHO orbit parameters namely : library point (l1 or l2), family (southern or northern) and
-                either Periselene radius, altitude, vertical extension or Jacobi constant
-
-            Attributs
-            ---------
-            halo : Halo orbit object
-                Departure Halo orbit object
-            nrho : NRHO orbit object
-                Arrival NRHO orbit object
-
-
-        """
-
-        # Mission.__init__(self, spacecraft_prm, n_states=7, n_controls=4, n_st_path_con=1, n_ct_path_con=1, n_event_con=13,
-        #                  n_f_par=0, **kwargs)
-
-        # Mission.__init__(self, spacecraft_prm, n_states=7, n_controls=4, n_event_con=13, n_st_path_con=0, n_ct_path_con=1,
-        #                  n_f_par=0, **kwargs)
-
-        Mission.__init__(self, spacecraft_prm, n_states=7, n_controls=4, n_event_con=13, n_st_path_con=0, n_ct_path_con=1, n_f_par=0, n_nodes=n_nodes)
-
-        # Generation of the departure and arrival orbits
-        self.halo_generation(halo_prm)
-        self.nrho_generation(nrho_prm)
-
-    def halo_generation(self, halo_prm):
-        """ Generation of the departure Halo orbit using the parameters given  
-            by the user.
-
-            Parameters
-            ----------
-            halo_prm : dict
-                Halo orbit parameters namely : library point (l1 or l2), family (southern or northern) and
-                either Orbit extension or Jacobi constant
-
-        """
-
-        # Libration point definition
-        libration_point = self.cr3bp.l1 if halo_prm['libration_point'] == 'l1' else self.cr3bp.l2
-
-        # Halo family
-        family = Halo.Family.southern if halo_prm['family'] == 'southern' else Halo.Family.northern
-
-        if 'Azdim' in halo_prm:
-            self.halo = Halo(self.cr3bp, libration_point, family, Azdim=halo_prm['Azdim'])
-            self.halo.interpolation()
-        elif 'Cjac' in halo_prm:
-            self.halo = Halo(self.cr3bp, libration_point, family, Cjac=halo_prm['Cjac'])
-            self.halo.interpolation()
-        else:
-            raise ValueError("Neither orbit extension or Jacobi constant has been setted")
-
-    def nrho_generation(self, nrho_prm):
-        """ Generation of the arrival NRHO orbit using the parameters given  
-            by the user.
-
-            Parameters
-            ----------
-            nrho_prm : dict
-                NRHO orbit parameters namely : library point (l1 or l2), family (southern or northern) and
-                either Periselene radius, altitude, vertical extension or Jacobi constant
-
-        """
-
-        # Libration point definition
-        libration_point = self.cr3bp.l1 if nrho_prm['libration_point'] == 'l1' else self.cr3bp.l2
-
-        # NRHO family
-        family = NRHO.Family.southern if nrho_prm['family'] == 'southern' else NRHO.Family.northern
-
-        if 'Azdim' in nrho_prm:
-            self.nrho = NRHO(self.cr3bp, libration_point, family, Azdim=nrho_prm['Azdim'])
-            self.nrho.interpolation()
-        elif 'Cjac' in nrho_prm:
-            # self.nrho = NRHO(self.cr3bp, libration_point, family, Cjac=nrho_prm['Cjac'])
-            # self.nrho.interpolation()
-            self.nrho = Halo(self.cr3bp, libration_point, Halo.Family.southern, Cjac=nrho_prm['Cjac'])
-            self.nrho.interpolation()
-        elif 'Rpdim' in nrho_prm:
-            self.nrho = NRHO(self.cr3bp, libration_point, family, Rpdim=nrho_prm['Rpdim'])
-            self.nrho.interpolation()
-        elif 'Altpdim' in nrho_prm:
-            self.nrho = NRHO(self.cr3bp, libration_point, family, Altpdim=nrho_prm['Altpdim'])
-            self.nrho.interpolation()
-        else:
-            raise ValueError("Neither periselene radius or altitude, vertical extension, \
-                                Jacobi constant or orbit's period")
-
-    def set_boundaries(self):
-        """ Setting of the states, controls, free-parameters, initial and final times
-                boundaries """
-
-        # X (syn. fram) [-]
-        self.low_bnd.states[0] = -0.5 #- self.cr3bp.mu #- 1.0 # peut-etre a modifier
-        self.upp_bnd.states[0] = 1.5 #- self.cr3bp.mu #+ 1.0 # peut-etre a modifier
-
-        # Y (syn. fram) [-]
-        self.low_bnd.states[1] = - 0.5
-        self.upp_bnd.states[1] = 0.5
-
-        # Z (syn. fram) [-]
-        self.low_bnd.states[2] = - 0.5
-        self.upp_bnd.states[2] = 0.5
-
-        # Vx (syn. fram) [-]
-        self.low_bnd.states[3] = - 1
-        self.upp_bnd.states[3] = 1
-
-        # Vy (syn. fram) [-]
-        self.low_bnd.states[4] = - 1
-        self.upp_bnd.states[4] = 1
-
-        # Vz (syn. fram) [-]
-        self.low_bnd.states[5] = - 1
-        self.upp_bnd.states[5] = 1
-
-        # Set the mass limits [-]
-        self.low_bnd.states[6] = 10**(-4)
-        self.upp_bnd.states[6] = self.spacecraft.mass0
-
-        # Set the thrust limits [-]
-        self.low_bnd.controls[0] = self.spacecraft.thrust_min 
-        self.upp_bnd.controls[0] = self.spacecraft.thrust_max 
-
-        # Set the limits for controls ux, uy, uz
-        for i in [1, 2, 3]:
-            self.low_bnd.controls[i] = -1
-            self.upp_bnd.controls[i] = 1
-
-        # Set the times limits
-        t_init  = self.initial_guess.time[0]
-        t_final = self.initial_guess.time[-1]
-
-        self.low_bnd.ti = self.upp_bnd.ti = t_init
-
-        self.low_bnd.tf = 0.5 * t_final
-        self.upp_bnd.tf = 1.5 * t_final
-
-    def path_constraints(self, states, controls, states_add, controls_add, controls_col, f_par):
-        """ Computation of the path constraints 
-
-            Parameters
-            ----------
-            states : ndarray
-                Matrix of the states
-            controls : ndarray 
-                Matrix of the controls
-            states_add : ndarray
-                Matrix of the states at additional nodes
-            controls_add : ndarray
-                Matrix of the controls at additional nodes
-            controls_col : ndarray
-                Matrix of the controls at collocation nodes
-            f_par : array
-                Array of the free parameters
-
-            Returns
-            -------
-            constraints_st : ndarray
-                States path constraints matrix
-            constraints_ct : ndarray
-                Controls path constraints matrix
-        """
-        st_path = np.ndarray((self.prm['n_st_path_con'],
-                            2*self.prm['n_nodes']-1), dtype=cppad_py.a_double)
-        ct_path = np.ndarray((self.prm['n_ct_path_con'],
-                            4*self.prm['n_nodes']-3), dtype=cppad_py.a_double)
-
-        # Positions in the synodic frame [-]
-        x = np.concatenate((states[0], states_add[0]))
-        y = np.concatenate((states[1], states_add[1]))
-        z = np.concatenate((states[2], states_add[2]))
-
-        # Thrust directions in the synodic frame [-]
-        ux = np.concatenate((controls[1], controls_add[1], controls_col[1]))
-        uy = np.concatenate((controls[2], controls_add[2], controls_col[2]))
-        uz = np.concatenate((controls[3], controls_add[3], controls_col[3]))
-
-        u2 = ux*ux + uy*uy + uz*uz
-
-        # r_nrm = np.sqrt( (x-(1-self.cr3bp.mu))*(x-(1-self.cr3bp.mu)) + y*y + z*z )
-
-        ct_path[0] = u2 - 1
-        # st_path[0] = r_nrm
-
-        return st_path, ct_path
-
-    def set_path_constraints_boundaries(self):
-        """ Setting of the path constraints boundaries """
-        self.low_bnd.ct_path[0] = self.upp_bnd.ct_path[0] = 0
-
-        # self.low_bnd.st_path[0] = 0 
-        # self.upp_bnd.st_path[0] = 1
-
-
-    def end_point_cost(self, ti, xi, tf, xf, f_prm):
-        """ Computation of the end point cost (Mayer term) 
-
-            Parameters
-            ----------
-            ti : float
-                Initial time value
-            xi : array
-                Initial states array
-            tf : float
-                Final time value
-            xf : array
-                Final states array
-            f_prm : array
-                Free parameters array
-
-            Returns
-            -------
-            float
-                Mayer term value
-
-        """
-        m0 = self.spacecraft.mass0
-
-        mf = xf[-1]
-        return - mf / m0
-
-    def event_constraints(self, xi, ui, xf, uf, ti, tf, f_prm):
-        """ Computation of the events constraints 
-
-            Parameters
-            ----------
-            xi : array
-                Array of states at initial time
-            ui : array
-                Array of controls at initial time
-            xf : array
-                Array of states at final time
-            uf : array
-                Array of controls at final time
-            ti : float
-                Value of initial time
-            tf : float
-                Value of final time
-            f_prm : array
-                Free parameters array
-
-            Returns
-            -------
-            constraints : array
-                Array of the event constraints
-
-        """
-
-        constraints = np.ndarray((self.prm['n_event_con'], 1),
-                                 dtype=cppad_py.a_double)
-
-        # Initial states
-        x0, y0, z0, vx0, vy0, vz0, m0 = self.initial_guess.states[:, 0]
-
-        # Final states (minus mass)
-        x_f, y_f, z_f, vx_f, vy_f, vz_f = self.initial_guess.states[:-1, -1]
-
-        # Position (init) [-]
-        constraints[0] = x0 - xi[0]
-        constraints[1] = y0 - xi[1]
-        constraints[2] = z0 - xi[2]
-
-        # Velocity (init) [-]
-        constraints[3] = vx0 - xi[3]
-        constraints[4] = vy0 - xi[4]
-        constraints[5] = vz0 - xi[5]
-
-        # Mass (init) [-]
-        constraints[6] = m0 - xi[6]
-
-        # Position (final) [-]
-        constraints[7] = x_f - xf[0]
-        constraints[8] = y_f - xf[1]
-        constraints[9] = z_f - xf[2]
-
-        # Velocity (final) [-]
-        constraints[10] = vx_f - xf[3]
-        constraints[11] = vy_f - xf[4]
-        constraints[12] = vz_f - xf[5]
-        
-
-        return constraints
-
-    def set_events_constraints_boundaries(self):
-        """ Setting of the events constraints boundaries """
-
-        # Position (init) [-]
-        self.low_bnd.event[0] = self.upp_bnd.event[0] = 0
-        self.low_bnd.event[1] = self.upp_bnd.event[1] = 0
-        self.low_bnd.event[2] = self.upp_bnd.event[2] = 0
-
-        # Velocity (init) [-]
-        self.low_bnd.event[3] = self.upp_bnd.event[3] = 0
-        self.low_bnd.event[4] = self.upp_bnd.event[4] = 0
-        self.low_bnd.event[5] = self.upp_bnd.event[5] = 0
-
-        # Mass (init) [-]
-        self.low_bnd.event[6] = self.upp_bnd.event[6] = 0
-
-        # Position (final) [-]
-        self.low_bnd.event[7] = self.upp_bnd.event[7] = 0
-        self.low_bnd.event[8] = self.upp_bnd.event[8] = 0
-        self.low_bnd.event[9] = self.upp_bnd.event[9] = 0
-
-        # Velocity (final) [-]
-        self.low_bnd.event[10] = self.upp_bnd.event[10] = 0
-        self.low_bnd.event[11] = self.upp_bnd.event[11] = 0
-        self.low_bnd.event[12] = self.upp_bnd.event[12] = 0
-
-    def set_initial_guess(self):
-        """ Setting of the initial guess for the states, controls, free-parameters
-                and time grid """
-
-        # Shifts for both the Halo and the NRHO orbits
-        # shift_h = 650
-        shift_h = 1000
-        shift_n = 100
-        
-        # Initialization of the states, controls and time
-        states = np.ndarray(shape=(7, self.prm['n_nodes']))
-        controls = np.ndarray(shape=(4, self.prm['n_nodes']))
-        time = np.zeros(self.prm['n_nodes'])
-
-        halo_st = self.halo.state_vec.transpose()[:6]
-        nrho_st = self.nrho.state_vec.transpose()[:6]
-
-        halo_st = np.roll(halo_st, shift_h, axis=1)
-        nrho_st = np.roll(nrho_st, shift_n, axis=1)
-
-        n_pts = len(self.halo.t_vec)
-        step = int(n_pts / (self.prm['n_nodes'] / 2))
-        # step = int(n_pts / (self.prm['n_nodes'] / 3))
-
-        # Set the states
-        states[:6, :int(self.prm['n_nodes']/2)] = halo_st[:, 0:-1:step]
-        states[:6, int(self.prm['n_nodes']/2):] = nrho_st[:, 0:-1:step]
-
-        self.plot_trajectory(states)
-
-        states[6] = self.spacecraft.mass0 * np.ones(self.prm['n_nodes'])
-
-        # Set the controls
-        controls[0] = np.zeros(self.prm['n_nodes'])
-
-        v_norm = np.sqrt( states[3]*states[3] + states[4]*states[4] + states[5]*states[5])
-        controls[1] = states[3] / v_norm
-        controls[2] = states[4] / v_norm
-        controls[3] = states[5] / v_norm
-
-        # Set the time
-        time = np.linspace(0, 4, self.prm['n_nodes'])
-
-        # with open('pickled_results/halo2nrho/halo2nrho_vel', 'rb') as earth2manifolds_pickles_vel:
-        #     pickled_results_vel = pickle.load(earth2manifolds_pickles_vel)
-        
-
-
-        # # Recovery of the states, controls, time and free parameters values from pickled results
-        # states_ = pickled_results_vel['opt_st']
-        # controls_ = pickled_results_vel['opt_ct']
-        # time_ = pickled_results_vel['opt_tm']
-
-        # Set the initial guess
-        self.initial_guess.states = states
-        self.initial_guess.controls = controls
-        self.initial_guess.time = time
-
-    def plot_trajectory(self, states):
-
-        fig = plt.figure()
-        ax = fig.gca(projection='3d')
-
-        # Extraction of the states
-        x = states[0]
-        y = states[1]
-        z = states[2]
-
-        # Orbit 
-        orbit_dep = self.halo
-        orbit_arr = self.nrho
-
-        x_dep = orbit_dep.state_vec[:, 0]
-        y_dep = orbit_dep.state_vec[:, 1]
-        z_dep = orbit_dep.state_vec[:, 2] 
-
-        x_arr = orbit_arr.state_vec[:, 0]
-        y_arr = orbit_arr.state_vec[:, 1]
-        z_arr = orbit_arr.state_vec[:, 2] 
-
-        # Plot the trajectory
-        ax.plot(x, y, z, label='Spacecraft trajectory')
-        ax.plot(x_dep, y_dep, z_dep, label='Departure orbit', color = 'green')
-        ax.plot(x_arr, y_arr, z_arr, label='Arrival orbit', color = 'red')
-        ax.plot([1], [0], [0], 'mo')
-
-        # self.nrho.single_manifold_computation(stability='stable', way='interior', theta=10, t_prop=7, max_internal_steps=4_000_000)
-
-        # # Plot manifolds from nrho orbit
-        # for i in range(len(self.nrho.manifolds['stable_interior'])):
-        # i = 9
-        # ax.plot(self.nrho.manifolds['stable_interior'][i].state_vec[:, 0], self.nrho.manifolds['stable_interior'][i].state_vec[:, 1], 
-        #                 self.nrho.manifolds['stable_interior'][i].state_vec[:, 2], '-')
-
-
-        plt.legend()
-        plt.show()
-
-if __name__ == '__main__':
-
-    # Spacecraft properties
-    m0 = 1000  # initial mass [kg]
-    m_dry = 10  # dry mass [kg]
-    thrust_max = 2  # maximum thrust [N]
-    thrust_min = 0  # minimum thrust [N]
-    Isp = 2000  # specific impulse [s]
-
-    spacecraft_prm = {'mass0': m0, 'mass_dry': m_dry, 'thrust_max': thrust_max, 'thrust_min': thrust_min, 'isp': Isp}
-
-    # Departure orbit
-    halo_prm = {'libration_point': 'l1', 'family': 'southern', 'Cjac': 3.10}
-
-    # Final orbit
-    nrho_prm = {'libration_point': 'l2', 'family': 'southern', 'Cjac': 3.05}
-
-
-    # Number of nodes
-    n_nodes = 200
-
-    # Instantiation of the problem
-    problem = HALO2NRHO(spacecraft_prm, halo_prm, nrho_prm, n_nodes=n_nodes)
-
-    # problem.plot_trajectory([1,1,1])
-    # input()
-
-    # Optimization options
-    # Possible linear solvers : ma27, ma57, ma77, ma86, mumps
-    linear_solver = 'ma86'
-
-    options = {'explicit_integration': False, 'linear_solver': linear_solver,
-                'plot_results': True, 'plot_physical_err': False, 'n_nodes': n_nodes, 'pickle_results': True, 'name': 'halo2nrho_direct'}
-
-    # Instantiation of the optimization
-    optimization = Optimization(problem=problem, **options)
-
-    # Launch of the optimization
-    optimization.run()
-
-    # Display the results
-    problem.plot_trajectory(optimization.results['opt_st'])
-    
\ No newline at end of file