diff --git a/sim/mujoco/model/walker.xml b/sim/mujoco/model/walker.xml
index 842e2f9..db77421 100644
--- a/sim/mujoco/model/walker.xml
+++ b/sim/mujoco/model/walker.xml
@@ -11,7 +11,7 @@
-
+
@@ -32,11 +32,11 @@
-
+
-
+
@@ -44,7 +44,7 @@
-
+
@@ -53,22 +53,22 @@
-
+
-
+
-
+
-
+
@@ -76,7 +76,7 @@
-
+
@@ -85,22 +85,22 @@
-
+
-
+
-
-
-
-
+
+
+
+
diff --git a/sim/mujoco/walker_sim/controller.py b/sim/mujoco/walker_sim/controller.py
index 4fe2f04..d7ea1d9 100644
--- a/sim/mujoco/walker_sim/controller.py
+++ b/sim/mujoco/walker_sim/controller.py
@@ -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)
diff --git a/sim/mujoco/walker_sim/run.py b/sim/mujoco/walker_sim/run.py
index 25afba2..511ef20 100644
--- a/sim/mujoco/walker_sim/run.py
+++ b/sim/mujoco/walker_sim/run.py
@@ -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}) "
diff --git a/sim/mujoco/walker_sim/sim.py b/sim/mujoco/walker_sim/sim.py
index 165c118..0bcfafb 100644
--- a/sim/mujoco/walker_sim/sim.py
+++ b/sim/mujoco/walker_sim/sim.py
@@ -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]),