🎨 Fixes gait in sim

simulation/play.py

@@ -34,7 +34,7 @@ joint_directions = np.array([-1, 1, 1, 1, 1, 1, -1, 1, 1, 1, 1, 1])
 
 kinematics = Kinematics()
 
-standby = kinematics.get_default_feet_pos()
+standby = KinConfig.default_feet_positions[:4, :3]
 
 body_state = BodyStateT(
     omega=0,

simulation/src/robot/gait.py

@@ -41,18 +41,27 @@ length_multipliers = np.array([-1.4, -1.0, -1.5, -1.5, -1.5, 0.0, 0.0, 0.0, 1.5,
 height_profile = np.array([0.0, 0.0, 0.9, 0.9, 0.9, 0.9, 0.9, 1.1, 1.1, 1.1, 0.0, 0.0])
 
 
-def sine_curve(length, angle, height, phase):
+def sine_curve(length, angle, depth, phase):
-    x, z = length * (1 - 2 * phase) * np.cos(angle), length * (1 - 2 * phase) * np.sin(angle)
+    x_polar = np.cos(angle)
-    y = height * np.cos(np.pi * (x + z) / (2 * length)) if length else 0
+    z_polar = np.sin(angle)
-    return np.array([x, z, y])
+
+    step = length * (1 - 2 * phase)
+    x = step * x_polar
+    z = step * z_polar
+    y = -depth * np.cos((np.pi * (x + z)) / (2 * length)) if length != 0 else 0
+    return np.array([x, y, z])
 
 
-def yaw_arc(feet, current):
+def yaw_arc(default_foot, current_foot):
-    return (
+    foot_mag = np.sqrt(default_foot[0] ** 2 + default_foot[2] ** 2)
-        np.pi / 2
+    foot_dir = np.arctan2(default_foot[2], default_foot[0])
-        + np.arctan2(feet[1], feet[0])
+    offset_x = current_foot[0] - default_foot[0]
-        + np.arctan2(np.linalg.norm(current[:2] - feet[:2]), np.linalg.norm(feet[:2]))
+    offset_z = current_foot[2] - default_foot[2]
-    )
+    offset_y = current_foot[1] - default_foot[1]
+    offset_mag = np.sqrt(offset_x**2 + offset_z**2)
+    offset_mod = np.arctan2(offset_mag, foot_mag)
+
+    return np.pi / 2.0 + foot_dir + offset_mod
 
 
 def get_control_points(length, angle, height):
@@ -61,7 +70,7 @@ def get_control_points(length, angle, height):
     x = length * length_multipliers * x_polar
     z = length * length_multipliers * z_polar
     y = height * height_profile
-    return np.stack([x, z, y], axis=1)
+    return np.stack([x, y, z], axis=1)
 
 
 def bezier_curve(length, angle, height, phase):
@@ -73,7 +82,7 @@ def bezier_curve(length, angle, height, phase):
 
 class GaitController:
     def __init__(self, default_position: np.ndarray):
-        self.default_position = default_position
+        self.default_position = default_position.copy()
         self.phase = 0.0
 
     def step(self, gait: GaitStateT, body: BodyStateT, dt: float):
@@ -93,17 +102,20 @@ class GaitController:
         length = np.hypot(step_x, step_z)
         if step_x < 0:
             length = -length
-        turn_amplitude = np.arctan2(step_z, length) * 2
+        turn_amplitude = np.arctan2(step_z, length)
 
-        new_feet = np.zeros_like(self.default_position)
+        new_feet = self.default_position.copy()
 
         for i, (default_foot, current_foot) in enumerate(zip(self.default_position, body["feet"])):
             phase = (self.phase + offsets[i]) % 1
             ph_norm, curve_fn, amp = self._phase_params(phase, stand_fraction, depth, height)
             delta_pos = curve_fn(length / 2, turn_amplitude, amp, ph_norm)
-            delta_rot = curve_fn(np.rad2deg(angle), yaw_arc(default_foot, current_foot), amp, ph_norm)
+            delta_rot = curve_fn(angle * 2, yaw_arc(default_foot, current_foot), amp, ph_norm)
-            new_feet[i][:2] = default_foot[:2] + delta_pos[:2] + delta_rot[:2]
+
-            # new_feet[i][3] = 1
+            new_feet[i][0] = default_foot[0] + delta_pos[0] + delta_rot[0] * 0.2
+            new_feet[i][2] = default_foot[2] + delta_pos[2] + delta_rot[2] * 0.2
+            if length or angle:
+                new_feet[i][1] = default_foot[1] + delta_pos[1] + delta_rot[1] * 0.2
 
         body["feet"] = new_feet
 

simulation/src/robot/kinematics.py

@@ -13,12 +13,14 @@ class KinConfig:
 
     mountOffsets = [[L / 2, 0, W / 2], [L / 2, 0, -W / 2], [-L / 2, 0, W / 2], [-L / 2, 0, -W / 2]]
 
-    default_feet_positions = [
+    default_feet_positions = np.array(
+        [
             [mountOffsets[0][0], 0, mountOffsets[0][2] + coxa, 1],
             [mountOffsets[1][0], 0, mountOffsets[1][2] - coxa, 1],
             [mountOffsets[2][0], 0, mountOffsets[2][2] + coxa, 1],
             [mountOffsets[3][0], 0, mountOffsets[3][2] - coxa, 1],
         ]
+    )
 
     # Max constants
     max_roll = 15 * np.pi / 2

simulation/src/utils/gui.py

@@ -26,22 +26,18 @@ class GUI:
         self.step_x_slider = p.addUserDebugParameter("Step x", -1, 1, 0)
         self.step_z_slider = p.addUserDebugParameter("Step z", -1, 1, 0)
         self.angle_slider = p.addUserDebugParameter("Angle", -1, 1, 0)
-        self.step_height_slider = p.addUserDebugParameter("Step height", 0, 50, 15)
+        self.step_height_slider = p.addUserDebugParameter("Step height", 0, 1, 0.5)
         self.step_depth_slider = p.addUserDebugParameter("Step depth", 0, 0.01, 0.002)
         self.speed_slider = p.addUserDebugParameter("Speed", 0, 2, 1)
-        self.stand_frac_slider = p.addUserDebugParameter("Stand frac", 0, 1, 0.5)
+        self.stand_frac_slider = p.addUserDebugParameter("Stand frac", 0, 1, 0.75)
 
-        self.gait_type_slider = p.addUserDebugParameter("Gait Type", 0, len(GaitType) - 1, 0)
-
-        # self.gait_type_slider = p.addUserDebugParameter("Gait Type", 0, len(GaitType) - 1, 0, paramType=p.GUI_ENUM,
-        #                               enumNames=[g.value for g in GaitType])
         self.last_gait_type = GaitType.TROT_GATE
 
     def update_gait_state(self, gait_state: GaitStateT):
-        gait_state["step_x"] = p.readUserDebugParameter(self.step_x_slider)
+        gait_state["step_x"] = p.readUserDebugParameter(self.step_x_slider) * KinConfig.max_step_length
-        gait_state["step_z"] = p.readUserDebugParameter(self.step_z_slider)
+        gait_state["step_z"] = p.readUserDebugParameter(self.step_z_slider) * KinConfig.max_step_length
         gait_state["step_angle"] = p.readUserDebugParameter(self.angle_slider)
-        gait_state["step_height"] = p.readUserDebugParameter(self.step_height_slider)
+        gait_state["step_height"] = p.readUserDebugParameter(self.step_height_slider) * KinConfig.max_step_height
         gait_state["step_depth"] = p.readUserDebugParameter(self.step_depth_slider)
         gait_state["step_velocity"] = p.readUserDebugParameter(self.speed_slider)
         gait_state["stand_frac"] = p.readUserDebugParameter(self.stand_frac_slider)
@@ -61,12 +57,6 @@ class GUI:
         body_state["pz"] = p.readUserDebugParameter(self.pivot_z_slider) * KinConfig.max_body_shift_z
 
     def update(self):
-        gait_type = GaitType(int(p.readUserDebugParameter(self.gait_type_slider)))
-        if gait_type != self.last_gait_type:
-            self.last_gait_type = gait_type
-            p.removeUserDebugItem(self.stand_frac_slider)
-            self.stand_frac_slider = p.addUserDebugParameter("Stand frac", 0, 1, default_stand_frac[gait_type])
-
         quadruped_pos, _ = p.getBasePositionAndOrientation(self.robot)
         p.resetDebugVisualizerCamera(
             cameraDistance=self.c_distance,