Added possibility to add periodicity constraint on the control inputs

src/hippopt/turnkey_planners/humanoid_kinodynamic/main_periodic_step.py

@@ -110,6 +110,7 @@ def get_planner_settings() -> walking_settings.Settings:
     settings.contact_velocity_control_cost_multiplier = 5.0
     settings.contact_force_control_cost_multiplier = 0.0001
     settings.final_state_expression_type = hippopt.ExpressionType.subject_to
+    settings.periodicity_expression_type = hippopt.ExpressionType.subject_to
     settings.casadi_function_options = {"cse": True}
     settings.casadi_opti_options = {"expand": True, "detect_simple_bounds": True}
     settings.casadi_solver_options = {
@@ -231,12 +232,12 @@ def compute_initial_state(
         number_of_joints=len(desired_joints),
     )
 
-    pf_ref.state.com = np.array([0.0, 0.0, 0.7])
+    pf_ref.state.com = np.array([0.15, 0.0, 0.7])
     desired_left_foot_pose = liecasadi.SE3.from_translation_and_rotation(
         np.array([0.0, 0.1, 0.0]), liecasadi.SO3.Identity()
     )
     desired_right_foot_pose = liecasadi.SE3.from_translation_and_rotation(
-        np.array([0.0, -0.1, 0.0]), liecasadi.SO3.Identity()
+        np.array([0.3, -0.1, 0.0]), liecasadi.SO3.Identity()
     )
     pf_ref.state.contact_points.left = (
         hp_rp.FootContactState.from_parent_frame_transform(
@@ -311,12 +312,12 @@ def compute_final_state(
         number_of_joints=len(desired_joints),
     )
 
-    pf_ref.state.com = np.array([0.15, 0.0, 0.7])
+    pf_ref.state.com = np.array([0.75, 0.0, 0.7])
     desired_left_foot_pose = liecasadi.SE3.from_translation_and_rotation(
-        np.array([0.0, 0.1, 0.0]), liecasadi.SO3.Identity()
+        np.array([0.60, 0.1, 0.0]), liecasadi.SO3.Identity()
     )
     desired_right_foot_pose = liecasadi.SE3.from_translation_and_rotation(
-        np.array([0.3, -0.1, 0.0]), liecasadi.SO3.Identity()
+        np.array([0.90, -0.1, 0.0]), liecasadi.SO3.Identity()
     )
     pf_ref.state.contact_points.left = (
         hp_rp.FootContactState.from_parent_frame_transform(

src/hippopt/turnkey_planners/humanoid_kinodynamic/planner.py

@@ -141,6 +141,8 @@ def __init__(self, settings: Settings) -> None:
             foot_name="right",
         )
 
+        self.add_periodicity_expression(all_contact_points)
+
     def get_function_inputs_dict(self):
         sym = self.ocp.symbolic_structure
         function_inputs = {
@@ -503,7 +505,9 @@ def add_robot_dynamics(self, all_contact_points: list, function_inputs: dict):
         )
         problem.add_dynamics(
             hp.dot(sym.system.centroidal_momentum) == centroidal_dynamics,  # noqa
-            x0=problem.initial(sym.initial_state.centroidal_momentum),  # noqa
+            x0=problem.initial(sym.initial_state.centroidal_momentum)
+            if self.settings.periodicity_expression_type is hp.ExpressionType.skip
+            else None,  # noqa
             dt=sym.dt,
             integrator=default_integrator,
             name="centroidal_momentum_dynamics",
@@ -806,6 +810,41 @@ def add_contact_point_regularization(
             scaling=self.settings.contact_force_control_cost_multiplier,
         )
 
+    def add_periodicity_expression(self, all_contact_points):
+        problem = self.ocp.problem
+        sym = self.ocp.symbolic_structure  # type: Variables
+        initial_controls = []
+        final_controls = []
+        for point in all_contact_points:
+            initial_controls.append(problem.initial(point.u_v))
+            initial_controls.append(problem.initial(point.f_dot))
+            final_controls.append(problem.final(point.u_v))
+            final_controls.append(problem.final(point.f_dot))
+        initial_controls.append(problem.initial(sym.system.centroidal_momentum))
+        final_controls.append(problem.final(sym.system.centroidal_momentum))
+        initial_controls.append(
+            problem.initial(sym.system.kinematics.base.linear_velocity)
+        )
+        initial_controls.append(
+            problem.initial(sym.system.kinematics.base.quaternion_velocity_xyzw)
+        )
+        initial_controls.append(
+            problem.initial(sym.system.kinematics.joints.velocities)
+        )
+        final_controls.append(problem.final(sym.system.kinematics.base.linear_velocity))
+        final_controls.append(
+            problem.final(sym.system.kinematics.base.quaternion_velocity_xyzw)
+        )
+        final_controls.append(problem.final(sym.system.kinematics.joints.velocities))
+        problem.add_expression(
+            mode=self.settings.periodicity_expression_type,
+            expression=cs.MX(
+                cs.vertcat(*initial_controls) == cs.vertcat(*final_controls)
+            ),
+            name="periodicity_expression",
+            scaling=self.settings.periodicity_expression_weight,
+        )
+
     def set_initial_guess(self, initial_guess: Variables) -> None:
         self.ocp.problem.set_initial_guess(initial_guess)
 

src/hippopt/turnkey_planners/humanoid_kinodynamic/settings.py

@@ -35,9 +35,11 @@ class Settings:
     minimum_joint_velocities: np.ndarray = dataclasses.field(default=None)
 
     final_state_expression_type: hp.ExpressionType = dataclasses.field(default=None)
-
     final_state_expression_weight: float = dataclasses.field(default=None)
 
+    periodicity_expression_type: hp.ExpressionType = dataclasses.field(default=None)
+    periodicity_expression_weight: float = dataclasses.field(default=None)
+
     contacts_centroid_cost_multiplier: float = dataclasses.field(default=None)
 
     com_linear_velocity_cost_weights: np.ndarray = dataclasses.field(default=None)
@@ -88,6 +90,8 @@ def __post_init__(self) -> None:
         self.maximum_angular_momentum = 10.0
         self.final_state_expression_type = hp.ExpressionType.skip
         self.final_state_expression_weight = 1.0
+        self.periodicity_expression_type = hp.ExpressionType.skip
+        self.periodicity_expression_weight = 1.0
 
     def is_valid(self) -> bool:
         number_of_joints = len(self.joints_name_list)