miller_ocp.py
def _set_initial_guesses(self):
        """
        Set the initial guess for the optimal control problem (states and controls)
        """
        # --- Initial guess --- #
        total_n_shooting = np.sum(self.n_shooting) + len(self.n_shooting)
        # Initialize state vector
        # if self.x is None:
        self.x = np.zeros((self.n_q + self.n_qdot, total_n_shooting))

        # determine v such that final z == 0
        v0 = 1 / 2 * 9.81 * self.duration  #
        # time vector
        data_point = np.linspace(0, self.duration, total_n_shooting)
        # parabolic trajectory on Z
        self.x[2, :] = v0 * data_point + -9.81 / 2 * data_point**2
        # Somersaults
        self.x[3, :] = np.hstack(
            (
                np.linspace(0, self.phase_proportions[0] * self.somersaults, self.n_shooting[0] + 1),
                np.linspace(self.phase_proportions[0] * self.somersaults, self.somersaults, self.n_shooting[1] + 1),
            )
        )
        # Twists
        self.x[5, :] = np.hstack(
            (np.linspace(0, self.twists, self.n_shooting[0] + 1), self.twists * np.ones(self.n_shooting[1] + 1))
        )

        # Handle second DoF of arms with Noise.
        self.x[6:9, :] = np.random.random((3, total_n_shooting)) * np.pi / 12 - np.pi / 24
        self.x[10, :] = np.random.random((1, total_n_shooting)) * np.pi / 2 - (np.pi - np.pi / 4)
        self.x[12, :] = np.random.random((1, total_n_shooting)) * np.pi / 2 + np.pi / 4
        self.x[13:15, :] = np.random.random((2, total_n_shooting)) * np.pi / 12 - np.pi / 24

        # velocity on Y
        self.x[self.n_q + 0, :] = self.velocity_x
        self.x[self.n_q + 1, :] = self.velocity_y
        self.x[self.n_q + 2, :] = self.vertical_velocity_0 - 9.81 * data_point
        # Somersaults rate
        self.x[self.n_q + 3, :] = self.somersault_rate_0
        # Twists rate
        self.x[self.n_q + 5, :] = self.twists / self.duration

        # random for other velocities
        self.x[self.n_q + 6 :, :] = (
            (np.random.random((self.n_qdot - self.nb_root, total_n_shooting)) * 2 - 1)
            * self.velocity_max
            * self.random_scale_qdot
        )

        # random for other velocities in phase 2 to only
        low_speed_idx = [self.n_q + 6, self.n_q + 7, self.n_q + 8, self.n_q + 13, self.n_q + 14]
        n_shooting_phase_0 = self.n_shooting[0] + 1
        n_shooting_phase_1 = self.n_shooting[1] + 1
        self.x[low_speed_idx, n_shooting_phase_0:] = (
            (np.random.random((len(low_speed_idx), n_shooting_phase_1)) * 2 - 1)
            * self.velocity_max_phase_transition
            * self.random_scale_qdot
        )

        if self.dynamics_type == MillerDynamics.IMPLICIT_TAU_DRIVEN_QDDDOT:
            qddot_random = (
                (np.random.random((self.n_qddot, total_n_shooting)) * 2 - 1) * self.qddot_max * self.random_scale_qddot
            )
            self.x = np.vstack((self.x, qddot_random))

        if self.dynamics_type == MillerDynamics.ROOT_IMPLICIT_QDDDOT:
            qddot_random = (
                (np.random.random((self.n_qddot, total_n_shooting)) * 2 - 1) * self.qddot_max * self.random_scale_qddot
            )

            self.x = np.vstack((self.x, qddot_random))

        self._set_initial_states(self.x)
        self._set_initial_controls()

        Set the initial guess for the optimal control problem (states and controls)
        
Set the initial guess for the optimal control problem (states and controls)
Set the initial guess for the optimal control problem ( states and controls )
def _set_initial_guesses(self):
        total_n_shooting = np.sum(self.n_shooting) + len(self.n_shooting)
        self.x = np.zeros((self.n_q + self.n_qdot, total_n_shooting))
        v0 = 1 / 2 * 9.81 * self.duration  
         time vector
        data_point = np.linspace(0, self.duration, total_n_shooting)
         parabolic trajectory on Z
        self.x[2, :] = v0 * data_point + -9.81 / 2 * data_point**2
         Somersaults
        self.x[3, :] = np.hstack(
            (
                np.linspace(0, self.phase_proportions[0] * self.somersaults, self.n_shooting[0] + 1),
                np.linspace(self.phase_proportions[0] * self.somersaults, self.somersaults, self.n_shooting[1] + 1),
            )
        )
         Twists
        self.x[5, :] = np.hstack(
            (np.linspace(0, self.twists, self.n_shooting[0] + 1), self.twists * np.ones(self.n_shooting[1] + 1))
        )
         Handle second DoF of arms with Noise.
        self.x[6:9, :] = np.random.random((3, total_n_shooting)) * np.pi / 12 - np.pi / 24
        self.x[10, :] = np.random.random((1, total_n_shooting)) * np.pi / 2 - (np.pi - np.pi / 4)
        self.x[12, :] = np.random.random((1, total_n_shooting)) * np.pi / 2 + np.pi / 4
        self.x[13:15, :] = np.random.random((2, total_n_shooting)) * np.pi / 12 - np.pi / 24
         velocity on Y
        self.x[self.n_q + 0, :] = self.velocity_x
        self.x[self.n_q + 1, :] = self.velocity_y
        self.x[self.n_q + 2, :] = self.vertical_velocity_0 - 9.81 * data_point
         Somersaults rate
        self.x[self.n_q + 3, :] = self.somersault_rate_0
         Twists rate
        self.x[self.n_q + 5, :] = self.twists / self.duration
         random for other velocities
        self.x[self.n_q + 6 :, :] = (
            (np.random.random((self.n_qdot - self.nb_root, total_n_shooting)) * 2 - 1)
            * self.velocity_max
            * self.random_scale_qdot
        )
         random for other velocities in phase 2 to only
        low_speed_idx = [self.n_q + 6, self.n_q + 7, self.n_q + 8, self.n_q + 13, self.n_q + 14]
        n_shooting_phase_0 = self.n_shooting[0] + 1
        n_shooting_phase_1 = self.n_shooting[1] + 1
        self.x[low_speed_idx, n_shooting_phase_0:] = (
            (np.random.random((len(low_speed_idx), n_shooting_phase_1)) * 2 - 1)
            * self.velocity_max_phase_transition
            * self.random_scale_qdot
        )
        if self.dynamics_type == MillerDynamics.IMPLICIT_TAU_DRIVEN_QDDDOT:
            qddot_random = (
                (np.random.random((self.n_qddot, total_n_shooting)) * 2 - 1) * self.qddot_max * self.random_scale_qddot
            )
            self.x = np.vstack((self.x, qddot_random))
        if self.dynamics_type == MillerDynamics.ROOT_IMPLICIT_QDDDOT:
            qddot_random = (
                (np.random.random((self.n_qddot, total_n_shooting)) * 2 - 1) * self.qddot_max * self.random_scale_qddot
            )
            self.x = np.vstack((self.x, qddot_random))
        self._set_initial_states(self.x)
        self._set_initial_controls()
