Fix state machine

Author: rdesarz

biped_walking_controller/preview_control.py

@@ -315,13 +315,18 @@ def __init__(self, params: WalkingFSMParams, initial_state=State.INIT):
         self.state = initial_state
         self.next_ss_state = State.SS_RIGHT
         self.t_start = 0.0
+        self.steps = None
+        self.step_idx = None
 
     def update(
         self,
         t: float,
         rf_contact_force: float,
         lf_contact_force: float,
     ):
+        if self.steps is None:
+            return
+
         delta_t = t - self.t_start
 
         if self.state == State.INIT:
@@ -347,17 +352,30 @@ def update(
         elif self.state == State.SS_RIGHT:
             if delta_t > 0.5 * self.params.t_ss and lf_contact_force > self.params.force_threshold:
                 self.t_start = t
-                self.state = State.DS
+                if self.step_idx == len(self.steps) - 2:
+                    self.state = State.END
+                else:
+                    self.state = State.DS
+                self.step_idx += 1
         elif self.state == State.SS_LEFT:
             if delta_t > 0.5 * self.params.t_ss and rf_contact_force > self.params.force_threshold:
                 self.t_start = t
-                self.state = State.DS
+                if self.step_idx == len(self.steps) - 2:
+                    self.state = State.END
+                else:
+                    self.state = State.DS
+                self.step_idx += 1
         elif self.state == State.END:
-            pass
+            if delta_t > self.params.t_end:
+                self.steps = None
 
     def get_current_state(self) -> State:
         return self.state
 
+    def update_steps(self, steps_pose):
+        self.steps = steps_pose
+        self.step_idx = 0
+
 
 class CentroidalPlanner:
     def __init__(

examples/example_5_walking_controller.py

@@ -197,6 +197,8 @@ def main():
         interp_fn=cubic_spline_interpolation,
     )
 
+    state_machine.update_steps(steps_pose)
+
     zmp_padded = np.vstack(
         [zmp_ref, np.repeat(zmp_ref[-1][None, :], ctrler_params.n_preview_steps, axis=0)]
     )