Improve tests

Author: rdesarz

biped_walking_controller/preview_control.py

@@ -301,7 +301,7 @@ class State(Enum):
 
 
 @dataclass
-class WalkingFSMParams:
+class WalkingStateMachineParams:
     t_init: float = 2.0  # [s]
     t_end: float = 2.0  # [s]
     t_ss: float = 0.8  # [s]
@@ -310,7 +310,7 @@ class WalkingFSMParams:
 
 
 class WalkingStateMachine:
-    def __init__(self, params: WalkingFSMParams, initial_state=State.INIT):
+    def __init__(self, params: WalkingStateMachineParams, initial_state=State.INIT):
         self.params = params
         self.state = initial_state
         self.next_ss_state = State.SS_RIGHT

examples/example_5_walking_controller.py

@@ -22,7 +22,7 @@
     compute_zmp_ref,
     cubic_spline_interpolation,
     WalkingStateMachine,
-    WalkingFSMParams,
+    WalkingStateMachineParams,
     State,
 )
 
@@ -183,7 +183,9 @@ def main():
         traj_generator=BezierCurveFootPathGenerator(max_height_foot),
     )
 
-    params = WalkingFSMParams(t_init=t_init, t_end=t_end, t_ss=t_ss, t_ds=t_ds, force_threshold=50)
+    params = WalkingStateMachineParams(
+        t_init=t_init, t_end=t_end, t_ss=t_ss, t_ds=t_ds, force_threshold=50
+    )
     state_machine = WalkingStateMachine(params=params, initial_state=State.INIT)
 
     zmp_ref = compute_zmp_ref(

tests/test_preview_control.py

@@ -4,7 +4,7 @@
 from biped_walking_controller.preview_control import (
     PreviewControllerParams,
     compute_preview_control_matrices,
-    WalkingFSMParams,
+    WalkingStateMachineParams,
     WalkingStateMachine,
     State,
 )
@@ -100,31 +100,53 @@ def test_gains_change_with_Qe(self):
 
 class TestWalkingPhase(unittest.TestCase):
     def setUp(self):
-        self.params = WalkingFSMParams()
+        self.params = WalkingStateMachineParams()
+        self.steps = np.ones([5, 3])  # Five steps
+
+    def test_begin_init(self):
+        wsm = WalkingStateMachine(self.params, initial_state=State.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(), State.INIT)
+
+    def test_init_to_ds(self):
+        wsm = WalkingStateMachine(self.params, initial_state=State.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(), State.SS_RIGHT)
 
     def test_begin_double_support(self):
         wsm = WalkingStateMachine(self.params, initial_state=State.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(), State.DS)
 
     def test_switch_to_single_support(self):
         wsm = WalkingStateMachine(self.params, initial_state=State.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(), State.SS_RIGHT)
 
     def test_do_not_switch_to_ds_if_beginning_of_phase(self):
         wsm = WalkingStateMachine(self.params, initial_state=State.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(), State.SS_RIGHT)
 
     def test_do_not_switch_to_ds_if_force_too_low(self):
         wsm = WalkingStateMachine(self.params, initial_state=State.SS_RIGHT)
+        wsm.update_steps(self.steps)
 
         wsm.update(
             t=self.params.t_ss * 0.75,
@@ -136,6 +158,7 @@ def test_do_not_switch_to_ds_if_force_too_low(self):
 
     def test_switch_to_ds_if_force_too_low_and_phase_close_to_end(self):
         wsm = WalkingStateMachine(self.params, initial_state=State.SS_LEFT)
+        wsm.update_steps(self.steps)
 
         wsm.update(
             t=self.params.t_ss * 0.75,