Reproduce same behavior

Author: rdesarz

biped_walking_controller/foot.py

@@ -511,7 +511,7 @@ def compute_swing_foot_pose(
     """
     t_ss = params.t_ss
 
-    if True:  # t_state <= t_ss:
+    if t_state <= t_ss:
         # Normal swing phase
         s = np.clip(t_state / t_ss, 0.0, 1.0)
 

biped_walking_controller/state_machine.py

@@ -197,9 +197,8 @@ def _try_transition_to_ds_or_end(self, t: float, contact_force: float):
         Otherwise the state transitions to DS and the step index is incremented.
         """
         if (
-            t - self.t_start
-            > self.params.t_ss
-            # and contact_force > self.params.force_threshold
+            t - self.t_start > 0.5 * self.params.t_ss
+            and contact_force > self.params.force_threshold
         ):
             self.t_start = t
             self.state = WalkingState.END if self._is_last_step() else WalkingState.DS

examples/example_5_walking_controller.py

@@ -36,6 +36,7 @@
     WalkingStateMachineParams,
     WalkingStateMachine,
     WalkingState,
+    Foot,
 )
 
 
@@ -55,7 +56,7 @@ def main():
     t_ds = 0.3  # Double support phase time window
     t_init = 2.0  # Initialization phase (transition from still position to first step)
     t_end = 0.4
-    n_steps = 5  # Number of steps executed by the robot
+    n_steps = 15  # Number of steps executed by the robot
     l_stride = 0.1  # Length of the stride
     max_height_foot = 0.01  # Maximal height of the swing foot
 
@@ -267,8 +268,7 @@ def main():
                 lf_start = steps_pose[step_idx - 1]
                 lf_target = steps_pose[step_idx + 1]
 
-            rf_pose = oMf_rf_tgt.translation
-
+            rf_pose = steps_pose[step_idx]
             lf_pose = compute_swing_foot_pose(
                 t_state=state_machine.get_elapsed_time_in_state(k * dt),
                 params=params,
@@ -283,23 +283,11 @@ def main():
             q_des, dq = solve_inverse_kinematics(
                 q_init,
                 com_target,
-                oMf_fixed_foot=oMf_rf_tgt,
+                oMf_fixed_foot=pin.SE3(oMf_rf_tgt.rotation, rf_pose),
                 oMf_moving_foot=oMf_lf,
                 oMf_torso=oMf_torso,
                 params=ik_sol_params,
             )
-
-            lf_next = compute_swing_foot_pose(
-                t_state=state_machine.get_elapsed_time_in_state((k + 1) * dt),
-                params=params,
-                step_start=lf_start,
-                step_target=lf_target,
-                swing_height=max_height_foot,
-                touchdown_extension_vel=touchdown_vel,
-                path_generator=generator,
-            )
-
-            oMf_lf_tgt = pin.SE3(oMf_lf_tgt.rotation, lf_next)
         elif state_machine.get_current_state() == WalkingState.SS_LEFT:
             if step_idx < 2:
                 rf_start = rf_initial_pose
@@ -311,7 +299,7 @@ def main():
             ik_sol_params.fixed_foot_frame = talos.left_foot_id
             ik_sol_params.moving_foot_frame = talos.right_foot_id
 
-            lf_pose = oMf_lf_tgt.translation
+            lf_pose = steps_pose[step_idx]
             rf_pose = compute_swing_foot_pose(
                 t_state=state_machine.get_elapsed_time_in_state(k * dt),
                 params=params,
@@ -326,35 +314,37 @@ def main():
             q_des, dq = solve_inverse_kinematics(
                 q_init,
                 com_target,
-                oMf_fixed_foot=oMf_lf_tgt,
+                oMf_fixed_foot=pin.SE3(oMf_lf_tgt.rotation, lf_pose),
                 oMf_moving_foot=oMf_rf,
                 oMf_torso=oMf_torso,
                 params=ik_sol_params,
             )
+        elif state_machine.get_current_state() == WalkingState.END:
 
-            rf_next = compute_swing_foot_pose(
-                t_state=state_machine.get_elapsed_time_in_state((k + 1) * dt),
-                params=params,
-                step_start=rf_start,
-                step_target=rf_target,
-                swing_height=max_height_foot,
-                touchdown_extension_vel=touchdown_vel,
-                path_generator=generator,
-            )
-
-            oMf_rf_tgt = pin.SE3(oMf_rf_tgt.rotation, rf_next)
         else:
-            lf_pose = oMf_lf_tgt.translation
-            rf_pose = oMf_rf_tgt.translation
+            if steps_ids[step_idx] == Foot.RIGHT:
+                if step_idx == 0:
+                    rf_pose = steps_pose[step_idx]
+                    lf_pose = lf_initial_pose
+                else:
+                    rf_pose = steps_pose[step_idx]
+                    lf_pose = steps_pose[step_idx - 1]
+            else:
+                if step_idx == 0:
+                    rf_pose = rf_initial_pose
+                    lf_pose = steps_pose[step_idx]
+                else:
+                    rf_pose = steps_pose[step_idx - 1]
+                    lf_pose = steps_pose[step_idx]
 
             ik_sol_params.fixed_foot_frame = talos.left_foot_id
             ik_sol_params.moving_foot_frame = talos.right_foot_id
 
             q_des, dq = solve_inverse_kinematics(
                 q_init,
                 com_target,
-                oMf_fixed_foot=oMf_lf_tgt,
-                oMf_moving_foot=oMf_rf_tgt,
+                oMf_fixed_foot=pin.SE3(oMf_lf_tgt.rotation, lf_pose),
+                oMf_moving_foot=pin.SE3(oMf_rf_tgt.rotation, rf_pose),
                 oMf_torso=oMf_torso,
                 params=ik_sol_params,
             )