def _set_initial_guesses ( self ) : total_n_shooting = np . sum ( self . n_shooting ) + len ( self . n_shooting ) self . x = np . zeros ( ( self . n_q + self . n_qdot , total_n_shooting ) ) v0 = 1 / 2 * 9.81 * self . duration data_point = np . linspace ( 0 , self . duration , total_n_shooting ) self . x [ 2 , : ] = v0 * data_point + - 9.81 / 2 * data_point ** 2 self . x [ 3 , : ] = np . hstack ( ( np . linspace ( 0 , self . phase_proportions [ 0 ] * self . somersaults , self . n_shooting [ 0 ] + 1 ) , np . linspace ( self . phase_proportions [ 0 ] * self . somersaults , self . somersaults , self . n_shooting [ 1 ] + 1 ) , ) ) self . x [ 5 , : ] = np . hstack ( ( np . linspace ( 0 , self . twists , self . n_shooting [ 0 ] + 1 ) , self . twists * np . ones ( self . n_shooting [ 1 ] + 1 ) ) ) self . x [ 6 : 9 , : ] = np . random . random ( ( 3 , total_n_shooting ) ) * np . pi / 12 - np . pi / 24 self . x [ 10 , : ] = np . random . random ( ( 1 , total_n_shooting ) ) * np . pi / 2 - ( np . pi - np . pi / 4 ) self . x [ 12 , : ] = np . random . random ( ( 1 , total_n_shooting ) ) * np . pi / 2 + np . pi / 4 self . x [ 13 : 15 , : ] = np . random . random ( ( 2 , total_n_shooting ) ) * np . pi / 12 - np . pi / 24 self . x [ self . n_q + 0 , : ] = self . velocity_x self . x [ self . n_q + 1 , : ] = self . velocity_y self . x [ self . n_q + 2 , : ] = self . vertical_velocity_0 - 9.81 * data_point self . x [ self . n_q + 3 , : ] = self . somersault_rate_0 self . x [ self . n_q + 5 , : ] = self . twists / self . duration self . x [ self . n_q + 6 : , : ] = ( ( np . random . random ( ( self . n_qdot - self . nb_root , total_n_shooting ) ) * 2 - 1 ) * self . velocity_max * self . random_scale_qdot ) low_speed_idx = [ self . n_q + 6 , self . n_q + 7 , self . n_q + 8 , self . n_q + 13 , self . n_q + 14 ] n_shooting_phase_0 = self . n_shooting [ 0 ] + 1 n_shooting_phase_1 = self . n_shooting [ 1 ] + 1 self . x [ low_speed_idx , n_shooting_phase_0 : ] = ( ( np . random . random ( ( len ( low_speed_idx ) , n_shooting_phase_1 ) ) * 2 - 1 ) * self . velocity_max_phase_transition * self . random_scale_qdot ) if self . dynamics_type == MillerDynamics . IMPLICIT_TAU_DRIVEN_QDDDOT : qddot_random = ( ( np . random . random ( ( self . n_qddot , total_n_shooting ) ) * 2 - 1 ) * self . qddot_max * self . random_scale_qddot ) self . x = np . vstack ( ( self . x , qddot_random ) ) if self . dynamics_type == MillerDynamics . ROOT_IMPLICIT_QDDDOT : qddot_random = ( ( np . random . random ( ( self . n_qddot , total_n_shooting ) ) * 2 - 1 ) * self . qddot_max * self . random_scale_qddot ) self . x = np . vstack ( ( self . x , qddot_random ) ) self . _set_initial_states ( self . x ) self . _set_initial_controls ( )
Set the initial guess for the optimal control problem (states and controls)
Set the initial guess for the optimal control problem ( states and controls )
"""
        Set the initial guess for the optimal control problem (states and controls)
        """ # --- Initial guess --- # # Initialize state vector # if self.x is None: # determine v such that final z == 0 # # time vector # parabolic trajectory on Z # Somersaults # Twists # Handle second DoF of arms with Noise. # velocity on Y # Somersaults rate # Twists rate # random for other velocities # random for other velocities in phase 2 to only
self