miller_ocp.py
def _set_objective_functions(self):
        """
        Set the multi-objective functions for each phase with specific weights
        """

        # --- Objective function --- #
        w_qdot = 1
        w_penalty = 1
        w_penalty_foot = 10
        w_penalty_core = 10
        w_track_final = 0.1
        w_angular_momentum_x = 100000
        w_angular_momentum_yz = 1000
        for i in range(len(self.n_shooting)):
            self.objective_functions.add(
                ObjectiveFcn.Lagrange.MINIMIZE_STATE,
                derivative=True,
                key="qdot",
                index=(6, 7, 8, 9, 10, 11, 12, 13, 14),
                weight=w_qdot,
                phase=i,
            )  # Regularization
            self.objective_functions.add(
                ObjectiveFcn.Mayer.MINIMIZE_MARKERS,
                derivative=True,
                reference_jcs=0,
                marker_index=6,
                weight=w_penalty,
                phase=i,
                node=Node.ALL_SHOOTING,
            )  # Right hand trajectory
            self.objective_functions.add(
                ObjectiveFcn.Mayer.MINIMIZE_MARKERS,
                derivative=True,
                reference_jcs=0,
                marker_index=11,
                weight=w_penalty,
                phase=i,
                node=Node.ALL_SHOOTING,
            )  # Left hand trajectory
            self.objective_functions.add(
                ObjectiveFcn.Mayer.MINIMIZE_MARKERS,  ########### Lagrange
                node=Node.ALL_SHOOTING,
                derivative=True,
                reference_jcs=0,
                marker_index=16,
                weight=w_penalty_foot,
                phase=i,
                quadratic=False,
            )  # feet trajectory
            self.objective_functions.add(
                ObjectiveFcn.Lagrange.MINIMIZE_STATE, index=(6, 7, 8, 13, 14), key="q", weight=w_penalty_core, phase=i
            )  # core DoFs

            self.objective_functions.add(
                ObjectiveFcn.Mayer.MINIMIZE_ANGULAR_MOMENTUM,
                node=Node.START,
                phase=0,
                weight=w_angular_momentum_x,
                quadratic=False,
                index=0,
            )
            self.objective_functions.add(
                ObjectiveFcn.Mayer.MINIMIZE_ANGULAR_MOMENTUM,
                node=Node.START,
                phase=0,
                weight=w_angular_momentum_yz,
                quadratic=True,
                index=[1, 2],
            )

        # Track momentum and Minimize delta momentum
        if self.extra_obj:
            for i in range(2):
                if (
                    self.dynamics_type == MillerDynamics.IMPLICIT_TAU_DRIVEN_QDDDOT
                    or self.dynamics_type == MillerDynamics.ROOT_IMPLICIT_QDDDOT
                ):
                    self.objective_functions.add(
                        ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="qdddot", phase=i, weight=1e-8
                    )
                if self.dynamics_type == MillerDynamics.IMPLICIT or self.dynamics_type == MillerDynamics.ROOT_IMPLICIT:
                    self.objective_functions.add(
                        ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="qddot", phase=i, weight=1e-4
                    )

        # Help to stay upright at the landing.
        self.objective_functions.add(
            ObjectiveFcn.Mayer.TRACK_STATE,
            index=(0, 1, 2),
            target=[0, 0, 0],
            key="q",
            weight=w_track_final,
            phase=1,
            node=Node.END,
        )
        self.objective_functions.add(
            ObjectiveFcn.Mayer.TRACK_STATE,
            index=3,
            target=self.somersaults - self.thorax_hips_xyz - self.slack_final_somersault / 2,
            key="q",
            weight=w_track_final,
            phase=1,
            node=Node.END,
        )
        self.objective_functions.add(
            ObjectiveFcn.Mayer.TRACK_STATE, index=4, target=0, key="q", weight=w_track_final, phase=1, node=Node.END
        )
        self.objective_functions.add(
            ObjectiveFcn.Mayer.TRACK_STATE,
            index=5,
            target=self.twists,
            key="q",
            weight=w_track_final,
            phase=1,
            node=Node.END,
        )

        slack_duration = 0.15
        self.objective_functions.add(
            ObjectiveFcn.Mayer.MINIMIZE_TIME,
            min_bound=self.phase_durations[0] - slack_duration,
            max_bound=self.phase_durations[0] + slack_duration,
            phase=0,
            weight=1e-6,
        )
        self.objective_functions.add(
            ObjectiveFcn.Mayer.MINIMIZE_TIME,
            min_bound=self.phase_durations[1] - slack_duration,
            max_bound=self.phase_durations[1] + slack_duration,
            phase=1,
            weight=1e-6,
        )

        Set the multi-objective functions for each phase with specific weights
        
