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/__init__.py
View File
simulation/src/envs/quadruped_env.py
+36 -17
View File
@@ -2,6 +2,7 @@ import gymnasium as gym
import pybullet as p
import pybullet_data
import numpy as np
import os
from enum import Enum
from src.utils.gui import GUI
@@ -16,16 +17,33 @@ class TerrainType(Enum):
class QuadrupedRobot:
def __init__(self, urdf_path, position=[0, 0, 0.3], orientation=[0, 0, 0], use_fixed_base=False):
print(f"Loading URDF from: {urdf_path}")
print(f"URDF file exists: {os.path.exists(urdf_path)}")
q_orientation = p.getQuaternionFromEuler(orientation)
self.robot_id = p.loadURDF(
urdf_path, position, q_orientation, useFixedBase=use_fixed_base)
try:
self.robot_id = p.loadURDF(urdf_path, position, q_orientation, useFixedBase=use_fixed_base)
print(f"Robot loaded successfully with ID: {self.robot_id}")
if self.robot_id >= 0:
print(f"Number of joints: {p.getNumJoints(self.robot_id)}")
else:
print("ERROR: Robot ID is negative, URDF loading failed!")
except Exception as e:
print(f"Error loading URDF: {e}")
raise
# Indices of the 12 actuated joints in the URDF (PyBullet joint indices)
# Order: FL(shoulder, leg, foot), FR(shoulder, leg, foot), RL(shoulder, leg, foot), RR(shoulder, leg, foot)
self.movable_joint_indices = [2, 3, 5, 7, 8, 10, 12, 13, 15, 17, 18, 20]
def get_movable_joint_names(self):
return [p.getJointInfo(self.robot_id, idx)[1].decode("utf-8") for idx in self.movable_joint_indices]
def get_observation(self):
position, orientation = p.getBasePositionAndOrientation(self.robot_id)
orientation = p.getEulerFromQuaternion(orientation)
velocity, angular_velocity = p.getBaseVelocity(self.robot_id)
joint_states = p.getJointStates(
self.robot_id, range(p.getNumJoints(self.robot_id)))
joint_states = p.getJointStates(self.robot_id, range(p.getNumJoints(self.robot_id)))
joint_positions = [state[0] for state in joint_states]
joint_velocities = [state[1] for state in joint_states]
return np.concatenate(
@@ -40,16 +58,19 @@ class QuadrupedRobot:
)
def apply_action(self, action):
# Apply target positions (radians) to actuated joints in fixed order
for i, position in enumerate(action):
p.setJointMotorControl2(
bodyIndex=self.robot_id,
jointIndex=i,
controlMode=p.POSITION_CONTROL,
targetPosition=position,
force=50, # 343 # / 100 for newtons - Fix mass
positionGain=0.5,
maxVelocity=13.09,
)
if i < len(self.movable_joint_indices):
joint_index = self.movable_joint_indices[i]
p.setJointMotorControl2(
bodyIndex=self.robot_id,
jointIndex=joint_index,
controlMode=p.POSITION_CONTROL,
targetPosition=position,
force=50, # 343 # / 100 for newtons - Fix mass
positionGain=0.5,
maxVelocity=13.09,
)
class QuadrupedEnv(gym.Env):
@@ -60,8 +81,7 @@ class QuadrupedEnv(gym.Env):
else:
p.connect(p.DIRECT)
self.observation_space = gym.spaces.Box(
low=-np.inf, high=np.inf, shape=(48,))
self.observation_space = gym.spaces.Box(low=-np.inf, high=np.inf, shape=(48,))
self.action_space = gym.spaces.Box(low=-1, high=1, shape=(18,))
p.setAdditionalSearchPath(pybullet_data.getDataPath())
@@ -95,8 +115,7 @@ class QuadrupedEnv(gym.Env):
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1)
p.configureDebugVisualizer(p.COV_ENABLE_PLANAR_REFLECTION, 1)
p.configureDebugVisualizer(p.COV_ENABLE_TINY_RENDERER, 0)
self.terrain = p.loadURDF(
"plane_transparent.urdf", useMaximalCoordinates=True)
self.terrain = p.loadURDF("plane_transparent.urdf", useMaximalCoordinates=True)
elif terrain_type == TerrainType.TERRAIN:
terrainShape = p.createCollisionShape(
shapeType=p.GEOM_HEIGHTFIELD, meshScale=[0.1, 0.1, 24], fileName="heightmaps/wm_height_out.png"
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)
simulation/src/utils/gui.py
+35 -47
View File
@@ -1,7 +1,7 @@
import pybullet as p
import numpy as np
from src.robot.kinematics import BodyStateT
from src.robot.kinematics import BodyStateT, KinConfig
from src.robot.gait import GaitStateT, GaitType, default_stand_frac, default_offset
@@ -10,36 +10,28 @@ class GUI:
self.robot = bot
self.c_yaw = 10
self.c_pitch = -17
self.c_distance = 5
self.c_distance = 1
self.x_slider = p.addUserDebugParameter("x", -50, 50, 0)
self.y_slider = p.addUserDebugParameter("y", -50, 50, 0)
self.z_slider = p.addUserDebugParameter("z", -50, 50, 0)
self.yaw_slider = p.addUserDebugParameter(
"yaw", -np.pi / 4, np.pi / 4, 0)
self.pitch_slider = p.addUserDebugParameter(
"pitch", -np.pi / 4, np.pi / 4, 0)
self.roll_slider = p.addUserDebugParameter(
"roll", -np.pi / 4, np.pi / 4, 0)
self.x_slider = p.addUserDebugParameter("x", -1, 1, 0)
self.y_slider = p.addUserDebugParameter("y", 0, 1, 0.5)
self.z_slider = p.addUserDebugParameter("z", -1, 1, 0)
self.yaw_slider = p.addUserDebugParameter("yaw", -1, 1, 0)
self.pitch_slider = p.addUserDebugParameter("pitch", -1, 1, 0)
self.roll_slider = p.addUserDebugParameter("roll", -1, 1, 0)
self.pivot_x_slider = p.addUserDebugParameter("pivot x", -50, 50, 0)
self.pivot_y_slider = p.addUserDebugParameter("pivot y", -50, 50, 0)
self.pivot_z_slider = p.addUserDebugParameter("pivot z", -50, 50, 0)
self.pivot_x_slider = p.addUserDebugParameter("pivot x", -1, 1, 0)
self.pivot_y_slider = p.addUserDebugParameter("pivot y", -1, 1, 0)
self.pivot_z_slider = p.addUserDebugParameter("pivot z", -1, 1, 0)
self.step_x_slider = p.addUserDebugParameter("Step x", -50, 50, 0)
self.step_z_slider = p.addUserDebugParameter("Step z", -50, 50, 0)
self.angle_slider = p.addUserDebugParameter(
"Angle", -np.pi / 4, np.pi / 4, 0)
self.step_height_slider = p.addUserDebugParameter(
"Step height", 0, 50, 15)
self.step_depth_slider = p.addUserDebugParameter(
"Step depth", 0, 0.01, 0.002)
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_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.5)
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)
# self.gait_type_slider = p.addUserDebugParameter("Gait Type", 0, len(GaitType) - 1, 0, paramType=p.GUI_ENUM,
# enumNames=[g.value for g in GaitType])
@@ -49,35 +41,31 @@ class GUI:
gait_state["step_x"] = p.readUserDebugParameter(self.step_x_slider)
gait_state["step_z"] = p.readUserDebugParameter(self.step_z_slider)
gait_state["step_angle"] = p.readUserDebugParameter(self.angle_slider)
gait_state["step_height"] = p.readUserDebugParameter(
self.step_height_slider)
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)
gait_state["step_height"] = p.readUserDebugParameter(self.step_height_slider)
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)
gait_state["offset"] = default_offset[self.last_gait_type]
def update_body_state(self, body_state: BodyStateT):
body_state["xm"] = p.readUserDebugParameter(self.x_slider)
body_state["ym"] = p.readUserDebugParameter(self.y_slider)
body_state["zm"] = p.readUserDebugParameter(self.z_slider)
body_state["omega"] = p.readUserDebugParameter(self.roll_slider)
body_state["phi"] = p.readUserDebugParameter(self.pitch_slider)
body_state["psi"] = p.readUserDebugParameter(self.yaw_slider)
body_state["px"] = p.readUserDebugParameter(self.pivot_x_slider)
body_state["py"] = p.readUserDebugParameter(self.pivot_y_slider)
body_state["pz"] = p.readUserDebugParameter(self.pivot_z_slider)
body_state["xm"] = p.readUserDebugParameter(self.x_slider) * KinConfig.max_body_shift_x
body_state["ym"] = (
p.readUserDebugParameter(self.y_slider) * KinConfig.body_height_range + KinConfig.min_body_height
)
body_state["zm"] = p.readUserDebugParameter(self.z_slider) * KinConfig.max_body_shift_z
body_state["omega"] = p.readUserDebugParameter(self.roll_slider) * KinConfig.max_roll
body_state["phi"] = p.readUserDebugParameter(self.pitch_slider) * KinConfig.max_pitch
body_state["psi"] = p.readUserDebugParameter(self.yaw_slider) * KinConfig.max_pitch
body_state["px"] = p.readUserDebugParameter(self.pivot_x_slider) * KinConfig.max_body_shift_x
body_state["py"] = p.readUserDebugParameter(self.pivot_y_slider) * KinConfig.max_body_shift_x
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)))
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])
self.stand_frac_slider = p.addUserDebugParameter("Stand frac", 0, 1, default_stand_frac[gait_type])
quadruped_pos, _ = p.getBasePositionAndOrientation(self.robot)
p.resetDebugVisualizerCamera(