2 use a state state machine to switch between walking phases

biped_walking_controller/plot.py

@@ -3,6 +3,8 @@
     compute_single_support_polygon,
 )
 
+from biped_walking_controller.preview_control import WalkingState
+
 
 def plot_steps(axes, steps_pose, step_shape):
     # Plot double support polygon
@@ -186,18 +188,30 @@ def traj2d(arr):  # drop last 2 samples as in your code
     ax2.set_title(f"{title_prefix} — plan view (x–y)")
 
 
-def plot_contact_forces(t, force_rf, force_lf, title="Contact force Fx"):
+def plot_contact_forces(
+    t: float,
+    force_rf: float,
+    force_lf: float,
+    states: WalkingState,
+    title: str = "Contact force Fx",
+):
     t = np.asarray(t).ravel()
     force_rf = np.asarray(force_rf).ravel()
     force_lf = np.asarray(force_lf).ravel()
     assert t.size == force_rf.size == force_lf.size
 
-    plt.figure(figsize=(12, 8), layout="constrained")
-    plt.plot(t, force_rf, label="right foot")
-    plt.plot(t, force_lf, label="left foot")
-    plt.xlabel("t [s]")
-    plt.ylabel("Normal force [N]")
-    plt.title(title)
-    plt.grid(True)
-    plt.legend()
-    plt.tight_layout()
+    fig, ax = plt.subplots(2, figsize=(12, 8), layout="constrained")
+    ax[0].plot(t, force_rf, label="right foot")
+    ax[0].plot(t, force_lf, label="left foot")
+    ax[0].set_xlabel("t [s]")
+    ax[0].set_ylabel("Normal force [N]")
+    ax[0].set_title(title)
+    ax[0].grid(True)
+    ax[0].legend()
+
+    ax[1].plot(t, states, label="state")
+    ax[1].set_xlabel("t [s]")
+    ax[1].set_ylabel("State [-]")
+    ax[1].set_title(title)
+    ax[1].grid(True)
+    ax[1].legend()

biped_walking_controller/preview_control.py

@@ -1,4 +1,8 @@
+import abc
+import typing
+from abc import abstractmethod
 from dataclasses import dataclass
+from enum import Enum
 
 import numpy as np
 from scipy.linalg import solve_discrete_are