Set the multi-objective functions for each phase with specific weights
Set the multi - objective functions for each phase with specific weights
def _set_objective_functions(self):
        w_qdot = 1
        w_penalty = 1
        w_penalty_foot = 10
        w_penalty_core = 10
        w_track_final = 0.1
        w_angular_momentum_x = 100000
        w_angular_momentum_yz = 1000
        for i in range(len(self.n_shooting)):
            self.objective_functions.add(
                ObjectiveFcn.Lagrange.MINIMIZE_STATE,
                derivative=True,
                key="qdot",
                index=(6, 7, 8, 9, 10, 11, 12, 13, 14),
                weight=w_qdot,
                phase=i,
            )  
            self.objective_functions.add(
                ObjectiveFcn.Mayer.MINIMIZE_MARKERS,
                derivative=True,
                reference_jcs=0,
                marker_index=6,
                weight=w_penalty,
                phase=i,
                node=Node.ALL_SHOOTING,
            )  
            self.objective_functions.add(
                ObjectiveFcn.Mayer.MINIMIZE_MARKERS,
                derivative=True,
                reference_jcs=0,
                marker_index=11,
                weight=w_penalty,
                phase=i,
                node=Node.ALL_SHOOTING,
            )  
            self.objective_functions.add(
                ObjectiveFcn.Mayer.MINIMIZE_MARKERS,  
                node=Node.ALL_SHOOTING,
                derivative=True,
                reference_jcs=0,
                marker_index=16,
                weight=w_penalty_foot,
                phase=i,
                quadratic=False,
            )  
            self.objective_functions.add(
                ObjectiveFcn.Lagrange.MINIMIZE_STATE, index=(6, 7, 8, 13, 14), key="q", weight=w_penalty_core, phase=i
            )  
            self.objective_functions.add(
                ObjectiveFcn.Mayer.MINIMIZE_ANGULAR_MOMENTUM,
                node=Node.START,
                phase=0,
                weight=w_angular_momentum_x,
                quadratic=False,
                index=0,
            )
            self.objective_functions.add(
                ObjectiveFcn.Mayer.MINIMIZE_ANGULAR_MOMENTUM,
                node=Node.START,
                phase=0,
                weight=w_angular_momentum_yz,
                quadratic=True,
                index=[1, 2],
            )
        if self.extra_obj:
            for i in range(2):
                if (
                    self.dynamics_type == MillerDynamics.IMPLICIT_TAU_DRIVEN_QDDDOT
                    or self.dynamics_type == MillerDynamics.ROOT_IMPLICIT_QDDDOT
                ):
                    self.objective_functions.add(
                        ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="qdddot", phase=i, weight=1e-8
                    )
                if self.dynamics_type == MillerDynamics.IMPLICIT or self.dynamics_type == MillerDynamics.ROOT_IMPLICIT:
                    self.objective_functions.add(
                        ObjectiveFcn.Lagrange.MINIMIZE_CONTROL, key="qddot", phase=i, weight=1e-4
                    )
        self.objective_functions.add(
            ObjectiveFcn.Mayer.TRACK_STATE,
            index=(0, 1, 2),
            target=[0, 0, 0],
            key="q",
            weight=w_track_final,
            phase=1,
            node=Node.END,
        )
        self.objective_functions.add(
            ObjectiveFcn.Mayer.TRACK_STATE,
            index=3,
            target=self.somersaults - self.thorax_hips_xyz - self.slack_final_somersault / 2,
            key="q",
            weight=w_track_final,
            phase=1,
            node=Node.END,
        )
        self.objective_functions.add(
            ObjectiveFcn.Mayer.TRACK_STATE, index=4, target=0, key="q", weight=w_track_final, phase=1, node=Node.END
        )
        self.objective_functions.add(
            ObjectiveFcn.Mayer.TRACK_STATE,
            index=5,
            target=self.twists,
            key="q",
            weight=w_track_final,
            phase=1,
            node=Node.END,
        )
        slack_duration = 0.15
        self.objective_functions.add(
            ObjectiveFcn.Mayer.MINIMIZE_TIME,
            min_bound=self.phase_durations[0] - slack_duration,
            max_bound=self.phase_durations[0] + slack_duration,
            phase=0,
            weight=1e-6,
        )
        self.objective_functions.add(
            ObjectiveFcn.Mayer.MINIMIZE_TIME,
            min_bound=self.phase_durations[1] - slack_duration,
            max_bound=self.phase_durations[1] + slack_duration,
            phase=1,
            weight=1e-6,
        )
