yair/walkersim
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

fix(mujoco): enable continuous crank rotation for linkage

Browse Source
This commit is contained in:
Ariel R.E.L
2026-02-17 14:00:53 +02:00
parent 2a495592a6
commit f9b0aee8cf
4 changed files with 27 additions and 27 deletions
Download Patch File Download Diff File Expand all files Collapse all files

30
sim/mujoco/model/walker.xml
View File

@@ -11,7 +11,7 @@
<default>
<joint damping="0.9" armature="0.005" frictionloss="0.02"/>
<geom friction="1.3 0.04 0.002" condim="4" solref="0.01 1" solimp="0.92 0.98 0.001"/>
<motor ctrllimited="true" ctrlrange="-28 28"/>
<motor ctrllimited="true" ctrlrange="-140 140"/>
</default>
<asset>
@@ -32,11 +32,11 @@
<!-- Near side (+Y): one crank motor drives two legs via tandem links -->
<body name="near_crank" pos="0 0.08 0">
<joint name="near_crank_joint" type="hinge" axis="0 1 0" range="-3.5 3.5" damping="0.2"/>
<joint name="near_crank_joint" type="hinge" axis="0 1 0" damping="0.2"/>
<geom type="capsule" fromto="0 0 0 -0.0024 0 -0.0457" size="0.007" mass="0.12" rgba="0.10 0.95 0.12 1"/>
<body name="near_leg_f" pos="-0.0024 0 -0.0457">
<joint name="near_leg_f_joint" type="hinge" axis="0 1 0" range="-2.2 2.2" damping="0.6"/>
<joint name="near_leg_f_joint" type="hinge" axis="0 1 0" damping="0.6"/>
<geom type="capsule" fromto="0 0 0 -0.2795 0 -0.3535" size="0.014" mass="0.42" rgba="0.30 0.62 0.92 1"/>
<site name="near_D_site" pos="-0.1460 0 -0.1728" size="0.006" rgba="1 1 0 1"/>
<site name="near_F_site" pos="-0.2795 0 -0.3535" size="0.007" rgba="1 0.8 0.1 1"/>
@@ -44,7 +44,7 @@
</body>
<body name="near_leg_g" pos="-0.0024 0 -0.0457">
<joint name="near_leg_g_joint" type="hinge" axis="0 1 0" range="-2.2 2.2" damping="0.6"/>
<joint name="near_leg_g_joint" type="hinge" axis="0 1 0" damping="0.6"/>
<geom type="capsule" fromto="0 0 0 0.2801 0 -0.3522" size="0.014" mass="0.42" rgba="0.36 0.68 0.95 1"/>
<site name="near_E_site" pos="0.1408 0 -0.1660" size="0.006" rgba="1 1 0 1"/>
<site name="near_G_site" pos="0.2801 0 -0.3522" size="0.007" rgba="1 0.8 0.1 1"/>
@@ -53,22 +53,22 @@
</body>
<body name="near_tendon_f" pos="-0.2715 0.08 -0.0020">
<joint name="near_tendon_f_joint" type="hinge" axis="0 1 0" range="-2.5 2.5" damping="0.8"/>
<joint name="near_tendon_f_joint" type="hinge" axis="0 1 0" damping="0.8"/>
<geom type="capsule" fromto="0 0 0 0.1231 0 -0.2165" size="0.011" mass="0.24" rgba="0.95 0.56 0.25 1"/>
</body>
<body name="near_tendon_g" pos="0.2744 0.08 -0.0040">
<joint name="near_tendon_g_joint" type="hinge" axis="0 1 0" range="-2.5 2.5" damping="0.8"/>
<joint name="near_tendon_g_joint" type="hinge" axis="0 1 0" damping="0.8"/>
<geom type="capsule" fromto="0 0 0 -0.1360 0 -0.2076" size="0.011" mass="0.24" rgba="0.95 0.56 0.25 1"/>
</body>
<!-- Far side (-Y): same linkage, separate crank motor -->
<body name="far_crank" pos="0 -0.08 0">
<joint name="far_crank_joint" type="hinge" axis="0 1 0" range="-3.5 3.5" damping="0.2"/>
<joint name="far_crank_joint" type="hinge" axis="0 1 0" damping="0.2"/>
<geom type="capsule" fromto="0 0 0 -0.0024 0 -0.0457" size="0.007" mass="0.12" rgba="0.10 0.95 0.12 0.65"/>
<body name="far_leg_f" pos="-0.0024 0 -0.0457">
<joint name="far_leg_f_joint" type="hinge" axis="0 1 0" range="-2.2 2.2" damping="0.6"/>
<joint name="far_leg_f_joint" type="hinge" axis="0 1 0" damping="0.6"/>
<geom type="capsule" fromto="0 0 0 -0.2795 0 -0.3535" size="0.014" mass="0.42" rgba="0.30 0.62 0.92 0.72"/>
<site name="far_D_site" pos="-0.1460 0 -0.1728" size="0.006" rgba="1 1 0 0.9"/>
<site name="far_F_site" pos="-0.2795 0 -0.3535" size="0.007" rgba="1 0.8 0.1 0.9"/>
@@ -76,7 +76,7 @@
</body>
<body name="far_leg_g" pos="-0.0024 0 -0.0457">
<joint name="far_leg_g_joint" type="hinge" axis="0 1 0" range="-2.2 2.2" damping="0.6"/>
<joint name="far_leg_g_joint" type="hinge" axis="0 1 0" damping="0.6"/>
<geom type="capsule" fromto="0 0 0 0.2801 0 -0.3522" size="0.014" mass="0.42" rgba="0.36 0.68 0.95 0.72"/>
<site name="far_E_site" pos="0.1408 0 -0.1660" size="0.006" rgba="1 1 0 0.9"/>
<site name="far_G_site" pos="0.2801 0 -0.3522" size="0.007" rgba="1 0.8 0.1 0.9"/>
@@ -85,22 +85,22 @@
</body>
<body name="far_tendon_f" pos="-0.2715 -0.08 -0.0020">
<joint name="far_tendon_f_joint" type="hinge" axis="0 1 0" range="-2.5 2.5" damping="0.8"/>
<joint name="far_tendon_f_joint" type="hinge" axis="0 1 0" damping="0.8"/>
<geom type="capsule" fromto="0 0 0 0.1231 0 -0.2165" size="0.011" mass="0.24" rgba="0.95 0.56 0.25 0.72"/>
</body>
<body name="far_tendon_g" pos="0.2744 -0.08 -0.0040">
<joint name="far_tendon_g_joint" type="hinge" axis="0 1 0" range="-2.5 2.5" damping="0.8"/>
<joint name="far_tendon_g_joint" type="hinge" axis="0 1 0" damping="0.8"/>
<geom type="capsule" fromto="0 0 0 -0.1360 0 -0.2076" size="0.011" mass="0.24" rgba="0.95 0.56 0.25 0.72"/>
</body>
</body>
</worldbody>
<equality>
<connect body1="near_leg_f" body2="near_tendon_f" anchor="-0.1484 0.08 0.2015"/>
<connect body1="near_leg_g" body2="near_tendon_g" anchor="0.1384 0.08 0.2084"/>
<connect body1="far_leg_f" body2="far_tendon_f" anchor="-0.1484 -0.08 0.2015"/>
<connect body1="far_leg_g" body2="far_tendon_g" anchor="0.1384 -0.08 0.2084"/>
<connect body1="near_leg_f" body2="near_tendon_f" anchor="-0.1484 0.08 -0.2185"/>
<connect body1="near_leg_g" body2="near_tendon_g" anchor="0.1384 0.08 -0.2116"/>
<connect body1="far_leg_f" body2="far_tendon_f" anchor="-0.1484 -0.08 -0.2185"/>
<connect body1="far_leg_g" body2="far_tendon_g" anchor="0.1384 -0.08 -0.2116"/>
</equality>
<actuator>

