Clean range

Author: rdesarz

biped_walking_controller/plot.py

@@ -145,6 +145,7 @@ def plot_feet_and_com(
         ax.set_ylabel(coord_labels[coord_idx.index(j)])
         ax.grid(True)
 
+    axes[1].set_ylim((-0.02, 0.06))
     axes[-1].set_xlabel("t [s]")
 
     # One combined legend outside

biped_walking_controller/preview_control.py

@@ -1,12 +1,11 @@
-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
 
+from biped_walking_controller.state_machine import WalkingState, Foot
+
 
 def linear_interpolation(t: np.ndarray, pos_begin: np.ndarray, pos_end: np.ndarray) -> np.ndarray:
     return (1 - t)[:, None] * pos_begin + t[:, None] * pos_end
@@ -263,6 +262,7 @@ def update_control(ctrl_mat: PreviewControllerMatrices, current_zmp, zmp_ref, x,
 
     return u, x_next, y_next
 
+
 def build_zmp_horizon(
     com_initial_target,
     t_horizon: float,

examples/example_5_walking_controller.py

@@ -55,10 +55,10 @@ def main():
     t_ss = 0.8  # Single support phase time window
     t_ds = 0.3  # Double support phase time window
     t_init = 2.0  # Initialization phase (transition from still position to first step)
-    t_end = 0.4
+    t_end = 2.0
     n_steps = 5  # Number of steps executed by the robot
     l_stride = 0.1  # Length of the stride
-    max_height_foot = 0.01  # Maximal height of the swing foot
+    max_height_foot = 0.03  # Maximal height of the swing foot
 
     # Preview controller parameters
     t_preview = 1.6  # Time horizon used for the preview controller
@@ -381,11 +381,11 @@ def main():
 
             lf_ref_pos[k] = lf_pose
             lf_pin_pos[k] = talos.data.oMf[talos.left_foot_id].translation
-            lf_pb_pos[k], _ = simulator.get_robot_frame_pos("leg_left_6_link")
+            # lf_pb_pos[k], _ = simulator.get_robot_frame_pos("leg_left_6_link")
 
             rf_ref_pos[k] = rf_pose
             rf_pin_pos[k] = talos.data.oMf[talos.right_foot_id].translation
-            rf_pb_pos[k], _ = simulator.get_robot_frame_pos("leg_right_6_link")
+            # rf_pb_pos[k], _ = simulator.get_robot_frame_pos("leg_right_6_link")
 
             rf_forces[k], lf_forces[k] = simulator.get_contact_forces()