Refactor

Author: rdesarz

biped_walking_controller/preview_control.py

@@ -334,44 +334,41 @@ def update(
         delta_t = t - self.t_start
 
         if self.state == State.INIT:
-            if delta_t > self.params.t_init:
-                if self.next_ss_state == State.SS_RIGHT:
-                    self.t_start = t
-                    self.state = State.SS_RIGHT
-                    self.next_ss_state = State.SS_LEFT
-                elif self.next_ss_state == State.SS_LEFT:
-                    self.t_start = t
-                    self.state = State.SS_LEFT
-                    self.next_ss_state = State.SS_RIGHT
+            self._try_transition_to_single_support(t, self.params.t_init)
+
         elif self.state == State.DS:
-            if delta_t > self.params.t_ds:
-                if self.next_ss_state == State.SS_RIGHT:
-                    self.t_start = t
-                    self.state = State.SS_RIGHT
-                    self.next_ss_state = State.SS_LEFT
-                elif self.next_ss_state == State.SS_LEFT:
-                    self.t_start = t
-                    self.state = State.SS_LEFT
-                    self.next_ss_state = State.SS_RIGHT
+            self._try_transition_to_single_support(t, self.params.t_ds)
+
         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
-                if self.step_idx == len(self.steps) - 2:
-                    self.state = State.END
-                else:
-                    self.state = State.DS
-                self.step_idx += 1
+            self._try_transition_to_ds_or_end(t, lf_contact_force)
+
         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
-                if self.step_idx == len(self.steps) - 2:
-                    self.state = State.END
-                else:
-                    self.state = State.DS
-                self.step_idx += 1
+            self._try_transition_to_ds_or_end(t, rf_contact_force)
+
         elif self.state == State.END:
             if delta_t > self.params.t_end:
                 self.steps = None
 
     def get_current_state(self) -> State:
         return self.state
+
+    def _switch_single_support_leg(self, t: float):
+        self.t_start = t
+        self.state = self.next_ss_state
+        self.next_ss_state = State.SS_LEFT if self.state == State.SS_RIGHT else State.SS_RIGHT
+
+    def _try_transition_to_single_support(self, t: float, duration: float):
+        if t - self.t_start > duration:
+            self._switch_single_support_leg(t)
+
+    def _is_last_step(self) -> bool:
+        return self.step_idx == len(self.steps) - 2
+
+    def _try_transition_to_ds_or_end(self, t: float, contact_force: float):
+        if (
+            t - self.t_start > 0.5 * self.params.t_ss
+            and contact_force > self.params.force_threshold
+        ):
+            self.t_start = t
+            self.state = State.END if self._is_last_step() else State.DS
+            self.step_idx += 1