def _set_objective_functions ( self ) : w_qdot = 1 w_penalty = 1 w_penalty_foot = 10 w_penalty_core = 10 w_track_final = 0.1 w_angular_momentum_x = 100000 w_angular_momentum_yz = 1000 for i in range ( len ( self . n_shooting ) ) : self . objective_functions . add ( ObjectiveFcn . Lagrange . MINIMIZE_STATE , derivative = True , key = "qdot" , index = ( 6 , 7 , 8 , 9 , 10 , 11 , 12 , 13 , 14 ) , weight = w_qdot , phase = i , ) self . objective_functions . add ( ObjectiveFcn . Mayer . MINIMIZE_MARKERS , derivative = True , reference_jcs = 0 , marker_index = 6 , weight = w_penalty , phase = i , node = Node . ALL_SHOOTING , ) self . objective_functions . add ( ObjectiveFcn . Mayer . MINIMIZE_MARKERS , derivative = True , reference_jcs = 0 , marker_index = 11 , weight = w_penalty , phase = i , node = Node . ALL_SHOOTING , ) self . objective_functions . add ( ObjectiveFcn . Mayer . MINIMIZE_MARKERS , node = Node . ALL_SHOOTING , derivative = True , reference_jcs = 0 , marker_index = 16 , weight = w_penalty_foot , phase = i , quadratic = False , ) self . objective_functions . add ( ObjectiveFcn . Lagrange . MINIMIZE_STATE , index = ( 6 , 7 , 8 , 13 , 14 ) , key = "q" , weight = w_penalty_core , phase = i ) self . objective_functions . add ( ObjectiveFcn . Mayer . MINIMIZE_ANGULAR_MOMENTUM , node = Node . START , phase = 0 , weight = w_angular_momentum_x , quadratic = False , index = 0 , ) self . objective_functions . add ( ObjectiveFcn . Mayer . MINIMIZE_ANGULAR_MOMENTUM , node = Node . START , phase = 0 , weight = w_angular_momentum_yz , quadratic = True , index = [ 1 , 2 ] , ) if self . extra_obj : for i in range ( 2 ) : if ( self . dynamics_type == MillerDynamics . IMPLICIT_TAU_DRIVEN_QDDDOT or self . dynamics_type == MillerDynamics . ROOT_IMPLICIT_QDDDOT ) : self . objective_functions . add ( ObjectiveFcn . Lagrange . MINIMIZE_CONTROL , key = "qdddot" , phase = i , weight = 1e-8 ) if self . dynamics_type == MillerDynamics . IMPLICIT or self . dynamics_type == MillerDynamics . ROOT_IMPLICIT : self . objective_functions . add ( ObjectiveFcn . Lagrange . MINIMIZE_CONTROL , key = "qddot" , phase = i , weight = 1e-4 ) self . objective_functions . add ( ObjectiveFcn . Mayer . TRACK_STATE , index = ( 0 , 1 , 2 ) , target = [ 0 , 0 , 0 ] , key = "q" , weight = w_track_final , phase = 1 , node = Node . END , ) self . objective_functions . add ( ObjectiveFcn . Mayer . TRACK_STATE , index = 3 , target = self . somersaults - self . thorax_hips_xyz - self . slack_final_somersault / 2 , key = "q" , weight = w_track_final , phase = 1 , node = Node . END , ) self . objective_functions . add ( ObjectiveFcn . Mayer . TRACK_STATE , index = 4 , target = 0 , key = "q" , weight = w_track_final , phase = 1 , node = Node . END ) self . objective_functions . add ( ObjectiveFcn . Mayer . TRACK_STATE , index = 5 , target = self . twists , key = "q" , weight = w_track_final , phase = 1 , node = Node . END , ) slack_duration = 0.15 self . objective_functions . add ( ObjectiveFcn . Mayer . MINIMIZE_TIME , min_bound = self . phase_durations [ 0 ] - slack_duration , max_bound = self . phase_durations [ 0 ] + slack_duration , phase = 0 , weight = 1e-6 , ) self . objective_functions . add ( ObjectiveFcn . Mayer . MINIMIZE_TIME , min_bound = self . phase_durations [ 1 ] - slack_duration , max_bound = self . phase_durations [ 1 ] + slack_duration , phase = 1 , weight = 1e-6 , )
Set the multi-objective functions for each phase with specific weights
Set the multi - objective functions for each phase with specific weights
"""
        Set the multi-objective functions for each phase with specific weights
        """ # --- Objective function --- # # Regularization # Right hand trajectory # Left hand trajectory ########### Lagrange # feet trajectory # core DoFs # Track momentum and Minimize delta momentum # Help to stay upright at the landing.
self