@@ -251,3 +255,197 @@ def update_control(ctrl_mat: PreviewControllerMatrices, current_zmp, zmp_ref, x,
     y_next[1:] = ctrl_mat.A @ y[1:] + ctrl_mat.B.ravel() * u[1]
 
     return u, x_next, y_next
+
+
+class WalkingState(Enum):
+    """Enumeration of the high-level walking phases."""
+
+    INIT = 0  # Initialization (pre-walk)
+    DS = 1  # Double support phase
+    SS_LEFT = 2  # Single support, left foot stance
+    SS_RIGHT = 3  # Single support, right foot stance
+    END = 4  # End of walking sequence
+
+
+@dataclass
+class WalkingStateMachineParams:
+    """Timing and contact parameters for the walking state machine.
+
+    Attributes
+    ----------
+    t_init : float
+        Duration of the initial phase before starting walking [s].
+    t_end : float
+        Duration of the final phase after the last step [s].
+    t_ss : float
+        Nominal duration of a single support phase [s].
+    t_ds : float
+        Nominal duration of a double support phase [s].
+    force_threshold : float
+        Contact force threshold used to detect foot contact [N].
+    """
+
+    t_init: float = 2.0  # [s]
+    t_end: float = 2.0  # [s]
+    t_ss: float = 0.8  # [s]
+    t_ds: float = 0.3  # [s]
+    force_threshold: float = 50  # [N]
+
+
+class WalkingStateMachine:
+    """Finite state machine controlling walking phases.
+
+    The state machine advances through INIT, DS, SS_LEFT, SS_RIGHT, and END
+    based on elapsed time and measured foot contact forces.
+    """
+
+    def __init__(
+        self,
+        params: WalkingStateMachineParams,
+        initial_state: WalkingState = WalkingState.INIT,
+    ):
+        """Initialize the walking state machine.
+
+        Parameters
+        ----------
+        params : WalkingStateMachineParams
+            Timing and contact parameters for the state machine.
+        initial_state : WalkingState, optional
+            Initial state of the machine, by default WalkingState.INIT.
+        """
+        self.params = params
+        self.state = initial_state
+        self.next_ss_state = WalkingState.SS_RIGHT
+        self.t_start = 0.0
+        self.steps = None
+        self.step_idx = None
+
+    def update_steps(self, steps_pose):
+        """Set the planned sequence of steps.
+
+        Parameters
+        ----------
+        steps_pose :
+            Container describing the planned footsteps (e.g. list/array of poses).
+            Only its length is used here to know when the last step is reached.
+        """
+        self.steps = steps_pose
+        self.step_idx = 0
+
+    def update(
+        self,
+        t: float,
+        rf_contact_force: float,
+        lf_contact_force: float,
+    ):
+        """Update the state machine given time and contact forces.
+
+        Parameters
+        ----------
+        t : float
+            Current time [s].
+        rf_contact_force : float
+            Measured contact force under the right foot [N].
+        lf_contact_force : float
+            Measured contact force under the left foot [N].
+
+        Notes
+        -----
+        This method updates the internal state of the machine in-place.
+        If no steps have been provided with `update_steps`, it returns immediately.
+        """
+        if self.steps is None:
+            return
+
+        delta_t = t - self.t_start
+
+        if self.state == WalkingState.INIT:
+            self._try_transition_to_single_support(t, self.params.t_init)
+
+        elif self.state == WalkingState.DS:
+            self._try_transition_to_single_support(t, self.params.t_ds)
+
+        elif self.state == WalkingState.SS_RIGHT:
+            self._try_transition_to_ds_or_end(t, lf_contact_force)
+
+        elif self.state == WalkingState.SS_LEFT:
+            self._try_transition_to_ds_or_end(t, rf_contact_force)
+
+        elif self.state == WalkingState.END:
+            if delta_t > self.params.t_end:
+                self.steps = None
+
+    def get_current_state(self) -> WalkingState:
+        """Return the current walking state.
+
+        Returns
+        -------
+        WalkingState
+            Current state of the finite state machine.
+        """
+        return self.state
+
+    def _switch_single_support_leg(self, t: float):
+        """Switch to the next single-support state and reset the phase timer.
+
+        Parameters
+        ----------
+        t : float
+            Current time [s], used as the new phase start time.
+        """
+        self.t_start = t
+        self.state = self.next_ss_state
+        self.next_ss_state = (
+            WalkingState.SS_LEFT if self.state == WalkingState.SS_RIGHT else WalkingState.SS_RIGHT
+        )
+
+    def _try_transition_to_single_support(self, t: float, duration: float):
+        """Transition from INIT/DS to the next single-support state if duration elapsed.
+
+        Parameters
+        ----------
+        t : float
+            Current time [s].
+        duration : float
+            Required time spent in the current phase before switching [s].
+        """
+        if t - self.t_start > duration:
+            self._switch_single_support_leg(t)
+
+    def _is_last_step(self) -> bool:
+        """Check whether the current step is the last one.
+
+        Returns
+        -------
+        bool
+            True if the current step index corresponds to the last planned step,
+            False otherwise.
+        """
+        return self.step_idx == len(self.steps) - 2
+
+    def _try_transition_to_ds_or_end(self, t: float, contact_force: float):
+        """Transition from single support to double support or END.
+
+        Parameters
+        ----------
+        t : float
+            Current time [s].
+        contact_force : float
+            Measured contact force of the swing foot [N].
+
+        Notes
+        -----
+        The transition is triggered when:
+        - at least half of `t_ss` has elapsed since `t_start`, and
+        - the swing foot contact force exceeds `force_threshold`.
+
+        If the current step is the last one, the next state is END.
+        Otherwise the state transitions to DS and the step index is incremented.
+        """
+        if (
+            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
+            self.step_idx += 1

biped_walking_controller/simulation.py

@@ -406,9 +406,9 @@ def __init__(self, dt, path_to_robot_urdf: Path, model, launch_gui=True):
         for j in range(pb.getNumJoints(self.robot_id)):
             info = pb.getJointInfo(self.robot_id, j)
             link_name = info[12].decode("ascii")
-            if link_name == model.get_lf_link_name():
+            if link_name == model.get_rf_link_name():
                 self.rf_link_id = info[0]
-            elif link_name == model.get_rf_link_name():
+            elif link_name == model.get_lf_link_name():
                 self.lf_link_id = info[0]
 
         # Build mapping between joints for both velocities and position

examples/example_1_lipm_preview_control.py

@@ -15,7 +15,8 @@
     BezierCurveFootPathGenerator,
 )
 
-if __name__ == "__main__":
+
+def main():
     # Parameters
     dt = 1 / 240.0  # 0.005  # Delta of time of the model simulation
 
@@ -203,3 +204,7 @@
 
     # Uncomment to save the plot
     # imageio.mimsave("img/traj.gif", frames[2:], fps=int(1/update_frequency * 2), loop=10) # Save at frame divided by 2
+
+
+if __name__ == "__main__":
+    main()

examples/example_2_feet_motion.py

@@ -3,7 +3,8 @@
 
 from biped_walking_controller.foot import compute_feet_path_and_poses, BezierCurveFootPathGenerator
 
-if __name__ == "__main__":
+
+def main():
     # Parameters
     dt = 0.005  # Delta of time of the model simulation
 
@@ -61,3 +62,7 @@
     axes[2].set_title("Feet trajectories on z-axis")
 
     plt.show()
+
+
+if __name__ == "__main__":
+    main()

examples/example_3_walk_inverse_kinematic.py

@@ -18,7 +18,8 @@
 from biped_walking_controller.model import Talos
 from biped_walking_controller.visualizer import Visualizer
 
-if __name__ == "__main__":
+
+def main():
     p = argparse.ArgumentParser()
     p.add_argument("--path-talos-data", type=Path, help="Path to talos_data root")
     p.add_argument(
@@ -177,3 +178,7 @@
         elapsed_dt = stop - start
         remaining_dt = dt - elapsed_dt
         sleep(max(0.0, remaining_dt))
+
+
+if __name__ == "__main__":
+    main()