From f9b0aee8cf4647b8fce2ed5f0299361bfbc48e9c Mon Sep 17 00:00:00 2001 From: ro Date: Tue, 17 Feb 2026 14:00:53 +0200 Subject: [PATCH] fix(mujoco): enable continuous crank rotation for linkage --- sim/mujoco/model/walker.xml | 30 ++++++++++++++--------------- sim/mujoco/walker_sim/controller.py | 19 +++++++----------- sim/mujoco/walker_sim/run.py | 1 + sim/mujoco/walker_sim/sim.py | 4 ++++ 4 files changed, 27 insertions(+), 27 deletions(-) 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]),