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

🐛 Fixes many smaller simulation pains

Browse Source
This commit is contained in:
Rune Harlyk
2025-10-10 18:19:14 +02:00
committed by Rune Harlyk
parent a198de05c2
commit 51ee910fb6
8 changed files with 222 additions and 138 deletions
Download Patch File Download Diff File Expand all files Collapse all files
simulation/src/robot/gait.py
+12 -16
View File
@@ -3,7 +3,10 @@ import numpy as np
from typing import TypedDict
from enum import Enum
from src.robot.kinematics import BodyStateT
try:
from src.robot.kinematics import BodyStateT
except ImportError:
from robot.kinematics import BodyStateT
class GaitType(Enum):
@@ -34,15 +37,12 @@ class GaitStateT(TypedDict):
gait_type: GaitType
length_multipliers = np.array(
[-1.4, -1.0, -1.5, -1.5, -1.5, 0.0, 0.0, 0.0, 1.5, 1.5, 1.4, 1.0])
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])
length_multipliers = np.array([-1.4, -1.0, -1.5, -1.5, -1.5, 0.0, 0.0, 0.0, 1.5, 1.5, 1.4, 1.0])
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)
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])
@@ -67,8 +67,7 @@ def get_control_points(length, angle, height):
def bezier_curve(length, angle, height, phase):
ctrl = get_control_points(length, angle, height)
n = len(ctrl) - 1
coeffs = np.array([math.comb(n, i) * (phase**i) *
((1 - phase) ** (n - i)) for i in range(n + 1)])
coeffs = np.array([math.comb(n, i) * (phase**i) * ((1 - phase) ** (n - i)) for i in range(n + 1)])
return np.sum(ctrl * coeffs[:, None], axis=0)
@@ -80,8 +79,7 @@ class GaitController:
def step(self, gait: GaitStateT, body: BodyStateT, dt: float):
step_x, step_z, angle = gait["step_x"], gait["step_z"], gait["step_angle"]
if not any((step_x, step_z, angle)):
body["feet"] = body["feet"] + \
(self.default_position - body["feet"]) * dt * 10
body["feet"] = body["feet"] + (self.default_position - body["feet"]) * dt * 10
self.phase = 0.0
return
@@ -101,12 +99,10 @@ class GaitController:
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)
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 + delta_rot
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
body["feet"] = new_feet
simulation/src/robot/kinematics.py
+78 -43
View File
@@ -1,7 +1,41 @@
import numpy as np
from typing import TypedDict, List
import config
class KinConfig:
# SPOTMICRO_ESP32 configuration (matching C++ version)
coxa = 60.5 / 100.0
coxa_offset = 10.0 / 100.0
femur = 111.2 / 100.0
tibia = 118.5 / 100.0
L = 207.5 / 100.0
W = 78.0 / 100.0
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],
]
# Max constants
max_roll = 15 * np.pi / 2
max_pitch = 15 * np.pi / 2
max_body_shift_x = W / 3
max_body_shift_z = W / 3
max_leg_reach = femur + tibia - coxa_offset
min_body_height = max_leg_reach * 0.45
max_body_height = max_leg_reach * 0.9
body_height_range = max_body_height - min_body_height
max_step_length = max_leg_reach * 0.8
max_step_height = max_leg_reach / 2
# Default constants
default_step_depth = 0.002
default_body_height = min_body_height + body_height_range / 2
default_step_height = default_body_height / 2
class BodyStateT(TypedDict):
@@ -56,68 +90,69 @@ def get_transformation_matrix(body_state):
class Kinematics:
def __init__(self, l1, l2, l3, l4, length, width):
self.l1 = float(l1)
self.l2 = float(l2)
self.l3 = float(l3)
self.l4 = float(l4)
self.length = float(length)
self.width = float(width)
self.deg2rad = np.pi / 180
def __init__(self):
# Use KinConfig constants (matching C++ version)
self.coxa = KinConfig.coxa
self.coxa_offset = KinConfig.coxa_offset
self.femur = KinConfig.femur
self.tibia = KinConfig.tibia
self.L = KinConfig.L
self.W = KinConfig.W
self.mount_offsets = np.array([
[self.length / 2, 0, self.width / 2],
[self.length / 2, 0, -self.width / 2],
[-self.length / 2, 0, self.width / 2],
[-self.length / 2, 0, -self.width / 2]
])
self.mount_offsets = np.array(KinConfig.mountOffsets)
self.inv_mount_rot = np.array([
[0, 0, -1],
[0, 1, 0],
[1, 0, 0]
])
self.inv_mount_rot = np.array([[0, 0, -1], [0, 1, 0], [1, 0, 0]])
# Cache for current state (matching C++ optimization)
self.current_state = None
def get_default_feet_pos(self):
feet = self.mount_offsets.copy()
feet[:, 1] = -1
feet[:, 2] += np.array([self.l1, -self.l1, self.l1, -self.l1])
return feet
# Return default feet positions matching C++ version exactly
feet = []
for i, offset in enumerate(self.mount_offsets):
y_pos = 0 # Ground level (matching C++ default_feet_positions)
z_offset = -self.coxa if i % 2 == 1 else self.coxa
feet.append([offset[0], y_pos, offset[2] + z_offset])
return np.array(feet)
def inverse_kinematics(self, body_state):
roll, pitch, yaw = np.deg2rad(body_state["omega"]), np.deg2rad(
body_state["phi"]), np.deg2rad(body_state["psi"])
# Check if state has changed (optimization from C++ version)
# if self.current_state == body_state:
# return self.current_angles if hasattr(self, "current_angles") else []
self.current_state = body_state.copy()
roll, pitch, yaw = np.deg2rad(body_state["omega"]), np.deg2rad(body_state["phi"]), np.deg2rad(body_state["psi"])
xm, ym, zm = body_state["xm"], body_state["ym"], body_state["zm"]
rot = self._rotation_matrix(roll, pitch, yaw)
inv_rot = rot.T
inv_tr = - \
inv_rot @ np.array([xm, ym, zm])
inv_tr = -inv_rot @ np.array([xm, ym, zm])
angles = []
for idx, foot_world in enumerate(body_state["feet"]):
foot_body = inv_rot @ foot_world + inv_tr
foot_local = self.inv_mount_rot @ (foot_body -
self.mount_offsets[idx])
foot_local = self.inv_mount_rot @ (foot_body - self.mount_offsets[idx])
x_local = -foot_local[0] if idx % 2 else foot_local[0]
angles.extend(self._leg_ik(x_local, foot_local[1], foot_local[2]))
leg_angles = self._leg_ik(x_local, foot_local[1], foot_local[2])
angles.extend(leg_angles)
self.current_angles = angles
return angles
def _leg_ik(self, x, y, z):
f = np.sqrt(max(0.0, x*x + y*y - self.l1*self.l1))
g = f - self.l2
h = np.sqrt(g*g + z*z)
# Match C++ implementation exactly
F = np.sqrt(max(0.0, x * x + y * y - self.coxa * self.coxa))
G = F - self.coxa_offset
H = np.sqrt(G * G + z * z)
t1 = -np.arctan2(y, x) - np.arctan2(f, -self.l1)
theta1 = -np.arctan2(y, x) - np.arctan2(F, -self.coxa)
D = (H * H - self.femur * self.femur - self.tibia * self.tibia) / (2 * self.femur * self.tibia)
theta3 = np.arccos(max(-1.0, min(1.0, D)))
theta2 = np.arctan2(z, G) - np.arctan2(self.tibia * np.sin(theta3), self.femur + self.tibia * np.cos(theta3))
d = (h*h - self.l3*self.l3 - self.l4*self.l4) / (2*self.l3*self.l4)
d = max(-1.0, min(1.0, d))
t3 = np.arccos(d)
t2 = np.arctan2(z, g) - np.arctan2(self.l4*np.sin(t3),
self.l3 + self.l4*np.cos(t3))
return t1, t2, t3
# Return angles in radians (matching web app)
return theta1, theta2, theta3
def _rotation_matrix(self, roll, pitch, yaw):
cr, sr = np.cos(roll), np.sin(roll)