diff --git a/biped_walking_controller/plot.py b/biped_walking_controller/plot.py index 69a5317..502ca13 100644 --- a/biped_walking_controller/plot.py +++ b/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() diff --git a/biped_walking_controller/preview_control.py b/biped_walking_controller/preview_control.py index 0e1242a..60db4b3 100644 --- a/biped_walking_controller/preview_control.py +++ b/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 diff --git a/biped_walking_controller/simulation.py b/biped_walking_controller/simulation.py index 876bc3b..d2e708c 100644 --- a/biped_walking_controller/simulation.py +++ b/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 diff --git a/examples/example_1_lipm_preview_control.py b/examples/example_1_lipm_preview_control.py index f908017..fd749a4 100644 --- a/examples/example_1_lipm_preview_control.py +++ b/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() diff --git a/examples/example_2_feet_motion.py b/examples/example_2_feet_motion.py index 9652265..013fbd1 100644 --- a/examples/example_2_feet_motion.py +++ b/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() diff --git a/examples/example_3_walk_inverse_kinematic.py b/examples/example_3_walk_inverse_kinematic.py index 420ba5b..9cf372d 100644 --- a/examples/example_3_walk_inverse_kinematic.py +++ b/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() diff --git a/tests/test_preview_control.py b/tests/test_preview_control.py index 5bf6bc7..b62b5eb 100644 --- a/tests/test_preview_control.py +++ b/tests/test_preview_control.py @@ -4,6 +4,9 @@ from biped_walking_controller.preview_control import ( PreviewControllerParams, compute_preview_control_matrices, + WalkingStateMachineParams, + WalkingStateMachine, + WalkingState, ) from numpy.linalg import eigvals @@ -95,5 +98,76 @@ def test_gains_change_with_Qe(self): self.assertFalse(np.allclose(base.Gd, alt.Gd)) +class TestWalkingPhase(unittest.TestCase): + def setUp(self): + self.params = WalkingStateMachineParams() + self.steps = np.ones([5, 3]) # Five steps + + def test_begin_init(self): + wsm = WalkingStateMachine(self.params, initial_state=WalkingState.INIT) + wsm.update_steps(self.steps) + + wsm.update(t=0.0, rf_contact_force=100.0, lf_contact_force=100.0) + + self.assertEqual(wsm.get_current_state(), WalkingState.INIT) + + def test_init_to_ds(self): + wsm = WalkingStateMachine(self.params, initial_state=WalkingState.INIT) + wsm.update_steps(self.steps) + wsm.update(t=0.0, rf_contact_force=100.0, lf_contact_force=100.0) + + wsm.update(t=self.params.t_init + 0.1, rf_contact_force=0.0, lf_contact_force=100.0) + + self.assertEqual(wsm.get_current_state(), WalkingState.SS_RIGHT) + + def test_begin_double_support(self): + wsm = WalkingStateMachine(self.params, initial_state=WalkingState.DS) + wsm.update_steps(self.steps) + + wsm.update(t=0.0, rf_contact_force=100.0, lf_contact_force=100.0) + + self.assertEqual(wsm.get_current_state(), WalkingState.DS) + + def test_switch_to_single_support(self): + wsm = WalkingStateMachine(self.params, initial_state=WalkingState.DS) + wsm.update_steps(self.steps) + + wsm.update(t=self.params.t_ds + 0.1, rf_contact_force=0.0, lf_contact_force=0.0) + + self.assertEqual(wsm.get_current_state(), WalkingState.SS_RIGHT) + + def test_do_not_switch_to_ds_if_beginning_of_phase(self): + wsm = WalkingStateMachine(self.params, initial_state=WalkingState.SS_RIGHT) + wsm.update_steps(self.steps) + + wsm.update(t=0.0, rf_contact_force=0.0, lf_contact_force=self.params.force_threshold + 10) + + self.assertEqual(wsm.get_current_state(), WalkingState.SS_RIGHT) + + def test_do_not_switch_to_ds_if_force_too_low(self): + wsm = WalkingStateMachine(self.params, initial_state=WalkingState.SS_RIGHT) + wsm.update_steps(self.steps) + + wsm.update( + t=self.params.t_ss * 0.75, + rf_contact_force=0.0, + lf_contact_force=self.params.force_threshold - 10, + ) + + self.assertEqual(wsm.get_current_state(), WalkingState.SS_RIGHT) + + def test_switch_to_ds_if_force_too_low_and_phase_close_to_end(self): + wsm = WalkingStateMachine(self.params, initial_state=WalkingState.SS_LEFT) + wsm.update_steps(self.steps) + + wsm.update( + t=self.params.t_ss * 0.75, + rf_contact_force=self.params.force_threshold + 10, + lf_contact_force=0.0, + ) + + self.assertEqual(wsm.get_current_state(), WalkingState.DS) + + if __name__ == "__main__": unittest.main()