From 46bb5f74b1928117964080bab0b1057f08be6cfe Mon Sep 17 00:00:00 2001 From: Rune Harlyk Date: Fri, 10 Oct 2025 18:54:42 +0200 Subject: [PATCH] =?UTF-8?q?=F0=9F=8E=A8=20Fixes=20gait=20in=20sim?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- simulation/play.py | 2 +- simulation/src/robot/gait.py | 46 +++++++++++++++++++----------- simulation/src/robot/kinematics.py | 14 +++++---- simulation/src/utils/gui.py | 20 ++++--------- 4 files changed, 43 insertions(+), 39 deletions(-) diff --git a/simulation/play.py b/simulation/play.py index a181833..a650257 100644 --- a/simulation/play.py +++ b/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, diff --git a/simulation/src/robot/gait.py b/simulation/src/robot/gait.py index 0e5d6b0..f9c23c4 100644 --- a/simulation/src/robot/gait.py +++ b/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): - x, z = length * (1 - 2 * phase) * np.cos(angle), length * (1 - 2 * phase) * np.sin(angle) - y = height * np.cos(np.pi * (x + z) / (2 * length)) if length else 0 - return np.array([x, z, y]) +def sine_curve(length, angle, depth, phase): + x_polar = np.cos(angle) + z_polar = np.sin(angle) + + 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): - return ( - np.pi / 2 - + np.arctan2(feet[1], feet[0]) - + np.arctan2(np.linalg.norm(current[:2] - feet[:2]), np.linalg.norm(feet[:2])) - ) +def yaw_arc(default_foot, current_foot): + foot_mag = np.sqrt(default_foot[0] ** 2 + default_foot[2] ** 2) + foot_dir = np.arctan2(default_foot[2], default_foot[0]) + offset_x = current_foot[0] - default_foot[0] + 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) - new_feet[i][:2] = default_foot[:2] + delta_pos[:2] + delta_rot[:2] - # new_feet[i][3] = 1 + delta_rot = curve_fn(angle * 2, yaw_arc(default_foot, current_foot), amp, ph_norm) + + 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 diff --git a/simulation/src/robot/kinematics.py b/simulation/src/robot/kinematics.py index 3e7a18c..773ac24 100644 --- a/simulation/src/robot/kinematics.py +++ b/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 = [ - [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], - ] + 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 diff --git a/simulation/src/utils/gui.py b/simulation/src/utils/gui.py index b3ab811..f99bcc1 100644 --- a/simulation/src/utils/gui.py +++ b/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_z"] = p.readUserDebugParameter(self.step_z_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) * 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,