Darkages/SpotMicroESP32-Leika
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

🎨 Fixes gait in sim

Browse Source
This commit is contained in:
Rune Harlyk
2025-10-10 18:54:42 +02:00
committed by Rune Harlyk
parent 89a0316fb4
commit 46bb5f74b1
4 changed files with 43 additions and 39 deletions
Download Patch File Download Diff File Expand all files Collapse all files
simulation/src/robot/gait.py
+29 -17
View File
@@ -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
simulation/src/robot/kinematics.py
+8 -6
View File
@@ -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
simulation/src/utils/gui.py
+5 -15
View File
@@ -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,