19
sim/mujoco/walker_sim/controller.py
View File

@@ -1,25 +1,20 @@
from __future__ import annotations
from dataclasses import dataclass
from math import pi, sin
from math import pi
import numpy as np
@dataclass
class PDGaitConfig:
frequency_hz: float = 1.4
frequency_hz: float = 0.9
kp: float = 24.0
kd: float = 2.5
near_bias: float = 0.0
near_amp: float = 1.0
near_phase_offset: float = 0.0
far_phase_offset: float = pi
def angle_wrap(rad: float) -> float:
return (rad + pi) % (2.0 * pi) - pi
class PDGaitController:
"""Phase-based crank PD controller for the tandem 4-leg linkage."""
@@ -45,13 +40,13 @@ class PDGaitController:
def desired_positions(self, t: float) -> np.ndarray:
w = 2.0 * pi * self.cfg.frequency_hz
phase_n = w * t
phase_n = w * t + self.cfg.near_phase_offset
phase_f = phase_n + self.cfg.far_phase_offset
q_des = np.array(
[
self.cfg.near_bias + self.cfg.near_amp * sin(phase_n),
self.cfg.near_bias + self.cfg.near_amp * sin(phase_f),
phase_n,
phase_f,
],
dtype=np.float64,
)
@@ -62,6 +57,6 @@ class PDGaitController:
for i, (aid, qpos_adr, qvel_adr, ctrl_min, ctrl_max) in enumerate(self._joint_info):
q = self.data.qpos[qpos_adr]
qd = self.data.qvel[qvel_adr]
err = angle_wrap(float(q_des[i] - q))
err = float(q_des[i] - q)
tau = self.cfg.kp * err - self.cfg.kd * qd
self.data.ctrl[aid] = np.clip(tau, ctrl_min, ctrl_max)

1
sim/mujoco/walker_sim/run.py
View File

@@ -22,6 +22,7 @@ def run_headless(model, data, seconds: float, controller: PDGaitController, samp
s = snapshot(data)
print(
f"t={s.t:6.3f} "
f"cranks=({s.near_crank_rad:+.2f},{s.far_crank_rad:+.2f}) "
f"torso=({s.torso_x:+.3f},{s.torso_z:+.3f}) "
f"nearF=({s.near_f_x:+.3f},{s.near_f_z:+.3f}) "
f"nearG=({s.near_g_x:+.3f},{s.near_g_z:+.3f}) "

4
sim/mujoco/walker_sim/sim.py
View File

@@ -14,6 +14,8 @@ DEFAULT_MODEL_PATH = ROOT / "model" / "walker.xml"
@dataclass
class StepLog:
t: float
near_crank_rad: float
far_crank_rad: float
torso_x: float
torso_z: float
near_f_x: float
@@ -59,6 +61,8 @@ def snapshot(data: mujoco.MjData) -> StepLog:
far_g = site_xyz(data, "far_G_site")
return StepLog(
t=float(data.time),
near_crank_rad=float(data.joint("near_crank_joint").qpos[0]),
far_crank_rad=float(data.joint("far_crank_joint").qpos[0]),
torso_x=float(torso[0]),
torso_z=float(torso[2]),
near_f_x=float(near_f[0]),