Specify the step length in the periodic step main

ami-iit · Nov 29, 2023 · bbc0e2d · bbc0e2d
1 parent b117b1d
commit bbc0e2d
Showing 1 changed file with 7 additions and 5 deletions.
diff --git a/src/hippopt/turnkey_planners/humanoid_kinodynamic/main_periodic_step.py b/src/hippopt/turnkey_planners/humanoid_kinodynamic/main_periodic_step.py
@@ -360,22 +360,24 @@ def get_references(
 
     horizon = planner_settings.horizon_length * planner_settings.time_step
 
+    step_length = 0.6
+
     contact_phases_guess = hp_rp.FeetContactPhasesDescriptor()
     contact_phases_guess.left = [
         hp_rp.FootContactPhaseDescriptor(
             transform=liecasadi.SE3.from_translation_and_rotation(
                 np.array([0.0, 0.1, 0.0]), liecasadi.SO3.Identity()
             ),
             mid_swing_transform=liecasadi.SE3.from_translation_and_rotation(
-                np.array([0.3, 0.1, 0.05]), liecasadi.SO3.Identity()
+                np.array([step_length / 2, 0.1, 0.05]), liecasadi.SO3.Identity()
             ),
             force=np.array([0, 0, 100.0]),
             activation_time=None,
             deactivation_time=horizon / 6.0,
         ),
         hp_rp.FootContactPhaseDescriptor(
             transform=liecasadi.SE3.from_translation_and_rotation(
-                np.array([0.6, 0.1, 0.0]), liecasadi.SO3.Identity()
+                np.array([step_length, 0.1, 0.0]), liecasadi.SO3.Identity()
             ),
             mid_swing_transform=None,
             force=np.array([0, 0, 100.0]),
@@ -387,18 +389,18 @@ def get_references(
     contact_phases_guess.right = [
         hp_rp.FootContactPhaseDescriptor(
             transform=liecasadi.SE3.from_translation_and_rotation(
-                np.array([0.3, -0.1, 0.0]), liecasadi.SO3.Identity()
+                np.array([step_length / 2, -0.1, 0.0]), liecasadi.SO3.Identity()
             ),
             mid_swing_transform=liecasadi.SE3.from_translation_and_rotation(
-                np.array([0.60, -0.1, 0.05]), liecasadi.SO3.Identity()
+                np.array([step_length, -0.1, 0.05]), liecasadi.SO3.Identity()
             ),
             force=np.array([0, 0, 100.0]),
             activation_time=None,
             deactivation_time=horizon * 2.0 / 3.0,
         ),
         hp_rp.FootContactPhaseDescriptor(
             transform=liecasadi.SE3.from_translation_and_rotation(
-                np.array([0.9, -0.1, 0.0]), liecasadi.SO3.Identity()
+                np.array([1.5 * step_length, -0.1, 0.0]), liecasadi.SO3.Identity()
             ),
             mid_swing_transform=None,
             force=np.array([0, 0, 100.0]),