diff --git a/.gitignore b/.gitignore index 0ecf76d..7386f0a 100644 --- a/.gitignore +++ b/.gitignore @@ -6,6 +6,11 @@ __pycache__/ *.py[cod] .venv/ venv/ +*.egg-info/ +.pytest_cache/ +.ruff_cache/ +.mypy_cache/ +.uv-cache/ # Build outputs build/ @@ -20,10 +25,16 @@ log/ *.bin *.axf +# Required vendored Odin runtime libraries in the early Sim2Real release +!05_software/real/sim2real/vendored/odin1_imu/build/ +!05_software/real/sim2real/vendored/odin1_imu/build/libodin1_imu_bridge.so +!05_software/real/sim2real/vendored/odin1_imu/lib/*.a + # Training outputs logs/ checkpoints/ wandb/ +sim2sim_log_*.txt # IDE and operating system files .idea/ diff --git a/01_doc/architecture/early_software_stack.md b/01_doc/architecture/early_software_stack.md new file mode 100644 index 0000000..c4eb485 --- /dev/null +++ b/01_doc/architecture/early_software_stack.md @@ -0,0 +1,22 @@ +# 第一代软件闭环 + +第一代 16DOF 软件的目标是先打通“训练、仿真验证、真机执行”闭环，而不是在初期建立复杂的分布式系统。 + +## 训练与策略 + +`05_software/train/rc_mjlab` 使用本地修改的 mjlab 和 MuJoCo 模型训练轮腿混合策略。训练任务包括 Flat、Rough 和 Crawl，腿部 12 个关节输出位置目标，4 个轮子输出速度目标。 + +## MuJoCo 与 Sim2Sim + +- `mujoco_sim` 用于不加载 RL 策略时的模型、动力学和控制调试。 +- `sim2sim` 加载训练策略，在独立 MuJoCo 环境中验证观测、动作、地形和导航行为。 +- 两者与训练任务共同使用 `rc_mjlab/mjcf`，避免早期模型定义不一致。 + +## 真机控制 + +- `ik_real` 先以逆运动学和轨迹插值验证电机控制链路。 +- `sim2real` 再将训练策略部署到 Python 真机运行时，加入 IMU、站立、安全和 Web 调试。 + +## 阶段特点 + +这一版本的优势是链路完整、模块直观，便于快速验证；局限是训练、仿真和部署仍存在重复资源，Python 真机运行时的实时性和系统集成能力有限。这些问题推动了后续统一训练版本和 ROS 2/C++ 部署架构。 diff --git a/05_software/README.md b/05_software/README.md index e4f8f3f..98c93e4 100644 --- a/05_software/README.md +++ b/05_software/README.md @@ -1,13 +1,37 @@ # 软件 -本目录用于整理 16DOF 轮足机器人的上层软件。 +本目录当前保存 16DOF 轮足机器人的第一代完整软件闭环。 ```text 05_software/ -├─ train/ # 强化学习训练任务和配置 -├─ sim/ # Sim2Sim 和独立仿真 -├─ real/ # Sim2Real 与 ROS 2 真机部署 -└─ tools/ # 导航、地图、诊断和转换工具 +├─ train/ +│ └─ rc_mjlab/ # 训练、MJCF、MuJoCo、Sim2Sim 和本地 mjlab 依赖 +└─ real/ + ├─ ik_real/ # IK 轨迹与早期真机控制 + └─ sim2real/ # 第一代 Python 策略真机部署 ``` -历史版本通过 Git Tag 保存，同一模块不使用 `old`、`final`、`v2` 等目录复制完整代码。 +## 数据流 + +```text +MJCF + mjlab task + | + v + PPO 训练策略 + | + +----> MuJoCo 独立模型调试 + | + +----> Sim2Sim 策略验证 + | + +----> Python Sim2Real ----> 电机 / IMU + +IK real --------------------------------> 电机 +``` + +`rc_mjlab` 在早期版本中是自包含工程。训练、MJCF、独立 MuJoCo、Sim2Sim 和策略权重通过相对路径绑定，因此本次保留其原始内部布局，没有为了目录外观拆散。 + +详细说明见： + +- [`train/README.md`](train/README.md) +- [`real/README.md`](real/README.md) +- [`../01_doc/architecture/early_software_stack.md`](../01_doc/architecture/early_software_stack.md) diff --git a/05_software/real/README.md b/05_software/real/README.md new file mode 100644 index 0000000..321c4fb --- /dev/null +++ b/05_software/real/README.md @@ -0,0 +1,21 @@ +# 第一代真机控制 + +本目录保存 16DOF 轮足机器人的早期真机控制实现。 + +## `ik_real` + +基于几何逆运动学和轨迹插值的真机控制探索，不依赖强化学习策略。主要用于验证电机接口、关节映射和姿态轨迹。 + +## `sim2real` + +第一代 Python 策略部署栈，包含： + +- 53D 观测到 16D 动作的策略运行时 +- 电机映射和真机 IO +- IMU 接入 +- 站立初始化与平衡 +- 运行时安全检查和阻尼刹车 +- Web 调试界面 +- 对齐、标定和独立检查工具 + +部署说明见 [`sim2real/README.md`](sim2real/README.md) 与 [`sim2real/DEPLOYMENT.md`](sim2real/DEPLOYMENT.md)。 diff --git a/05_software/real/ik_real/README.md b/05_software/real/ik_real/README.md new file mode 100644 index 0000000..b2a6b9b --- /dev/null +++ b/05_software/real/ik_real/README.md @@ -0,0 +1,9 @@ +# IK 真机控制探索 + +该目录保存强化学习部署前的逆运动学真机控制代码。 + +- `sim2real_control_api.py`：真机控制接口 +- `trajectory_interpolator.py`：关节/姿态轨迹插值 +- `sim_to_real_deploy_beifen.py`：早期部署脚本备份 + +文件名中的 `beifen` 来自原始资料。为保持早期版本可追溯性，本次归档不修改源码和文件名。 diff --git a/05_software/real/ik_real/sim2real_control_api.py b/05_software/real/ik_real/sim2real_control_api.py new file mode 100644 index 0000000..d6061c3 --- /dev/null +++ b/05_software/real/ik_real/sim2real_control_api.py @@ -0,0 +1,274 @@ +"""Sim-to-real control/IK API extracted from mature mujoco_sim controller. + +This module provides a deployment-friendly wrapper around: +- wheel mode posture control +- trot swing-leg IK + wheel assist +- differential wheel speed mapping + +No MuJoCo runtime is required for using the API itself. +For trot IK, Pinocchio model is used via Dynamics. +""" + +from dataclasses import dataclass +from typing import Dict, Optional, Tuple +import numpy as np + +from config import ( + LEG_NAMES, + LEG_JOINTS, + WHEEL_JOINT, + DEFAULT_JOINT_ANGLES, + WHEEL_RADIUS, + WHEEL_TRACK, + WHEEL_VEL_MAX, + KP_ROLL, + KP_PITCH, + GAIT_FREQ, + GAIT_DUTY, + SWING_HEIGHT, + PHASE_OFFSETS, +) +from dynamics import Dynamics + + +@dataclass +class DeployState: + """Minimal state for deployment control.""" + + rpy: np.ndarray # (3,) roll, pitch, yaw + + +class Sim2RealControlAPI: + """Deployment-friendly control and IK API. + + Supported modes: + - wheel: wheel differential drive + leg posture hold + - trot: swing foot IK + stance posture + wheel assist + """ + + def __init__(self): + self.mode = "wheel" + self.prone = False + + self.vel_x = 0.0 + self.vel_y = 0.0 + self.yaw_rate = 0.0 + self.height = 0.33 + + self._gait_phase = 0.0 + self._smooth_vx = 0.0 + self._smooth_vy = 0.0 + self._smooth_yaw = 0.0 + + self._default_q = np.array([ + DEFAULT_JOINT_ANGLES["hip_abduction"], + DEFAULT_JOINT_ANGLES["hip_pitch"], + DEFAULT_JOINT_ANGLES["knee"], + ]) + + self.dynamics = Dynamics() + self._swing_start_foot = {leg: np.zeros(3) for leg in LEG_NAMES} + self._last_contact = {leg: True for leg in LEG_NAMES} + + def set_mode(self, mode: str): + if mode not in ("wheel", "trot"): + raise ValueError("mode must be one of: wheel, trot") + self.mode = mode + + def set_command(self, vel_x: float, vel_y: float, yaw_rate: float, height: Optional[float] = None): + self.vel_x = float(vel_x) + self.vel_y = float(vel_y) + self.yaw_rate = float(yaw_rate) + if height is not None: + self.height = float(height) + + def compute( + self, + state: DeployState, + dt: float, + q_pin: Optional[np.ndarray] = None, + dq_pin: Optional[np.ndarray] = None, + ) -> Tuple[np.ndarray, np.ndarray]: + """Compute leg and wheel commands. + + Returns: + leg_targets: (12,) [fl(3), fr(3), rl(3), rr(3)] + wheel_targets: (4,) [fl, fr, rl, rr] in rad/s + + Notes: + - wheel mode does not require q_pin/dq_pin + - trot mode requires q_pin/dq_pin for IK/FK through Pinocchio + """ + alpha = min(float(dt) * 3.0, 1.0) + self._smooth_vx += alpha * (self.vel_x - self._smooth_vx) + self._smooth_vy += alpha * (self.vel_y - self._smooth_vy) + self._smooth_yaw += alpha * (self.yaw_rate - self._smooth_yaw) + + if self.prone: + return self._prone_mode() + + if self.mode == "wheel": + return self._wheel_mode(state) + + if q_pin is None or dq_pin is None: + raise ValueError("trot mode requires q_pin and dq_pin") + return self._trot_mode(state, float(dt), q_pin, dq_pin) + + def to_joint_dict(self, leg_targets: np.ndarray, wheel_targets: np.ndarray) -> Dict[str, float]: + """Convert array commands to named joint-command dictionary.""" + out: Dict[str, float] = {} + for i, leg in enumerate(LEG_NAMES): + out[f"{leg}_{LEG_JOINTS[0]}"] = float(leg_targets[i * 3 + 0]) + out[f"{leg}_{LEG_JOINTS[1]}"] = float(leg_targets[i * 3 + 1]) + out[f"{leg}_{LEG_JOINTS[2]}"] = float(leg_targets[i * 3 + 2]) + out[f"{leg}_{WHEEL_JOINT}"] = float(wheel_targets[i]) + return out + + def _prone_mode(self): + leg_targets = np.zeros(12) + for i, leg in enumerate(LEG_NAMES): + side = 1.0 if leg[1] == "l" else -1.0 + leg_targets[i * 3 + 0] = side * 0.3 + leg_targets[i * 3 + 1] = 1.5 + leg_targets[i * 3 + 2] = -2.65 + return leg_targets, np.zeros(4) + + def _wheel_mode(self, state: DeployState): + wheel_targets = self._differential_drive(self._smooth_vx, self._smooth_yaw) + leg_targets = self._posture_control(state) + return leg_targets, wheel_targets + + def _posture_control(self, state: DeployState) -> np.ndarray: + leg_targets = np.zeros(12) + + _H = [0.157, 0.248, 0.311, 0.366, 0.411, 0.448] + _HIP = [1.5, 1.2, 1.0, 0.8, 0.6, 0.4] + _KNEE = [-2.5, -2.1, -1.8, -1.5, -1.2, -0.9] + + h_clamp = np.clip(self.height, _H[0], _H[-1]) + q_hip_base = float(np.interp(h_clamp, _H, _HIP)) + q_knee_base = float(np.interp(h_clamp, _H, _KNEE)) + + roll_corr = -KP_ROLL * float(state.rpy[0]) + pitch_corr = -KP_PITCH * float(state.rpy[1]) + lateral_lean = 0.3 * self.vel_y + + for i, leg in enumerate(LEG_NAMES): + side = 1.0 if leg[1] == "l" else -1.0 + leg_targets[i * 3 + 0] = np.clip(side * roll_corr + lateral_lean, -0.5, 0.5) + leg_targets[i * 3 + 1] = np.clip(q_hip_base + pitch_corr, -1.0, 2.5) + leg_targets[i * 3 + 2] = np.clip(q_knee_base, -2.6, -0.3) + + return leg_targets + + def _trot_mode(self, state: DeployState, dt: float, q_pin: np.ndarray, dq_pin: np.ndarray): + self._gait_phase = (self._gait_phase + dt * GAIT_FREQ) % 1.0 + + contacts: Dict[str, bool] = {} + for leg in LEG_NAMES: + phase = (self._gait_phase + PHASE_OFFSETS[leg]) % 1.0 + contacts[leg] = bool(phase < GAIT_DUTY) + + self.dynamics.update(q_pin, dq_pin) + + leg_targets = np.zeros(12) + wheel_targets = np.zeros(4) + + for i, leg in enumerate(LEG_NAMES): + if contacts[leg]: + leg_targets[i * 3:(i + 1) * 3] = self._stance_leg_target(state, leg) + self._swing_start_foot[leg] = self.dynamics.get_foot_pos(leg) + self._last_contact[leg] = True + wheel_targets[i] = self._differential_drive_single(self._smooth_vx, self._smooth_yaw, leg) + else: + swing_phase = self._get_swing_phase(leg) + target_foot = self._compute_swing_target(leg, state, swing_phase) + q_ik = self.dynamics.inverse_kinematics(leg, target_foot, q_pin) + leg_targets[i * 3:(i + 1) * 3] = q_ik + self._last_contact[leg] = False + wheel_targets[i] = 0.0 + + return leg_targets, wheel_targets + + def _stance_leg_target(self, state: DeployState, leg: str) -> np.ndarray: + _H = [0.157, 0.248, 0.311, 0.366, 0.411, 0.448] + _HIP = [1.5, 1.2, 1.0, 0.8, 0.6, 0.4] + _KNEE = [-2.5, -2.1, -1.8, -1.5, -1.2, -0.9] + + h_clamp = np.clip(self.height, _H[0], _H[-1]) + q_hip = float(np.interp(h_clamp, _H, _HIP)) + q_knee = float(np.interp(h_clamp, _H, _KNEE)) + + roll_corr = -KP_ROLL * float(state.rpy[0]) + pitch_corr = -KP_PITCH * float(state.rpy[1]) + side = 1.0 if leg[1] == "l" else -1.0 + lateral_lean = 0.3 * self.vel_y + + return np.array([ + np.clip(side * roll_corr + lateral_lean, -0.5, 0.5), + np.clip(q_hip + pitch_corr, -1.0, 2.5), + np.clip(q_knee, -2.6, -0.3), + ]) + + def _differential_drive(self, vel_x: float, yaw_rate: float) -> np.ndarray: + vel_left = (vel_x - 0.5 * WHEEL_TRACK * yaw_rate) / WHEEL_RADIUS + vel_right = (vel_x + 0.5 * WHEEL_TRACK * yaw_rate) / WHEEL_RADIUS + targets = np.zeros(4) + for i, leg in enumerate(LEG_NAMES): + targets[i] = vel_left if leg[1] == "l" else vel_right + return np.clip(targets, -WHEEL_VEL_MAX, WHEEL_VEL_MAX) + + def _differential_drive_single(self, vel_x: float, yaw_rate: float, leg: str) -> float: + if leg[1] == "l": + v = (vel_x - 0.5 * WHEEL_TRACK * yaw_rate) / WHEEL_RADIUS + else: + v = (vel_x + 0.5 * WHEEL_TRACK * yaw_rate) / WHEEL_RADIUS + return float(np.clip(v, -WHEEL_VEL_MAX, WHEEL_VEL_MAX)) + + def _get_swing_phase(self, leg: str) -> float: + phase = (self._gait_phase + PHASE_OFFSETS[leg]) % 1.0 + if phase < GAIT_DUTY: + return 0.0 + return (phase - GAIT_DUTY) / (1.0 - GAIT_DUTY) + + def _compute_swing_target(self, leg: str, state: DeployState, swing_phase: float) -> np.ndarray: + p_start = self._swing_start_foot[leg] + p_end = self._compute_touchdown(leg, state) + + s = swing_phase + s_mj = 10 * s**3 - 15 * s**4 + 6 * s**5 + + pos = p_start + (p_end - p_start) * s_mj + + z_lift = 64.0 * s**3 * (1.0 - s)**3 + pos[2] = p_start[2] + SWING_HEIGHT * z_lift + + return pos + + def _compute_touchdown(self, leg: str, state: DeployState) -> np.ndarray: + td = self._swing_start_foot[leg].copy() + + t_stance = (1.0 / GAIT_FREQ) * GAIT_DUTY + yaw = float(state.rpy[2]) + c, s = np.cos(yaw), np.sin(yaw) + R_z = np.array([[c, -s, 0], [s, c, 0], [0, 0, 1]]) + cmd_vel_world = R_z @ np.array([self._smooth_vx, self._smooth_vy, 0.0]) + + td[0] += cmd_vel_world[0] * t_stance * 0.5 + td[1] += cmd_vel_world[1] * t_stance * 0.5 + td[2] = WHEEL_RADIUS + return td + + +if __name__ == "__main__": + api = Sim2RealControlAPI() + api.set_mode("wheel") + api.set_command(vel_x=0.3, vel_y=0.0, yaw_rate=0.0, height=0.33) + + state = DeployState(rpy=np.array([0.0, 0.0, 0.0])) + leg, wheel = api.compute(state=state, dt=0.004) + cmd = api.to_joint_dict(leg, wheel) + + print("Example wheel-mode command:") + for k, v in sorted(cmd.items()): + print(f"{k}: {v:.6f}") diff --git a/05_software/real/ik_real/sim_to_real_deploy_beifen.py b/05_software/real/ik_real/sim_to_real_deploy_beifen.py new file mode 100644 index 0000000..86ed1cd --- /dev/null +++ b/05_software/real/ik_real/sim_to_real_deploy_beifen.py @@ -0,0 +1,234 @@ +#!/usr/bin/env python3 +import argparse, sys, threading, time +from dataclasses import dataclass +from pathlib import Path +import numpy as np + +sys.path.append('/home/rc2/work/rcwork/control') +from drivers.motor_driver import RobStrideDriver + +ROOT = Path('/home/rc2/work/rcwork/wheelleg_deploy_swj/wheelleg_deploy/wheelleg_mjlab/beifen') +sys.path += [str(ROOT), str(ROOT / 'mujoco_sim')] +from sim2real_control_api import DeployState, Sim2RealControlAPI # type: ignore + +sys.path.append('/home/rc2/work/rcwork') +from trajectory_interpolator import TrajectoryInterpolator + +LEGS = ('fl','fr','rl','rr') +LJ = ('hip_abduction_joint','hip_pitch_joint','knee_joint') +WJ = 'wheel_joint' +JNS = [f'{l}_{j}' for l in LEGS for j in (*LJ, WJ)] + +@dataclass +class Cfg: + mid:int; model:str; sign:float; off:float; bus:str + +class Deploy: + def __init__(self, can1, can2, hz=100.0, use_interpolation=True, interp_method='quintic', interp_time=1.5): + self.d1, self.d2 = RobStrideDriver(can1, False), RobStrideDriver(can2, False) + self.api = Sim2RealControlAPI(); self.dt = 1.0/hz; self.lk = threading.Lock() + self.run = False; self.enabled = False; self.estop = True + self.mode='stand'; self.vx=0.0; self.vy=0.0; self.yaw=0.0; self.h=0.33; self.roll=0.0; self.pitch=0.0 + self.prone = False + self.kp_leg, self.kd_leg, self.kd_wheel = 80.0, 2.5, 2.0 + self.cfg = self._cfg(); self.q=np.zeros(23); self.dq=np.zeros(22) + + self.use_interpolation = use_interpolation + if self.use_interpolation: + self.interpolator = TrajectoryInterpolator(method=interp_method, transition_time=interp_time) + print(f'[Interpolation] Enabled: method={interp_method}, transition_time={interp_time}s') + else: + self.interpolator = None + print('[Interpolation] Disabled') + + def _cfg(self): + sign={'fl_hip_abduction_joint':-1,'fl_hip_pitch_joint':-1,'fl_knee_joint':-1,'fl_wheel_joint':-1,'fr_hip_abduction_joint':-1,'fr_hip_pitch_joint':1,'fr_knee_joint':1,'fr_wheel_joint':1,'rl_hip_abduction_joint':1,'rl_hip_pitch_joint':-1,'rl_knee_joint':-1,'rl_wheel_joint':-1,'rr_hip_abduction_joint':1,'rr_hip_pitch_joint':1,'rr_knee_joint':1,'rr_wheel_joint':1} + off={'fl_hip_abduction_joint':0.003,'fl_hip_pitch_joint':0.030,'fl_knee_joint':0.028,'fl_wheel_joint':0.0,'fr_hip_abduction_joint':0.004,'fr_hip_pitch_joint':0.038,'fr_knee_joint':0.011,'fr_wheel_joint':0.0,'rl_hip_abduction_joint':0.019,'rl_hip_pitch_joint':-0.034,'rl_knee_joint':0.025,'rl_wheel_joint':0.0,'rr_hip_abduction_joint':-0.001,'rr_hip_pitch_joint':0.039,'rr_knee_joint':0.018,'rr_wheel_joint':0.0} + ids={'fl_hip_abduction_joint':1,'fl_hip_pitch_joint':2,'fl_knee_joint':3,'fl_wheel_joint':4,'fr_hip_abduction_joint':5,'fr_hip_pitch_joint':6,'fr_knee_joint':7,'fr_wheel_joint':8,'rl_hip_abduction_joint':1,'rl_hip_pitch_joint':2,'rl_knee_joint':3,'rl_wheel_joint':4,'rr_hip_abduction_joint':5,'rr_hip_pitch_joint':6,'rr_knee_joint':7,'rr_wheel_joint':8} + bus={k:('can1' if k.startswith('f') else 'can2') for k in JNS} + return {jn:Cfg(ids[jn],'rs-06',float(sign[jn]),float(off[jn]),bus[jn]) for jn in JNS} + + def _drv(self, jn): return self.d1 if self.cfg[jn].bus=='can1' else self.d2 + + def connect(self): + self.d1.connect(); self.d2.connect() + for jn,c in self.cfg.items(): self._drv(jn).add_motor(jn,c.mid,c.model) + + def enable_all(self): + for jn in JNS: self._drv(jn).enable(jn) + with self.lk: self.enabled=True; self.estop=False; self.vx=self.vy=self.yaw=0.0 + + def disable_all(self): + for jn in JNS: self._drv(jn).disable(jn) + with self.lk: self.enabled=False + + def clear(self): + for jn in JNS: self._drv(jn).clear_warnings(jn) + + def set_estop(self,on): + with self.lk: + self.estop=on + if on: self.vx=self.vy=self.yaw=0.0 + if on: self.disable_all() + + def set_prone(self,on): + with self.lk: self.prone=on; self.api.prone=on + + def _update_pin(self,leg,wheel): + q=np.zeros(23); dq=np.zeros(22); q[2]=self.h; q[6]=1.0 + for i,_ in enumerate(LEGS): + b=7+i*4; q[b:b+3]=leg[i*3:i*3+3]; dq[6+i*4+3]=wheel[i] + self.q,self.dq=q,dq + + def step(self): + with self.lk: + if self.estop or (not self.enabled): return + m=self.mode; vx=float(np.clip(self.vx,-0.8,0.8)); vy=float(np.clip(self.vy,-0.5,0.5)); yaw=float(np.clip(self.yaw,-3,3)); h=float(np.clip(self.h,0.157,0.448)); r=float(np.clip(self.roll,-0.4,0.4)); p=float(np.clip(self.pitch,-0.4,0.4)); prone=self.prone + cm='trot' if m=='trot' else 'wheel' + if m=='stand': vx=vy=yaw=0.0 + self.api.prone=prone; self.api.set_mode(cm); self.api.set_command(vx,vy,yaw,height=h) + st=DeployState(rpy=np.array([r,p,0.0])) + leg,wheel = self.api.compute(st,self.dt,self.q,self.dq) if cm=='trot' else self.api.compute(st,self.dt) + self._update_pin(leg,wheel); cmd=self.api.to_joint_dict(leg,wheel) + + if self.use_interpolation and self.interpolator is not None: + leg_cmd = {jn: cmd[jn] for jn in JNS if not jn.endswith(WJ)} + self.interpolator.set_target(leg_cmd) + smooth_cmd = self.interpolator.update(self.dt) + for jn in leg_cmd: + cmd[jn] = smooth_cmd[jn] + + for jn in JNS: + d=self._drv(jn); c=self.cfg[jn] + if jn.endswith(WJ): d.control_mit(jn,0.0,c.sign*float(cmd.get(jn,0.0)),0.0,self.kd_wheel,0.0) + else: d.control_mit(jn,c.sign*float(cmd[jn])+c.off,0.0,self.kp_leg,self.kd_leg,0.0) + + def loop(self): + while self.run: + t=time.time() + try: self.step() + except Exception as e: print('[control]',e) + time.sleep(max(0.0,self.dt-(time.time()-t))) + + def start(self): self.run=True; threading.Thread(target=self.loop,daemon=True).start() + def stop(self): + self.run=False; time.sleep(0.05) + try: self.disable_all() + finally: self.d1.disconnect(); self.d2.disconnect() + + def status(self): + with self.lk: return f'mode={self.mode} en={self.enabled} estop={self.estop} prone={self.prone} vx={self.vx:.2f} vy={self.vy:.2f} yaw={self.yaw:.2f} h={self.h:.3f}' + +class CLI: + def __init__(self,d): self.d=d + def run(self): + print('enable disable clear estop_on estop_off prone_on prone_off status') + print('mode stand|wheel|trot, vx vy yaw h roll pitch, stop, quit') + print('interp_on interp_off interp_time , interp_method linear|cubic|quintic|cosine') + while True: + try: s=input('cmd> ').strip().lower() + except (EOFError,KeyboardInterrupt): s='quit' + if s in ('quit','exit'): break + if s=='enable': self.d.enable_all(); continue + if s=='disable': self.d.disable_all(); continue + if s=='clear': self.d.clear(); continue + if s=='estop_on': self.d.set_estop(True); continue + if s=='estop_off': self.d.set_estop(False); continue + if s=='prone_on': self.d.set_prone(True); continue + if s=='prone_off': self.d.set_prone(False); continue + if s=='status': print(self.d.status()); continue + if s=='stop': + with self.d.lk: self.d.vx=self.d.vy=self.d.yaw=0.0 + continue + if s=='interp_on': + with self.d.lk: self.d.use_interpolation=True + print('Interpolation enabled'); continue + if s=='interp_off': + with self.d.lk: self.d.use_interpolation=False + print('Interpolation disabled'); continue + if s.startswith('interp_time '): + try: + t=float(s.split()[1]) + if self.d.interpolator: self.d.interpolator.set_transition_time(t) + print(f'Interpolation time set to {t}s') + except Exception as e: print(f'Error: {e}') + continue + if s.startswith('interp_method '): + try: + method=s.split()[1] + if self.d.interpolator: self.d.interpolator.set_method(method) + print(f'Interpolation method set to {method}') + except Exception as e: print(f'Error: {e}') + continue + if s.startswith('mode '): + m=s.split()[1] + if m in ('stand','wheel','trot'): + with self.d.lk: self.d.mode=m + else: print('bad mode') + continue + try: + k,v=s.split()[0],float(s.split()[1]) + with self.d.lk: + if k=='vx': self.d.vx=v + elif k=='vy': self.d.vy=v + elif k=='yaw': self.d.yaw=v + elif k=='h': self.d.h=v + elif k=='roll': self.d.roll=v + elif k=='pitch': self.d.pitch=v + else: print('unknown') + except Exception: print('unknown/bad') + +class GUI: + def __init__(self,d): + import tkinter as tk + from tkinter import ttk + self.d=d; self.root=tk.Tk(); self.root.title('WheelLeg Deploy') + f=ttk.Frame(self.root,padding=8); f.grid(row=0,column=0,sticky='nsew') + self.state=tk.StringVar(value='E-STOP ON'); ttk.Label(f,textvariable=self.state).grid(row=0,column=0,columnspan=4,sticky='w') + ttk.Button(f,text='Enable',command=self.en).grid(row=1,column=0) + ttk.Button(f,text='Disable',command=self.dis).grid(row=1,column=1) + ttk.Button(f,text='E-STOP ON',command=lambda:self.es(True)).grid(row=1,column=2) + ttk.Button(f,text='E-STOP OFF',command=lambda:self.es(False)).grid(row=1,column=3) + ttk.Button(f,text='Prone ON',command=lambda:self.pr(True)).grid(row=2,column=2) + ttk.Button(f,text='Prone OFF',command=lambda:self.pr(False)).grid(row=2,column=3) + self.mode=tk.StringVar(value='stand'); self.vx=tk.DoubleVar(value=0.0); self.vy=tk.DoubleVar(value=0.0); self.yaw=tk.DoubleVar(value=0.0); self.h=tk.DoubleVar(value=0.33) + self.roll=tk.DoubleVar(value=0.0); self.pitch=tk.DoubleVar(value=0.0) + cb=ttk.Combobox(f,textvariable=self.mode,values=['stand','wheel','trot'],state='readonly'); cb.grid(row=3,column=0,columnspan=2,sticky='ew'); cb.bind('<>',lambda _:self.sync()) + ttk.Button(f,text='Stop',command=self.stp).grid(row=3,column=3) + self.sl(f,4,'vx',self.vx,-0.8,0.8); self.sl(f,5,'vy',self.vy,-0.5,0.5); self.sl(f,6,'yaw',self.yaw,-3,3); self.sl(f,7,'height',self.h,0.157,0.448); self.sl(f,8,'roll',self.roll,-0.4,0.4); self.sl(f,9,'pitch',self.pitch,-0.4,0.4) + self.info=tk.StringVar(value=''); ttk.Label(f,textvariable=self.info).grid(row=10,column=0,columnspan=4,sticky='w'); self.tick() + def sl(self,f,r,n,v,lo,hi): + from tkinter import ttk + ttk.Label(f,text=n).grid(row=r,column=0,sticky='w'); ttk.Scale(f,from_=lo,to=hi,variable=v,command=lambda _:self.sync()).grid(row=r,column=1,columnspan=3,sticky='ew') + def sync(self): + with self.d.lk: + self.d.mode=self.mode.get(); self.d.vx=float(self.vx.get()); self.d.vy=float(self.vy.get()); self.d.yaw=float(self.yaw.get()); self.d.h=float(self.h.get()); self.d.roll=float(self.roll.get()); self.d.pitch=float(self.pitch.get()) + def en(self): self.d.enable_all(); self.state.set('Enabled') + def dis(self): self.d.disable_all(); self.state.set('Disabled') + def es(self,on): self.d.set_estop(on); self.state.set('E-STOP ON' if on else 'E-STOP OFF') + def pr(self,on): self.d.set_prone(on) + def stp(self): + with self.d.lk: self.d.vx=self.d.vy=self.d.yaw=0.0 + self.vx.set(0.0); self.vy.set(0.0); self.yaw.set(0.0) + def tick(self): self.info.set(self.d.status()); self.root.after(150,self.tick) + def run(self): self.root.mainloop() + +def main(): + ap=argparse.ArgumentParser() + ap.add_argument('--port-can1',default='/dev/can1') + ap.add_argument('--port-can2',default='/dev/can2') + ap.add_argument('--hz',type=float,default=100.0) + ap.add_argument('--no-gui',action='store_true') + ap.add_argument('--no-interp',action='store_true',help='Disable trajectory interpolation') + ap.add_argument('--interp-method',default='quintic',choices=['linear','cubic','quintic','cosine'],help='Interpolation method') + ap.add_argument('--interp-time',type=float,default=0.3,help='Interpolation transition time (seconds)') + a=ap.parse_args() + d=Deploy(a.port_can1,a.port_can2,a.hz,use_interpolation=not a.no_interp,interp_method=a.interp_method,interp_time=a.interp_time); d.connect(); d.start() + try: + cli=CLI(d); t=threading.Thread(target=cli.run,daemon=True); t.start() + if a.no_gui: + while t.is_alive(): time.sleep(0.2) + else: GUI(d).run() + finally: d.stop() + +if __name__=='__main__': main() diff --git a/05_software/real/ik_real/trajectory_interpolator.py b/05_software/real/ik_real/trajectory_interpolator.py new file mode 100644 index 0000000..82fb31c --- /dev/null +++ b/05_software/real/ik_real/trajectory_interpolator.py @@ -0,0 +1,237 @@ +#!/usr/bin/env python3 +""" +轨迹插值模块 - 用于平滑关节角度过渡，避免突变和冲击 + +支持多种插值方法： +- linear: 线性插值 +- cubic: 三次多项式（速度连续） +- quintic: 五次多项式（速度和加速度连续，最平滑） +- cosine: 余弦 S 曲线 + +使用示例： + interp = TrajectoryInterpolator(method='quintic', transition_time=0.5) + + # 设置新目标 + interp.set_target({'joint1': 1.5, 'joint2': 0.8}) + + # 每个控制周期调用 + smooth_q = interp.update(dt=0.01, current_q={'joint1': 0.5, 'joint2': 0.3}) +""" + +import time +from typing import Dict, Optional +import numpy as np + + +class TrajectoryInterpolator: + def __init__(self, method: str = 'quintic', transition_time: float = 0.5): + """ + 初始化轨迹插值器 + + Args: + method: 插值方法 ('linear', 'cubic', 'quintic', 'cosine') + transition_time: 过渡时间（秒） + """ + self.method = method + self.transition_time = transition_time + + self.q_start: Dict[str, float] = {} + self.q_target: Dict[str, float] = {} + self.q_current: Dict[str, float] = {} + + self.transition_start_time: Optional[float] = None + self.is_transitioning = False + + self._interpolation_funcs = { + 'linear': self._linear, + 'cubic': self._cubic, + 'quintic': self._quintic, + 'cosine': self._cosine, + } + + if method not in self._interpolation_funcs: + raise ValueError(f"Unknown interpolation method: {method}. " + f"Available: {list(self._interpolation_funcs.keys())}") + + def set_target(self, q_target: Dict[str, float], force_restart: bool = False): + """ + 设置新的目标角度，开始新的过渡 + + Args: + q_target: 目标关节角度字典 {joint_name: angle} + force_restart: 是否强制重新开始过渡（即使已经在过渡中） + """ + if not self.q_current: + self.q_current = q_target.copy() + self.q_target = q_target.copy() + self.q_start = q_target.copy() + self.is_transitioning = False + return + + if not force_restart and self.is_transitioning: + self.q_target = q_target.copy() + return + + self.q_start = self.q_current.copy() + self.q_target = q_target.copy() + self.transition_start_time = time.time() + self.is_transitioning = True + + def update(self, dt: float, current_q: Optional[Dict[str, float]] = None) -> Dict[str, float]: + """ + 更新插值状态，返回当前应该下发的平滑角度 + + Args: + dt: 时间步长（秒） + current_q: 可选的当前实际角度（用于初始化或同步） + + Returns: + 平滑后的关节角度字典 + """ + if current_q is not None and not self.q_current: + self.q_current = current_q.copy() + self.q_start = current_q.copy() + self.q_target = current_q.copy() + return self.q_current.copy() + + if not self.is_transitioning: + return self.q_target.copy() + + elapsed = time.time() - self.transition_start_time + + if elapsed >= self.transition_time: + self.q_current = self.q_target.copy() + self.is_transitioning = False + return self.q_current.copy() + + s = elapsed / self.transition_time + alpha = self._interpolation_funcs[self.method](s) + + self.q_current = {} + for joint_name in self.q_target: + start_val = self.q_start.get(joint_name, 0.0) + target_val = self.q_target[joint_name] + self.q_current[joint_name] = start_val + (target_val - start_val) * alpha + + return self.q_current.copy() + + def reset(self, q_init: Optional[Dict[str, float]] = None): + """ + 重置插值器状态 + + Args: + q_init: 初始角度，如果为 None 则清空所有状态 + """ + if q_init is None: + self.q_start = {} + self.q_target = {} + self.q_current = {} + else: + self.q_start = q_init.copy() + self.q_target = q_init.copy() + self.q_current = q_init.copy() + + self.transition_start_time = None + self.is_transitioning = False + + def is_done(self) -> bool: + """返回是否已完成当前过渡""" + return not self.is_transitioning + + def set_transition_time(self, t: float): + """动态修改过渡时间""" + self.transition_time = max(0.01, t) + + def set_method(self, method: str): + """动态修改插值方法""" + if method not in self._interpolation_funcs: + raise ValueError(f"Unknown method: {method}") + self.method = method + + @staticmethod + def _linear(s: float) -> float: + """线性插值：alpha = s""" + return np.clip(s, 0.0, 1.0) + + @staticmethod + def _cubic(s: float) -> float: + """三次多项式：alpha = 3s² - 2s³""" + s = np.clip(s, 0.0, 1.0) + return 3.0 * s**2 - 2.0 * s**3 + + @staticmethod + def _quintic(s: float) -> float: + """五次多项式：alpha = 10s³ - 15s⁴ + 6s⁵""" + s = np.clip(s, 0.0, 1.0) + return 10.0 * s**3 - 15.0 * s**4 + 6.0 * s**5 + + @staticmethod + def _cosine(s: float) -> float: + """余弦 S 曲线：alpha = (1 - cos(πs)) / 2""" + s = np.clip(s, 0.0, 1.0) + return (1.0 - np.cos(np.pi * s)) / 2.0 + + +if __name__ == '__main__': + import matplotlib.pyplot as plt + + print("轨迹插值模块测试") + print("=" * 60) + + methods = ['linear', 'cubic', 'quintic', 'cosine'] + colors = ['blue', 'green', 'red', 'purple'] + + fig, (ax1, ax2) = plt.subplots(1, 2, figsize=(12, 5)) + + for method, color in zip(methods, colors): + interp = TrajectoryInterpolator(method=method, transition_time=1.0) + + interp.reset({'joint1': 0.0}) + interp.set_target({'joint1': 1.0}) + + times = [] + positions = [] + velocities = [] + + t = 0.0 + dt = 0.01 + last_pos = 0.0 + + while t <= 1.0: + q = interp.update(dt) + pos = q['joint1'] + vel = (pos - last_pos) / dt if t > 0 else 0.0 + + times.append(t) + positions.append(pos) + velocities.append(vel) + + last_pos = pos + t += dt + + ax1.plot(times, positions, label=method, color=color, linewidth=2) + ax2.plot(times, velocities, label=method, color=color, linewidth=2) + + ax1.set_xlabel('时间 (s)') + ax1.set_ylabel('位置 (rad)') + ax1.set_title('不同插值方法的位置曲线') + ax1.legend() + ax1.grid(True, alpha=0.3) + + ax2.set_xlabel('时间 (s)') + ax2.set_ylabel('速度 (rad/s)') + ax2.set_title('不同插值方法的速度曲线') + ax2.legend() + ax2.grid(True, alpha=0.3) + + plt.tight_layout() + plt.savefig('/home/rc2/work/rcwork/trajectory_interpolation_comparison.png', dpi=150) + print("已保存对比图到: trajectory_interpolation_comparison.png") + + print("\n测试完成") + print("=" * 60) + print("推荐使用:") + print(" - quintic: 最平滑，速度和加速度连续") + print(" - cosine: 平滑且计算简单") + print(" - cubic: 速度连续，比 quintic 稍快") + print(" - linear: 最简单但速度会突变") diff --git a/05_software/real/sim2real/DEPLOYMENT.md b/05_software/real/sim2real/DEPLOYMENT.md new file mode 100644 index 0000000..f787b7b --- /dev/null +++ b/05_software/real/sim2real/DEPLOYMENT.md @@ -0,0 +1,50 @@ +# `sim2real` 部署说明 + +## 模型 + +当前只使用： + +- `sim2real/policies/model_rough.pt` + +## 模型契约 + +- `obs_dim = 53` +- `action_dim = 16` +- 单帧输入 +- 无 `base_lin_vel` +- 无 `height_scan` + +## 启动流程 + +1. 连接硬件 +2. 使能电机 +3. 从当前实测姿态起立 +4. 进入 `stand_balance` 闭环站立 +5. prime 当前观测 +6. 进入 `50Hz` runtime + +## 为什么这样改 + +- 之前版本在 `startup` 后只维持固定 `STAND_POSE` +- 实机上纯 PD 不足以持续抗姿态扰动 +- 现在增加独立站立闭环，先保证身体支撑，再进入策略 + +## 运行开关 + +- `config.yaml > policy.enable_zero_cmd_suppression` +- `config.yaml > stand_balance.enabled` +- `config.yaml > policy.hold_zero_command_pose` +- `config.yaml > policy.command_release_s` + +## 纯 Python 命令 + +默认前提：当前目录就是 `sim2real/` + +```bash +python -m pip install -r requirements-orin.txt +python tools/alignment_check.py --policy policies/model_rough.pt --manifest deployment_manifest.yaml +python tools/standalone_check.py +python main.py --dry-run +python main.py +python web/server.py --host 0.0.0.0 --port 8080 +``` diff --git a/05_software/real/sim2real/FACTS_AND_ASSUMPTIONS.md b/05_software/real/sim2real/FACTS_AND_ASSUMPTIONS.md new file mode 100644 index 0000000..5de97bd --- /dev/null +++ b/05_software/real/sim2real/FACTS_AND_ASSUMPTIONS.md @@ -0,0 +1,48 @@ +# `FACTS_AND_ASSUMPTIONS` + +## 已确认 + +- 当前部署模型：`sim2real/policies/model_rough.pt` +- 源模型：`model_2000.pt` +- actor 输入：`53D` +- actor 输出：`16D` +- 当前 actor 不吃 `base_lin_vel` +- 当前 actor 不吃 `height_scan` + +## 当前观测顺序 + +1. `base_ang_vel * 0.25` +2. `projected_gravity` +3. `command` +4. `joint_pos_rel`（12） +5. `joint_vel_rel * 0.05`（12） +6. `wheel_vel * 0.05`（4） +7. `last_actions`（16） + +## 当前控制定义 + +- 控制频率：`50Hz` +- 腿缩放：`0.125 / 0.25` +- 轮缩放：`5.0` +- 腿 LPF：`5Hz` +- 轮 LPF：`15Hz` + +## 当前仍依赖现场一致的部分 + +- IMU 安装方向与上一版校正一致 +- 当前 MJCF / 电机参数对应这次重新训练后的模型 +- 电机零位、方向、接线已按当前硬件修正 + +## 本次实现边界 + +不再支持： + +- `crawl` 模型 +- 多策略切换 +- `318D` 历史输入 +- 旧版 `startup.start_pose` + +## 本次排查结论 + +代码应只围绕当前 rough 模型运行。 +如果后续模型结构再改，必须重新核对观测、动作缩放、控制频率和部署文档。 diff --git a/05_software/real/sim2real/ORIN_NANO_DEPLOYMENT.md b/05_software/real/sim2real/ORIN_NANO_DEPLOYMENT.md new file mode 100644 index 0000000..42104c6 --- /dev/null +++ b/05_software/real/sim2real/ORIN_NANO_DEPLOYMENT.md @@ -0,0 +1,45 @@ +# `Orin Nano` 部署说明 + +## 是否必须转 ONNX + +不必须。 + +当前优先级仍然是： + +1. 先保证观测、动作、站立控制对齐 +2. 再测 `50Hz` 实际环路稳定性 +3. 最后才决定是否转 `ONNX/TensorRT` + +## 当前代码重点 + +- `stand_balance` 已加入 `main.py` 和 `web/session.py` +- 启动后先站稳，再允许策略接管 +- `PolicyRunner.step()` 仍保留零命令抑制开关，默认开启 + +## Orin 上先测什么 + +- 机器人能否在不启动策略时，仅靠 `startup + stand_balance` 稳定站住 +- `loop_dt_ms` +- `imu_age_ms` +- 电机 stale +- policy forward 耗时 + +## 纯 Python 部署命令 + +默认前提：当前目录就是 `sim2real/` + +```bash +python3 -m pip install -r requirements-orin.txt +python3 tools/alignment_check.py --policy policies/model_rough.pt --manifest deployment_manifest.yaml +python3 tools/standalone_check.py +python3 main.py --dry-run +python3 main.py +python3 web/server.py --host 0.0.0.0 --port 8080 +``` + +## 首轮实机建议 + +1. 先不启动策略 +2. 只验证 `startup -> stand_balance` +3. 站稳后再启动策略 +4. 只给很小的 `vx / vy / yaw` diff --git a/05_software/real/sim2real/README.md b/05_software/real/sim2real/README.md new file mode 100644 index 0000000..8d13446 --- /dev/null +++ b/05_software/real/sim2real/README.md @@ -0,0 +1,77 @@ +# `sim2real` + +当前版本只部署现在这套 `53D -> 16D` 模型，不再兼容旧版 `crawl`、多策略和历史观测。 + +## 当前部署模型 + +- 使用文件：`sim2real/policies/model_rough.pt` +- 来源文件：`model_2000.pt` + +## 当前 actor 输入 + +- 单帧 `53D` +- 顺序： + - `base_ang_vel * 0.25` + - `projected_gravity` + - `command` + - `joint_pos_rel`（12） + - `joint_vel_rel * 0.05`（12） + - `wheel_vel * 0.05`（4） + - `last_actions`（16） + +不包含： + +- `base_lin_vel` +- `height_scan` + +## 当前控制参数 + +- 控制频率：`50Hz` +- 站立保持：`startup/stand_balance.py` +- `hip_abduction` 缩放：`0.125` +- 其他腿关节缩放：`0.25` +- 轮速缩放：`5.0` +- 腿 LPF：`5Hz` +- 轮 LPF：`15Hz` +- 零命令抑制：默认开启，可通过 `config.yaml > policy.enable_zero_cmd_suppression` 关闭 +- 零命令保持：默认开启，可通过 `config.yaml > policy.hold_zero_command_pose` 控制 +- 首次命令解锁：默认开启，可通过 `config.yaml > policy.require_active_command_to_release` 控制 + +## 当前站立逻辑 + +- `startup`：从实测姿态过渡到默认站姿 +- `stand_balance`：根据 `IMU roll/pitch + gyro` 动态修正四条腿目标 +- `runtime`：只有站立稳定后才进入策略控制 + +这次改动的重点是：策略不再承担“先把身体撑住”的职责。 + +另外当前部署逻辑改成： + +- 零命令时默认不让策略直接接管腿和轮，保持站立目标 +- 命令从零变为非零时，策略输出在 `command_release_s` 内平滑放开 + +## 启动命令 + +默认前提：当前目录就是 `sim2real/` + +纯 `python`： + +```bash +python -m pip install -r requirements-orin.txt +python tools/alignment_check.py --policy policies/model_rough.pt --manifest deployment_manifest.yaml +python tools/standalone_check.py +python main.py --dry-run +python main.py +python web/server.py --host 0.0.0.0 --port 8080 +``` + +Windows 本机： + +```bash +D:\Minicoda3\envs\py10\python.exe -m pip install -r requirements-orin.txt +D:\Minicoda3\envs\py10\python.exe tools\alignment_check.py --policy policies\model_rough.pt --manifest deployment_manifest.yaml +D:\Minicoda3\envs\py10\python.exe tools\standalone_check.py +D:\Minicoda3\envs\py10\python.exe main.py +D:\Minicoda3\envs\py10\python.exe web\server.py --host 0.0.0.0 --port 8080 +``` +#sim2real/policies/model_rough.pt \ No newline at end of file diff --git a/05_software/real/sim2real/config.yaml b/05_software/real/sim2real/config.yaml new file mode 100644 index 0000000..6ef5bed --- /dev/null +++ b/05_software/real/sim2real/config.yaml @@ -0,0 +1,74 @@ +can1_port: "/dev/can1" +can2_port: "/dev/can2" +motor_model: "rs-02" +debug: false + +control_freq: 50 +imu_lib_path: null + +controller: + kp_leg: 80.0 + kd_leg: 2.5 + kd_wheel: 2.0 + max_vx: 0.8 + max_vy: 0.3 + max_yaw_rate: 0.5 + +policy: + enable_zero_cmd_suppression: true + hold_zero_command_pose: true + command_release_s: 0.35 + require_active_command_to_release: true + zero_cmd_use_yaw_rate: false + action_scale: [0.125, 0.25, 0.25, 0.125, 0.25, 0.25, 0.125, 0.25, 0.25, 0.125, 0.25, 0.25, 5.0, 5.0, 5.0, 5.0] + release_command_hold_s: 0.12 + release_posture_max_err: 0.35 + release_target_blend_s: 0.30 + +stand_balance: + enabled: true + height: 0.33 + kp_roll: 0.85 + kp_pitch: 0.70 + kd_roll_rate: 0.03 + kd_pitch_rate: 0.025 + lateral_lean_gain: 0.0 + hip_abduction_clip: 0.45 + hip_pitch_clip: [-1.0, 2.5] + knee_clip: [-2.6, -0.3] + stable_roll_deg: 6.0 + stable_pitch_deg: 8.0 + stable_gyro_deg_s: 45.0 + enter_hold_s: 1.0 + profile_h: [0.157, 0.248, 0.311, 0.366, 0.411, 0.448] + profile_hip: [1.5, 1.2, 1.0, 0.8, 0.6, 0.4] + profile_knee: [-2.5, -2.1, -1.8, -1.5, -1.2, -0.9] + +startup: + enabled: true + wait_for_enter_before_rise: false + soft_hold_duration: 1.0 + ramp_kp_time: 1.0 + transition_time_min: 2.0 + transition_time_max: 6.0 + transition_seconds_per_rad: 1.5 + timeout_extra: 3.0 + hold_time: 1.0 + settle_pos_threshold: 0.30 + settle_vel_threshold: 0.6 + progress_log_interval: 0.5 + max_dev_warn: 1.5 + max_dev_abort: 3.0 + require_user_confirm: true + +safety: + enabled: true + max_target_offset: 0.6 + max_ang_vel: 10.0 + max_tilt_z: -0.3 + clip_to_brake: 3 + imu_age_warn_ms: 60.0 + imu_age_stop_ms: 200.0 + +log_dir: "logs" +log_every: 1 diff --git a/05_software/real/sim2real/deployment_manifest.yaml b/05_software/real/sim2real/deployment_manifest.yaml new file mode 100644 index 0000000..994daaf --- /dev/null +++ b/05_software/real/sim2real/deployment_manifest.yaml @@ -0,0 +1,71 @@ +model: + path: "/home/rc2/work/rcwork/real/rc_mjlab/rc_mjlab/model_rough.pt" + obs_dim: 53 + action_dim: 16 + enable_zero_cmd_suppression: true + +observation: + terms: + - name: base_ang_vel + dim: 3 + scale: 0.25 + - name: projected_gravity + dim: 3 + - name: command + dim: 3 + - name: joint_pos_rel + dim: 12 + - name: joint_vel_rel + dim: 12 + scale: 0.05 + - name: wheel_vel + dim: 4 + scale: 0.05 + - name: last_actions + dim: 16 + +action: + scale: + - 0.125 + - 0.25 + - 0.25 + - 0.125 + - 0.25 + - 0.25 + - 0.125 + - 0.25 + - 0.25 + - 0.125 + - 0.25 + - 0.25 + - 5.0 + - 5.0 + - 5.0 + - 5.0 + default_dof_pos: + - 0.0 + - 0.9 + - -1.8 + - 0.0 + - 0.9 + - -1.8 + - 0.0 + - 0.9 + - -1.8 + - 0.0 + - 0.9 + - -1.8 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + +control: + control_freq_hz: 50 + leg_lpf_hz: 5 + wheel_lpf_hz: 15 + +safety: + zero_cmd_lin_thresh: 0.05 + zero_cmd_yaw_thresh: 0.05 + zero_yaw_rate_thresh: 0.10 diff --git a/05_software/real/sim2real/input_dev/__init__.py b/05_software/real/sim2real/input_dev/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/05_software/real/sim2real/input_dev/keyboard.py b/05_software/real/sim2real/input_dev/keyboard.py new file mode 100644 index 0000000..d4a271f --- /dev/null +++ b/05_software/real/sim2real/input_dev/keyboard.py @@ -0,0 +1,89 @@ +"""键盘控制器 — 兼容 sim2sim/input_dev/keyboard.py 的接口与平滑参数。""" +import numpy as np + +try: + from pynput import keyboard + PYNPUT_AVAILABLE = True +except ImportError: + PYNPUT_AVAILABLE = False + keyboard = None # type: ignore + + +class KeyboardCommandController: + """方向键 + AD 键的键盘指令源。 + 指令: [vx, vy, yaw_rate]，平滑加减速；空格触发急停标志。 + """ + + def __init__(self, + max_x_vel: float = 0.8, + max_y_vel: float = 0.3, + max_yaw_vel: float = 0.5, + acc_step: float = 0.05, + dec_step: float = 0.1): + if not PYNPUT_AVAILABLE: + raise RuntimeError("pynput 不可用，无法使用键盘控制；改用其他输入源。") + + self.current_cmd = np.zeros(3, dtype=np.float32) + self.max_x_vel = max_x_vel + self.max_y_vel = max_y_vel + self.max_yaw_vel = max_yaw_vel + self.acc_step = acc_step + self.dec_step = dec_step + + self._pressed = set() + self._estop = False + self.listener = keyboard.Listener( + on_press=self._on_press, on_release=self._on_release + ) + + def start(self): + self.listener.start() + print("[Keyboard] 启动。↑↓ 前后, ←→ 转向, A/D 横移, SPACE 急停") + + def stop(self): + try: + self.listener.stop() + except Exception: + pass + + def _on_press(self, key): + self._pressed.add(key) + if key == keyboard.Key.space: + self._estop = True + + def _on_release(self, key): + self._pressed.discard(key) + + def is_estop_triggered(self) -> bool: + return self._estop + + def reset_estop(self): + self._estop = False + + def get_command(self) -> np.ndarray: + target = np.zeros(3, dtype=np.float32) + if keyboard.Key.up in self._pressed: + target[0] += self.max_x_vel + if keyboard.Key.down in self._pressed: + target[0] -= self.max_x_vel + if keyboard.Key.left in self._pressed: + target[2] += self.max_yaw_vel + if keyboard.Key.right in self._pressed: + target[2] -= self.max_yaw_vel + try: + if keyboard.KeyCode.from_char('a') in self._pressed: + target[1] += self.max_y_vel + if keyboard.KeyCode.from_char('d') in self._pressed: + target[1] -= self.max_y_vel + except Exception: + pass + + for i, max_v in enumerate((self.max_x_vel, self.max_y_vel, self.max_yaw_vel)): + step = self.acc_step if target[i] != 0 else self.dec_step + if i == 2: + step *= 2.0 + if self.current_cmd[i] < target[i]: + self.current_cmd[i] = min(self.current_cmd[i] + step, target[i]) + else: + self.current_cmd[i] = max(self.current_cmd[i] - step, target[i]) + return self.current_cmd.copy() diff --git a/05_software/real/sim2real/interface/__init__.py b/05_software/real/sim2real/interface/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/05_software/real/sim2real/interface/imu_client.py b/05_software/real/sim2real/interface/imu_client.py new file mode 100644 index 0000000..ce65280 --- /dev/null +++ b/05_software/real/sim2real/interface/imu_client.py @@ -0,0 +1,136 @@ +"""Odin1 IMU 客户端封装。 + +核心改动相对 sim_rl/odin1/python/odin1_imu.py： +- 自动加载默认 .so 路径，调用方只需要 IMUClient(lib_path=...) +- 启动后做一次"重力对齐" — 用静止时的加速度计读数初始化 Mahony 滤波器， + 把首步姿态偏差从可能的 5°+ 降到 0.3° 内。这是方法论 D4 的关键一步。 +- 数据老化检测：若 imu_age_ms > stale_threshold 则报警（不阻塞）。 +""" +import sys +import time +from pathlib import Path +from typing import Optional + +import numpy as np + + +class IMUClient: + """Odin1 IMU 包装。 + + Args: + lib_path: libodin1_imu_bridge.so 的绝对路径；None 则按方法论 1.2 中 + 约定的相对位置寻找。 + gravity_align_samples: 启动时取多少帧加速度计平均值用于姿态初始化 + stale_threshold_ms: 单帧数据超过该 age 视为陈旧 + """ + + def __init__(self, lib_path: Optional[str] = None, gravity_align_samples: int = 50, + stale_threshold_ms: float = 50.0): + # 优先级 1: vendored/odin1_imu（独立部署模式） + # 优先级 2: ../../odin1/odin1/python（开发模式，即 sim_rl/odin1/odin1/python） + sim2real_root = Path(__file__).resolve().parents[1] + candidates = [ + sim2real_root / "vendored" / "odin1_imu", + sim2real_root.parents[1] / "odin1" / "odin1" / "python", + ] + for cand in candidates: + if cand.exists() and str(cand) not in sys.path: + sys.path.insert(0, str(cand)) + break + try: + from odin1_imu import Odin1ImuClient # type: ignore + except ImportError as e: + raise ImportError( + f"无法导入 Odin1ImuClient，已尝试的路径: {[str(c) for c in candidates]}: {e}" + ) + + # lib_path 默认查找：vendored/odin1_imu/build/libodin1_imu_bridge.so → 开发路径 + if lib_path is None: + so_candidates = [ + sim2real_root / "vendored" / "odin1_imu" / "build" / "libodin1_imu_bridge.so", + sim2real_root / "vendored" / "odin1_imu" / "libodin1_imu_bridge.so", + sim2real_root.parents[1] / "odin1" / "odin1" / "build" / "libodin1_imu_bridge.so", + ] + for so in so_candidates: + if so.exists(): + lib_path = str(so) + break + + self._client = Odin1ImuClient(lib_path=lib_path) + self._gravity_align_samples = gravity_align_samples + self._stale_threshold_ms = stale_threshold_ms + self._initial_gravity: Optional[np.ndarray] = None + # 用本机时钟追踪数据新鲜度（stamp_ns 是设备单调时钟，不能和 time.time 混算） + self._last_seq: int = -1 + self._last_fresh_time: float = 0.0 + + def version(self) -> str: + return self._client.version() + + def start(self, timeout_ms: int = 8000): + """启动 IMU 流，并采集若干帧用于重力对齐。""" + self._client.start(timeout_ms=timeout_ms) + self._wait_for_stream() + self._initial_gravity = self._collect_gravity_samples() + self._last_fresh_time = time.time() + + def stop(self): + try: + self._client.stop() + except Exception: + pass + + @property + def initial_gravity(self) -> Optional[np.ndarray]: + """启动后的初始重力向量（机身坐标系），用于初始化 Mahony 四元数。""" + return self._initial_gravity + + def get_latest(self): + """返回 (gyro[3], accel[3], age_ms, fresh)；fresh=False 表示无新数据。""" + sample = self._client.get_latest() + if sample is None: + return (np.zeros(3, dtype=np.float32), + np.array([0.0, 0.0, 9.81], dtype=np.float32), + -1.0, False) + gyro = np.array([sample.gyro_x, sample.gyro_y, sample.gyro_z], dtype=np.float32) + accel = np.array([sample.accel_x, sample.accel_y, sample.accel_z], dtype=np.float32) + # 用 stamp_ns 判断是否有新数据，因为 sequence 字段在 C++ 中可能没有赋值，导致永远为 0 + stamp = getattr(sample, "stamp_ns", 0) + now = time.time() + if stamp != self._last_seq: + self._last_seq = stamp + self._last_fresh_time = now + fresh = True + else: + fresh = False + age_ms = (now - self._last_fresh_time) * 1000.0 + return gyro, accel, age_ms, fresh + + # ---- 内部方法 ---- + def _wait_for_stream(self, timeout: float = 3.0): + deadline = time.time() + timeout + while time.time() < deadline: + if self._client.wait_for_data(timeout_ms=200): + # 有数据进来后清空一次队列以保证后续 get_latest 拿到的都是最新 + while self._client.pop_sample() is not None: + pass + return + raise RuntimeError("IMU 启动超时，未收到任何样本") + + def _collect_gravity_samples(self) -> np.ndarray: + accels = [] + for _ in range(self._gravity_align_samples): + sample = self._client.pop_sample() + if sample is None: + if not self._client.wait_for_data(timeout_ms=100): + continue + sample = self._client.pop_sample() + if sample is None: + continue + accels.append([sample.accel_x, sample.accel_y, sample.accel_z]) + if not accels: + print("[IMU] 警告: 重力对齐期间未收到样本，使用默认重力 [0,0,-9.81]") + return np.array([0.0, 0.0, -9.81], dtype=np.float32) + gravity = np.mean(accels, axis=0).astype(np.float32) + print(f"[IMU] 重力对齐完成: g_body = {gravity}") + return gravity diff --git a/05_software/real/sim2real/interface/motor_driver.py b/05_software/real/sim2real/interface/motor_driver.py new file mode 100644 index 0000000..5cafd3f --- /dev/null +++ b/05_software/real/sim2real/interface/motor_driver.py @@ -0,0 +1,310 @@ +"""RobStride 电机驱动包装。 + +职责： +- 封装 ik_real 中 RobStrideDriver 的 enable/disable/clear/control_mit 调用 +- **真实的丢包检测**：旧版用「value=0 启发式」会误判（电机回机械零位时也是 0）。 + 新方案： + 1. 调用 process_messages 前快照所有电机的 (pos, vel, torque) + 2. 调用后比较：状态变了 → 这一帧有新反馈；状态完全没变 → 累计 stale_count + 3. stale_count 超过阈值才沿用上一帧（方法论 3.4.2） + 仍然不完美（电机长时间静止确实会有连续多帧 state 不变），但比 0 启发式可靠。 +- 通过 driver_factory 由调用方注入：远程 Linux 主机用 RobStrideDriver， + 本地 Windows 调试可用 Mock。 +""" +from dataclasses import dataclass +import threading +from typing import Callable, Dict, List, Optional, Tuple + +import numpy as np + +from interface.motor_mapping import MotorMapping + + +@dataclass +class MotorReading: + position: float + velocity: float + torque: float = 0.0 + fresh: bool = False # True 表示本帧驱动板有新反馈 + + +class HardwareIO: + """统一的电机+IMU总线接口（不含策略），主控调用这一层。 + + Args: + driver_factory: () -> (drv1, drv2)，由调用方注入；返回的对象需要满足： + connect()/disconnect()/disable(name)/enable(name)/clear_warnings(name) + add_motor(name, mid, model)/process_messages() + control_mit(name, q, dq, kp, kd, tau) + .motors: dict[name -> motor], motor.state.position / .velocity / .torque + config: yaml 解析后的字典 + """ + + def __init__(self, driver_factory: Callable[[str, str, bool], Tuple[object, object]], + motor_model: str, can1_port: str, can2_port: str, debug: bool = False, + stale_frames_to_holdover: int = 2): + self.mapper = MotorMapping() + drv1, drv2 = driver_factory(can1_port, can2_port, debug) + self.driver_can1 = drv1 + self.driver_can2 = drv2 + self.motor_model = motor_model + self.stale_frames_to_holdover = stale_frames_to_holdover + + # 上一帧反馈（按 (bus, can_id) 索引），用于丢包兜底 + self._last_pos: Dict[Tuple[int, int], float] = {} + self._last_vel: Dict[Tuple[int, int], float] = {} + self._last_torque: Dict[Tuple[int, int], float] = {} + # 每个电机连续多少帧没收到新反馈 + self._stale_counts: Dict[Tuple[int, int], int] = {} + # 第一次必须读到才能解锁，避免初始化时直接用零位发送大力矩 + self._initialized = False + + self.lock = threading.Lock() + + # 累计诊断 + self.holdover_total = 0 # 累计被沿用上一帧的次数 + + # ---- 总线管理 ---- + def connect(self): + self.driver_can1.connect() + self.driver_can2.connect() + for jk in self.mapper.SIM_JOINT_ORDER: + leg, joint = jk + bus, mid = self.mapper.CAN_ID_MAP[jk] + name = f"{leg}_{joint}" + drv = self.driver_can1 if bus == 1 else self.driver_can2 + drv.add_motor(name, mid, self.motor_model) + self._stale_counts[(bus, mid)] = 0 + + def disconnect(self): + try: + self.driver_can1.disconnect() + finally: + self.driver_can2.disconnect() + + def enable_all(self): + for drv in (self.driver_can1, self.driver_can2): + for name in drv.motors: + drv.clear_warnings(name) + drv.enable(name) + + def disable_all(self): + for drv in (self.driver_can1, self.driver_can2): + for name in drv.motors: + drv.disable(name) + + # ---- 状态读取 ---- + def _snapshot_state(self) -> Dict[Tuple[int, int], Tuple[float, float, float, int]]: + """快照所有电机的 (pos, vel, torque, update_count)，process_messages 前后比较即可判 fresh。""" + snap: Dict[Tuple[int, int], Tuple[float, float, float, int]] = {} + for drv_idx, drv in enumerate((self.driver_can1, self.driver_can2)): + bus = drv_idx + 1 + for name, motor in drv.motors.items(): + parts = name.split("_", 1) + if len(parts) != 2: + continue + key = (parts[0], parts[1]) + if key not in self.mapper.CAN_ID_MAP: + continue + _, mid = self.mapper.CAN_ID_MAP[key] + s = motor.state + snap[(bus, mid)] = (s.position, s.velocity, s.torque, getattr(s, "update_count", 0)) + return snap + + def read_state(self) -> Tuple[np.ndarray, np.ndarray, np.ndarray, Dict[str, object]]: + """返回 (sim_joint_pos[16], sim_joint_vel[16], sim_joint_torque[16], debug_info)。""" + with self.lock: + # 1) 抓取上一次的状态作为「pre」快照（基线） + pre = self._snapshot_state() + + # 2) 拉取本帧反馈 + self.driver_can1.process_messages() + self.driver_can2.process_messages() + + # 3) 抓取「post」快照 + post = self._snapshot_state() + + # 4) 比较：state 元组变了 → 本帧有新反馈，stale_count 清零；否则 stale_count++ + per_motor_fresh: Dict[Tuple[int, int], bool] = {} + for key in post: + fresh = (pre.get(key) != post[key]) + per_motor_fresh[key] = fresh + if fresh: + self._stale_counts[key] = 0 + else: + self._stale_counts[key] += 1 + + # 5) 取出本帧 pos/vel；若该电机连续多帧没刷新，沿用上一帧（方法论 3.4.2） + real_pos: Dict[Tuple[int, int], float] = {} + real_vel: Dict[Tuple[int, int], float] = {} + real_torque: Dict[Tuple[int, int], float] = {} + holdover_this_frame = 0 + for key, (pos, vel, tor, _) in post.items(): + if (not per_motor_fresh[key]) and self._stale_counts[key] >= self.stale_frames_to_holdover: + # 长时间不刷新视作丢包：沿用上一帧 + if key in self._last_pos: + real_pos[key] = self._last_pos[key] + real_vel[key] = self._last_vel[key] + real_torque[key] = self._last_torque[key] + holdover_this_frame += 1 + else: + real_pos[key] = pos + real_vel[key] = vel + real_torque[key] = tor + else: + real_pos[key] = pos + real_vel[key] = vel + real_torque[key] = tor + + self.holdover_total += holdover_this_frame + # 缓存本帧（即便部分是 holdover 也缓存） + self._last_pos = real_pos.copy() + self._last_vel = real_vel.copy() + self._last_torque = real_torque.copy() + if not self._initialized: + self._initialized = True + + cur_pos = self.mapper.real_to_sim(real_pos) + cur_vel = self.mapper.real_vel_to_sim(real_vel) + cur_torque = self.mapper.real_vel_to_sim(real_torque) + + # 诊断信息 + stale_max = max(self._stale_counts.values()) if self._stale_counts else 0 + n_stale_motors = sum(1 for c in self._stale_counts.values() + if c >= self.stale_frames_to_holdover) + # 按 SIM_JOINT_ORDER 排列的每个电机连续丢帧数 + per_motor_stale = [ + self._stale_counts.get(self.mapper.CAN_ID_MAP[jk], 99) + for jk in self.mapper.SIM_JOINT_ORDER + ] + return cur_pos, cur_vel, cur_torque, { + "holdover_this_frame": holdover_this_frame, + "stale_max": stale_max, + "n_stale_motors": n_stale_motors, + "fresh_count": sum(1 for v in per_motor_fresh.values() if v), + "per_motor_stale": per_motor_stale, + } + + def passive_poll(self): + """发送全 0 (0刚度0阻尼0力矩) 的 MIT 指令给所有电机。 + 目的：在 ENABLED 状态下，不产生力矩地索要反馈（因为 RobStride 在 MIT 模式下必须有指令才反馈）。""" + with self.lock: + for jk in self.mapper.SIM_JOINT_ORDER: + bus, mid = self.mapper.CAN_ID_MAP[jk] + name = f"{jk[0]}_{jk[1]}" + drv = self.driver_can1 if bus == 1 else self.driver_can2 + if name in drv.motors: + drv.control_mit(name, 0.0, 0.0, 0.0, 0.0, 0.0) + + # ---- 控制下发 ---- + def send_control(self, target_angles: np.ndarray, kp_leg: float, kd_leg: float, + kd_wheel: float): + """与 sim2sim 的 PD 模型对齐： + - 腿: position 控制，目标角度由 target_angles[:12] 给出，kp/kd 来自配置 + - 轮: velocity 控制，目标速度由 target_angles[12:] 给出，kd 阻尼 + """ + with self.lock: + if target_angles.shape != (16,): + raise ValueError("target_angles must be (16,)") + + real_targets = self.mapper.sim_to_real(target_angles.astype(np.float32)) + + # 轮毂速度目标暂且用 0，如果 target_angles 里包含了速度，就在 policy 那里处理， + # 这里的 target_angles 是 pose 目标，轮毂作为连续旋转关节其实位置控制没有意义。 + # 为了兼容旧代码，这里构造一个 16 维的 velocity array，只有后 4 个是目标（如果当作速度的话）。 + vel_targets = np.zeros(16, dtype=np.float32) + vel_targets[12:] = target_angles[12:].astype(np.float32) + real_wheel = self.mapper.sim_vel_to_real(vel_targets) + + for jk in self.mapper.SIM_JOINT_ORDER: + leg, joint = jk + bus, mid = self.mapper.CAN_ID_MAP[jk] + name = f"{leg}_{joint}" + drv = self.driver_can1 if bus == 1 else self.driver_can2 + if name not in drv.motors: + continue + + if joint == "wheel": + v = real_wheel[(bus, mid)] + drv.control_mit(name, 0.0, v, 0.0, kd_wheel, 0.0) + else: + q = real_targets[(bus, mid)] + drv.control_mit(name, q, 0.0, kp_leg, kd_leg, 0.0) + + def damping_brake(self, kd_leg: float, kd_wheel: float): + """急停模式：所有关节卸载刚度，仅保留阻尼。 + 对应 270_SimToReal 方法论 97.11 Level 2 "刹车"。 + """ + with self.lock: + for jk in self.mapper.SIM_JOINT_ORDER: + leg, joint = jk + bus, _ = self.mapper.CAN_ID_MAP[jk] + name = f"{leg}_{joint}" + drv = self.driver_can1 if bus == 1 else self.driver_can2 + if name not in drv.motors: + continue + kd = kd_wheel if joint == "wheel" else kd_leg + drv.control_mit(name, 0.0, 0.0, 0.0, kd, 0.0) + + def wait_feedback_ready(self, max_attempts: int = 20, + poll_interval: float = 0.05) -> Tuple[bool, list]: + """enable 后调用：尝试 max_attempts 次读总线，等所有 16 个电机 + 都至少给出一帧反馈。 + 返回 (all_ready, missing_motors)；missing_motors 是 (bus, mid, name) 列表。 + """ + import time + seen: Dict[Tuple[int, int], bool] = { + self.mapper.CAN_ID_MAP[jk]: False for jk in self.mapper.SIM_JOINT_ORDER + } + # 用第一次读到的 (pos, vel, torque) 三元组的"非零"或"已变化"作为反馈到达的判据。 + # 启动瞬间所有 motor.state 默认全 0，要么收到反馈让其变化，要么收到反馈但值确实是 0。 + # 退化情况下电机静止时 vel=0 且 pos=机械零位也=0，那种情况只能等多帧确认。 + snap_prev = self._snapshot_state() + for attempt in range(max_attempts): + with self.lock: + self.driver_can1.process_messages() + self.driver_can2.process_messages() + snap_cur = self._snapshot_state() + for key, fields_cur in snap_cur.items(): + if seen[key]: + continue + fields_prev = snap_prev.get(key) + # 任一字段不为 0 → 一定有反馈（因为初始值都是 0） + if any(v != 0.0 for v in fields_cur): + seen[key] = True + # 与上一次快照不同 → 一定有反馈（即便都很小） + elif fields_prev is not None and fields_cur != fields_prev: + seen[key] = True + snap_prev = snap_cur + if all(seen.values()): + return True, [] + time.sleep(poll_interval) + + # 超时：列出仍未反馈的电机 + missing = [] + rev_can = {v: k for k, v in self.mapper.CAN_ID_MAP.items()} + for key, ok in seen.items(): + if not ok: + leg, joint = rev_can[key] + missing.append((key[0], key[1], f"{leg}_{joint}")) + return False, missing + + def read_measured_pose(self) -> np.ndarray: + """返回 (16,) 当前实测 sim 坐标系下的关节位置。 + 会先 process_messages 一次保证拿到本帧。 + """ + self.driver_can1.process_messages() + self.driver_can2.process_messages() + real_pos: Dict[Tuple[int, int], float] = {} + for drv_idx, drv in enumerate((self.driver_can1, self.driver_can2)): + bus = drv_idx + 1 + for name, motor in drv.motors.items(): + parts = name.split("_", 1) + if len(parts) != 2: + continue + key = (parts[0], parts[1]) + if key not in self.mapper.CAN_ID_MAP: + continue + _, mid = self.mapper.CAN_ID_MAP[key] + real_pos[(bus, mid)] = motor.state.position + return self.mapper.real_to_sim(real_pos) diff --git a/05_software/real/sim2real/interface/motor_mapping.py b/05_software/real/sim2real/interface/motor_mapping.py new file mode 100644 index 0000000..27ed77a --- /dev/null +++ b/05_software/real/sim2real/interface/motor_mapping.py @@ -0,0 +1,99 @@ +"""仿真→实机电机映射。 + +数据来源：sim_rl/ik_real/sim_to_real_deploy_beifen.py 和 +sim_rl/sim2real/motor_mapping.py 中的 sign / offset / can_id 表（已在实机上验证）。 +关节顺序与 rc_mjlab/sim2sim 完全一致：[12 个腿关节] + [4 个轮子]。 +""" +from typing import Dict, Tuple + +import numpy as np + + +class MotorMapping: + LEG_NAMES = ("fl", "fr", "rl", "rr") + JOINT_NAMES = ("hip_abduction", "hip_pitch", "knee", "wheel") + + SIM_JOINT_ORDER = ( + ("fl", "hip_abduction"), ("fl", "hip_pitch"), ("fl", "knee"), + ("fr", "hip_abduction"), ("fr", "hip_pitch"), ("fr", "knee"), + ("rl", "hip_abduction"), ("rl", "hip_pitch"), ("rl", "knee"), + ("rr", "hip_abduction"), ("rr", "hip_pitch"), ("rr", "knee"), + ("fl", "wheel"), ("fr", "wheel"), ("rl", "wheel"), ("rr", "wheel"), + ) + SIM_INDEX_MAP = {jk: i for i, jk in enumerate(SIM_JOINT_ORDER)} + + CAN_ID_MAP: Dict[Tuple[str, str], Tuple[int, int]] = { + ("fl", "hip_abduction"): (1, 1), ("fl", "hip_pitch"): (1, 2), + ("fl", "knee"): (1, 3), ("fl", "wheel"): (1, 4), + ("fr", "hip_abduction"): (1, 5), ("fr", "hip_pitch"): (1, 6), + ("fr", "knee"): (1, 7), ("fr", "wheel"): (1, 8), + ("rl", "hip_abduction"): (2, 1), ("rl", "hip_pitch"): (2, 2), + ("rl", "knee"): (2, 3), ("rl", "wheel"): (2, 4), + ("rr", "hip_abduction"): (2, 5), ("rr", "hip_pitch"): (2, 6), + ("rr", "knee"): (2, 7), ("rr", "wheel"): (2, 8), + } + + DIRECTION_MAP: Dict[Tuple[str, str], int] = { + ("fl", "hip_abduction"): -1, ("fl", "hip_pitch"): -1, + ("fl", "knee"): -1, ("fl", "wheel"): -1, + ("fr", "hip_abduction"): -1, ("fr", "hip_pitch"): 1, + ("fr", "knee"): 1, ("fr", "wheel"): 1, + ("rl", "hip_abduction"): 1, ("rl", "hip_pitch"): -1, + ("rl", "knee"): -1, ("rl", "wheel"): -1, + ("rr", "hip_abduction"): 1, ("rr", "hip_pitch"): 1, + ("rr", "knee"): 1, ("rr", "wheel"): 1, + } + + ZERO_OFFSET_MAP: Dict[Tuple[str, str], float] = { + ("fl", "hip_abduction"): 0.003, ("fl", "hip_pitch"): 0.030, + ("fl", "knee"): 0.028, ("fl", "wheel"): 0.000, + ("fr", "hip_abduction"): 0.004, ("fr", "hip_pitch"): 0.038, + ("fr", "knee"): 0.011, ("fr", "wheel"): 0.000, + ("rl", "hip_abduction"): 0.019, ("rl", "hip_pitch"): -0.034, + ("rl", "knee"): 0.025, ("rl", "wheel"): 0.000, + ("rr", "hip_abduction"): -0.001, ("rr", "hip_pitch"): 0.039, + ("rr", "knee"): 0.018, ("rr", "wheel"): 0.000, + } + + def __init__(self): + self.num_motors = len(self.SIM_JOINT_ORDER) + self._sign = np.array([self.DIRECTION_MAP[jk] for jk in self.SIM_JOINT_ORDER], dtype=np.float32) + self._offset = np.array([self.ZERO_OFFSET_MAP[jk] for jk in self.SIM_JOINT_ORDER], dtype=np.float32) + + def sim_to_real(self, sim_angles: np.ndarray) -> Dict[Tuple[int, int], float]: + if len(sim_angles) != 16: + raise ValueError(f"expected 16 sim angles, got {len(sim_angles)}") + out: Dict[Tuple[int, int], float] = {} + for i, jk in enumerate(self.SIM_JOINT_ORDER): + real = float(self._sign[i] * sim_angles[i] + self._offset[i]) + out[self.CAN_ID_MAP[jk]] = real + return out + + def sim_vel_to_real(self, sim_vels: np.ndarray) -> Dict[Tuple[int, int], float]: + # 速度只受方向影响，不应用 offset。 + out: Dict[Tuple[int, int], float] = {} + for i, jk in enumerate(self.SIM_JOINT_ORDER): + out[self.CAN_ID_MAP[jk]] = float(self._sign[i] * sim_vels[i]) + return out + + def real_to_sim(self, real_pos: Dict[Tuple[int, int], float]) -> np.ndarray: + out = np.zeros(16, dtype=np.float32) + for i, jk in enumerate(self.SIM_JOINT_ORDER): + v = real_pos.get(self.CAN_ID_MAP[jk]) + if v is None: + continue + out[i] = (v - self._offset[i]) / self._sign[i] + return out + + def real_vel_to_sim(self, real_vel: Dict[Tuple[int, int], float]) -> np.ndarray: + out = np.zeros(16, dtype=np.float32) + for i, jk in enumerate(self.SIM_JOINT_ORDER): + v = real_vel.get(self.CAN_ID_MAP[jk]) + if v is None: + continue + out[i] = v / self._sign[i] + return out + + def joint_name_at(self, idx: int) -> str: + leg, joint = self.SIM_JOINT_ORDER[idx] + return f"{leg}_{joint}_joint" diff --git a/05_software/real/sim2real/interface/real_io.py b/05_software/real/sim2real/interface/real_io.py new file mode 100644 index 0000000..6ff6480 --- /dev/null +++ b/05_software/real/sim2real/interface/real_io.py @@ -0,0 +1,135 @@ +import time +from typing import Callable, Dict, Tuple + +import numpy as np + +from interface.imu_client import IMUClient +from interface.motor_driver import HardwareIO +from tools.math_utils import LowPassFilter, MahonyFilter, get_gravity_orientation + + +class RealIO: + def __init__( + self, + driver_factory: Callable[[str, str, bool], Tuple[object, object]], + motor_model: str, + can1_port: str, + can2_port: str, + imu_lib_path: str, + control_dt: float = 0.02, + kp_leg: float = 80.0, + kd_leg: float = 2.5, + kd_wheel: float = 2.0, + debug: bool = False, + ): + self.control_dt = control_dt + self.kp_leg = kp_leg + self.kd_leg = kd_leg + self.kd_wheel = kd_wheel + + print("[RealIO] 初始化电机驱动...") + self.hw = HardwareIO(driver_factory, motor_model, can1_port, can2_port, debug) + print("[RealIO] 初始化 IMU...") + self.imu = IMUClient(lib_path=imu_lib_path) + + self.imu_filter = MahonyFilter(kp=2.0, ki=0.0, dt=control_dt) + self.quat_wxyz = np.array([1.0, 0.0, 0.0, 0.0], dtype=np.float32) + + self.lpf_legs = LowPassFilter(cutoff_freq=5.0, dt=control_dt, dim=12) + self.lpf_wheels = LowPassFilter(cutoff_freq=15.0, dt=control_dt, dim=4) + + self._last_imu_age_ms = -1.0 + self._last_imu_fresh = False + + def connect(self, imu_timeout_ms: int = 8000): + self.hw.connect() + self.imu.start(timeout_ms=imu_timeout_ms) + if self.imu.initial_gravity is not None: + self.imu_filter.reset_with_accel(self.imu.initial_gravity) + self.quat_wxyz = self.imu_filter.q.copy() + + def disconnect(self): + try: + self.hw.disable_all() + finally: + self.imu.stop() + self.hw.disconnect() + + def enable_motors(self): + self.hw.enable_all() + + def disable_motors(self): + self.hw.disable_all() + + def damping_brake(self): + self.hw.damping_brake(self.kd_leg, self.kd_wheel) + + def wait_feedback_ready(self, max_attempts: int = 20, poll_interval: float = 0.05): + return self.hw.wait_feedback_ready(max_attempts=max_attempts, poll_interval=poll_interval) + + def read_measured_pose(self) -> np.ndarray: + return self.hw.read_measured_pose() + + def read_state(self) -> Dict[str, object]: + joint_pos, joint_vel, joint_torque, motor_diag = self.hw.read_state() + gyro, accel, age_ms, fresh = self.imu.get_latest() + self._last_imu_age_ms = age_ms + self._last_imu_fresh = fresh + self.quat_wxyz = self.imu_filter.update(accel, gyro) + projected_gravity = get_gravity_orientation(self.quat_wxyz) + + return { + "joint_pos": joint_pos, + "joint_vel": joint_vel, + "joint_torque": joint_torque, + "imu_gyro": gyro, + "imu_accel": accel, + "quat_wxyz": self.quat_wxyz.copy(), + "projected_gravity": projected_gravity, + "imu_age_ms": age_ms, + "imu_fresh": fresh, + "motor_stale": motor_diag, + } + + def get_obs_policy( + self, + state: Dict[str, object], + command: np.ndarray, + default_dof_pos: np.ndarray, + last_actions_raw: np.ndarray, + ) -> np.ndarray: + gyro = state["imu_gyro"] + joint_pos = state["joint_pos"] + joint_vel = state["joint_vel"] + projected_gravity = state["projected_gravity"] + + base_ang_vel = (gyro * 0.25).astype(np.float32) + joint_pos_rel = (joint_pos[:12] - default_dof_pos[:12]).astype(np.float32) + joint_vel_leg = (joint_vel[:12] * 0.05).astype(np.float32) + wheel_vel = (joint_vel[12:] * 0.05).astype(np.float32) + + return np.concatenate( + [ + base_ang_vel, + projected_gravity, + command.astype(np.float32), + joint_pos_rel, + joint_vel_leg, + wheel_vel, + last_actions_raw, + ] + ).astype(np.float32) + + def send_actions(self, scaled_actions: np.ndarray, default_dof_pos: np.ndarray): + act = (scaled_actions + default_dof_pos).astype(np.float32) + act = np.clip(act, -100.0, 100.0) + act[:12] = self.lpf_legs.filter(act[:12]) + act[12:] = self.lpf_wheels.filter(act[12:]) + self.hw.send_control(act, self.kp_leg, self.kd_leg, self.kd_wheel) + return act + + def hold_pose(self, sim_target_pose: np.ndarray, kp_scale: float = 1.0): + target = np.clip(sim_target_pose.astype(np.float32), -100.0, 100.0) + kp_scale = float(np.clip(kp_scale, 0.0, 1.0)) + self.hw.send_control(target, self.kp_leg * kp_scale, self.kd_leg, self.kd_wheel) + return target diff --git a/05_software/real/sim2real/main.py b/05_software/real/sim2real/main.py new file mode 100644 index 0000000..245f78a --- /dev/null +++ b/05_software/real/sim2real/main.py @@ -0,0 +1,725 @@ +"""CLI entrypoint for current sim2real deployment.""" + +import argparse +import os +import sys +import threading +import time +from pathlib import Path + +import numpy as np +import yaml + +sys.path.insert(0, str(Path(__file__).resolve().parent)) + +from input_dev.keyboard import KeyboardCommandController +from interface.real_io import RealIO +from policy.policy_runner import PolicyRunner +from safety.runtime_guard import GuardLevel, RuntimeGuard +from safety.safety_monitor import SafetyLevel, SafetyMonitor +from startup.pose_initializer import PoseInitFailed, PoseInitializer, STAND_POSE +from startup.stand_balance import StandBalanceController +from tools.logger import LogBundle +from tools.math_utils import get_gravity_orientation + +JOINT_LABELS = LogBundle.JOINT_LABELS + + +def make_real_driver_factory(): + def factory(can1_port, can2_port, debug): + sim2real_root = Path(__file__).resolve().parent + for path in ( + sim2real_root / "vendored", + "/home/rc2/work/rcwork/control", + "/home/rc2/work/rcwork", + ): + path_str = str(path) + if path_str not in sys.path and Path(path).exists(): + sys.path.append(path_str) + from drivers.motor_driver import RobStrideDriver # type: ignore + + return RobStrideDriver(can1_port, debug), RobStrideDriver(can2_port, debug) + + return factory + + +def make_dry_driver_factory(): + class MockMotor: + def __init__(self): + class State: + position = 0.0 + velocity = 0.0 + torque = 0.0 + + self.state = State() + + class MockDriver: + def __init__(self, port, debug): + self.port = port + self.motors = {} + + def connect(self): ... + def disconnect(self): ... + def add_motor(self, name, motor_id, model): self.motors[name] = MockMotor() + def enable(self, name): ... + def disable(self, name): ... + def clear_warnings(self, name): ... + def process_messages(self): ... + def control_mit(self, *args, **kwargs): ... + + def factory(can1_port, can2_port, debug): + return MockDriver(can1_port, debug), MockDriver(can2_port, debug) + + return factory + + +def _sleep_to(next_exec: float) -> float: + slack = next_exec - time.perf_counter() + if slack > 0: + time.sleep(slack) + return next_exec + 0.0 + return time.perf_counter() + + +def build_action_diag( + *, + joint_pos: np.ndarray, + default_pose: np.ndarray, + raw: np.ndarray, + scaled: np.ndarray, + tentative: np.ndarray, + cmd: np.ndarray, + zero_command: bool, + runtime_released: bool, + release_alpha: float, + safety_details: dict | None = None, +) -> dict: + details = dict(safety_details or {}) + joint_indices = list(details.get("joint_indices", [])) + pos_err = tentative - joint_pos + leg_offset = tentative[:12] - default_pose[:12] + diag = { + "joint_indices": joint_indices, + "joint_names": [JOINT_LABELS[i] for i in joint_indices if 0 <= i < len(JOINT_LABELS)], + "cmd": cmd.tolist(), + "zero_command": bool(zero_command), + "runtime_released": bool(runtime_released), + "release_alpha": float(release_alpha), + "max_raw": float(np.max(np.abs(raw))) if raw.size else 0.0, + "max_scaled": float(np.max(np.abs(scaled[:12]))) if scaled.size else 0.0, + "max_target": float(np.max(np.abs(tentative[:12]))) if tentative.size else 0.0, + } + if joint_indices: + primary = int(joint_indices[0]) + diag.update( + { + "primary_joint_index": primary, + "primary_joint_name": JOINT_LABELS[primary], + "primary_target": float(tentative[primary]), + "primary_default": float(default_pose[primary]), + "primary_measured": float(joint_pos[primary]), + "primary_pos_err": float(pos_err[primary]), + "primary_raw": float(raw[primary]), + "primary_scaled": float(scaled[primary]), + } + ) + if primary < 12: + diag["primary_leg_offset"] = float(leg_offset[primary]) + details.update(diag) + return details + + +def policy_release_cfg(cfg: dict) -> dict[str, float]: + policy_cfg = cfg.get("policy", {}) + return { + "command_hold_s": max(float(policy_cfg.get("release_command_hold_s", 0.12)), 0.0), + "posture_max_err": max(float(policy_cfg.get("release_posture_max_err", 0.35)), 0.0), + "target_blend_s": max(float(policy_cfg.get("release_target_blend_s", 0.30)), 1e-3), + } + + +def compute_release_metrics(runner: PolicyRunner, state: dict, hold_target: np.ndarray, cmd: np.ndarray) -> dict: + joint_pos = np.asarray(state["joint_pos"], dtype=np.float32) + default_pose = np.asarray(runner.default_dof_pos, dtype=np.float32) + hold_target = np.asarray(hold_target, dtype=np.float32) + planar_cmd, yaw_cmd = runner.command_activation_metrics(cmd) + return { + "planar_cmd": float(planar_cmd), + "yaw_cmd": float(yaw_cmd), + "max_hold_err": float(np.max(np.abs(joint_pos[:12] - hold_target[:12]))), + "max_default_err": float(np.max(np.abs(joint_pos[:12] - default_pose[:12]))), + "max_hold_default_gap": float(np.max(np.abs(hold_target[:12] - default_pose[:12]))), + } + + +def blend_runtime_target( + runner: PolicyRunner, + hold_target: np.ndarray, + policy_target: np.ndarray, + release_alpha: float, + target_blend_s: float, + control_dt: float, +) -> np.ndarray: + blend = min(1.0, release_alpha * (runner.command_release_s / max(target_blend_s, control_dt))) + return ((1.0 - blend) * hold_target + blend * policy_target).astype(np.float32) + + +def compute_target_error_metrics( + state: dict, + hold_target: np.ndarray, + policy_target: np.ndarray, +) -> dict[str, float]: + joint_pos = np.asarray(state["joint_pos"], dtype=np.float32) + hold_target = np.asarray(hold_target, dtype=np.float32) + policy_target = np.asarray(policy_target, dtype=np.float32) + return { + "hold_target_max_err": float(np.max(np.abs(joint_pos[:12] - hold_target[:12]))), + "policy_target_max_err": float(np.max(np.abs(joint_pos[:12] - policy_target[:12]))), + "hold_policy_max_gap": float(np.max(np.abs(hold_target[:12] - policy_target[:12]))), + } + + +def main(): + parser = argparse.ArgumentParser() + parser.add_argument("--config", default=str(Path(__file__).parent / "config.yaml")) + parser.add_argument("--policy", default=None) + parser.add_argument("--dry-run", action="store_true") + args = parser.parse_args() + + with open(args.config, "r", encoding="utf-8") as file_obj: + cfg = yaml.safe_load(file_obj) + + sim2real_root = Path(__file__).resolve().parent + policy_path = Path(args.policy) if args.policy else sim2real_root / "policies" / "model_rough.pt" + if not policy_path.exists(): + print(f"[Main] policy not found: {policy_path}") + sys.exit(1) + + control_dt = 1.0 / float(cfg["control_freq"]) + driver_factory = make_dry_driver_factory() if args.dry_run else make_real_driver_factory() + + logger = LogBundle(cfg["log_dir"]) + logger.event( + "CONFIG_LOADED", + config_path=args.config, + policy=str(policy_path), + dry_run=args.dry_run, + control_freq=cfg["control_freq"], + motor_model=cfg["motor_model"], + ) + + io = RealIO( + driver_factory=driver_factory, + motor_model=cfg["motor_model"], + can1_port=cfg["can1_port"], + can2_port=cfg["can2_port"], + imu_lib_path=cfg.get("imu_lib_path"), + control_dt=control_dt, + kp_leg=cfg["controller"]["kp_leg"], + kd_leg=cfg["controller"]["kd_leg"], + kd_wheel=cfg["controller"]["kd_wheel"], + debug=cfg.get("debug", False), + ) + runner = PolicyRunner( + policy_path, + enable_zero_cmd_suppression=cfg.get("policy", {}).get("enable_zero_cmd_suppression", True), + hold_zero_command_pose=cfg.get("policy", {}).get("hold_zero_command_pose", True), + command_release_s=cfg.get("policy", {}).get("command_release_s", 0.35), + action_scale=np.asarray( + cfg.get("policy", {}).get( + "action_scale", + [0.125, 0.25, 0.25, 0.125, 0.25, 0.25, 0.125, 0.25, 0.25, 0.125, 0.25, 0.25, 5.0, 5.0, 5.0, 5.0], + ), + dtype=np.float32, + ), + zero_cmd_use_yaw_rate=cfg.get("policy", {}).get("zero_cmd_use_yaw_rate", False), + ) + require_active_command = cfg.get("policy", {}).get("require_active_command_to_release", True) + keyboard = KeyboardCommandController( + max_x_vel=cfg["controller"]["max_vx"], + max_y_vel=cfg["controller"]["max_vy"], + max_yaw_vel=cfg["controller"]["max_yaw_rate"], + ) + safety = SafetyMonitor( + max_target_offset=cfg["safety"]["max_target_offset"], + max_ang_vel=cfg["safety"]["max_ang_vel"], + max_tilt_z=cfg["safety"]["max_tilt_z"], + clip_to_brake=cfg["safety"]["clip_to_brake"], + ) + safety.reset() + guard = RuntimeGuard( + max_ang_vel=cfg["safety"]["max_ang_vel"], + max_tilt_z=cfg["safety"]["max_tilt_z"], + imu_age_warn_ms=cfg["safety"].get("imu_age_warn_ms", 60.0), + imu_age_stop_ms=cfg["safety"].get("imu_age_stop_ms", 200.0), + ) + initializer = PoseInitializer( + io, + control_dt=control_dt, + transition_time_min=cfg["startup"].get("transition_time_min", 2.0), + transition_time_max=cfg["startup"].get("transition_time_max", 6.0), + transition_seconds_per_rad=cfg["startup"].get("transition_seconds_per_rad", 1.5), + hold_time=cfg["startup"]["hold_time"], + settle_pos_threshold=cfg["startup"]["settle_pos_threshold"], + settle_vel_threshold=cfg["startup"]["settle_vel_threshold"], + timeout_extra=cfg["startup"].get("timeout_extra", 3.0), + progress_log_interval=cfg["startup"]["progress_log_interval"], + ramp_kp_time=cfg["startup"].get("ramp_kp_time", 1.0), + soft_hold_duration=cfg["startup"].get("soft_hold_duration", 1.0), + max_dev_warn=cfg["startup"].get("max_dev_warn", 1.5), + max_dev_abort=cfg["startup"].get("max_dev_abort", 3.0), + ) + initializer.attach(logger=logger, guard=guard, keyboard=keyboard) + stand_balance = StandBalanceController(cfg.get("stand_balance", {}), control_dt=control_dt) + + print("\n[Main] connecting hardware...") + keyboard.start() + try: + io.connect() + logger.event("CAN_IMU_CONNECTED", initial_gravity=io.imu.initial_gravity) + except Exception as exc: + logger.event("HARDWARE_CONNECT_FAILED", error=str(exc)) + keyboard.stop() + logger.close() + raise + + try: + io.enable_motors() + logger.event("MOTORS_ENABLED") + time.sleep(0.5) + + target_pose = initializer.transition_to_stand_from_current(target_pose=STAND_POSE) if cfg["startup"]["enabled"] else STAND_POSE.copy() + + if stand_balance.enabled: + logger.event("STAND_BALANCE_BEGIN") + print("[Main] waiting for stand-balance to settle...") + stand_balance.reset() + next_exec = time.perf_counter() + while True: + state = io.read_state() + target_pose = stand_balance.compute_target(state, np.zeros(3, dtype=np.float32)) + io.hold_pose(target_pose, kp_scale=1.0) + debug = stand_balance.last_debug + if stand_balance.is_stable(): + logger.event( + "STAND_BALANCE_STABLE", + roll_deg=float(np.degrees(debug.roll)), + pitch_deg=float(np.degrees(debug.pitch)), + ) + break + next_exec += control_dt + next_exec = _sleep_to(next_exec) + logger.event("STAND_BALANCE_END") + + if cfg["startup"]["require_user_confirm"]: + print("[Main] standing complete. Press Enter to release policy control...") + done = threading.Event() + + def _wait(): + try: + input() + except EOFError: + pass + done.set() + + threading.Thread(target=_wait, daemon=True).start() + if not initializer.hold_until_user_confirm(target_pose, done): + raise PoseInitFailed("WAIT_USER interrupted") + + print("[Main] priming current observation...") + logger.event("PRIME_BEGIN") + zero_cmd = np.zeros(3, dtype=np.float32) + next_exec = time.perf_counter() + for index in range(1): + if stand_balance.enabled: + state = io.read_state() + target_pose = stand_balance.compute_target(state, zero_cmd) + io.hold_pose(target_pose, kp_scale=1.0) + else: + io.hold_pose(target_pose, kp_scale=1.0) + state = io.read_state() + obs = io.get_obs_policy(state, zero_cmd, runner.default_dof_pos, runner.last_actions) + if index == 0: + runner.reset(prime_obs=obs) + logger.state( + phase="PRIME", + joint_pos=state["joint_pos"], + joint_vel=state["joint_vel"], + joint_torque=state.get("joint_torque", np.zeros(16, dtype=np.float32)), + target_pose=target_pose, + raw_action=None, + gyro=state["imu_gyro"], + accel=state["imu_accel"], + quat=state["quat_wxyz"], + proj_gravity=state["projected_gravity"], + command=zero_cmd, + imu_age_ms=float(state["imu_age_ms"]), + loop_dt_ms=0.0, + kp_scale=1.0, + ) + next_exec += control_dt + next_exec = _sleep_to(next_exec) + logger.event("PRIME_END") + + print("[Main] entering 50Hz control loop... (space = estop)") + logger.event("RUNTIME_BEGIN") + next_exec = time.perf_counter() + loop_count = 0 + last_print = next_exec + log_every = int(cfg.get("log_every", 1)) + recent_dt_ms = [] + runtime_released = not require_active_command + release_cfg = policy_release_cfg(cfg) + release_active_time = 0.0 + + while True: + loop_t0 = time.perf_counter() + cmd = keyboard.get_command() + state = io.read_state() + obs = io.get_obs_policy(state, cmd, runner.default_dof_pos, runner.last_actions) + zero_command = runner._is_zero_command(cmd, state["imu_gyro"]) + + obs_nan = bool(np.any(np.isnan(obs)) or np.any(np.isinf(obs))) + if obs_nan: + logger.event("OBS_NAN", obs_max=float(np.nanmax(obs))) + io.damping_brake() + break + + if not runtime_released and zero_command: + raw = np.zeros(16, dtype=np.float32) + scaled = np.zeros(16, dtype=np.float32) + target_hold = stand_balance.compute_target(state, np.zeros(3, dtype=np.float32)) if stand_balance.enabled else runner.default_dof_pos.copy() + actual_target = io.hold_pose(target_hold, kp_scale=1.0) + policy_target = runner.default_dof_pos.copy() + release_metrics = compute_release_metrics(runner, state, target_hold, cmd) + target_metrics = compute_target_error_metrics(state, target_hold, policy_target) + release_active_time = 0.0 + safety_decision = SafetyMonitor().check( + target_pose=target_hold, + default_pose=runner.default_dof_pos, + imu_gyro=state["imu_gyro"], + projected_gravity=state["projected_gravity"], + estop_triggered=keyboard.is_estop_triggered(), + ) + guard_decision = guard.check( + imu_gyro=state["imu_gyro"], + projected_gravity=state["projected_gravity"], + imu_age_ms=float(state["imu_age_ms"]), + estop_triggered=keyboard.is_estop_triggered(), + extra_nan_arrays=(target_hold,), + ) + else: + target_hold = stand_balance.compute_target(state, np.zeros(3, dtype=np.float32)) if stand_balance.enabled else runner.default_dof_pos.copy() + release_metrics = compute_release_metrics(runner, state, target_hold, cmd) + if not runtime_released: + release_active_time += control_dt if runner.is_command_active(cmd) else 0.0 + active_ready = release_active_time >= release_cfg["command_hold_s"] + posture_ready = release_metrics["max_hold_err"] <= release_cfg["posture_max_err"] + if active_ready and posture_ready: + runtime_released = True + logger.event( + "RUNTIME_COMMAND_RELEASED", + cmd=cmd.tolist(), + active_hold_s=release_active_time, + max_hold_err=release_metrics["max_hold_err"], + max_default_err=release_metrics["max_default_err"], + max_hold_default_gap=release_metrics["max_hold_default_gap"], + ) + else: + reasons = [] + if not active_ready: + reasons.append(f"cmd_hold<{release_cfg['command_hold_s']:.2f}s") + if not posture_ready: + reasons.append(f"hold_err>{release_cfg['posture_max_err']:.3f}") + logger.event( + "RUNTIME_RELEASE_BLOCKED", + reason=",".join(reasons), + cmd=cmd.tolist(), + active_hold_s=release_active_time, + max_hold_err=release_metrics["max_hold_err"], + max_default_err=release_metrics["max_default_err"], + max_hold_default_gap=release_metrics["max_hold_default_gap"], + ) + raw = np.zeros(16, dtype=np.float32) + scaled = np.zeros(16, dtype=np.float32) + actual_target = io.hold_pose(target_hold, kp_scale=1.0) + policy_target = runner.default_dof_pos.copy() + target_metrics = compute_target_error_metrics(state, target_hold, policy_target) + safety_decision = SafetyMonitor().check( + target_pose=target_hold, + default_pose=runner.default_dof_pos, + imu_gyro=state["imu_gyro"], + projected_gravity=state["projected_gravity"], + estop_triggered=keyboard.is_estop_triggered(), + ) + guard_decision = guard.check( + imu_gyro=state["imu_gyro"], + projected_gravity=state["projected_gravity"], + imu_age_ms=float(state["imu_age_ms"]), + estop_triggered=keyboard.is_estop_triggered(), + extra_nan_arrays=(target_hold,), + ) + loop_dt_ms = (time.perf_counter() - loop_t0) * 1000.0 + if log_every and (loop_count % log_every == 0): + motor_diag = state.get("motor_stale", {}) + logger.state( + phase="RUNTIME", + joint_pos=state["joint_pos"], + joint_vel=state["joint_vel"], + joint_torque=state.get("joint_torque", np.zeros(16, dtype=np.float32)), + target_pose=actual_target, + raw_action=raw, + gyro=state["imu_gyro"], + accel=state["imu_accel"], + quat=state["quat_wxyz"], + proj_gravity=state["projected_gravity"], + command=cmd, + imu_age_ms=float(state["imu_age_ms"]), + loop_dt_ms=loop_dt_ms, + safety_level=int(safety_decision.level), + guard_level=int(guard_decision.level), + holdover=int(motor_diag.get("holdover_this_frame", 0)), + stale_max=int(motor_diag.get("stale_max", 0)), + fresh_count=int(motor_diag.get("fresh_count", 16)), + kp_scale=1.0, + nan_flag=0, + kp_leg_cmd=float(io.kp_leg), + kd_leg_cmd=float(io.kd_leg), + kd_wheel_cmd=float(io.kd_wheel), + runtime_release_alpha=0.0, + runtime_release_hold_s=release_active_time, + runtime_blend_ratio=0.0, + hold_target_max_err=target_metrics["hold_target_max_err"], + policy_target_max_err=target_metrics["policy_target_max_err"], + hold_policy_max_gap=target_metrics["hold_policy_max_gap"], + target_source="runtime_hold", + clip_primary_joint="", + safety_reason=f"release_blocked:{','.join(reasons)}", + guard_reason=guard_decision.reason, + ) + next_exec += control_dt + next_exec = _sleep_to(next_exec) + loop_count += 1 + continue + scaled, raw = runner.step(obs) + act_nan = bool(np.any(np.isnan(raw)) or np.any(np.isinf(raw))) + if act_nan: + logger.event("ACTION_NAN") + io.damping_brake() + break + + policy_target = (scaled + runner.default_dof_pos).astype(np.float32) + tentative = blend_runtime_target( + runner, + target_hold, + policy_target, + float(getattr(runner, "_command_release_alpha", 0.0)), + release_cfg["target_blend_s"], + control_dt, + ) + scaled = tentative - runner.default_dof_pos + target_metrics = compute_target_error_metrics(state, target_hold, policy_target) + runtime_blend_ratio = min( + 1.0, + float(getattr(runner, "_command_release_alpha", 0.0)) + * (runner.command_release_s / max(release_cfg["target_blend_s"], control_dt)), + ) + projected_gravity = get_gravity_orientation(state["quat_wxyz"]) + + guard_decision = guard.check( + imu_gyro=state["imu_gyro"], + projected_gravity=projected_gravity, + imu_age_ms=float(state["imu_age_ms"]), + estop_triggered=keyboard.is_estop_triggered(), + extra_nan_arrays=(raw, tentative), + ) + if guard_decision.level == GuardLevel.STOP: + logger.event("GUARD_STOP", phase="RUNTIME", reason=guard_decision.reason) + io.damping_brake() + break + + safety_decision = safety.check( + target_pose=tentative, + default_pose=runner.default_dof_pos, + imu_gyro=state["imu_gyro"], + projected_gravity=projected_gravity, + estop_triggered=keyboard.is_estop_triggered(), + ) + if safety_decision.level == SafetyLevel.ESTOP: + logger.event("SAFETY_ESTOP", reason=safety_decision.message) + io.damping_brake() + break + if safety_decision.level == SafetyLevel.BRAKE: + safety_diag = build_action_diag( + joint_pos=state["joint_pos"], + default_pose=runner.default_dof_pos, + raw=raw, + scaled=scaled, + tentative=tentative, + cmd=cmd, + zero_command=zero_command, + runtime_released=runtime_released, + release_alpha=float(getattr(runner, "_command_release_alpha", 0.0)), + safety_details=safety_decision.details, + ) + logger.event( + "SAFETY_BRAKE", + reason=safety_decision.message, + details=safety_diag, + primary_joint=safety_diag.get("primary_joint_name"), + primary_offset=safety_diag.get("primary_leg_offset"), + primary_target=safety_diag.get("primary_target"), + primary_measured=safety_diag.get("primary_measured"), + primary_raw=safety_diag.get("primary_raw"), + primary_scaled=safety_diag.get("primary_scaled"), + cmd=cmd.tolist(), + release_alpha=float(getattr(runner, "_command_release_alpha", 0.0)), + ) + io.damping_brake() + break + if safety_decision.level == SafetyLevel.CLIP and safety_decision.clipped_target is not None: + scaled = safety_decision.clipped_target - runner.default_dof_pos + safety_diag = build_action_diag( + joint_pos=state["joint_pos"], + default_pose=runner.default_dof_pos, + raw=raw, + scaled=scaled, + tentative=tentative, + cmd=cmd, + zero_command=zero_command, + runtime_released=runtime_released, + release_alpha=float(getattr(runner, "_command_release_alpha", 0.0)), + safety_details=safety_decision.details, + ) + logger.event( + "SAFETY_CLIP", + reason=safety_decision.message, + details=safety_diag, + primary_joint=safety_diag.get("primary_joint_name"), + primary_offset=safety_diag.get("primary_leg_offset"), + primary_target=safety_diag.get("primary_target"), + primary_measured=safety_diag.get("primary_measured"), + primary_raw=safety_diag.get("primary_raw"), + primary_scaled=safety_diag.get("primary_scaled"), + max_raw=float(np.max(np.abs(raw))), + cmd=cmd.tolist(), + release_alpha=float(getattr(runner, "_command_release_alpha", 0.0)), + ) + + actual_target = io.send_actions(scaled, runner.default_dof_pos) + loop_dt_ms = (time.perf_counter() - loop_t0) * 1000.0 + + if log_every and (loop_count % log_every == 0): + motor_diag = state.get("motor_stale", {}) + logger.state( + phase="RUNTIME", + joint_pos=state["joint_pos"], + joint_vel=state["joint_vel"], + joint_torque=state.get("joint_torque", np.zeros(16, dtype=np.float32)), + target_pose=actual_target, + raw_action=raw, + gyro=state["imu_gyro"], + accel=state["imu_accel"], + quat=state["quat_wxyz"], + proj_gravity=projected_gravity, + command=cmd, + imu_age_ms=float(state["imu_age_ms"]), + loop_dt_ms=loop_dt_ms, + safety_level=int(safety_decision.level), + guard_level=int(guard_decision.level), + holdover=int(motor_diag.get("holdover_this_frame", 0)), + stale_max=int(motor_diag.get("stale_max", 0)), + fresh_count=int(motor_diag.get("fresh_count", 16)), + kp_scale=1.0, + nan_flag=int(obs_nan or act_nan), + kp_leg_cmd=float(io.kp_leg), + kd_leg_cmd=float(io.kd_leg), + kd_wheel_cmd=float(io.kd_wheel), + runtime_release_alpha=float(getattr(runner, "_command_release_alpha", 0.0)), + runtime_release_hold_s=release_active_time, + runtime_blend_ratio=runtime_blend_ratio, + hold_target_max_err=target_metrics["hold_target_max_err"], + policy_target_max_err=target_metrics["policy_target_max_err"], + hold_policy_max_gap=target_metrics["hold_policy_max_gap"], + target_source="runtime_blend" if runtime_blend_ratio < 0.999 else "runtime_policy", + clip_primary_joint=str((safety_decision.details or {}).get("primary_joint_name", "")), + clip_primary_target=float((safety_decision.details or {}).get("primary_target", 0.0) or 0.0), + clip_primary_measured=float((safety_decision.details or {}).get("primary_measured", 0.0) or 0.0), + clip_primary_default=float((safety_decision.details or {}).get("primary_default", 0.0) or 0.0), + clip_primary_pos_err=float((safety_decision.details or {}).get("primary_pos_err", 0.0) or 0.0), + clip_primary_raw=float((safety_decision.details or {}).get("primary_raw", 0.0) or 0.0), + clip_primary_scaled=float((safety_decision.details or {}).get("primary_scaled", 0.0) or 0.0), + safety_reason=( + f"{safety_decision.message};zero_cmd={int(zero_command)};" + f"released={int(runtime_released)};alpha={getattr(runner, '_command_release_alpha', 0.0):.2f};" + f"max_raw={float(np.max(np.abs(raw))):.2f};" + f"clip={((safety_decision.details or {}).get('joint_indices', []))}" + ), + guard_reason=guard_decision.reason, + ) + + next_exec += control_dt + slack = next_exec - time.perf_counter() + if slack > 0: + coarse = slack - 0.002 + if coarse > 0: + time.sleep(coarse) + while time.perf_counter() < next_exec: + pass + elif slack < -control_dt: + logger.event("LOOP_OVERRUN", over_ms=-slack * 1000.0) + next_exec = time.perf_counter() + + recent_dt_ms.append(loop_dt_ms) + if len(recent_dt_ms) > 50: + recent_dt_ms.pop(0) + if len(recent_dt_ms) == 50: + median_dt = float(np.median(recent_dt_ms)) + if median_dt > 22.0: + logger.event("SLOW_LOOP_TREND", median_dt_ms=median_dt) + recent_dt_ms.clear() + + loop_count += 1 + if time.perf_counter() - last_print > 1.0: + print( + f"[Loop] cmd=[{cmd[0]:+.2f},{cmd[1]:+.2f},{cmd[2]:+.2f}] " + f"|raw|={float(np.max(np.abs(raw))):.2f} " + f"zero={int(zero_command)} rel={int(runtime_released)} " + f"alpha={getattr(runner, '_command_release_alpha', 0.0):.2f} " + f"imu_age={state['imu_age_ms']:.1f}ms " + f"holdover={io.hw.holdover_total} " + f"safety={int(safety_decision.level)}" + ) + last_print = time.perf_counter() + + except PoseInitFailed as exc: + print(f"[Main] startup aborted: {exc}") + logger.event("POSE_INIT_FAILED", error=str(exc)) + except KeyboardInterrupt: + print("\n[Main] Ctrl+C received, stopping...") + logger.event("KEYBOARD_INTERRUPT") + except Exception as exc: + import traceback + + print(f"\n[Main] exception: {exc}") + traceback.print_exc() + logger.event("UNEXPECTED_ERROR", error=str(exc), traceback=traceback.format_exc()) + finally: + print("[Main] cleaning up...") + try: + io.damping_brake() + time.sleep(0.05) + logger.event("DAMPING_BRAKE_APPLIED") + except Exception as exc: + logger.event("DAMPING_BRAKE_FAILED", error=str(exc)) + try: + io.disconnect() + logger.event("HARDWARE_DISCONNECTED") + finally: + keyboard.stop() + logger.close() + os._exit(0) + + +if __name__ == "__main__": + main() diff --git a/05_software/real/sim2real/mjcf/meshes/base_link.STL b/05_software/real/sim2real/mjcf/meshes/base_link.STL new file mode 100644 index 0000000..015035f Binary files /dev/null and b/05_software/real/sim2real/mjcf/meshes/base_link.STL differ diff --git a/05_software/real/sim2real/mjcf/meshes/fl_hip_abduction_Link.STL b/05_software/real/sim2real/mjcf/meshes/fl_hip_abduction_Link.STL new file mode 100644 index 0000000..971a78e Binary files /dev/null and b/05_software/real/sim2real/mjcf/meshes/fl_hip_abduction_Link.STL differ diff --git a/05_software/real/sim2real/mjcf/meshes/fl_hip_pitch_Link.STL b/05_software/real/sim2real/mjcf/meshes/fl_hip_pitch_Link.STL new file mode 100644 index 0000000..e1803e0 Binary files /dev/null and b/05_software/real/sim2real/mjcf/meshes/fl_hip_pitch_Link.STL differ diff --git a/05_software/real/sim2real/mjcf/meshes/fl_knee_Link.STL b/05_software/real/sim2real/mjcf/meshes/fl_knee_Link.STL new file mode 100644 index 0000000..cc5f40c Binary files /dev/null and b/05_software/real/sim2real/mjcf/meshes/fl_knee_Link.STL differ diff --git a/05_software/real/sim2real/mjcf/meshes/fl_wheel_Link.STL b/05_software/real/sim2real/mjcf/meshes/fl_wheel_Link.STL new file mode 100644 index 0000000..c3e124b Binary files /dev/null and b/05_software/real/sim2real/mjcf/meshes/fl_wheel_Link.STL differ diff --git a/05_software/real/sim2real/mjcf/meshes/fr_hip_abduction_Link.STL b/05_software/real/sim2real/mjcf/meshes/fr_hip_abduction_Link.STL new file mode 100644 index 0000000..8a15920 Binary files /dev/null and b/05_software/real/sim2real/mjcf/meshes/fr_hip_abduction_Link.STL differ diff --git a/05_software/real/sim2real/mjcf/meshes/fr_hip_pitch_Link.STL b/05_software/real/sim2real/mjcf/meshes/fr_hip_pitch_Link.STL new file mode 100644 index 0000000..909e5ae Binary files /dev/null and b/05_software/real/sim2real/mjcf/meshes/fr_hip_pitch_Link.STL differ diff --git a/05_software/real/sim2real/mjcf/meshes/fr_knee_Link.STL b/05_software/real/sim2real/mjcf/meshes/fr_knee_Link.STL new file mode 100644 index 0000000..802f2eb Binary files /dev/null and b/05_software/real/sim2real/mjcf/meshes/fr_knee_Link.STL differ diff --git a/05_software/real/sim2real/mjcf/meshes/fr_wheel_Link.STL b/05_software/real/sim2real/mjcf/meshes/fr_wheel_Link.STL new file mode 100644 index 0000000..9c17db1 Binary files /dev/null and b/05_software/real/sim2real/mjcf/meshes/fr_wheel_Link.STL differ diff --git a/05_software/real/sim2real/mjcf/meshes/rl_hip_abduction_Link.STL b/05_software/real/sim2real/mjcf/meshes/rl_hip_abduction_Link.STL new file mode 100644 index 0000000..0680c00 Binary files /dev/null and b/05_software/real/sim2real/mjcf/meshes/rl_hip_abduction_Link.STL differ diff --git a/05_software/real/sim2real/mjcf/meshes/rl_hip_pitch_Link.STL b/05_software/real/sim2real/mjcf/meshes/rl_hip_pitch_Link.STL new file mode 100644 index 0000000..ab8ffa7 Binary files /dev/null and b/05_software/real/sim2real/mjcf/meshes/rl_hip_pitch_Link.STL differ diff --git a/05_software/real/sim2real/mjcf/meshes/rl_knee_Link.STL b/05_software/real/sim2real/mjcf/meshes/rl_knee_Link.STL new file mode 100644 index 0000000..5d64c75 Binary files /dev/null and b/05_software/real/sim2real/mjcf/meshes/rl_knee_Link.STL differ diff --git a/05_software/real/sim2real/mjcf/meshes/rl_wheel_Link.STL b/05_software/real/sim2real/mjcf/meshes/rl_wheel_Link.STL new file mode 100644 index 0000000..5bab538 Binary files /dev/null and b/05_software/real/sim2real/mjcf/meshes/rl_wheel_Link.STL differ diff --git a/05_software/real/sim2real/mjcf/meshes/rr_hip_abduction_Link.STL b/05_software/real/sim2real/mjcf/meshes/rr_hip_abduction_Link.STL new file mode 100644 index 0000000..4013ccf Binary files /dev/null and b/05_software/real/sim2real/mjcf/meshes/rr_hip_abduction_Link.STL differ diff --git a/05_software/real/sim2real/mjcf/meshes/rr_hip_pitch_Link.STL b/05_software/real/sim2real/mjcf/meshes/rr_hip_pitch_Link.STL new file mode 100644 index 0000000..5f81a5f Binary files /dev/null and b/05_software/real/sim2real/mjcf/meshes/rr_hip_pitch_Link.STL differ diff --git a/05_software/real/sim2real/mjcf/meshes/rr_knee_Link.STL b/05_software/real/sim2real/mjcf/meshes/rr_knee_Link.STL new file mode 100644 index 0000000..28dccab Binary files /dev/null and b/05_software/real/sim2real/mjcf/meshes/rr_knee_Link.STL differ diff --git a/05_software/real/sim2real/mjcf/meshes/rr_wheel_Link.STL b/05_software/real/sim2real/mjcf/meshes/rr_wheel_Link.STL new file mode 100644 index 0000000..a9d9ecb Binary files /dev/null and b/05_software/real/sim2real/mjcf/meshes/rr_wheel_Link.STL differ diff --git a/05_software/real/sim2real/mjcf/scene.xml b/05_software/real/sim2real/mjcf/scene.xml new file mode 100644 index 0000000..155ffb2 --- /dev/null +++ b/05_software/real/sim2real/mjcf/scene.xml @@ -0,0 +1,22 @@ + + + + diff --git a/05_software/real/sim2real/mjcf/sim2sim_temp.xml b/05_software/real/sim2real/mjcf/sim2sim_temp.xml new file mode 100644 index 0000000..80306ee --- /dev/null +++ b/05_software/real/sim2real/mjcf/sim2sim_temp.xml @@ -0,0 +1,327 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/05_software/real/sim2real/mjcf/wheelleg.xml b/05_software/real/sim2real/mjcf/wheelleg.xml new file mode 100644 index 0000000..960a3cb --- /dev/null +++ b/05_software/real/sim2real/mjcf/wheelleg.xml @@ -0,0 +1,157 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/05_software/real/sim2real/policies/model_rough.pt b/05_software/real/sim2real/policies/model_rough.pt new file mode 100644 index 0000000..f0be4c3 Binary files /dev/null and b/05_software/real/sim2real/policies/model_rough.pt differ diff --git a/05_software/real/sim2real/policy/__init__.py b/05_software/real/sim2real/policy/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/05_software/real/sim2real/policy/policy_runner.py b/05_software/real/sim2real/policy/policy_runner.py new file mode 100644 index 0000000..58960f3 --- /dev/null +++ b/05_software/real/sim2real/policy/policy_runner.py @@ -0,0 +1,176 @@ +from pathlib import Path + +import numpy as np +import torch +import torch.nn as nn + + +class PolicyMLP(nn.Module): + def __init__(self, obs_dim: int, action_dim: int): + super().__init__() + self.register_buffer("obs_mean", torch.zeros(obs_dim)) + self.register_buffer("obs_std", torch.ones(obs_dim)) + self.net = nn.Sequential( + nn.Linear(obs_dim, 512), + nn.ELU(), + nn.Linear(512, 256), + nn.ELU(), + nn.Linear(256, 128), + nn.ELU(), + nn.Linear(128, action_dim), + ) + + def forward(self, x: torch.Tensor) -> torch.Tensor: + x = (x - self.obs_mean) / torch.clamp(self.obs_std, min=1e-6) + return self.net(x) + + +def load_policy(model_path: Path, device: torch.device) -> PolicyMLP: + checkpoint = torch.load(model_path, map_location=device, weights_only=False) + state_dict = checkpoint["actor_state_dict"] + + input_key = "mlp.0.weight" if "mlp.0.weight" in state_dict else "net.0.weight" + output_key = "mlp.6.weight" if "mlp.6.weight" in state_dict else "net.6.weight" + obs_dim = int(state_dict[input_key].shape[1]) + action_dim = int(state_dict[output_key].shape[0]) + + model = PolicyMLP(obs_dim=obs_dim, action_dim=action_dim) + remapped_state_dict: dict[str, torch.Tensor] = {} + for key, value in state_dict.items(): + if key.startswith("mlp."): + remapped_state_dict[key.replace("mlp.", "net.")] = value + elif key.startswith("net."): + remapped_state_dict[key] = value + elif key == "obs_normalizer._mean": + remapped_state_dict["obs_mean"] = value.squeeze() + elif key == "obs_normalizer._var": + remapped_state_dict["obs_std"] = torch.sqrt(value.squeeze() + 1e-5) + + model.load_state_dict(remapped_state_dict, strict=False) + model.eval() + model.to(device) + model.expected_obs_dim = obs_dim + model.expected_action_dim = action_dim + return model + + +class PolicyRunner: + BASE_OBS_DIM = 53 + DEFAULT_STAND_POSE = np.array( + [ + 0.0, 0.9, -1.8, + 0.0, 0.9, -1.8, + 0.0, 0.9, -1.8, + 0.0, 0.9, -1.8, + 0.0, 0.0, 0.0, 0.0, + ], + dtype=np.float32, + ) + + def __init__( + self, + policy_path: Path, + device: torch.device | None = None, + enable_zero_cmd_suppression: bool = True, + hold_zero_command_pose: bool = True, + command_release_s: float = 0.35, + action_scale: np.ndarray | None = None, + zero_cmd_use_yaw_rate: bool = True, + ): + self.device = device or torch.device("cuda" if torch.cuda.is_available() else "cpu") + self.policy_path = Path(policy_path) + self.enable_zero_cmd_suppression = bool(enable_zero_cmd_suppression) + self.hold_zero_command_pose = bool(hold_zero_command_pose) + self.command_release_s = max(float(command_release_s), 1e-3) + print(f"[PolicyRunner] device={self.device}, policy={self.policy_path}") + self.policy = load_policy(self.policy_path, self.device) + if self.policy.expected_obs_dim != self.BASE_OBS_DIM: + raise ValueError( + f"Unsupported policy obs dim {self.policy.expected_obs_dim}. " + f"Current sim2real only supports {self.BASE_OBS_DIM}-D actor observations." + ) + + self.default_dof_pos = self.DEFAULT_STAND_POSE.copy() + self.last_actions = np.zeros(16, dtype=np.float32) + + self.action_scale = np.asarray( + action_scale + if action_scale is not None + else [ + 0.125, 0.25, 0.25, + 0.125, 0.25, 0.25, + 0.125, 0.25, 0.25, + 0.125, 0.25, 0.25, + 5.0, 5.0, 5.0, 5.0, + ], + dtype=np.float32, + ) + if self.action_scale.shape != (16,): + raise ValueError(f"action_scale must be shape (16,), got {self.action_scale.shape}") + + self.zero_cmd_lin_thresh = 0.05 + self.zero_cmd_yaw_thresh = 0.05 + self.zero_yaw_rate_thresh = 0.10 + self.zero_cmd_use_yaw_rate = bool(zero_cmd_use_yaw_rate) + self._command_release_alpha = 0.0 + print( + f"[PolicyRunner] obs_dim={self.policy.expected_obs_dim}, " + f"base_obs_dim={self.BASE_OBS_DIM}, history=1, " + f"action_dim={self.policy.expected_action_dim}, " + f"zero_cmd_suppression={self.enable_zero_cmd_suppression}, " + f"hold_zero_command_pose={self.hold_zero_command_pose}" + ) + + def reset(self, prime_obs: np.ndarray | None = None) -> None: + self.last_actions = np.zeros(16, dtype=np.float32) + self._command_release_alpha = 0.0 + + def _is_zero_command(self, command: np.ndarray, base_ang_vel: np.ndarray) -> bool: + cmd_is_zero = ( + np.linalg.norm(command[:2]) < self.zero_cmd_lin_thresh + and abs(command[2]) < self.zero_cmd_yaw_thresh + ) + if not self.zero_cmd_use_yaw_rate: + return cmd_is_zero + return cmd_is_zero and abs(base_ang_vel[2]) < self.zero_yaw_rate_thresh + + def command_activation_metrics(self, command: np.ndarray) -> tuple[float, float]: + command = np.asarray(command, dtype=np.float32) + planar = float(np.linalg.norm(command[:2])) + yaw = float(abs(command[2])) + return planar, yaw + + def is_command_active(self, command: np.ndarray) -> bool: + planar, yaw = self.command_activation_metrics(command) + return planar >= self.zero_cmd_lin_thresh or yaw >= self.zero_cmd_yaw_thresh + + def step(self, obs: np.ndarray) -> tuple[np.ndarray, np.ndarray]: + obs = np.asarray(obs, dtype=np.float32) + expected_obs_dim = int(self.policy.expected_obs_dim) + if obs.shape[0] != expected_obs_dim: + raise ValueError( + f"Observation dim mismatch: got {obs.shape[0]}, expected {expected_obs_dim}." + ) + + obs_tensor = torch.tensor(obs, dtype=torch.float32, device=self.device).unsqueeze(0) + with torch.no_grad(): + raw_actions = self.policy(obs_tensor).squeeze(0).cpu().numpy() + + raw_actions = np.clip(raw_actions, -10.0, 10.0).astype(np.float32) + command = obs[6:9] + base_ang_vel = obs[0:3] / 0.25 + zero_command = self._is_zero_command(command, base_ang_vel) + if zero_command: + self._command_release_alpha = 0.0 + if self.hold_zero_command_pose: + raw_actions[:] = 0.0 + elif self.enable_zero_cmd_suppression: + raw_actions[12:16] = 0.0 + raw_actions[:12] *= 0.5 + else: + self._command_release_alpha = min(1.0, self._command_release_alpha + 0.02 / self.command_release_s) + raw_actions *= self._command_release_alpha + + self.last_actions = raw_actions.copy() + scaled_actions = raw_actions * self.action_scale + return scaled_actions, raw_actions diff --git a/05_software/real/sim2real/requirements-orin.txt b/05_software/real/sim2real/requirements-orin.txt new file mode 100644 index 0000000..d65726b --- /dev/null +++ b/05_software/real/sim2real/requirements-orin.txt @@ -0,0 +1,7 @@ +numpy +PyYAML +torch +pyserial + +# Optional: +# pynput # only needed for CLI keyboard control diff --git a/05_software/real/sim2real/safety/__init__.py b/05_software/real/sim2real/safety/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/05_software/real/sim2real/safety/runtime_guard.py b/05_software/real/sim2real/safety/runtime_guard.py new file mode 100644 index 0000000..5ffecd1 --- /dev/null +++ b/05_software/real/sim2real/safety/runtime_guard.py @@ -0,0 +1,78 @@ +"""通用运行期守护：每个控制周期调用一次，无副作用，只做检查。 + +设计原则： +- 守护函数本身不下发动作、不打印（除非 verbose），只返回判定 +- 调用方决定收到 GuardStop 时怎么办（damping_brake 或 raise） +- 起立期 / 等待期 / 主循环都共用同一组检查 +""" +from dataclasses import dataclass +from enum import IntEnum +from typing import Optional + +import numpy as np + + +class GuardLevel(IntEnum): + OK = 0 + WARN = 1 # 仅记录，不停 + STOP = 2 # 主调方应立刻 damping_brake + 退出当前阶段 + + +@dataclass +class GuardDecision: + level: GuardLevel + reason: str # 触发时人类可读说明，OK 时为空 + + +class RuntimeGuard: + """启动/起立/主循环共用的安全守护。 + + 不监控目标位置范围（那是 SafetyMonitor 的职责）。这里只关心 + 机身整体状态：是否倾倒、是否翻滚、是否检测到 NaN、用户是否按急停。 + """ + + def __init__(self, + max_ang_vel: float = 12.0, + max_tilt_z: float = -0.30, + imu_age_warn_ms: float = 60.0, + imu_age_stop_ms: float = 200.0): + self.max_ang_vel = max_ang_vel + self.max_tilt_z = max_tilt_z + self.imu_age_warn_ms = imu_age_warn_ms + self.imu_age_stop_ms = imu_age_stop_ms + + def check(self, + imu_gyro: np.ndarray, + projected_gravity: np.ndarray, + imu_age_ms: float, + estop_triggered: bool, + extra_nan_arrays: tuple = ()) -> GuardDecision: + # 1) 用户急停 + if estop_triggered: + return GuardDecision(GuardLevel.STOP, "user E-stop") + + # 2) NaN 检查（任意输入数组中出现 NaN） + for arr in (imu_gyro, projected_gravity, *extra_nan_arrays): + if arr is None: + continue + if np.any(np.isnan(arr)) or np.any(np.isinf(arr)): + return GuardDecision(GuardLevel.STOP, "NaN/Inf detected in observation/action") + + # 3) IMU 数据陈旧 + if imu_age_ms > self.imu_age_stop_ms: + return GuardDecision(GuardLevel.STOP, f"IMU stale {imu_age_ms:.0f}ms") + warned_imu = imu_age_ms > self.imu_age_warn_ms + + # 4) 倾倒 + if projected_gravity[2] > self.max_tilt_z: + return GuardDecision(GuardLevel.STOP, + f"tilt: g_z={projected_gravity[2]:.3f}") + + # 5) 角速度爆表 + ang_norm = float(np.linalg.norm(imu_gyro)) + if ang_norm > self.max_ang_vel: + return GuardDecision(GuardLevel.STOP, f"ang_vel overflow: |w|={ang_norm:.2f}") + + if warned_imu: + return GuardDecision(GuardLevel.WARN, f"IMU age {imu_age_ms:.0f}ms") + return GuardDecision(GuardLevel.OK, "") diff --git a/05_software/real/sim2real/safety/safety_monitor.py b/05_software/real/sim2real/safety/safety_monitor.py new file mode 100644 index 0000000..0b2ad72 --- /dev/null +++ b/05_software/real/sim2real/safety/safety_monitor.py @@ -0,0 +1,107 @@ +"""三级安全监控（对应方法论 97.11）。 + +Level 0: 正常 +Level 1: 限幅（位置/速度异常）— 截断目标位置幅值，记录连续触发次数 +Level 2: 刹车（连续限幅 N 次 / IMU 角速度过大 / 倾倒）— 卸载刚度只留阻尼 +Level 3: 急停（用户触发）— 让上层断电 + +设计原则：监控只判定，不直接关电机；返回 SafetyDecision 由上层决策。 +""" +from dataclasses import dataclass +from enum import IntEnum +from typing import Any, Optional + +import numpy as np + + +class SafetyLevel(IntEnum): + NORMAL = 0 + CLIP = 1 + BRAKE = 2 + ESTOP = 3 + + +@dataclass +class SafetyDecision: + level: SafetyLevel + message: str + clipped_target: Optional[np.ndarray] + details: Optional[dict[str, Any]] = None + + +class SafetyMonitor: + """安全监控（按 50Hz 控制频率调用）。 + + Args: + max_target_offset: 单关节相对默认位姿的最大偏离 (rad) + max_ang_vel: IMU 角速度模 (rad/s) + max_tilt_rad: 机身重力 z 轴投影低于该值认为已严重倾倒 + clip_to_brake: 连续 clip 多少帧升级为刹车 + """ + + def __init__(self, + max_target_offset: float = 0.6, + max_ang_vel: float = 10.0, + max_tilt_z: float = -0.3, + clip_to_brake: int = 3): + self.max_target_offset = max_target_offset + self.max_ang_vel = max_ang_vel + self.max_tilt_z = max_tilt_z # projected_gravity z 应当 ~ -1，明显小于 -0.3 视作倾倒 + self.clip_to_brake = clip_to_brake + self.consecutive_clips = 0 + + def check(self, + target_pose: np.ndarray, + default_pose: np.ndarray, + imu_gyro: np.ndarray, + projected_gravity: np.ndarray, + estop_triggered: bool) -> SafetyDecision: + if estop_triggered: + return SafetyDecision(SafetyLevel.ESTOP, "user E-stop", None, None) + + # 倾倒（projected_gravity[2] 应在 -1 附近，越接近 0 越倾斜） + if projected_gravity[2] > self.max_tilt_z: + return SafetyDecision( + SafetyLevel.BRAKE, + f"tilt detected: g_z={projected_gravity[2]:.3f}", + None, + {"g_z": float(projected_gravity[2])}, + ) + + # 角速度爆表（猛烈翻滚） + if np.linalg.norm(imu_gyro) > self.max_ang_vel: + return SafetyDecision( + SafetyLevel.BRAKE, + f"angular velocity overflow: |w|={np.linalg.norm(imu_gyro):.2f}", + None, + {"ang_vel_norm": float(np.linalg.norm(imu_gyro))}, + ) + + # 目标位置偏离过大 → 截断到允许范围 + offset_leg = target_pose[:12] - default_pose[:12] + clipped_offset = np.clip(offset_leg, -self.max_target_offset, self.max_target_offset) + if not np.allclose(offset_leg, clipped_offset): + self.consecutive_clips += 1 + clipped = target_pose.copy() + clipped[:12] = default_pose[:12] + clipped_offset + exceeded = np.where(np.abs(offset_leg) > self.max_target_offset)[0].tolist() + max_offset = float(np.max(np.abs(offset_leg))) + details = { + "joint_indices": exceeded, + "max_leg_offset": max_offset, + "consecutive_clips": int(self.consecutive_clips), + } + if self.consecutive_clips >= self.clip_to_brake: + return SafetyDecision( + SafetyLevel.BRAKE, + f"clipped {self.consecutive_clips} frames in a row", + clipped, + details, + ) + return SafetyDecision(SafetyLevel.CLIP, "target leg offset out of range", clipped, details) + + self.consecutive_clips = 0 + return SafetyDecision(SafetyLevel.NORMAL, "", None, None) + + def reset(self): + self.consecutive_clips = 0 diff --git a/05_software/real/sim2real/startup/__init__.py b/05_software/real/sim2real/startup/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/05_software/real/sim2real/startup/pose_initializer.py b/05_software/real/sim2real/startup/pose_initializer.py new file mode 100644 index 0000000..7e70b4c --- /dev/null +++ b/05_software/real/sim2real/startup/pose_initializer.py @@ -0,0 +1,329 @@ +"""起立姿态初始化器（实测起点版本）。 + +设计： + - 不再假设机器人的物理起始姿态（不再有 CRAWL_POSE / GROUND_POSE 起点） + - enable 后从 io.read_measured_pose() 读 16 关节实测，直接作为插值起点 + - 余弦插值到 STAND_POSE，transition_time 根据最大偏差自适应 + - 全程 RuntimeGuard 守护（空格急停/倾倒/翻滚/NaN/IMU 陈旧） + - 50Hz 写 LogBundle CSV（phase 字段标识阶段） + +Phase 流程： + STARTUP_SOFT_HOLD — 软起步保持实测姿态，kp 从 0.125 渐升到 1.0 + STARTUP_TRANSITION — 实测起点 → STAND 余弦插值 + STARTUP_HOLD_AFTER — 站稳后保持 1 秒 +""" +import time +from typing import Optional + +import numpy as np + +from safety.runtime_guard import GuardLevel, RuntimeGuard +from tools.logger import LogBundle +from tools.math_utils import get_gravity_orientation + + +# 仅作为目标姿态使用（训练侧 default_dof_pos） +STAND_POSE = np.array([ + 0.0, 0.9, -1.8, + 0.0, 0.9, -1.8, + 0.0, 0.9, -1.8, + 0.0, 0.9, -1.8, + 0.0, 0.0, 0.0, 0.0, +], dtype=np.float32) + + +class PoseInitFailed(RuntimeError): + """起立流程触发安全停止。main.py 捕获后立即 damping_brake。""" + + +class PoseInitializer: + def __init__(self, real_io, control_dt: float = 0.02, + transition_time_min: float = 2.0, + transition_time_max: float = 6.0, + transition_seconds_per_rad: float = 1.5, + hold_time: float = 1.0, + settle_pos_threshold: float = 0.12, + settle_vel_threshold: float = 0.6, + timeout_extra: float = 3.0, + progress_log_interval: float = 0.5, + ramp_kp_time: float = 1.0, + soft_hold_duration: float = 1.0, + max_dev_warn: float = 1.5, + max_dev_abort: float = 3.0): + """ + Args: + transition_time_min/max/_per_rad: 自适应公式 + t = clip(min, max, max_dev * seconds_per_rad) + timeout_extra: 起立超时 = transition_time + timeout_extra + soft_hold_duration: 起立前先在实测姿态保持几秒，期间 kp ramp-up + max_dev_warn: 最大偏差超过此值打警告（仅日志） + max_dev_abort: 最大偏差超过此值直接 PoseInitFailed（拒绝起立） + """ + self.io = real_io + self.control_dt = control_dt + self.transition_time_min = transition_time_min + self.transition_time_max = transition_time_max + self.transition_seconds_per_rad = transition_seconds_per_rad + self.hold_time = hold_time + self.settle_pos_threshold = settle_pos_threshold + self.settle_vel_threshold = settle_vel_threshold + self.timeout_extra = timeout_extra + self.progress_log_interval = progress_log_interval + self.ramp_kp_time = ramp_kp_time + self.soft_hold_duration = soft_hold_duration + self.max_dev_warn = max_dev_warn + self.max_dev_abort = max_dev_abort + + self.logger: Optional[LogBundle] = None + self.guard: Optional[RuntimeGuard] = None + self.keyboard = None + + def attach(self, logger: LogBundle, guard: RuntimeGuard, keyboard): + self.logger = logger + self.guard = guard + self.keyboard = keyboard + + # ---- 通用每周期工作 ---- + def _tick(self, phase: str, sim_target: np.ndarray, kp_scale: float, next_exec: float): + """读状态 → guard 检查 → 写日志 → 锁帧。返回 (state_dict, next_exec)。 + 若 guard.STOP，立即抛 PoseInitFailed。""" + loop_t0 = time.perf_counter() + + state = self.io.read_state() + proj_g = get_gravity_orientation(state["quat_wxyz"]) + + guard_dec = None + if self.guard is not None: + estop = bool(self.keyboard and self.keyboard.is_estop_triggered()) + guard_dec = self.guard.check( + imu_gyro=state["imu_gyro"], + projected_gravity=proj_g, + imu_age_ms=float(state["imu_age_ms"]), + estop_triggered=estop, + extra_nan_arrays=(sim_target, state["joint_pos"], state["joint_vel"]), + ) + + if self.logger is not None: + motor_diag = state.get("motor_stale", {}) + self.logger.state( + phase=phase, + joint_pos=state["joint_pos"], + joint_vel=state["joint_vel"], + joint_torque=state.get("joint_torque", np.zeros(16, dtype=np.float32)), + target_pose=sim_target, + raw_action=None, + gyro=state["imu_gyro"], + accel=state["imu_accel"], + quat=state["quat_wxyz"], + proj_gravity=proj_g, + command=np.zeros(3, dtype=np.float32), + imu_age_ms=float(state["imu_age_ms"]), + loop_dt_ms=(time.perf_counter() - loop_t0) * 1000.0, + safety_level=0, + guard_level=int(guard_dec.level) if guard_dec else 0, + holdover=int(motor_diag.get("holdover_this_frame", 0)), + stale_max=int(motor_diag.get("stale_max", 0)), + fresh_count=int(motor_diag.get("fresh_count", 16)), + kp_scale=kp_scale, + nan_flag=int(np.any(np.isnan(state["joint_pos"]))), + kp_leg_cmd=float(self.io.kp_leg * kp_scale), + kd_leg_cmd=float(self.io.kd_leg), + kd_wheel_cmd=float(self.io.kd_wheel), + target_source="startup_hold", + guard_reason=guard_dec.reason if guard_dec else "", + ) + + if guard_dec is not None and guard_dec.level == GuardLevel.STOP: + if self.logger: + self.logger.event("GUARD_STOP", phase=phase, reason=guard_dec.reason) + raise PoseInitFailed(f"[{phase}] {guard_dec.reason}") + + next_exec += self.control_dt + slack = next_exec - time.perf_counter() + if slack > 0: + coarse = slack - 0.002 + if coarse > 0: + time.sleep(coarse) + while time.perf_counter() < next_exec: + pass + else: + next_exec = time.perf_counter() + return state, next_exec + + # ---- 主入口：从实测姿态起立到 STAND ---- + def transition_to_stand_from_current(self, + target_pose: Optional[np.ndarray] = None + ) -> np.ndarray: + """完整起立流程： + 1. 读实测起点 + 2. 偏差检查（warn / abort） + 3. SOFT_HOLD：保持实测姿态 + kp ramp-up + 4. TRANSITION：余弦插值到 target，transition_time 自适应 + 5. HOLD_AFTER：保持 1 秒 + 返回最终 target_pose（供主循环使用）。 + """ + if target_pose is None: + target_pose = STAND_POSE.copy() + target_pose = target_pose.astype(np.float32).copy() + target_pose[12:] = 0.0 + + # === 1. 读实测起点（要求电机反馈完整）=== + ok, missing = self.io.wait_feedback_ready(max_attempts=20, poll_interval=0.05) + if not ok: + msg = f"feedback incomplete: {len(missing)} motors no response: {missing[:4]}" + if self.logger: + self.logger.event("STARTUP_NO_FEEDBACK", + missing=[m[2] for m in missing]) + raise PoseInitFailed(msg) + + start_pose = self.io.read_measured_pose().astype(np.float32).copy() + start_pose[12:] = 0.0 # 轮子起点固定为 0 速度 + + # === 2. 偏差检查 === + diff = np.abs(start_pose[:12] - target_pose[:12]) + max_dev = float(np.max(diff)) + max_dev_joint = int(np.argmax(diff)) + transition_time = float(np.clip( + max_dev * self.transition_seconds_per_rad, + self.transition_time_min, self.transition_time_max + )) + timeout = transition_time + self.timeout_extra + + if self.logger: + self.logger.event( + "STARTUP_PLAN", + start_pose_leg=start_pose[:12].tolist(), + target_pose_leg=target_pose[:12].tolist(), + max_dev=max_dev, + max_dev_joint_idx=max_dev_joint, + transition_time=transition_time, + timeout=timeout, + ) + print(f"[PoseInit] 实测起点最大偏差 {max_dev:.3f} rad (关节 idx={max_dev_joint}); " + f"transition_time={transition_time:.2f}s") + + if max_dev > self.max_dev_abort: + raise PoseInitFailed( + f"实测起点偏差过大 ({max_dev:.2f} rad > abort 阈值 " + f"{self.max_dev_abort})；请检查电机是否在合理姿势" + ) + if max_dev > self.max_dev_warn: + print(f"[PoseInit] WARNING 偏差 {max_dev:.2f} rad > {self.max_dev_warn}; " + f"起立可能比较剧烈") + if self.logger: + self.logger.event("STARTUP_LARGE_DEV", max_dev=max_dev) + + # === 3. SOFT_HOLD：实测姿态 + kp ramp-up === + if self.logger: + self.logger.event("STARTUP_SOFT_HOLD_BEGIN", + duration=self.soft_hold_duration, + ramp_kp_time=self.ramp_kp_time, + ramp_kp_min=0.125) + n = max(1, int(self.soft_hold_duration / max(self.control_dt, 1e-3))) + next_exec = time.perf_counter() + t0 = next_exec + ramp_min = 0.125 + for i in range(n): + elapsed = time.perf_counter() - t0 + if elapsed < self.ramp_kp_time: + kp_scale = ramp_min + (1.0 - ramp_min) * (elapsed / self.ramp_kp_time) + else: + kp_scale = 1.0 + self.io.hold_pose(start_pose, kp_scale=kp_scale) + _s, next_exec = self._tick("STARTUP_SOFT_HOLD", start_pose, kp_scale, next_exec) + if self.logger: + self.logger.event("STARTUP_SOFT_HOLD_END") + + # === 4. TRANSITION：余弦插值 === + if self.logger: + self.logger.event("STARTUP_TRANSITION_BEGIN", + transition_time=transition_time, timeout=timeout) + print(f"[PoseInit] 起立: transition={transition_time:.2f}s, " + f"hold={self.hold_time}s, timeout={timeout:.2f}s") + + t0 = time.perf_counter() + last_log = t0 + reached = False + hold_start: Optional[float] = None + next_exec = t0 + + while True: + now = time.perf_counter() + elapsed = now - t0 + phase = min(1.0, elapsed / max(transition_time, 1e-3)) + + if elapsed > timeout: + if self.logger: + self.logger.event("STARTUP_TIMEOUT", elapsed=elapsed) + raise PoseInitFailed( + f"transition timeout after {elapsed:.2f}s, target not reached" + ) + + blend = 0.5 - 0.5 * np.cos(np.pi * phase) + blended = start_pose.astype(np.float32).copy() + blended[:12] = start_pose[:12] + blend * (target_pose[:12] - start_pose[:12]) + blended[12:] = 0.0 + self.io.hold_pose(blended, kp_scale=1.0) + state, next_exec = self._tick("STARTUP_TRANSITION", blended, 1.0, next_exec) + + joint_pos = state["joint_pos"] + joint_vel = state["joint_vel"] + pos_err = float(np.max(np.abs(joint_pos[:12] - target_pose[:12]))) + vel_err = float(np.max(np.abs(joint_vel[:12]))) + + if now - last_log >= self.progress_log_interval: + msg = (f"[PoseInit] phase={phase*100:5.1f}% | " + f"max_pos_err={pos_err:.3f} | max_vel={vel_err:.3f}") + print(msg) + if self.logger: + self.logger.event("STARTUP_PROGRESS", + phase=phase, pos_err=pos_err, vel_err=vel_err) + last_log = now + + if (phase >= 1.0 + and pos_err <= self.settle_pos_threshold + and vel_err <= self.settle_vel_threshold): + if not reached: + reached = True + hold_start = now + if self.logger: + self.logger.event("STARTUP_REACHED", + pos_err=pos_err, vel_err=vel_err) + print(f"[PoseInit] 已到位，保持 {self.hold_time:.2f}s") + elif hold_start is not None and now - hold_start >= self.hold_time: + break + elif phase >= 1.0: + reached = False + hold_start = None + + # === 5. HOLD_AFTER === + if self.logger: + self.logger.event("STARTUP_HOLD_AFTER_BEGIN", duration=self.hold_time) + n_hold = max(1, int(self.hold_time / max(self.control_dt, 1e-3))) + next_exec = time.perf_counter() + for _ in range(n_hold): + self.io.hold_pose(target_pose, kp_scale=1.0) + _s, next_exec = self._tick("STARTUP_HOLD_AFTER", target_pose, 1.0, next_exec) + + if self.logger: + self.logger.event("STARTUP_TRANSITION_END") + print("[PoseInit] 默认站姿初始化完成") + return target_pose + + # ---- 等用户回车（外部调用，期间持续保持） ---- + def hold_until_user_confirm(self, target_pose: np.ndarray, evt) -> bool: + """阻塞循环到 evt.is_set()，期间持续 PD 保持站姿、跑 guard、写日志。 + 返回 True 正常确认，False 因 guard.STOP 中止。""" + if self.logger: + self.logger.event("WAIT_USER_BEGIN") + next_exec = time.perf_counter() + while not evt.is_set(): + self.io.hold_pose(target_pose, kp_scale=1.0) + try: + _s, next_exec = self._tick("WAIT_USER", target_pose, 1.0, next_exec) + except PoseInitFailed as e: + print(f"[PoseInit] WAIT_USER 期间触发停止: {e}") + return False + if self.logger: + self.logger.event("WAIT_USER_END") + return True diff --git a/05_software/real/sim2real/startup/stand_balance.py b/05_software/real/sim2real/startup/stand_balance.py new file mode 100644 index 0000000..ff83b9e --- /dev/null +++ b/05_software/real/sim2real/startup/stand_balance.py @@ -0,0 +1,120 @@ +from __future__ import annotations + +from dataclasses import dataclass +from typing import Any, Dict + +import numpy as np + + +@dataclass +class StandBalanceDebug: + roll: float + pitch: float + roll_rate: float + pitch_rate: float + hip_base: float + knee_base: float + roll_corr: float + pitch_corr: float + stable: bool + + +class StandBalanceController: + def __init__(self, cfg: Dict[str, Any], control_dt: float): + self.enabled = bool(cfg.get("enabled", True)) + self.control_dt = float(control_dt) + self.height = float(cfg.get("height", 0.33)) + self.kp_roll = float(cfg.get("kp_roll", 0.85)) + self.kp_pitch = float(cfg.get("kp_pitch", 0.70)) + self.kd_roll_rate = float(cfg.get("kd_roll_rate", 0.03)) + self.kd_pitch_rate = float(cfg.get("kd_pitch_rate", 0.025)) + self.lateral_lean_gain = float(cfg.get("lateral_lean_gain", 0.0)) + self.hip_abduction_clip = float(cfg.get("hip_abduction_clip", 0.45)) + self.hip_pitch_clip = tuple(cfg.get("hip_pitch_clip", [-1.0, 2.5])) + self.knee_clip = tuple(cfg.get("knee_clip", [-2.6, -0.3])) + self.stable_roll_deg = float(cfg.get("stable_roll_deg", 6.0)) + self.stable_pitch_deg = float(cfg.get("stable_pitch_deg", 8.0)) + self.stable_gyro_deg_s = float(cfg.get("stable_gyro_deg_s", 45.0)) + self.enter_hold_s = float(cfg.get("enter_hold_s", 1.0)) + + self.profile_h = np.asarray( + cfg.get("profile_h", [0.157, 0.248, 0.311, 0.366, 0.411, 0.448]), + dtype=np.float32, + ) + self.profile_hip = np.asarray( + cfg.get("profile_hip", [1.5, 1.2, 1.0, 0.8, 0.6, 0.4]), + dtype=np.float32, + ) + self.profile_knee = np.asarray( + cfg.get("profile_knee", [-2.5, -2.1, -1.8, -1.5, -1.2, -0.9]), + dtype=np.float32, + ) + self._stable_time = 0.0 + self._last_debug = StandBalanceDebug(0.0, 0.0, 0.0, 0.0, 0.9, -1.8, 0.0, 0.0, False) + + @property + def last_debug(self) -> StandBalanceDebug: + return self._last_debug + + def reset(self) -> None: + self._stable_time = 0.0 + + def _estimate_roll_pitch(self, projected_gravity: np.ndarray) -> tuple[float, float]: + gx, gy, gz = [float(v) for v in projected_gravity] + roll = float(np.arctan2(-gy, max(1e-6, -gz))) + pitch = float(np.arctan2(gx, np.sqrt(max(1e-6, gy * gy + gz * gz)))) + return roll, pitch + + def _base_leg_pose(self) -> tuple[float, float]: + h_clamp = float(np.clip(self.height, float(self.profile_h[0]), float(self.profile_h[-1]))) + hip = float(np.interp(h_clamp, self.profile_h, self.profile_hip)) + knee = float(np.interp(h_clamp, self.profile_h, self.profile_knee)) + return hip, knee + + def compute_target(self, state: Dict[str, Any], command: np.ndarray | None = None) -> np.ndarray: + projected_gravity = np.asarray(state["projected_gravity"], dtype=np.float32) + imu_gyro = np.asarray(state["imu_gyro"], dtype=np.float32) + cmd = np.zeros(3, dtype=np.float32) if command is None else np.asarray(command, dtype=np.float32) + + hip_base, knee_base = self._base_leg_pose() + roll, pitch = self._estimate_roll_pitch(projected_gravity) + roll_rate = float(imu_gyro[0]) + pitch_rate = float(imu_gyro[1]) + + roll_corr = -self.kp_roll * roll - self.kd_roll_rate * roll_rate + pitch_corr = -self.kp_pitch * pitch - self.kd_pitch_rate * pitch_rate + lateral_lean = self.lateral_lean_gain * float(cmd[1]) + + target = np.zeros(16, dtype=np.float32) + for leg_idx in range(4): + side = 1.0 if leg_idx in (0, 2) else -1.0 + target[leg_idx * 3 + 0] = float( + np.clip(side * roll_corr + lateral_lean, -self.hip_abduction_clip, self.hip_abduction_clip) + ) + target[leg_idx * 3 + 1] = float( + np.clip(hip_base + pitch_corr, self.hip_pitch_clip[0], self.hip_pitch_clip[1]) + ) + target[leg_idx * 3 + 2] = float(np.clip(knee_base, self.knee_clip[0], self.knee_clip[1])) + target[12:] = 0.0 + + stable = ( + abs(np.degrees(roll)) <= self.stable_roll_deg + and abs(np.degrees(pitch)) <= self.stable_pitch_deg + and max(abs(np.degrees(roll_rate)), abs(np.degrees(pitch_rate))) <= self.stable_gyro_deg_s + ) + self._stable_time = self._stable_time + self.control_dt if stable else 0.0 + self._last_debug = StandBalanceDebug( + roll=roll, + pitch=pitch, + roll_rate=roll_rate, + pitch_rate=pitch_rate, + hip_base=hip_base, + knee_base=knee_base, + roll_corr=roll_corr, + pitch_corr=pitch_corr, + stable=stable, + ) + return target + + def is_stable(self) -> bool: + return self._stable_time >= self.enter_hold_s diff --git a/05_software/real/sim2real/tools/__init__.py b/05_software/real/sim2real/tools/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/05_software/real/sim2real/tools/alignment_check.py b/05_software/real/sim2real/tools/alignment_check.py new file mode 100644 index 0000000..59e9785 --- /dev/null +++ b/05_software/real/sim2real/tools/alignment_check.py @@ -0,0 +1,171 @@ +"""Offline deployment alignment check for the current 53-D rough policy.""" + +import argparse +from pathlib import Path +import sys + +import numpy as np +import torch +import yaml + +sys.path.insert(0, str(Path(__file__).resolve().parents[1])) + +from interface.motor_mapping import MotorMapping # noqa: E402 +from policy.policy_runner import PolicyRunner # noqa: E402 + + +def _load_manifest(manifest_path: Path) -> dict: + with open(manifest_path, "r", encoding="utf-8") as f: + return yaml.safe_load(f) + + +def check(policy_path: Path, manifest_path: Path | None = None) -> int: + issues: list[tuple[str, str]] = [] + manifest = _load_manifest(manifest_path) if manifest_path is not None else None + + expected = ( + ("fl", "hip_abduction"), ("fl", "hip_pitch"), ("fl", "knee"), + ("fr", "hip_abduction"), ("fr", "hip_pitch"), ("fr", "knee"), + ("rl", "hip_abduction"), ("rl", "hip_pitch"), ("rl", "knee"), + ("rr", "hip_abduction"), ("rr", "hip_pitch"), ("rr", "knee"), + ("fl", "wheel"), ("fr", "wheel"), ("rl", "wheel"), ("rr", "wheel"), + ) + if MotorMapping.SIM_JOINT_ORDER != expected: + issues.append(("joint_order", "MotorMapping.SIM_JOINT_ORDER mismatch")) + else: + print("[Check] joint order: PASS") + + manifest_enable_zero_cmd = True + if manifest is not None: + manifest_enable_zero_cmd = bool( + manifest.get("model", {}).get("enable_zero_cmd_suppression", True) + ) + + runner = PolicyRunner( + policy_path, + device=torch.device("cpu"), + enable_zero_cmd_suppression=manifest_enable_zero_cmd, + ) + obs_mean = runner.policy.obs_mean.detach().cpu().numpy() + obs_std = runner.policy.obs_std.detach().cpu().numpy() + if np.allclose(obs_mean, 0.0) and np.allclose(obs_std, 1.0): + print("[Check] obs normalizer: PASS (identity)") + else: + print( + f"[Check] obs normalizer: PASS " + f"(mean range=[{obs_mean.min():.3f},{obs_mean.max():.3f}], " + f"std range=[{obs_std.min():.3f},{obs_std.max():.3f}])" + ) + if (obs_std < 1e-6).any(): + issues.append( + ( + "normalizer_zero_std", + f"obs_std has near-zero entries: {np.where(obs_std < 1e-6)[0].tolist()}", + ) + ) + + raw_zero = np.zeros(runner.BASE_OBS_DIM, dtype=np.float32) + runner.reset(prime_obs=raw_zero) + _, raw = runner.step(raw_zero) + if np.max(np.abs(raw)) > 5.0: + issues.append( + ( + "output_range", + f"raw action too large under zero obs: {np.max(np.abs(raw)):.3f}", + ) + ) + else: + print(f"[Check] zero-obs output range: PASS (max|raw|={np.max(np.abs(raw)):.3f})") + + expected_default = np.array([0.0, 0.9, -1.8] * 4 + [0.0] * 4, dtype=np.float32) + if not np.allclose(runner.default_dof_pos, expected_default): + issues.append(("default_pose_mismatch", f"default_dof_pos mismatch: {runner.default_dof_pos}")) + else: + print("[Check] default_dof_pos: PASS") + + if runner.BASE_OBS_DIM != 53: + issues.append(("obs_dim", f"base obs dim {runner.BASE_OBS_DIM} != 53")) + else: + print("[Check] actor obs dim: PASS (53)") + + if manifest is not None: + declared_model = manifest.get("model", {}) + declared_action = manifest.get("action", {}) + declared_safety = manifest.get("safety", {}) + declared_control = manifest.get("control", {}) + + if int(declared_model.get("obs_dim", -1)) != runner.policy.expected_obs_dim: + issues.append( + ( + "manifest_obs_dim", + f"manifest obs_dim {declared_model.get('obs_dim')} != policy {runner.policy.expected_obs_dim}", + ) + ) + else: + print("[Check] manifest obs_dim: PASS") + + if int(declared_model.get("action_dim", -1)) != runner.policy.expected_action_dim: + issues.append( + ( + "manifest_action_dim", + f"manifest action_dim {declared_model.get('action_dim')} != policy {runner.policy.expected_action_dim}", + ) + ) + else: + print("[Check] manifest action_dim: PASS") + + declared_default = np.asarray(declared_action.get("default_dof_pos", []), dtype=np.float32) + if declared_default.shape != runner.default_dof_pos.shape or not np.allclose( + declared_default, runner.default_dof_pos + ): + issues.append(("manifest_default_pose", "manifest default_dof_pos mismatch")) + else: + print("[Check] manifest default_dof_pos: PASS") + + declared_scale = np.asarray(declared_action.get("scale", []), dtype=np.float32) + if declared_scale.shape != runner.action_scale.shape or not np.allclose( + declared_scale, runner.action_scale + ): + issues.append(("manifest_action_scale", "manifest action scale mismatch")) + else: + print("[Check] manifest action scale: PASS") + + if float(declared_safety.get("zero_cmd_lin_thresh", -1.0)) != runner.zero_cmd_lin_thresh: + issues.append(("manifest_zero_cmd_lin_thresh", "manifest zero_cmd_lin_thresh mismatch")) + if float(declared_safety.get("zero_cmd_yaw_thresh", -1.0)) != runner.zero_cmd_yaw_thresh: + issues.append(("manifest_zero_cmd_yaw_thresh", "manifest zero_cmd_yaw_thresh mismatch")) + if float(declared_safety.get("zero_yaw_rate_thresh", -1.0)) != runner.zero_yaw_rate_thresh: + issues.append(("manifest_zero_yaw_rate_thresh", "manifest zero_yaw_rate_thresh mismatch")) + if bool(declared_model.get("enable_zero_cmd_suppression", True)) != runner.enable_zero_cmd_suppression: + issues.append(("manifest_zero_cmd_switch", "manifest zero-command suppression switch mismatch")) + else: + print("[Check] manifest zero-command suppression: PASS") + + if int(declared_control.get("control_freq_hz", -1)) != 50: + issues.append(("manifest_control_freq", "manifest control_freq_hz must be 50")) + else: + print("[Check] manifest control freq: PASS") + + if issues: + print("\n" + "=" * 60) + print(f"Alignment check failed: {len(issues)} issue(s)") + for tag, msg in issues: + print(f" [{tag}] {msg}") + return 1 + + print("\n" + "=" * 60) + print("All offline alignment checks passed.") + return 0 + + +def main(): + parser = argparse.ArgumentParser() + parser.add_argument("--policy", type=str, required=True, help="Path to policy .pt") + parser.add_argument("--manifest", type=str, default=None, help="Optional deployment manifest yaml") + args = parser.parse_args() + manifest = Path(args.manifest) if args.manifest else None + sys.exit(check(Path(args.policy), manifest)) + + +if __name__ == "__main__": + main() diff --git a/05_software/real/sim2real/tools/calibrate_offsets.py b/05_software/real/sim2real/tools/calibrate_offsets.py new file mode 100644 index 0000000..662132e --- /dev/null +++ b/05_software/real/sim2real/tools/calibrate_offsets.py @@ -0,0 +1,163 @@ +"""零位偏移标定向导。 + +用途：把机器人摆到 sim2sim/训练侧的 stand 默认姿态（人工摆好）， +跑这个脚本，它会读 16 个电机的当前位置，反算每个电机的 ZERO_OFFSET。 + +关键公式（与 motor_mapping.py 一致）： + real = sign * sim + offset +当 sim = stand_default 时： + offset = real - sign * stand_default + +⚠️ 使用前置条件： +1. 已运行过 motor_driver_direction_test 类的脚本，确认每个电机的 sign 是对的； + sign 错的话本工具会算出错误的 offset 看起来很对，但发动作时机器人会反向冲撞 +2. 机器人物理上摆到 stand 姿态：四条腿微弯曲、轮子接地、机身水平 +3. 电机已 enable 并清除告警 + +输出：把打印出来的 ZERO_OFFSET_MAP 字段直接覆盖 motor_mapping.py 中的对应字典。 +""" +import argparse +import sys +import time +from pathlib import Path + +import numpy as np + +sys.path.insert(0, str(Path(__file__).resolve().parents[1])) + +from interface.motor_mapping import MotorMapping # noqa: E402 +from policy.policy_runner import PolicyRunner # noqa: E402 + + +def main(): + parser = argparse.ArgumentParser() + parser.add_argument("--can1-port", default="/dev/can1") + parser.add_argument("--can2-port", default="/dev/can2") + parser.add_argument("--motor-model", default="rs-02") + parser.add_argument("--samples", type=int, default=100, + help="平均采样帧数（去抖动）") + parser.add_argument("--target-pose", default="stand", choices=["stand", "crawl"], + help="标定时机器人摆的物理姿态") + parser.add_argument("--no-enable", action="store_true", + help="不主动 enable 电机（仅读取，适合手动转关节标定）") + args = parser.parse_args() + + # 真机驱动注入（路径优先级与 main.py 一致：vendored/drivers > /home/rc2/...） + sim2real_root = Path(__file__).resolve().parents[1] + for path in (sim2real_root / "vendored", + "/home/rc2/work/rcwork/control", + "/home/rc2/work/rcwork"): + sp = str(path) + if sp not in sys.path and Path(path).exists(): + sys.path.append(sp) + from drivers.motor_driver import RobStrideDriver # type: ignore + + mapper = MotorMapping() + drv1 = RobStrideDriver(args.can1_port, debug=False) + drv2 = RobStrideDriver(args.can2_port, debug=False) + drv1.connect() + drv2.connect() + + for jk in mapper.SIM_JOINT_ORDER: + leg, joint = jk + bus, mid = mapper.CAN_ID_MAP[jk] + name = f"{leg}_{joint}" + (drv1 if bus == 1 else drv2).add_motor(name, mid, args.motor_model) + + if not args.no_enable: + print("[Calib] Enable 电机以读取状态...（已就位则可加 --no-enable 跳过）") + for drv in (drv1, drv2): + for name in drv.motors: + drv.clear_warnings(name) + drv.enable(name) + time.sleep(0.5) + + # 选择标定姿态 + if args.target_pose == "stand": + sim_pose = PolicyRunner.DEFAULT_STAND_POSE.copy() # [0,0.9,-1.8] x4 + zeros + else: + sim_pose = np.array([ + 0.4, 1.65, -2.55, -0.4, 1.65, -2.55, + 0.4, 1.65, -2.55, -0.4, 1.65, -2.55, + 0.0, 0.0, 0.0, 0.0, + ], dtype=np.float32) + + print(f"\n[Calib] 请把机器人物理摆成 {args.target_pose.upper()} 姿态：") + if args.target_pose == "stand": + print(" 四条腿髋外展=0, 髋俯仰=0.9rad(~52°), 膝=-1.8rad(~-103°), 轮接地") + else: + print(" 内收外展 ±0.4rad, 髋俯仰=1.65rad, 膝=-2.55rad（深蹲下趴）") + print(" 轮子可以保持任意角度，offset 强制为 0") + print(" 按回车开始采样...") + try: + input() + except EOFError: + pass + + print(f"\n[Calib] 开始采样 {args.samples} 帧并平均...") + pos_acc = np.zeros(16, dtype=np.float64) + valid = 0 + for i in range(args.samples): + drv1.process_messages() + drv2.process_messages() + real_pos = {} + for drv_idx, drv in enumerate((drv1, drv2)): + bus = drv_idx + 1 + for name, motor in drv.motors.items(): + parts = name.split("_", 1) + if len(parts) != 2: + continue + key = (parts[0], parts[1]) + if key not in mapper.CAN_ID_MAP: + continue + _, mid = mapper.CAN_ID_MAP[key] + real_pos[(bus, mid)] = motor.state.position + if len(real_pos) == 16: + ordered = np.array([real_pos[mapper.CAN_ID_MAP[jk]] + for jk in mapper.SIM_JOINT_ORDER], dtype=np.float64) + pos_acc += ordered + valid += 1 + time.sleep(0.02) + + if valid < args.samples * 0.5: + print(f"[Calib] 警告: 只收到 {valid}/{args.samples} 帧反馈，标定可能不可靠") + real_avg = pos_acc / max(valid, 1) + + # 反算 offset：offset = real - sign * sim + sign = mapper._sign + offsets = real_avg - sign * sim_pose + + # 轮子 offset 强制 0 + for i, jk in enumerate(mapper.SIM_JOINT_ORDER): + if jk[1] == "wheel": + offsets[i] = 0.0 + + # 打印结果（按 motor_mapping.py 的字典格式） + print("\n" + "=" * 64) + print(f"[Calib] 标定完成（{valid} 帧平均）") + print("=" * 64) + print("把以下字典覆盖 sim2real/interface/motor_mapping.py 中的 ZERO_OFFSET_MAP:") + print() + print(" ZERO_OFFSET_MAP = {") + for i, jk in enumerate(mapper.SIM_JOINT_ORDER): + leg, joint = jk + cur = offsets[i] + old = mapper.ZERO_OFFSET_MAP[jk] + delta = cur - old + marker = " *" if abs(delta) > 0.01 else "" + print(f' ("{leg}", "{joint:13s}"): {cur:>+8.4f}, ' + f'# old={old:+.4f} delta={delta:+.4f}{marker}') + print(" }") + print("\n标记 * 的项与现表偏离 > 0.01 rad，请重点核对该关节的 sign 是否正确。\n") + + # Disable + if not args.no_enable: + for drv in (drv1, drv2): + for name in drv.motors: + drv.disable(name) + drv1.disconnect() + drv2.disconnect() + + +if __name__ == "__main__": + main() diff --git a/05_software/real/sim2real/tools/logger.py b/05_software/real/sim2real/tools/logger.py new file mode 100644 index 0000000..f7483f1 --- /dev/null +++ b/05_software/real/sim2real/tools/logger.py @@ -0,0 +1,239 @@ +"""Logging helpers for sim2real runs. + +Each session writes: +- `state.csv`: high-rate state stream +- `events.jsonl`: event / milestone stream +""" + +import json +import time +from datetime import datetime +from pathlib import Path +from typing import Any, Dict, Optional + +import numpy as np + + +class LogBundle: + """One session directory containing state CSV and event JSONL.""" + + JOINT_LABELS = ( + "fl_hip_abd", "fl_hip_pitch", "fl_knee", + "fr_hip_abd", "fr_hip_pitch", "fr_knee", + "rl_hip_abd", "rl_hip_pitch", "rl_knee", + "rr_hip_abd", "rr_hip_pitch", "rr_knee", + "fl_wheel", "fr_wheel", "rl_wheel", "rr_wheel", + ) + + def __init__(self, log_root: str = "logs"): + timestamp = datetime.now().strftime("%Y%m%d_%H%M%S") + self.dir = Path(log_root) / timestamp + self.dir.mkdir(parents=True, exist_ok=True) + + self.state_path = self.dir / "state.csv" + self.events_path = self.dir / "events.jsonl" + + self._state_fp = open(self.state_path, "w", encoding="utf-8") + self._events_fp = open(self.events_path, "w", encoding="utf-8") + self._t0 = time.time() + self._closed = False + + self._write_state_header() + self.event("LOG_START", session_dir=str(self.dir)) + print(f"[Log] {self.dir}") + + def _write_state_header(self): + cols = ["t", "t_rel", "phase"] + cols += [f"{joint}_pos" for joint in self.JOINT_LABELS] + cols += [f"{joint}_vel" for joint in self.JOINT_LABELS] + cols += [f"{joint}_tau" for joint in self.JOINT_LABELS] + cols += [f"{joint}_tgt" for joint in self.JOINT_LABELS] + cols += [f"{joint}_raw" for joint in self.JOINT_LABELS] + cols += ["gyro_x", "gyro_y", "gyro_z"] + cols += ["accel_x", "accel_y", "accel_z"] + cols += ["quat_w", "quat_x", "quat_y", "quat_z"] + cols += ["pgrav_x", "pgrav_y", "pgrav_z"] + cols += ["cmd_vx", "cmd_vy", "cmd_yaw"] + cols += ["imu_age_ms", "loop_dt_ms"] + cols += ["safety_level", "guard_level"] + cols += ["holdover", "stale_max", "fresh_count"] + cols += ["kp_scale", "nan_flag"] + cols += ["kp_leg_cmd", "kd_leg_cmd", "kd_wheel_cmd"] + cols += ["runtime_release_alpha", "runtime_release_hold_s", "runtime_blend_ratio"] + cols += ["hold_target_max_err", "policy_target_max_err", "hold_policy_max_gap"] + cols += ["target_source_code"] + cols += [ + "clip_primary_joint_index", + "clip_primary_joint", + "clip_primary_target", + "clip_primary_measured", + "clip_primary_default", + "clip_primary_pos_err", + "clip_primary_raw", + "clip_primary_scaled", + ] + cols += ["safety_reason", "guard_reason"] + self._state_fp.write(",".join(cols) + "\n") + self._state_fp.flush() + + def state( + self, + phase: str, + joint_pos: np.ndarray, + joint_vel: np.ndarray, + joint_torque: np.ndarray, + target_pose: np.ndarray, + raw_action: Optional[np.ndarray], + gyro: np.ndarray, + accel: np.ndarray, + quat: np.ndarray, + proj_gravity: np.ndarray, + command: np.ndarray, + imu_age_ms: float, + loop_dt_ms: float, + safety_level: int = 0, + guard_level: int = 0, + holdover: int = 0, + stale_max: int = 0, + fresh_count: int = 16, + kp_scale: float = 1.0, + nan_flag: int = 0, + kp_leg_cmd: float = 0.0, + kd_leg_cmd: float = 0.0, + kd_wheel_cmd: float = 0.0, + runtime_release_alpha: float = 0.0, + runtime_release_hold_s: float = 0.0, + runtime_blend_ratio: float = 0.0, + hold_target_max_err: float = 0.0, + policy_target_max_err: float = 0.0, + hold_policy_max_gap: float = 0.0, + target_source: str = "", + clip_primary_joint: str = "", + clip_primary_target: float = 0.0, + clip_primary_measured: float = 0.0, + clip_primary_default: float = 0.0, + clip_primary_pos_err: float = 0.0, + clip_primary_raw: float = 0.0, + clip_primary_scaled: float = 0.0, + safety_reason: str = "", + guard_reason: str = "", + ): + if self._closed: + return + if target_pose is None: + target_pose = np.zeros(16, dtype=np.float32) + if raw_action is None: + raw_action = np.zeros(16, dtype=np.float32) + + now = time.time() + numeric_values = [] + numeric_values += joint_pos.tolist() + numeric_values += joint_vel.tolist() + numeric_values += joint_torque.tolist() + numeric_values += target_pose.tolist() + numeric_values += raw_action.tolist() + numeric_values += gyro.tolist() + numeric_values += accel.tolist() + numeric_values += quat.tolist() + numeric_values += proj_gravity.tolist() + numeric_values += command.tolist() + numeric_values += [imu_age_ms, loop_dt_ms] + numeric_values += [safety_level, guard_level, holdover, stale_max, fresh_count, kp_scale, nan_flag] + numeric_values += [kp_leg_cmd, kd_leg_cmd, kd_wheel_cmd] + numeric_values += [runtime_release_alpha, runtime_release_hold_s, runtime_blend_ratio] + numeric_values += [hold_target_max_err, policy_target_max_err, hold_policy_max_gap] + numeric_values += [_target_source_code(target_source)] + numeric_values += [_csv_numeric_joint_index(clip_primary_joint)] + numeric_values += [ + clip_primary_target, + clip_primary_measured, + clip_primary_default, + clip_primary_pos_err, + clip_primary_raw, + clip_primary_scaled, + ] + + parts = [f"{now:.6f}", f"{now - self._t0:.6f}", phase] + parts += [f"{value:.6f}" for value in numeric_values] + parts += [_csv_escape(clip_primary_joint), _csv_escape(safety_reason), _csv_escape(guard_reason)] + self._state_fp.write(",".join(parts) + "\n") + + def event(self, kind: str, **fields: Any): + if self._closed: + return + record = {"t": time.time(), "t_rel": time.time() - self._t0, "kind": kind} + for key, value in fields.items(): + if isinstance(value, np.ndarray): + record[key] = value.tolist() + elif isinstance(value, (np.integer, np.floating)): + record[key] = value.item() + else: + record[key] = value + self._events_fp.write(json.dumps(record, ensure_ascii=False) + "\n") + self._events_fp.flush() + if kind != "STATE_TICK": + print(f"[Event {record['t_rel']:7.2f}s] {kind} {_short_fields(fields)}") + + def flush(self): + if not self._closed: + self._state_fp.flush() + self._events_fp.flush() + + def close(self): + if self._closed: + return + self.event("LOG_END") + self._state_fp.flush() + self._state_fp.close() + self._events_fp.flush() + self._events_fp.close() + self._closed = True + print(f"[Log] saved -> {self.dir}") + + +def _csv_escape(text: str) -> str: + if not text: + return "" + return text.replace(",", ";").replace("\n", " ").replace("\r", " ") + + +def _csv_numeric_joint_index(joint_name: str) -> float: + if not joint_name: + return -1.0 + try: + return float(LogBundle.JOINT_LABELS.index(joint_name)) + except ValueError: + return -1.0 + + +def _target_source_code(target_source: str) -> float: + mapping = { + "": -1.0, + "startup_hold": 0.0, + "stand_balance": 1.0, + "runtime_hold": 2.0, + "runtime_blend": 3.0, + "runtime_policy": 4.0, + } + return mapping.get(target_source, 99.0) + + +def _short_fields(fields: Dict[str, Any]) -> str: + parts = [] + for key, value in fields.items(): + if isinstance(value, (list, tuple, np.ndarray)): + arr = np.asarray(value).ravel() + if arr.size > 4: + continue + try: + parts.append(f"{key}=[{','.join(f'{float(x):.2f}' for x in arr)}]") + except (TypeError, ValueError): + parts.append(f"{key}={list(arr)[:4]}") + elif isinstance(value, float): + parts.append(f"{key}={value:.3f}") + else: + parts.append(f"{key}={value}") + return " ".join(parts) + + +SimpleLogger = LogBundle diff --git a/05_software/real/sim2real/tools/math_utils.py b/05_software/real/sim2real/tools/math_utils.py new file mode 100644 index 0000000..fd1cbe5 --- /dev/null +++ b/05_software/real/sim2real/tools/math_utils.py @@ -0,0 +1,108 @@ +"""数学工具 — 与 rc_mjlab/sim2sim/tools/math_utils.py 数值完全一致。""" +import numpy as np + + +def get_gravity_orientation(quat_wxyz: np.ndarray) -> np.ndarray: + qw, qx, qy, qz = quat_wxyz + gx = 2.0 * (-qz * qx + qw * qy) + gy = -2.0 * (qz * qy + qw * qx) + gz = 1.0 - 2.0 * (qw * qw + qz * qz) + return np.array([gx, gy, gz], dtype=np.float32) + + +def quat_rotate_inverse(quat_wxyz: np.ndarray, v: np.ndarray) -> np.ndarray: + q_w = quat_wxyz[0] + q_vec = quat_wxyz[1:] + a = v * (2.0 * q_w * q_w - 1.0) + b = np.cross(q_vec, v) * q_w * 2.0 + c = q_vec * np.dot(q_vec, v) * 2.0 + return a - b + c + + +def quat_from_accel(accel: np.ndarray) -> np.ndarray: + """用静止重力方向初始化机身姿态四元数。 + + 思想：仿真启动时 quat = [1,0,0,0] 隐含"机身完全水平"，但真机摆在地面上 + pitch/roll 通常各自有几度偏差，会让 projected_gravity 一开始就错。 + 用加速度计读数与 [0,0,-1] 的最短旋转作为初值，可以把首步重力误差 + 降到 IMU 噪声级。 + """ + g_meas = accel / (np.linalg.norm(accel) + 1e-9) + g_ref = np.array([0.0, 0.0, 1.0], dtype=np.float32) + cross = np.cross(g_ref, g_meas) + dot = float(np.dot(g_ref, g_meas)) + if dot < -0.999999: + return np.array([0.0, 1.0, 0.0, 0.0], dtype=np.float32) + s = float(np.sqrt((1.0 + dot) * 2.0)) + q = np.array([s * 0.5, cross[0] / s, cross[1] / s, cross[2] / s], dtype=np.float32) + return q / (np.linalg.norm(q) + 1e-9) + + +class LowPassFilter: + """一阶 IIR 低通，alpha 公式与训练侧 rc_mjlab/src/robot/mdp/lowpass_actions.py + `_lowpass_weights` 完全一致： + + alpha = 1 - exp(-2π · cutoff_freq / control_freq) + = 1 - exp(-2π · cutoff_freq · dt) + + 注意：这与 rc_mjlab/sim2sim/interface/mujoco_io.py 用的近似公式 + (dt / (dt + 1/(2π·fc))) 数值上不同，在 15Hz 截止时差约 30%。 + 我们以训练侧为准，因为策略是在那个滤波下学的。 + """ + + def __init__(self, cutoff_freq: float, dt: float, dim: int): + self.alpha = float(1.0 - np.exp(-2.0 * np.pi * cutoff_freq * dt)) + self.y_prev = None + + def filter(self, x: np.ndarray) -> np.ndarray: + if self.y_prev is None: + self.y_prev = x.copy() + y = self.alpha * x + (1.0 - self.alpha) * self.y_prev + self.y_prev = y.copy() + return y + + def reset(self): + self.y_prev = None + + +class MahonyFilter: + """互补滤波器：高频用陀螺仪积分，低频用加速度计修正。""" + + def __init__(self, kp: float = 2.0, ki: float = 0.0, dt: float = 0.02): + self.kp = kp + self.ki = ki + self.dt = dt + self.q = np.array([1.0, 0.0, 0.0, 0.0], dtype=np.float32) + self.e_int = np.zeros(3, dtype=np.float32) + + def reset_with_accel(self, accel: np.ndarray): + self.q = quat_from_accel(accel) + self.e_int.fill(0.0) + + def update(self, accel: np.ndarray, gyro: np.ndarray) -> np.ndarray: + norm_a = float(np.linalg.norm(accel)) + if norm_a > 1e-6: + a = accel / norm_a + q = self.q + v = np.array([ + 2.0 * (q[1] * q[3] - q[0] * q[2]), + 2.0 * (q[0] * q[1] + q[2] * q[3]), + q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3], + ], dtype=np.float32) + e = np.cross(a, v) + if self.ki > 0.0: + self.e_int += e * self.dt + else: + self.e_int.fill(0.0) + gyro = gyro + self.kp * e + self.ki * self.e_int + + q = self.q + q_dot = 0.5 * np.array([ + -q[1] * gyro[0] - q[2] * gyro[1] - q[3] * gyro[2], + q[0] * gyro[0] + q[2] * gyro[2] - q[3] * gyro[1], + q[0] * gyro[1] - q[1] * gyro[2] + q[3] * gyro[0], + q[0] * gyro[2] + q[1] * gyro[1] - q[2] * gyro[0], + ], dtype=np.float32) + self.q += q_dot * self.dt + self.q /= (np.linalg.norm(self.q) + 1e-9) + return self.q diff --git a/05_software/real/sim2real/tools/standalone_check.py b/05_software/real/sim2real/tools/standalone_check.py new file mode 100644 index 0000000..9ac4173 --- /dev/null +++ b/05_software/real/sim2real/tools/standalone_check.py @@ -0,0 +1,97 @@ +"""Check whether `sim2real/` is self-contained enough for direct deployment.""" + +from __future__ import annotations + +import importlib +from pathlib import Path +import sys + +sys.path.insert(0, str(Path(__file__).resolve().parents[1])) + + +REQUIRED_FILES = [ + "config.yaml", + "deployment_manifest.yaml", + "main.py", + "policies/model_rough.pt", + "policy/policy_runner.py", + "interface/real_io.py", + "interface/imu_client.py", + "interface/motor_driver.py", + "vendored/drivers/motor_driver.py", + "vendored/drivers/usb_can_adapter.py", + "vendored/odin1_imu/odin1_imu.py", + "vendored/odin1_imu/build/libodin1_imu_bridge.so", + "mjcf/wheelleg.xml", +] + +REQUIRED_IMPORTS = [ + "numpy", + "yaml", + "torch", + "serial", +] + +OPTIONAL_IMPORTS = [ + ("pynput", "only needed for CLI keyboard control"), +] + + +def check() -> int: + root = Path(__file__).resolve().parents[1] + issues: list[str] = [] + warnings: list[str] = [] + + print(f"[Check] sim2real root: {root}") + + for rel in REQUIRED_FILES: + path = root / rel + if path.exists(): + print(f"[Check] file: PASS {rel}") + else: + issues.append(f"missing required file: {rel}") + + for module_name in REQUIRED_IMPORTS: + try: + importlib.import_module(module_name) + print(f"[Check] import: PASS {module_name}") + except Exception as exc: + issues.append(f"missing python dependency `{module_name}`: {exc}") + + for module_name, note in OPTIONAL_IMPORTS: + try: + importlib.import_module(module_name) + print(f"[Check] optional import: PASS {module_name}") + except Exception: + warnings.append(f"optional dependency `{module_name}` not found ({note})") + + index_html = (root / "web" / "static" / "index.html").read_text(encoding="utf-8") + if "https://unpkg.com/three@" in index_html: + warnings.append( + "web 3D viewer depends on remote three.js CDN; CLI/web backend are standalone, " + "but full offline 3D viewer is not bundled yet" + ) + + if issues: + print("\n" + "=" * 60) + print("Standalone deployment check: FAIL") + for item in issues: + print(f"- {item}") + else: + print("\n" + "=" * 60) + print("Standalone deployment check: PASS") + + if warnings: + print("\nWarnings:") + for item in warnings: + print(f"- {item}") + + return 1 if issues else 0 + + +def main(): + raise SystemExit(check()) + + +if __name__ == "__main__": + main() diff --git a/05_software/real/sim2real/vendored/__init__.py b/05_software/real/sim2real/vendored/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/05_software/real/sim2real/vendored/drivers/__init__.py b/05_software/real/sim2real/vendored/drivers/__init__.py new file mode 100644 index 0000000..0f1cd3d --- /dev/null +++ b/05_software/real/sim2real/vendored/drivers/__init__.py @@ -0,0 +1,3 @@ +from drivers.motor_driver import RobStrideDriver, RobStrideMotor, MotorState +from drivers.motor_params import CommunicationType, ParamIndex, RunMode +from drivers.usb_can_adapter import DmUsbAdapter diff --git a/05_software/real/sim2real/vendored/drivers/motor_driver.py b/05_software/real/sim2real/vendored/drivers/motor_driver.py new file mode 100644 index 0000000..653b7c4 --- /dev/null +++ b/05_software/real/sim2real/vendored/drivers/motor_driver.py @@ -0,0 +1,371 @@ +import struct +import time +import queue +import numpy as np +from typing import Dict, Optional, Any, List +from dataclasses import dataclass + +from drivers.usb_can_adapter import DmUsbAdapter +from drivers.motor_params import ( + CommunicationType, ParamIndex, ParamType, + MODEL_MIT_POSITION_TABLE, MODEL_MIT_VELOCITY_TABLE, + MODEL_MIT_TORQUE_TABLE, MODEL_MIT_KP_TABLE, MODEL_MIT_KD_TABLE, + get_pack_format, PARAM_TABLE +) + +@dataclass +class MotorState: + position: float = 0.0 + velocity: float = 0.0 + torque: float = 0.0 + temperature: float = 0.0 + current: float = 0.0 + update_count: int = 0 + +class RobStrideMotor: + def __init__(self, name: str, motor_id: int, model: str): + """ + 初始化电机对象。 + + :param name: 电机名称 (例如 "knee") + :param motor_id: 电机 ID + :param model: 电机型号 (例如 "rs-06") + """ + self.name = name + self.id = motor_id + self.model = model + self.state = MotorState() + + def update_state(self, pos: float, vel: float, torque: float, temp: float, current: float = 0.0): + """ + 更新电机状态。 + """ + self.state.position = pos + self.state.velocity = vel + self.state.torque = torque + self.state.temperature = temp + self.state.update_count += 1 + if current != 0.0: + self.state.current = current + +class RobStrideDriver: + def __init__(self, port: str, debug: bool = False): + """ + 初始化驱动器。 + + :param port: 串口名称 + :param debug: 是否开启调试模式 + """ + self.adapter = DmUsbAdapter(port, debug=debug) + self.motors: Dict[str, RobStrideMotor] = {} + self.motors_by_id: Dict[int, RobStrideMotor] = {} + self.host_id = 0xFD # 根据文档，主机 ID 默认为 0xFD + + self.parameter_values = {} # 读取参数缓存: (motor_id, param_index) -> value + + def connect(self): + """连接到底层适配器。""" + self.adapter.open() + print(f"已连接到 RobStride 驱动器，端口: {self.adapter.serial.port}") + # 设置 CAN 波特率为 1000kbps (Index 0) + self.adapter.set_can_baudrate(0) + + def disconnect(self): + """断开连接。""" + self.adapter.close() + print("已断开 RobStride 驱动器连接") + + def set_can_id(self, current_id: int, new_id: int): + """ + 设置电机 CAN ID。 + + :param current_id: 当前电机 ID + :param new_id: 新电机 ID + """ + # Type 7: Set CAN ID + # Bits 23-16: New ID (Preset ID) + # Bits 15-8: Master ID + # Bits 7-0: Target ID + extra_data = (new_id << 8) | self.host_id + self._send_command(CommunicationType.SET_CAN_ID, extra_data, current_id) + print(f"已发送 ID 修改指令: {current_id} -> {new_id} (Master: {self.host_id})") + + def scan_motors(self, timeout: float = 0.1) -> List[int]: + """ + 快速扫描总线上的电机 (ID 1-127)。 + + :param timeout: 等待响应的超时时间 + :return: 发现的电机 ID 列表 + """ + found_ids = [] + print("正在快速扫描所有电机 (ID 1-127)...") + + # 清空缓冲区 + while self.adapter.read_can_frame(): + pass + + # 快速发送查询指令 + for dev_id in range(1, 128): + # 发送获取设备 ID 命令 + self._send_command(CommunicationType.GET_DEVICE_ID, self.host_id, dev_id) + + # 等待响应 + start_time = time.time() + while time.time() - start_time < timeout: + frame = self.adapter.read_can_frame() + if frame: + can_id, data, cmd, ide, rtr = frame + if not ide: continue + + # 解析回复 + # 通信类型 0 (GET_DEVICE_ID/Status) + comm_type = (can_id >> 24) & 0x1F + + if comm_type == CommunicationType.GET_DEVICE_ID: # Type 0 + # Type 0 回复格式: + # Bits 23-8: Status info + # Bits 7-0: Motor ID + extra_data = (can_id >> 8) & 0xFFFF + motor_id = extra_data & 0xFF # Device ID + + if motor_id not in found_ids: + print(f"发现电机 ID: {motor_id}") + found_ids.append(motor_id) + + return sorted(found_ids) + + + def add_motor(self, name: str, motor_id: int, model: str): + """ + 添加电机到控制列表。 + + :param name: 电机名称 + :param motor_id: 电机 ID + :param model: 电机型号 + """ + motor = RobStrideMotor(name, motor_id, model) + self.motors[name] = motor + self.motors_by_id[motor_id] = motor + + def _send_command(self, comm_type: int, extra_data: int, device_id: int, data: bytes = b''): + # 构建 29 位扩展 CAN ID + # Bits 28-24: 通信类型 (Communication Type) + # Bits 23-8: 额外数据 (Extra Data) + # Bits 7-0: 设备 ID (Device ID) + can_id = (comm_type << 24) | (extra_data << 8) | device_id + + # 通过适配器发送 + # RobStride 使用扩展帧 + self.adapter.send_can_frame(can_id, data, extended=True) + + def enable(self, motor_name: str): + """使能电机。""" + motor = self.motors[motor_name] + self._send_command(CommunicationType.ENABLE, self.host_id, motor.id) + + def disable(self, motor_name: str): + """失能电机 (Type 4: Stop)。""" + motor = self.motors[motor_name] + # Data: 全 0 + data = bytes([0x00]*8) + self._send_command(CommunicationType.DISABLE, self.host_id, motor.id, data) + + def clear_warnings(self, motor_name: str): + """ + 清除警告/故障 (Type 4: Stop Motor with Byte0=1)。 + 根据文档 Type 4: Byte[0]=1 时清除故障。 + """ + motor = self.motors[motor_name] + data = bytes([0x01] + [0x00]*7) + self._send_command(CommunicationType.DISABLE, self.host_id, motor.id, data) + + def set_zero_position(self, motor_name: str): + """设置电机当前位置为零点。""" + motor = self.motors[motor_name] + # Type 6: Set Zero Position + # Data: Byte0=1 + data = bytes([0x01] + [0x00]*7) + self._send_command(CommunicationType.SET_ZERO_POSITION, self.host_id, motor.id, data) + + def control_mit(self, motor_name: str, + position: float, velocity: float, + kp: float, kd: float, torque: float): + """ + 发送 MIT 控制指令。 + + :param motor_name: 电机名称 + :param position: 期望位置 (rad) + :param velocity: 期望速度 (rad/s) + :param kp: 位置增益 + :param kd: 速度增益 + :param torque: 前馈力矩 (Nm) + """ + motor = self.motors[motor_name] + model = motor.model + + # 获取限制值 + p_limit = MODEL_MIT_POSITION_TABLE.get(model, 12.5) + v_limit = MODEL_MIT_VELOCITY_TABLE.get(model, 50.0) + t_limit = MODEL_MIT_TORQUE_TABLE.get(model, 60.0) + kp_limit = MODEL_MIT_KP_TABLE.get(model, 500.0) + kd_limit = MODEL_MIT_KD_TABLE.get(model, 5.0) + + # 限幅 + position = np.clip(position, -p_limit, p_limit) + velocity = np.clip(velocity, -v_limit, v_limit) + kp = np.clip(kp, 0, kp_limit) + kd = np.clip(kd, 0, kd_limit) + torque = np.clip(torque, -t_limit, t_limit) + + # 转换为 uint16 + # Position: [-L, L] -> [0, 65535] + p_u16 = int(((position / p_limit) + 1.0) * 32767.0) + p_u16 = np.clip(p_u16, 0, 65535) + + # Velocity: [-L, L] -> [0, 65535] + v_u16 = int(((velocity / v_limit) + 1.0) * 32767.0) + v_u16 = np.clip(v_u16, 0, 65535) + + # Kp: [0, L] -> [0, 65535] + kp_u16 = int((kp / kp_limit) * 65535.0) + kp_u16 = np.clip(kp_u16, 0, 65535) + + # Kd: [0, L] -> [0, 65535] + kd_u16 = int((kd / kd_limit) * 65535.0) + kd_u16 = np.clip(kd_u16, 0, 65535) + + # Torque: [-L, L] -> [0, 65535] (发送在 Extra Data 域) + t_u16 = int(((torque / t_limit) + 1.0) * 32767.0) + t_u16 = np.clip(t_u16, 0, 65535) + + # 打包数据 (大端序) + data = struct.pack('>HHHH', p_u16, v_u16, kp_u16, kd_u16) + + # 发送 + self._send_command(CommunicationType.OPERATION_CONTROL, t_u16, motor.id, data) + + def read_parameter(self, motor_id: int, param_index: int): + """ + 发送读取参数指令 (Type 17)。 + """ + # Type 17 + # Data: Index (2B) + 00 00 + 00 00 00 00 + data = struct.pack('> 24) & 0x1F + + if comm_type == CommunicationType.READ_PARAMETER: + # 解析参数读取反馈 (Type 17) + extra_data = (can_id >> 8) & 0xFFFF + success_flag = (extra_data >> 8) & 0xFF + motor_id = extra_data & 0xFF + + if success_flag == 0: # 0 表示成功 + if len(data) >= 8: + param_index = struct.unpack('> 8) & 0xFFFF + motor_id = extra_data & 0xFF + if motor_id in self.motors_by_id: + motor = self.motors_by_id[motor_id] + self._parse_feedback(motor, data) + + count += 1 + + def _parse_feedback(self, motor: RobStrideMotor, data: bytes): + if len(data) < 8: + return + + # 解包大端序数据 + p_u16, v_u16, t_i16, temp_u16 = struct.unpack('>HHHH', data) + + model = motor.model + p_limit = MODEL_MIT_POSITION_TABLE.get(model, 12.5) + v_limit = MODEL_MIT_VELOCITY_TABLE.get(model, 50.0) + t_limit = MODEL_MIT_TORQUE_TABLE.get(model, 60.0) + + # 转换回浮点数 + pos = (float(p_u16) / 32767.0 - 1.0) * p_limit + vel = (float(v_u16) / 32767.0 - 1.0) * v_limit + torque = (float(t_i16) / 32767.0 - 1.0) * t_limit + temp = float(temp_u16) * 0.1 + + motor.update_state(pos, vel, torque, temp) diff --git a/05_software/real/sim2real/vendored/drivers/motor_params.py b/05_software/real/sim2real/vendored/drivers/motor_params.py new file mode 100644 index 0000000..0930c19 --- /dev/null +++ b/05_software/real/sim2real/vendored/drivers/motor_params.py @@ -0,0 +1,422 @@ +import numpy as np +import struct + +class CommunicationType: + """ + 电机通信类型定义 (Bit28~24) + 参考说明书 4.1 章节 + + 通信 ID 结构 (29位扩展帧): + | Bit 28-24 | Bit 23-8 | Bit 7-0 | + | 通信类型 | 数据区2 | 目标地址 | + """ + GET_DEVICE_ID = 0 # 获取设备 ID 和 64 位 MCU 唯一标识符 (Type 0) + OPERATION_CONTROL = 1 # 运控模式电机控制指令 (MIT 模式) (Type 1) + OPERATION_STATUS = 2 # 电机反馈数据 (标准反馈帧) (Type 2) + ENABLE = 3 # 电机使能运行 (Type 3) + DISABLE = 4 # 电机停止运行 (可用于清除故障) (Type 4) + SET_ZERO_POSITION = 6 # 设置电机机械零位 (设置当前位置为零点) (Type 6) + SET_CAN_ID = 7 # 设置电机 CAN ID (立即生效，需保存) (Type 7) + READ_PARAMETER = 17 # 单个参数读取 (Type 17, 0x11) + WRITE_PARAMETER = 18 # 单个参数写入 (Type 18, 0x12, 掉电丢失) + FAULT_REPORT = 21 # 故障反馈帧 (Type 21, 0x15) + SAVE_PARAMETERS = 22 # 电机数据保存帧 (保存所有参数到 Flash) (Type 22) + SET_BAUDRATE = 23 # 电机波特率修改帧 (重新上电生效) (Type 23) + ACTIVE_REPORT = 24 # 电机主动上报设置帧 (开启/关闭主动上报) (Type 24) + PROTOCOL_SWITCH = 25 # 电机协议修改帧 (切换 Canopen/MIT/私有协议) (Type 25) + READ_VERSION = 26 # 版本号读取帧 (Type 26) + +class RunMode: + """ + 电机运行模式 (参数索引 0x7005) + 参考说明书 4.3 章节 + """ + MIT = 0 # 运控模式 (默认): 适用于高动态响应控制 + POS_PP = 1 # 位置模式 (PP): 梯形加减速位置控制 + SPEED = 2 # 速度模式: 闭环速度控制 + CURRENT = 3 # 电流模式: 闭环力矩(电流)控制 + POS_CSP = 5 # 位置模式 (CSP): 循环同步位置模式 (适用于周期性指令) + +class BaudRate: + """ + 电机波特率 (通信类型 23) + 参考说明书 4.1 通信类型 23 + 注意: 修改后需重新上电生效 + """ + BAUD_1M = 1 # 1 Mbps (默认) + BAUD_500K = 2 # 500 Kbps + BAUD_250K = 3 # 250 Kbps + BAUD_125K = 4 # 125 Kbps + +class ActiveReportStatus: + """ + 电机主动上报状态 (通信类型 24) + 参考说明书 4.1 通信类型 24 + """ + DISABLE = 0 # 关闭主动上报 (默认) + ENABLE = 1 # 开启主动上报 (默认间隔 10ms, 可通过 EP_SCAN_TIME 修改) + +class ProtocolType: + """ + 电机协议类型 (通信类型 25) + 参考说明书 4.2.4 章节 + 注意: 切换协议后需重新上电生效 + """ + PRIVATE = 0 # 私有协议 (默认): 使用 29 位扩展帧 + CANOPEN = 1 # CANopen 协议: 符合 CiA 402 标准 + MIT = 2 # MIT 协议 (标准帧): 使用 11 位标准帧 + +class ParamType: + """ + 参数数据类型定义 + + - 私有协议 (Type 17/18) 参数表主要使用 UINT8/UINT16/UINT32/FLOAT + - CANopen 对象字典会用到有符号类型 (INTEGER8/16/32) + """ + UINT8 = 0 # 无符号 8 位整数 + UINT16 = 1 # 无符号 16 位整数 + UINT32 = 2 # 无符号 32 位整数 + FLOAT = 3 # 32 位浮点数 (IEEE 754) + INT8 = 4 # 有符号 8 位整数 + INT16 = 5 # 有符号 16 位整数 + INT32 = 6 # 有符号 32 位整数 + +class ErrorCode: + """ + 异常状态 fault 值位定义 + + 说明书位置: + - 章节 6 (Mit) 的“异常状态应答帧”对 fault 值 bit 位做了明确描述 + - 私有协议 Type 21 故障反馈帧也会携带 fault/warning 值 + """ + OVER_TEMP = 1 << 0 # bit0: 电机过温故障 (默认 >145°C) + DRIVE_CHIP = 1 << 1 # bit1: 驱动芯片故障 (DRV8353 等报告错误) + UNDER_VOLTAGE = 1 << 2 # bit2: 欠压故障 (电压 < 12V) + OVER_VOLTAGE = 1 << 3 # bit3: 过压故障 (电压 > 60V) + CURRENT_B_OVER = 1 << 4 # bit4: B 相电流采样过流 + CURRENT_C_OVER = 1 << 5 # bit5: C 相电流采样过流 + ENCODER_NOT_CALIB = 1 << 7 # bit7: 编码器未标定 + HARDWARE_ERR = 1 << 8 # bit8: 硬件识别故障 + POS_INIT_ERR = 1 << 9 # bit9: 位置初始化故障 + LOAD_BLOCK = 1 << 14 # bit14: 堵转过载算法保护 + CURRENT_A_OVER = 1 << 16 # bit16: A 相电流采样过流 + +class WarningCode: + """ + 预警状态 warning 值位定义 (Type 21 Byte 4-7) + """ + OVER_TEMP_WARNING = 1 << 0 # bit0: 电机过温预警 (默认 >135°C) + +class DriveFault1: + """ + 驱动芯片故障码 1 (0x3024) - DRV8353 状态寄存器 1 + 参考说明书 3.3.7 章节 + """ + VDS_LC = 1 << 0 # VDS overcurrent on C low-side (C相下管VDS过流) + VDS_HC = 1 << 1 # VDS overcurrent on C high-side (C相上管VDS过流) + VDS_LB = 1 << 2 # VDS overcurrent on B low-side (B相下管VDS过流) + VDS_HB = 1 << 3 # VDS overcurrent on B high-side (B相上管VDS过流) + VDS_LA = 1 << 4 # VDS overcurrent on A low-side (A相下管VDS过流) + VDS_HA = 1 << 5 # VDS overcurrent on A high-side (A相上管VDS过流) + OTSD = 1 << 6 # Overtemperature shutdown (过温关断) + UVLO = 1 << 7 # Undervoltage lockout (欠压锁定) + GDF = 1 << 8 # Gate drive fault (栅极驱动故障) + VDS_OCP = 1 << 9 # VDS monitor overcurrent (VDS 监控过流) + FAULT = 1 << 10 # Logic OR of FAULT status (故障状态逻辑或) + +class DriveFault2: + """ + 驱动芯片故障码 2 (0x3025) - DRV8353 状态寄存器 2 + 参考说明书 3.3.7 章节 + """ + VGS_LC = 1 << 0 # Gate drive fault on C low-side (C相下管栅极故障) + VGS_HC = 1 << 1 # Gate drive fault on C high-side (C相上管栅极故障) + VGS_LB = 1 << 2 # Gate drive fault on B low-side (B相下管栅极故障) + VGS_HB = 1 << 3 # Gate drive fault on B high-side (B相上管栅极故障) + VGS_LA = 1 << 4 # Gate drive fault on A low-side (A相下管栅极故障) + VGS_HA = 1 << 5 # Gate drive fault on A high-side (A相上管栅极故障) + GDUV = 1 << 6 # VCP charge pump / VGLS undervoltage (电荷泵欠压) + OTW = 1 << 7 # Overtemperature warning (过温预警) + SC_OC = 1 << 8 # Overcurrent on phase C sense amplifier (C相采样过流) + SB_OC = 1 << 9 # Overcurrent on phase B sense amplifier (B相采样过流) + SA_OC = 1 << 10 # Overcurrent on phase A sense amplifier (A相采样过流) + +class MotorParams: + """ + 电机物理参数限制 (用于 MIT 模式数据压缩) + 参考说明书 4.1 通信类型 1 + + 注意: + - P_MIN/MAX: 位置范围 (RS03: -12.57 ~ 12.57 rad) + - V_MIN/MAX: 速度范围 (RS03: -20 ~ 20 rad/s) + - T_MIN/MAX: 力矩范围 (RS03: -60 ~ 60 Nm) + - KP/KD: 刚度和阻尼系数范围 + """ + def __init__(self, + p_min: float = -12.57, + p_max: float = 12.57, # RS03: -12.57 ~ 12.57 rad (约 -4pi ~ 4pi) + v_min: float = -20.0, + v_max: float = 20.0, # RS03: -20 ~ 20 rad/s + kp_min: float = 0.0, + kp_max: float = 5000.0, # RS03: 0 ~ 5000 + kd_min: float = 0.0, + kd_max: float = 100.0, # RS03: 0 ~ 100 + t_min: float = -60.0, + t_max: float = 60.0): # RS03: -60 ~ 60 Nm + self.P_MIN = p_min + self.P_MAX = p_max + self.V_MIN = v_min + self.V_MAX = v_max + self.KP_MIN = kp_min + self.KP_MAX = kp_max + self.KD_MIN = kd_min + self.KD_MAX = kd_max + self.T_MIN = t_min + self.T_MAX = t_max + +class ParamIndex: + """ + 电机参数索引表 (Index) + 参考说明书 4.1 可读写单个参数列表 + """ + RUN_MODE = 0x7005 # 运行模式: 0:运控, 1:PP, 2:速度, 3:电流, 5:CSP (W/R) + IQ_REF = 0x7006 # 电流模式 Iq 指令 (-43~43A) (W/R) + SPD_REF = 0x700A # 转速模式转速指令 (-20~20rad/s) (W/R) + LIMIT_TORQUE = 0x700B # 转矩限制 (0~60Nm) (W/R) + CUR_KP = 0x7010 # 电流 Kp (默认 0.17) (W/R) + CUR_KI = 0x7011 # 电流 Ki (默认 0.012) (W/R) + CUR_FILT_GAIN = 0x7014 # 电流滤波系数 (0~1.0, 默认 0.1) (W/R) + LOC_REF = 0x7016 # 位置模式角度指令 (rad) (W/R) + LIMIT_SPD = 0x7017 # 位置模式(CSP)速度限制 (0~20rad/s) (W/R) + LIMIT_CUR = 0x7018 # 速度/位置模式电流限制 (0~43A) (W/R) + MECH_POS = 0x7019 # 负载端计圈机械角度 (rad) (Read Only) + IQF = 0x701A # Iq 滤波值 (A) (Read Only) + MECH_VEL = 0x701B # 负载端转速 (rad/s) (Read Only) + VBUS = 0x701C # 母线电压 (V) (Read Only) + LOC_KP = 0x701E # 位置环 Kp (默认 60) (W/R) + SPD_KP = 0x701F # 速度环 Kp (默认 6) (W/R) + SPD_KI = 0x7020 # 速度环 Ki (默认 0.02) (W/R) + SPD_FILT_GAIN = 0x7021 # 速度滤波值 (默认 0.1) (W/R) + ACC_RAD = 0x7022 # 速度模式加速度 (默认 20rad/s^2) (W/R) + VEL_MAX = 0x7024 # 位置模式(PP)速度 (默认 10rad/s) (W/R) + ACC_SET = 0x7025 # 位置模式(PP)加速度 (默认 10rad/s^2) (W/R) + EP_SCAN_TIME = 0x7026 # 主动上报时间 (1=10ms, +1=+5ms) (W) + CAN_TIMEOUT = 0x7028 # CAN 超时阈值 (20000=1s, 0=禁用) (W) + ZERO_STA = 0x7029 # 零点标志位 (0: 0~2pi, 1: -pi~pi) (W) + DAMPER = 0x702A # 阻尼开关 (1: 取消关机反驱保护) (W/R) + ADD_OFFSET = 0x702B # 零位偏置 (rad) (W/R) + +class CanopenIndex: + """ + CANopen 对象字典常用索引 + 参考说明书第 5 章 (Canopen) + """ + ERROR_CODE = 0x603F # 错误码 + CONTROLWORD = 0x6040 # 控制字 + STATUSWORD = 0x6041 # 状态字 + MODES_OF_OPERATION = 0x6060 # 运行模式 + MODES_OF_OPERATION_DISPLAY = 0x6061 # 当前运行模式显示 + POSITION_DEMAND_VALUE = 0x6062 # 位置指令值 + POSITION_ACTUAL_VALUE = 0x6064 # 位置实际值 + POSITION_WINDOW = 0x6067 # 位置窗口 + POSITION_WINDOW_TIME = 0x6068 # 位置窗口时间 + VELOCITY_DEMAND_VALUE = 0x606B # 速度指令值 + VELOCITY_ACTUAL_VALUE = 0x606C # 速度实际值 + TARGET_TORQUE = 0x6071 # 目标力矩 (0.1% 额定力矩) + TORQUE_ACTUAL_VALUE = 0x6077 # 力矩实际值 + CURRENT_ACTUAL_VALUE = 0x6078 # 电流实际值 + DC_LINK_CIRCUIT_VOLTAGE = 0x6079 # 母线电压 + TARGET_POSITION = 0x607A # 目标位置 + PROFILE_VELOCITY = 0x6081 # 轮廓速度 + PROFILE_ACCELERATION = 0x6083 # 轮廓加速度 + TARGET_VELOCITY = 0x60FF # 目标速度 + +class CanopenModeOfOperation: + """CANopen 模式 (6060)""" + PP = 1 # Profile Position Mode + SPEED = 3 # Profile Velocity Mode + TORQUE = 4 # Profile Torque Mode + CSP = 5 # Cyclic Synchronous Position Mode + HOMING = 6 # Homing Mode + +class CanopenControlword: + """CANopen 控制字 (6040) 常用值""" + SHUTDOWN = 0x0006 # Shutdown + SWITCH_ON = 0x0007 # Switch On + ENABLE_OPERATION = 0x000F # Enable Operation + DISABLE_VOLTAGE = 0x0001 # Disable Voltage + QUICK_STOP = 0x000B # Quick Stop + +# CANopen 协议切换帧 (扩展帧) +# 说明书 5.10: 29 位 ID 为 0xFFF，数据区 Byte0~6 固定 01~06，Byte7=F_CMD(协议类型) +CANOPEN_PROTOCOL_SWITCH_EXT_ID = 0xFFF + +class MitStdCommandType: + """ + MIT 标准帧指令类型 (对应说明书第 6 章的指令 1~11) + + 标准帧 ID (11位) 结构: + | Bit 10-8 | Bit 7-0 | + | 模式/指令 | 电机 ID | + + 注意: + - 指令 1~9: CAN ID 的 Bit10~8 为 0，通过数据区 Payload 区分功能 + - 指令 10: CAN ID 的 Bit10~8 为 1 (位置模式) + - 指令 11: CAN ID 的 Bit10~8 为 2 (速度模式) + """ + ENABLE = 1 # 指令 1: 电机使能运行 + STOP = 2 # 指令 2: 电机停止运行 + DYNAMIC_PARAM = 3 # 指令 3: MIT 动态参数 + SET_ZERO = 4 # 指令 4: 设置零点 (非位置模式) + CLEAR_ERROR_OR_READ_STATUS = 5 # 指令 5: 清错 / 读取异常状态 + SET_RUN_MODE = 6 # 指令 6: 设置运行模式 + SET_MOTOR_CAN_ID = 7 # 指令 7: 修改电机 CANID + SET_PROTOCOL = 8 # 指令 8: 修改电机协议 (重新上电生效) + SET_MASTER_CAN_ID = 9 # 指令 9: 修改主机 CANID + POS_CONTROL = 10 # 指令 10: 位置模式控制指令 (ID Bit10-8=1) + SPEED_CONTROL = 11 # 指令 11: 速度模式控制指令 (ID Bit10-8=2) + +def get_mit_can_id_mode(cmd_type: int) -> int: + """ + 获取 MIT 标准帧 CAN ID 的 Bit10~8 值 + + :param cmd_type: MitStdCommandType 枚举值 + :return: 模式位 (0, 1, 或 2) + """ + if cmd_type in (MitStdCommandType.POS_CONTROL,): + return 1 + elif cmd_type in (MitStdCommandType.SPEED_CONTROL,): + return 2 + else: + # 指令 1~9 (以及其他潜在指令) 默认为 0 + return 0 + +def build_mit_std_id(cmd_type: int, motor_id: int) -> int: + """ + 构建 MIT 标准帧 11 位 CAN ID + + :param cmd_type: MitStdCommandType 枚举值 + :param motor_id: 电机 ID (0~127) + :return: 11 位 CAN ID + """ + mode = get_mit_can_id_mode(cmd_type) + return ((mode & 0x07) << 8) | (motor_id & 0xFF) + +class MitPayloads: + """ + MIT 协议特殊指令的固定 Payload 定义 (指令 1, 2, 4, 5, 6, 7, 8, 9) + 部分指令的 Payload 末尾字节需要根据参数动态修改 + """ + # 指令 1: FF FF FF FF FF FF FF FC + ENABLE = b'\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFC' + + # 指令 2: FF FF FF FF FF FF FF FD + STOP = b'\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFD' + + # 指令 3: 动态参数 (全 0 或根据参数设置) + DYNAMIC_PARAM_ZERO = b'\x00\x00\x00\x00\x00\x00\x00\x00' + + # 指令 4: FF FF FF FF FF FF FF FE + SET_ZERO = b'\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFE' + + # 指令 5: FF FF FF FF FF FF FF FB (清除错误) + # 若 F_CMD (Byte6) 为 0xFF 则清除错误，否则为读取异常状态 + CLEAR_ERROR = b'\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFB' + + # 指令 6: FF FF FF FF FF FF [Mode] FC + # Template, last 2 bytes are [Mode, FC] + SET_RUN_MODE_PREFIX = b'\xFF\xFF\xFF\xFF\xFF\xFF' + + # 指令 7: FF FF FF FF FF FF [NewID] FA + SET_MOTOR_CAN_ID_PREFIX = b'\xFF\xFF\xFF\xFF\xFF\xFF' + + # 指令 8: FF FF FF FF FF FF [Protocol] FD + SET_PROTOCOL_PREFIX = b'\xFF\xFF\xFF\xFF\xFF\xFF' + + # 指令 9: FF FF FF FF FF FF [MasterID] 01 + SET_MASTER_CAN_ID_PREFIX = b'\xFF\xFF\xFF\xFF\xFF\xFF' + + +# 参数表配置: (参数名, 数据类型, 字节数) +PARAM_TABLE = { + ParamIndex.RUN_MODE: ("run_mode", ParamType.UINT8, 1), + ParamIndex.IQ_REF: ("iq_ref", ParamType.FLOAT, 4), + ParamIndex.SPD_REF: ("spd_ref", ParamType.FLOAT, 4), + ParamIndex.LIMIT_TORQUE: ("limit_torque", ParamType.FLOAT, 4), + ParamIndex.CUR_KP: ("cur_kp", ParamType.FLOAT, 4), + ParamIndex.CUR_KI: ("cur_ki", ParamType.FLOAT, 4), + ParamIndex.CUR_FILT_GAIN: ("cur_filt_gain", ParamType.FLOAT, 4), + ParamIndex.LOC_REF: ("loc_ref", ParamType.FLOAT, 4), + ParamIndex.LIMIT_SPD: ("limit_spd", ParamType.FLOAT, 4), + ParamIndex.LIMIT_CUR: ("limit_cur", ParamType.FLOAT, 4), + ParamIndex.MECH_POS: ("mechPos", ParamType.FLOAT, 4), + ParamIndex.IQF: ("iqf", ParamType.FLOAT, 4), + ParamIndex.MECH_VEL: ("mechVel", ParamType.FLOAT, 4), + ParamIndex.VBUS: ("VBUS", ParamType.FLOAT, 4), + ParamIndex.LOC_KP: ("loc_kp", ParamType.FLOAT, 4), + ParamIndex.SPD_KP: ("spd_kp", ParamType.FLOAT, 4), + ParamIndex.SPD_KI: ("spd_ki", ParamType.FLOAT, 4), + ParamIndex.SPD_FILT_GAIN: ("spd_filt_gain", ParamType.FLOAT, 4), + ParamIndex.ACC_RAD: ("acc_rad", ParamType.FLOAT, 4), + ParamIndex.VEL_MAX: ("vel_max", ParamType.FLOAT, 4), + ParamIndex.ACC_SET: ("acc_set", ParamType.FLOAT, 4), + ParamIndex.EP_SCAN_TIME: ("EPScan_time", ParamType.UINT16, 2), + ParamIndex.CAN_TIMEOUT: ("cantimeout", ParamType.UINT32, 4), + ParamIndex.ZERO_STA: ("zero_sta", ParamType.UINT8, 1), + ParamIndex.DAMPER: ("damper", ParamType.UINT8, 1), + ParamIndex.ADD_OFFSET: ("add_offset", ParamType.FLOAT, 4), +} + +MODEL_MIT_POSITION_TABLE = { + "rs-00": 4 * np.pi, "rs-01": 4 * np.pi, "rs-02": 4 * np.pi, + "rs-03": 4 * np.pi, "rs-04": 4 * np.pi, "rs-05": 4 * np.pi, "rs-06": 4 * np.pi, + "el-05": 4 * np.pi, +} + +MODEL_MIT_VELOCITY_TABLE = { + "rs-00": 50, "rs-01": 44, "rs-02": 44, + "rs-03": 50, "rs-04": 15, "rs-05": 33, "rs-06": 20, + "el-05": 50, +} + +MODEL_MIT_TORQUE_TABLE = { + "rs-00": 17, "rs-01": 17, "rs-02": 17, + "rs-03": 60, "rs-04": 120, "rs-05": 17, "rs-06": 60, + "el-05": 6, +} + +MODEL_MIT_KP_TABLE = { + "rs-00": 500.0, "rs-01": 500.0, "rs-02": 500.0, + "rs-03": 5000.0, "rs-04": 5000.0, "rs-05": 500.0, "rs-06": 5000.0, + "el-05": 500.0, +} + +MODEL_MIT_KD_TABLE = { + "rs-00": 5.0, "rs-01": 5.0, "rs-02": 5.0, + "rs-03": 100.0, "rs-04": 100.0, "rs-05": 5.0, "rs-06": 100.0, + "el-05": 5.0, +} + +def get_pack_format(param_type): + """ + 获取 struct.pack 的格式字符串和字节大小 + + 说明: + - Type 17/18 参数读写使用小端序 + - CANopen SDO 数据同样通常按小端序解释 (取决于实现) + """ + if param_type == ParamType.UINT8: + return ' None: + """ + 发送 CAN 帧。 + + :param can_id: CAN 标识符 (标准帧或扩展帧) + :param data: 数据负载 (最多 8 字节) + :param extended: True 为扩展帧 (29位), False 为标准帧 (11位) + :param remote: True 为远程帧, False 为数据帧 + :param feedback: True 请求设备反馈 (CMD 0x01), False 不反馈 (CMD 0x03) + """ + if len(data) > 8: + raise ValueError("CAN 数据不能超过 8 字节") + + # 填充数据到 8 字节 + data_padded = data + b'\x00' * (8 - len(data)) + + cmd = 0x01 if feedback else 0x03 + send_count = 1 + interval = 10 # 默认 10ms + id_type = 1 if extended else 0 + frame_type = 1 if remote else 0 + data_len = len(data) + + # 构建帧 (30 字节) + frame = bytearray(30) + frame[0] = 0x55 + frame[1] = 0xAA + frame[2] = 0x1E # 长度 + frame[3] = cmd + + # 发送次数 (4 字节, 小端序) + frame[4:8] = struct.pack(' Optional[Tuple[int, bytes, int, bool, bool]]: + """ + 如果缓冲区中有可用数据，读取一帧 CAN 数据。 + + :return: 元组 (can_id, data, cmd, extended, remote) 或者 None (如果没有完整帧) + """ + # 读取可用数据 + if self.serial.in_waiting: + raw_data = self.serial.read(self.serial.in_waiting) + self.data_buffer.extend(raw_data) + + # 检查完整帧 (16 字节) + while len(self.data_buffer) >= self.RECV_FRAME_LEN: + # 查找帧头 + try: + header_idx = self.data_buffer.index(self.RECV_HEADER) + except ValueError: + # 没有找到帧头，清空缓冲区（保留最后几个字节以防截断） + self.data_buffer = self.data_buffer[-(self.RECV_FRAME_LEN-1):] + return None + + # 检查从帧头开始是否有足够字节 + if len(self.data_buffer) - header_idx < self.RECV_FRAME_LEN: + # 保留从帧头开始的数据 + self.data_buffer = self.data_buffer[header_idx:] + return None + + # 检查帧尾 + if self.data_buffer[header_idx + self.RECV_FRAME_LEN - 1] != self.RECV_TAIL: + # 无效帧，跳过该帧头继续查找 + self.data_buffer = self.data_buffer[header_idx + 1:] + continue + + # 提取有效帧 + frame = self.data_buffer[header_idx : header_idx + self.RECV_FRAME_LEN] + self.data_buffer = self.data_buffer[header_idx + self.RECV_FRAME_LEN:] + + if self.debug: + print(f"[DEBUG] 解析帧: {frame.hex()}") + + # 解析帧 + cmd = frame[1] + format_byte = frame[2] + + data_len = format_byte & 0x3F + ide = bool((format_byte >> 6) & 0x01) + rtr = bool((format_byte >> 7) & 0x01) + + can_id = struct.unpack(' 0) otherwise + */ + int lidar_get_mapping_result(device_handle device, const char* dest_dir, const char* file_name); + +/** + * @brief Set the image mask file for the device + * + * Read & send specified image mask file to device + * + * @param device Handle to the target device + * @param abs_path Absolute path to the image mask file (e.g., mask.png) + * @return int 0 on success, -1 on failure, -2 if file transfer in progress + */ + int lidar_set_image_mask(device_handle device, const char* abs_path); + + + /** + * @brief enable device log + * + * + * @param device Handle to the target device + * @param dest_dir Destination directory to save the logs + * @return int 0 on success, -1 on failure + */ + int lidar_enable_encrypted_device_log(device_handle device, const char* dest_dir); + + +/** + * @brief Set the depth parameters for the device + * + * This function must be called before starting data stream. + * + * @param device Handle to the target device + * @param params Pointer to the depth parameters to set + * @return int 0 on success, negative error code on failure + */ +int lidar_set_depth_parameter(device_handle device, const lidar_depth_para_t *params); + +/** + * @brief Enable or disable IMU smooth sending feature + * + * When enabled, IMU data will be sent at precise intervals (default 400Hz) + * using a dedicated high-priority thread to reduce jitter and timing variance. + * When disabled, IMU data will be sent immediately upon reception. + * + * @param enable 1 to enable smooth sending, 0 to disable + * @return int 0 on success, -1 on failure + */ +int lidar_enable_imu_smooth_sending(int enable); + +/** + * @brief Set IMU smooth sending frequency + * + * Set the target frequency for IMU smooth sending. Only effective when + * smooth sending is enabled via lidar_enable_imu_smooth_sending(). + * + * @param frequency_hz Target frequency in Hz (1-1000 Hz, recommended 400 Hz) + * @return int 0 on success, -1 on failure + */ +int lidar_set_imu_smooth_frequency(uint32_t frequency_hz); + +#ifdef __cplusplus +} +#endif + +#endif // LIDAR_API_H \ No newline at end of file diff --git a/05_software/real/sim2real/vendored/odin1_imu/include/lidar_api_type.h b/05_software/real/sim2real/vendored/odin1_imu/include/lidar_api_type.h new file mode 100644 index 0000000..4d127ae --- /dev/null +++ b/05_software/real/sim2real/vendored/odin1_imu/include/lidar_api_type.h @@ -0,0 +1,242 @@ +/* +Copyright 2025 Manifold Tech Ltd.(www.manifoldtech.com.co) +Licensed under the Apache License, Version 2.0 (the "License"); +you may not use this file except in compliance with the License. +You may obtain a copy of the License at + http://www.apache.org/licenses/LICENSE-2.0 +Unless required by applicable law or agreed to in writing, software +distributed under the License is distributed on an "AS IS" BASIS, +WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +See the License for the specific language governing permissions and +limitations under the License. +*/ +#ifndef LIDAR_TYPES_H +#define LIDAR_TYPES_H + +#include +#include +#include + +#ifdef __cplusplus +extern "C" { +#endif + +#define LIDAR_SERIAL_MAX 64 +#define LIDAR_MODEL_MAX 64 +#define LIDAR_IP_MAX 64 + +typedef void * device_handle; + +typedef enum { + LIDAR_LOG_ERROR = 0, + LIDAR_LOG_WARN, + LIDAR_LOG_INFO, + LIDAR_LOG_DEBUG, +} lidar_log_level_e; + +typedef enum { + LIDAR_OTA_ALGORITHM, + LIDAR_OTA_FIRMWARE, + LIDAR_OTA_SCRIPT, + LIDAR_OTA_CALIBRATION +} lidar_ota_type_e; + +typedef enum { + LIDAR_MODE_RAW, + LIDAR_MODE_SLAM, +} lidar_mode_e; + +typedef enum { + LIDAR_DT_NONE = 0, + LIDAR_DT_RAW_RGB, + LIDAR_DT_RAW_IMU, + LIDAR_DT_RAW_DTOF, + LIDAR_DT_SLAM_CLOUD, + LIDAR_DT_SLAM_ODOMETRY, + LIDAR_DT_DEV_STATUS, + LIDAR_DT_SLAM_ODOMETRY_HIGHFREQ, + LIDAR_DT_SLAM_ODOMETRY_TF, + LIDAR_DT_SLAM_WIWC, + LIDAR_DT_NTP +} lidar_data_type_e; + +typedef struct { + int8_t serial[LIDAR_SERIAL_MAX]; + int8_t model[LIDAR_MODEL_MAX]; + bool online; + uint32_t initial_state; +} lidar_device_info_t; + +typedef struct { + float x, y, z; + float intensity; +} lidar_point_t; + + +typedef struct { + float intrinsics[9]; + float extrinsics[16]; +} lidar_calibration_t; + + +#define DEVICE_MAX_CH_NUMBER 4 + +typedef struct { + uint64_t timestamp_ns; + int64_t pos[3]; + int64_t orient[4]; +} ros2_odom_convert_t; + +typedef struct { + uint64_t timestamp_ns; + int64_t pos[3]; + int64_t orient[4]; + int64_t linear_velocity[3]; + int64_t angular_velocity[3]; + double pose_cov[36]; + double twist_cov[36]; +} ros_odom_convert_complete_t; + +typedef struct { + float accel_x; + float accel_y; + float accel_z; + float gyro_x; + float gyro_y; + float gyro_z; + uint64_t stamp; + uint64_t sequence; +} imu_convert_data_t; + + typedef struct { + uint32_t length; + uint64_t sequence; + uint64_t timestamp; + uint64_t interval; + void* pAddr; + uint32_t width; + uint32_t height; +} buffer_List_t; + +typedef struct { + double delay; + double offset; +} ptp_sync_data_t; + +typedef struct capture_Image_List_t { + uint32_t imageCount; + buffer_List_t imageList[DEVICE_MAX_CH_NUMBER]; +} capture_Image_List_t; + +typedef struct { + uint32_t type; + capture_Image_List_t stream; +} lidar_data_t; + +typedef void (*lidar_device_callback_t)(const lidar_device_info_t* device, bool attach); +typedef void (*lidar_data_callback_t)(const lidar_data_t *data, void *user_data); + +typedef struct { + lidar_data_callback_t data_callback; + void *user_data; +} lidar_data_callback_info_t; + +typedef struct { + int major; + int minor; + int patch; +}lidar_version_t; + +typedef struct { + lidar_version_t kernel_version; + lidar_version_t mcu_version; + lidar_version_t soc_version; + lidar_version_t Daemon_proc_version; + lidar_version_t slam_version; +} lidar_fireware_version_t; + +/** + * @brief RGB image sensor frame rate + * + */ + typedef struct{ + + int configured_odr; /* rgb image sensor configured output data rate */ + int tx_odr; /* rgb image sensor tx output data rate */ + +} lidar_rgb_sensor_status_t; + +/** + * @brief DTOF Lidar frame rate + * + */ +typedef struct{ + + int configured_odr; /* dtof lidar sensor configured output data rate */ + int tx_odr; /* dtof lidar sensor tx output data rate */ + int subframe_odr; /* dtof lidar sensor subframe output data rate */ + short tx_temp; /* dtof lidar tx module temp */ + short rx_temp; /* dtof lidar rx module temp */ + +} lidar_dtof_sensor_status_t; + +/** + * @brief IMU Sensor + * + */ +typedef struct{ + + int configured_odr; /* imu sensor configured output data rate */ + int tx_odr; /* imu sensor tx output data rate */ + +} lidar_imu_sensor_status_t; + +typedef struct{ + + int package_temp; /* soc package temp */ + int cpu_temp; /* cpu temp */ + int center_temp; /* center temp */ + int gpu_temp; /* gpu temp */ + int npu_temp; /* npu temp */ + +} lidar_soc_thermal_t; +typedef struct +{ + double uptime_seconds; + lidar_soc_thermal_t soc_thermal; + + int cpu_use_rate[8]; /* cpu usage rate */ + int ram_use_rate; /* ram usage rate */ + + lidar_rgb_sensor_status_t rgb_sensor; + lidar_dtof_sensor_status_t dtof_sensor; + lidar_imu_sensor_status_t imu_sensor; + + int slam_cloud_tx_odr; /* slam cloud tx output data rate */ + int slam_odom_tx_odr; /* slam odom tx output data rate */ + int slam_odom_highfreq_tx_odr; /* slam odom high freq tx output data rate */ + +} lidar_device_status_t; + +typedef enum { + LIDAR_DEVICE_NONE = 0, + LIDAR_DEVICE_NOT_INITIALIZED, + LIDAR_DEVICE_INITIALIZED, + LIDAR_DEVICE_STREAMING, + LIDAR_DEVICE_STREAM_STOPPED, +} lidar_device_initial_state_e; + +typedef enum { + LIDAR_DEPTH_ODR_10HZ = 0, + LIDAR_DEPTH_ODR_14_5HZ, +} lidar_depth_odr_e; + +typedef struct { + lidar_depth_odr_e odr; +} lidar_depth_para_t; + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/05_software/real/sim2real/vendored/odin1_imu/include/odin1_imu_bridge.h b/05_software/real/sim2real/vendored/odin1_imu/include/odin1_imu_bridge.h new file mode 100644 index 0000000..0f6bba8 --- /dev/null +++ b/05_software/real/sim2real/vendored/odin1_imu/include/odin1_imu_bridge.h @@ -0,0 +1,86 @@ +#ifndef ODIN1_IMU_BRIDGE_H +#define ODIN1_IMU_BRIDGE_H + +#include + +#ifdef __cplusplus +extern "C" { +#endif + +/** + * 输入: 无 + * 输出: odin1_imu_sample_t + * 作用: 描述一帧 IMU 数据, 供 C/C++/Python 共享使用 + */ +typedef struct odin1_imu_sample_t { + float accel_x; + float accel_y; + float accel_z; + float gyro_x; + float gyro_y; + float gyro_z; + uint64_t stamp_ns; + uint64_t sequence; +} odin1_imu_sample_t; + +/** + * 输入: 无 + * 输出: const char* + * 作用: 返回当前 bridge 的版本字符串 + */ +const char* odin1_imu_version(void); + +/** + * 输入: timeout_ms[int] + * 输出: int, 0 表示成功, 非 0 表示失败 + * 作用: 初始化 SDK, 等待设备连接并开始 IMU 数据流 + */ +int odin1_imu_start(int timeout_ms); + +/** + * 输入: 无 + * 输出: 无 + * 作用: 停止数据流并释放 SDK 资源 + */ +void odin1_imu_stop(void); + +/** + * 输入: 无 + * 输出: int, 1 表示运行中, 0 表示未运行 + * 作用: 返回当前 bridge 是否处于运行状态 + */ +int odin1_imu_is_running(void); + +/** + * 输入: timeout_ms[int] + * 输出: int, 1 表示有数据可读, 0 表示超时, 负数表示异常 + * 作用: 阻塞等待 IMU 数据到达 + */ +int odin1_imu_wait_for_data(int timeout_ms); + +/** + * 输入: out_sample[odin1_imu_sample_t*] + * 输出: int, 1 表示成功取出一帧, 0 表示队列为空, 负数表示异常 + * 作用: 从内部队列中弹出一帧 IMU 数据 + */ +int odin1_imu_pop_sample(odin1_imu_sample_t* out_sample); + +/** + * 输入: out_sample[odin1_imu_sample_t*] + * 输出: int, 1 表示成功读取, 0 表示当前还没有数据, 负数表示异常 + * 作用: 获取最近一帧 IMU 数据, 不会从队列中删除 + */ +int odin1_imu_get_latest(odin1_imu_sample_t* out_sample); + +/** + * 输入: 无 + * 输出: const char* + * 作用: 返回最近一次错误信息 + */ +const char* odin1_imu_last_error(void); + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/05_software/real/sim2real/vendored/odin1_imu/lib/liblydHostApi_arm.a b/05_software/real/sim2real/vendored/odin1_imu/lib/liblydHostApi_arm.a new file mode 100644 index 0000000..67f027c Binary files /dev/null and b/05_software/real/sim2real/vendored/odin1_imu/lib/liblydHostApi_arm.a differ diff --git a/05_software/real/sim2real/vendored/odin1_imu/odin1_imu.py b/05_software/real/sim2real/vendored/odin1_imu/odin1_imu.py new file mode 100644 index 0000000..66119f2 --- /dev/null +++ b/05_software/real/sim2real/vendored/odin1_imu/odin1_imu.py @@ -0,0 +1,124 @@ +#!/usr/bin/python3 +"""ODIN1 IMU ctypes 封装.""" + +from __future__ import annotations + +import ctypes +from pathlib import Path +from typing import Iterator, Optional + + +class Odin1ImuSample(ctypes.Structure): + """输入: 无; 输出: Odin1ImuSample; 作用: 映射 C++ bridge 的 IMU 结构体.""" + + _fields_ = [ + ("accel_x", ctypes.c_float), + ("accel_y", ctypes.c_float), + ("accel_z", ctypes.c_float), + ("gyro_x", ctypes.c_float), + ("gyro_y", ctypes.c_float), + ("gyro_z", ctypes.c_float), + ("stamp_ns", ctypes.c_uint64), + ("sequence", ctypes.c_uint64), + ] + + +class Odin1ImuClient: + """输入: lib_path[Optional[str|Path]]; 输出: Odin1ImuClient; 作用: 提供 Python 对 ODIN1 IMU bridge 的访问接口.""" + + def __init__(self, lib_path: Optional[str | Path] = None) -> None: + self._project_root = Path(__file__).resolve().parents[1] + resolved_path = Path(lib_path) if lib_path else self._project_root / "build" / "libodin1_imu_bridge.so" + self._lib = ctypes.CDLL(str(resolved_path)) + self._configure_signatures() + + def _configure_signatures(self) -> None: + """输入: 无; 输出: 无; 作用: 配置 ctypes 函数签名.""" + + self._lib.odin1_imu_version.restype = ctypes.c_char_p + + self._lib.odin1_imu_start.argtypes = [ctypes.c_int] + self._lib.odin1_imu_start.restype = ctypes.c_int + + self._lib.odin1_imu_stop.argtypes = [] + self._lib.odin1_imu_stop.restype = None + + self._lib.odin1_imu_is_running.argtypes = [] + self._lib.odin1_imu_is_running.restype = ctypes.c_int + + self._lib.odin1_imu_wait_for_data.argtypes = [ctypes.c_int] + self._lib.odin1_imu_wait_for_data.restype = ctypes.c_int + + self._lib.odin1_imu_pop_sample.argtypes = [ctypes.POINTER(Odin1ImuSample)] + self._lib.odin1_imu_pop_sample.restype = ctypes.c_int + + self._lib.odin1_imu_get_latest.argtypes = [ctypes.POINTER(Odin1ImuSample)] + self._lib.odin1_imu_get_latest.restype = ctypes.c_int + + self._lib.odin1_imu_last_error.argtypes = [] + self._lib.odin1_imu_last_error.restype = ctypes.c_char_p + + def version(self) -> str: + """输入: 无; 输出: str; 作用: 获取 C++ bridge 版本号.""" + + return self._lib.odin1_imu_version().decode("utf-8") + + def last_error(self) -> str: + """输入: 无; 输出: str; 作用: 获取最近一次 bridge 错误信息.""" + + return self._lib.odin1_imu_last_error().decode("utf-8") + + def start(self, timeout_ms: int = 5000) -> None: + """输入: timeout_ms[int]; 输出: 无; 作用: 启动 IMU 数据接收.""" + + result = self._lib.odin1_imu_start(timeout_ms) + if result != 0: + raise RuntimeError(f"启动 ODIN1 IMU 失败: {self.last_error()} (code={result})") + + def stop(self) -> None: + """输入: 无; 输出: 无; 作用: 停止 IMU 数据接收.""" + + self._lib.odin1_imu_stop() + + def is_running(self) -> bool: + """输入: 无; 输出: bool; 作用: 返回 bridge 是否仍在运行.""" + + return bool(self._lib.odin1_imu_is_running()) + + def wait_for_data(self, timeout_ms: int = 1000) -> bool: + """输入: timeout_ms[int]; 输出: bool; 作用: 等待 IMU 数据到达.""" + + result = self._lib.odin1_imu_wait_for_data(timeout_ms) + if result < 0: + raise RuntimeError(f"等待 IMU 数据失败: {self.last_error()} (code={result})") + return bool(result) + + def pop_sample(self) -> Optional[Odin1ImuSample]: + """输入: 无; 输出: Optional[Odin1ImuSample]; 作用: 从队列中取出一帧 IMU 数据.""" + + sample = Odin1ImuSample() + result = self._lib.odin1_imu_pop_sample(ctypes.byref(sample)) + if result < 0: + raise RuntimeError(f"读取 IMU 队列失败: {self.last_error()} (code={result})") + return sample if result == 1 else None + + def get_latest(self) -> Optional[Odin1ImuSample]: + """输入: 无; 输出: Optional[Odin1ImuSample]; 作用: 获取最近一帧 IMU 数据.""" + + sample = Odin1ImuSample() + result = self._lib.odin1_imu_get_latest(ctypes.byref(sample)) + if result < 0: + raise RuntimeError(f"读取最新 IMU 数据失败: {self.last_error()} (code={result})") + return sample if result == 1 else None + + def iter_samples(self, timeout_ms: int = 1000) -> Iterator[Odin1ImuSample]: + """输入: timeout_ms[int]; 输出: Iterator[Odin1ImuSample]; 作用: 连续迭代输出 IMU 数据.""" + + while self.is_running(): + if not self.wait_for_data(timeout_ms): + continue + while True: + sample = self.pop_sample() + if sample is None: + break + yield sample diff --git a/05_software/real/sim2real/vendored/odin1_imu/src/odin1_imu_bridge.cpp b/05_software/real/sim2real/vendored/odin1_imu/src/odin1_imu_bridge.cpp new file mode 100644 index 0000000..91069c2 --- /dev/null +++ b/05_software/real/sim2real/vendored/odin1_imu/src/odin1_imu_bridge.cpp @@ -0,0 +1,376 @@ +#include "odin1_imu_bridge.h" + +#include "lidar_api.h" +#include "lidar_api_type.h" + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace { + +constexpr const char* kBridgeVersion = "0.1.0"; +constexpr std::size_t kMaxQueueSize = 1024; +constexpr int kDefaultMode = LIDAR_MODE_SLAM; + +std::atomic g_running{false}; +std::atomic g_sdk_initialized{false}; +std::atomic g_device_connected{false}; +std::atomic g_stream_started{false}; + +device_handle g_device = nullptr; + +std::mutex g_state_mutex; +std::mutex g_queue_mutex; +std::condition_variable g_queue_cv; +std::deque g_queue; +odin1_imu_sample_t g_latest_sample{}; +bool g_has_latest_sample = false; + +std::mutex g_error_mutex; +std::string g_last_error = "bridge not started"; + +/** + * 输入: message[const std::string&] + * 输出: 无 + * 作用: 线程安全地记录最近一次错误信息 + */ +void set_last_error(const std::string& message) { + std::lock_guard lock(g_error_mutex); + g_last_error = message; +} + +/** + * 输入: 无 + * 输出: 无 + * 作用: 清空内部 IMU 队列和最近一帧缓存 + */ +void clear_queue_locked_state() { + std::lock_guard lock(g_queue_mutex); + g_queue.clear(); + g_latest_sample = {}; + g_has_latest_sample = false; +} + +/** + * 输入: raw_sample[const imu_convert_data_t*] + * 输出: odin1_imu_sample_t + * 作用: 将 SDK IMU 结构转换为 bridge 对外结构 + */ +odin1_imu_sample_t convert_sample(const imu_convert_data_t* raw_sample) { + odin1_imu_sample_t converted{}; + if (raw_sample == nullptr) { + return converted; + } + + converted.accel_x = raw_sample->accel_x; + converted.accel_y = raw_sample->accel_y; + converted.accel_z = raw_sample->accel_z; + converted.gyro_x = raw_sample->gyro_x; + converted.gyro_y = raw_sample->gyro_y; + converted.gyro_z = raw_sample->gyro_z; + converted.stamp_ns = raw_sample->stamp; + converted.sequence = raw_sample->sequence; + return converted; +} + +/** + * 输入: 无 + * 输出: 无 + * 作用: 安全关闭当前设备与 SDK 资源 + */ +void cleanup_device_and_sdk() { + std::lock_guard lock(g_state_mutex); + + if (g_device != nullptr) { + try { + if (g_stream_started.load()) { + lidar_deactivate_stream_type(g_device, LIDAR_DT_RAW_IMU); + lidar_stop_stream(g_device, kDefaultMode); + g_stream_started = false; + } + + lidar_unregister_stream_callback(g_device); + lidar_close_device(g_device); + lidar_destory_device(g_device); + } catch (...) { + } + g_device = nullptr; + } + + if (g_sdk_initialized.load()) { + try { + lidar_system_deinit(); + } catch (...) { + } + g_sdk_initialized = false; + } + + g_device_connected = false; +} + +/** + * 输入: data[const lidar_data_t*], user_data[void*] + * 输出: 无 + * 作用: 接收 SDK 回调中的 IMU 数据并写入内部缓存队列 + */ +void lidar_data_callback(const lidar_data_t* data, void* user_data) { + (void)user_data; + + if (!g_running.load() || data == nullptr) { + return; + } + + if (data->type != LIDAR_DT_RAW_IMU) { + return; + } + + if (data->stream.imageList[0].pAddr == nullptr) { + set_last_error("sdk imu callback returned null payload"); + return; + } + + const auto* raw_sample = + static_cast(data->stream.imageList[0].pAddr); + odin1_imu_sample_t sample = convert_sample(raw_sample); + + { + std::lock_guard lock(g_queue_mutex); + if (g_queue.size() >= kMaxQueueSize) { + g_queue.pop_front(); + } + g_queue.push_back(sample); + g_latest_sample = sample; + g_has_latest_sample = true; + } + + g_queue_cv.notify_all(); +} + +/** + * 输入: device_info[const lidar_device_info_t*], attach[bool] + * 输出: 无 + * 作用: 响应 SDK 设备插拔事件并启动 IMU 数据流 + */ +void lidar_device_callback(const lidar_device_info_t* device_info, bool attach) { + if (!g_running.load()) { + return; + } + + if (!attach) { + g_device_connected = false; + g_stream_started = false; + return; + } + + if (device_info == nullptr) { + set_last_error("sdk device callback returned null device info"); + return; + } + + std::lock_guard lock(g_state_mutex); + + if (g_device != nullptr) { + return; + } + + device_handle device_handle_local = nullptr; + if (lidar_create_device(const_cast(device_info), &device_handle_local) != 0) { // SDK接口，来源: include/lidar_api.h + set_last_error("lidar_create_device failed"); + return; + } + + if (lidar_open_device(device_handle_local) != 0) { // SDK接口，来源: include/lidar_api.h + set_last_error("lidar_open_device failed"); + lidar_destory_device(device_handle_local); // SDK接口，来源: include/lidar_api.h + return; + } + + lidar_data_callback_info_t callback_info{}; + callback_info.data_callback = lidar_data_callback; + callback_info.user_data = nullptr; + if (lidar_register_stream_callback(device_handle_local, callback_info) != 0) { // SDK接口，来源: include/lidar_api.h + set_last_error("lidar_register_stream_callback failed"); + lidar_close_device(device_handle_local); // SDK接口，来源: include/lidar_api.h + lidar_destory_device(device_handle_local); // SDK接口，来源: include/lidar_api.h + return; + } + + uint32_t dtof_subframe_odr = 0; + if (lidar_start_stream(device_handle_local, kDefaultMode, dtof_subframe_odr) != 0) { // SDK接口，来源: include/lidar_api.h + (void)dtof_subframe_odr; + set_last_error("lidar_start_stream failed"); + lidar_unregister_stream_callback(device_handle_local); // SDK接口，来源: include/lidar_api.h + lidar_close_device(device_handle_local); // SDK接口，来源: include/lidar_api.h + lidar_destory_device(device_handle_local); // SDK接口，来源: include/lidar_api.h + return; + } + + if (lidar_activate_stream_type(device_handle_local, LIDAR_DT_RAW_IMU) != 0) { // SDK接口，来源: include/lidar_api.h + set_last_error("lidar_activate_stream_type(raw_imu) failed"); + lidar_stop_stream(device_handle_local, kDefaultMode); // SDK接口，来源: include/lidar_api.h + lidar_unregister_stream_callback(device_handle_local); // SDK接口，来源: include/lidar_api.h + lidar_close_device(device_handle_local); // SDK接口，来源: include/lidar_api.h + lidar_destory_device(device_handle_local); // SDK接口，来源: include/lidar_api.h + return; + } + + g_device = device_handle_local; + g_stream_started = true; + g_device_connected = true; + set_last_error(""); + g_queue_cv.notify_all(); +} + +} // namespace + +extern "C" { + +/** + * 输入: 无 + * 输出: const char* + * 作用: 返回当前 bridge 的版本字符串 + */ +const char* odin1_imu_version(void) { + return kBridgeVersion; +} + +/** + * 输入: timeout_ms[int] + * 输出: int, 0 表示成功, 非 0 表示失败 + * 作用: 初始化 SDK, 等待设备连接并开始 IMU 数据流 + */ +int odin1_imu_start(int timeout_ms) { + if (timeout_ms <= 0) { + timeout_ms = 5000; + } + + if (g_running.load()) { + return 0; + } + + clear_queue_locked_state(); + set_last_error("waiting for odin1 device"); + + if (lidar_system_init(lidar_device_callback) != 0) { // SDK接口，来源: include/lidar_api.h + set_last_error("lidar_system_init failed"); + return -1; + } + + g_sdk_initialized = true; + g_running = true; + + const auto deadline = std::chrono::steady_clock::now() + std::chrono::milliseconds(timeout_ms); + while (std::chrono::steady_clock::now() < deadline) { + if (g_device_connected.load()) { + return 0; + } + std::this_thread::sleep_for(std::chrono::milliseconds(50)); + } + + set_last_error("timeout waiting for odin1 imu stream"); + odin1_imu_stop(); + return -2; +} + +/** + * 输入: 无 + * 输出: 无 + * 作用: 停止数据流并释放 SDK 资源 + */ +void odin1_imu_stop(void) { + g_running = false; + cleanup_device_and_sdk(); + clear_queue_locked_state(); + g_queue_cv.notify_all(); +} + +/** + * 输入: 无 + * 输出: int, 1 表示运行中, 0 表示未运行 + * 作用: 返回当前 bridge 是否处于运行状态 + */ +int odin1_imu_is_running(void) { + return g_running.load() ? 1 : 0; +} + +/** + * 输入: timeout_ms[int] + * 输出: int, 1 表示有数据可读, 0 表示超时, 负数表示异常 + * 作用: 阻塞等待 IMU 数据到达 + */ +int odin1_imu_wait_for_data(int timeout_ms) { + if (!g_running.load()) { + return -1; + } + + std::unique_lock lock(g_queue_mutex); + const bool ready = g_queue_cv.wait_for( + lock, + std::chrono::milliseconds(timeout_ms > 0 ? timeout_ms : 1000), + [] { return !g_queue.empty() || !g_running.load(); }); + + if (!g_running.load()) { + return -1; + } + + return ready && !g_queue.empty() ? 1 : 0; +} + +/** + * 输入: out_sample[odin1_imu_sample_t*] + * 输出: int, 1 表示成功取出一帧, 0 表示队列为空, 负数表示异常 + * 作用: 从内部队列中弹出一帧 IMU 数据 + */ +int odin1_imu_pop_sample(odin1_imu_sample_t* out_sample) { + if (out_sample == nullptr) { + set_last_error("odin1_imu_pop_sample received null output pointer"); + return -1; + } + + std::lock_guard lock(g_queue_mutex); + if (g_queue.empty()) { + return 0; + } + + *out_sample = g_queue.front(); + g_queue.pop_front(); + return 1; +} + +/** + * 输入: out_sample[odin1_imu_sample_t*] + * 输出: int, 1 表示成功读取, 0 表示当前还没有数据, 负数表示异常 + * 作用: 获取最近一帧 IMU 数据, 不会从队列中删除 + */ +int odin1_imu_get_latest(odin1_imu_sample_t* out_sample) { + if (out_sample == nullptr) { + set_last_error("odin1_imu_get_latest received null output pointer"); + return -1; + } + + std::lock_guard lock(g_queue_mutex); + if (!g_has_latest_sample) { + return 0; + } + + *out_sample = g_latest_sample; + return 1; +} + +/** + * 输入: 无 + * 输出: const char* + * 作用: 返回最近一次错误信息 + */ +const char* odin1_imu_last_error(void) { + std::lock_guard lock(g_error_mutex); + return g_last_error.c_str(); +} + +} // extern "C" diff --git a/05_software/real/sim2real/web/__init__.py b/05_software/real/sim2real/web/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/05_software/real/sim2real/web/server.py b/05_software/real/sim2real/web/server.py new file mode 100644 index 0000000..eb3b786 --- /dev/null +++ b/05_software/real/sim2real/web/server.py @@ -0,0 +1,353 @@ +"""Minimal HTTP + SSE server for the sim2real web console.""" + +from __future__ import annotations + +import argparse +import json +import queue +import sys +import threading +import time +import traceback +from http.server import BaseHTTPRequestHandler, ThreadingHTTPServer +from pathlib import Path +from urllib.parse import urlparse + +import yaml + +sys.path.insert(0, str(Path(__file__).resolve().parents[1])) + +from web.session import RobotSession # noqa: E402 + + +SESSION: "RobotSession" = None # type: ignore + + +def make_real_factory(): + def outer(): + def factory(can1_port, can2_port, debug): + sim2real_root = Path(__file__).resolve().parents[1] + for path in ( + sim2real_root / "vendored", + "/home/rc2/work/rcwork/control", + "/home/rc2/work/rcwork", + ): + path_str = str(path) + if path_str not in sys.path and Path(path).exists(): + sys.path.append(path_str) + from drivers.motor_driver import RobStrideDriver # type: ignore + + return RobStrideDriver(can1_port, debug), RobStrideDriver(can2_port, debug) + + return factory + + return outer + + +def make_dry_factory(): + def outer(): + class MockMotor: + def __init__(self): + class State: + position = 0.0 + velocity = 0.0 + torque = 0.0 + + self.state = State() + + class MockDriver: + def __init__(self, port, debug): + self.port = port + self.motors = {} + + def connect(self): + pass + + def disconnect(self): + pass + + def add_motor(self, name, motor_id, model): + self.motors[name] = MockMotor() + + def enable(self, name): + pass + + def disable(self, name): + pass + + def clear_warnings(self, name): + pass + + def process_messages(self): + pass + + def control_mit(self, *args, **kwargs): + pass + + def factory(can1_port, can2_port, debug): + return MockDriver(can1_port, debug), MockDriver(can2_port, debug) + + return factory + + return outer + + +def _send_json(handler: BaseHTTPRequestHandler, code: int, obj): + body = json.dumps(obj, ensure_ascii=False).encode("utf-8") + handler.send_response(code) + handler.send_header("Content-Type", "application/json; charset=utf-8") + handler.send_header("Content-Length", str(len(body))) + handler.send_header("Cache-Control", "no-store") + handler.end_headers() + handler.wfile.write(body) + + +def _send_static(handler: BaseHTTPRequestHandler, path: Path, content_type: str): + if not path.exists(): + handler.send_error(404, str(path)) + return + body = path.read_bytes() + handler.send_response(200) + handler.send_header("Content-Type", content_type) + handler.send_header("Content-Length", str(len(body))) + handler.end_headers() + handler.wfile.write(body) + + +class Handler(BaseHTTPRequestHandler): + server_version = "Sim2RealConsole/1.1" + + def log_message(self, fmt, *args): + if "GET /events" in (fmt % args): + return + super().log_message(fmt, *args) + + def do_GET(self): + url = urlparse(self.path) + if url.path in ("/", "/index.html"): + return _send_static(self, Path(__file__).parent / "static" / "index.html", "text/html; charset=utf-8") + if url.path == "/static/app.js": + return _send_static(self, Path(__file__).parent / "static" / "app.js", "application/javascript; charset=utf-8") + if url.path == "/static/style.css": + return _send_static(self, Path(__file__).parent / "static" / "style.css", "text/css; charset=utf-8") + if url.path.startswith("/static/viewer/"): + viewer_file = url.path.split("/static/viewer/", 1)[1] + viewer_path = Path(__file__).parent / "static" / "viewer" / viewer_file + content_type = "text/javascript" if not viewer_file.endswith(".css") else "text/css" + return _send_static(self, viewer_path, content_type) + if url.path.startswith("/meshes/"): + mesh_name = url.path.split("/meshes/", 1)[1] + mesh_path = Path(__file__).resolve().parents[1] / "mjcf" / "meshes" / mesh_name + if not mesh_path.exists(): + return self.send_error(404, f"mesh not found: {mesh_name}") + self.send_response(200) + self.send_header("Content-Type", "application/octet-stream") + self.send_header("Content-Length", str(mesh_path.stat().st_size)) + self.send_header("Cache-Control", "max-age=3600") + self.end_headers() + with open(mesh_path, "rb") as file_obj: + while True: + chunk = file_obj.read(64 * 1024) + if not chunk: + break + self.wfile.write(chunk) + return + if url.path.startswith("/mjcf/"): + mjcf_name = url.path.split("/mjcf/", 1)[1] + mjcf_path = Path(__file__).resolve().parents[1] / "mjcf" / mjcf_name + if not mjcf_path.exists(): + return self.send_error(404, f"mjcf not found: {mjcf_name}") + self.send_response(200) + self.send_header("Content-Type", "application/xml; charset=utf-8") + self.send_header("Content-Length", str(mjcf_path.stat().st_size)) + self.end_headers() + self.wfile.write(mjcf_path.read_bytes()) + return + if url.path == "/api/status": + return _send_json(self, 200, SESSION.get_status()) + if url.path == "/api/debug": + return _send_json(self, 200, SESSION.get_debug_snapshot()) + if url.path == "/api/logs": + return _send_json(self, 200, {"sessions": SESSION.list_logs()}) + if url.path.startswith("/api/logs/"): + parts = url.path.split("/") + if len(parts) >= 5: + session_id = parts[3] + filename = parts[4] + file_path = Path(SESSION.cfg.get("log_dir", "logs")) / session_id / filename + if file_path.exists() and filename in ("state.csv", "events.jsonl"): + self.send_response(200) + self.send_header( + "Content-Type", + "text/csv" if filename.endswith("csv") else "application/json", + ) + self.send_header("Content-Disposition", f'attachment; filename="{session_id}_{filename}"') + self.send_header("Content-Length", str(file_path.stat().st_size)) + self.end_headers() + with open(file_path, "rb") as file_obj: + while True: + chunk = file_obj.read(64 * 1024) + if not chunk: + break + self.wfile.write(chunk) + return + return self.send_error(404) + if url.path == "/events": + return self._handle_sse() + return self.send_error(404, self.path) + + def do_POST(self): + url = urlparse(self.path) + try: + length = int(self.headers.get("Content-Length", "0")) + body = self.rfile.read(length) if length else b"" + data = json.loads(body) if body else {} + except Exception as exc: + SESSION.note_api_error() + return _send_json(self, 400, {"error": f"bad body: {exc}"}) + + try: + result = self._handle_post(url.path, data) + except Exception as exc: + SESSION.note_api_error() + return _send_json( + self, + 500, + { + "error": f"{type(exc).__name__}: {exc}", + "traceback": traceback.format_exc(), + }, + ) + if result is None: + return self.send_error(404) + return _send_json(self, 200, {"ok": True, **(result if isinstance(result, dict) else {})}) + + def _handle_post(self, path: str, data: dict): + if path == "/api/connect": + return {"queued": SESSION.connect(dry_run=bool(data.get("dry_run", False)))} + if path == "/api/disconnect": + return {"queued": SESSION.disconnect()} + if path == "/api/enable": + return {"queued": SESSION.enable_motors()} + if path == "/api/disable": + return {"queued": SESSION.disable_motors()} + if path == "/api/test_motor": + return { + "queued": SESSION.test_motor( + leg=data["leg"], + joint=data["joint"], + delta_rad=float(data.get("delta_rad", 0.1)), + kp=float(data.get("kp", 5.0)), + kd=float(data.get("kd", 1.0)), + duration_s=float(data.get("duration_s", 1.0)), + ) + } + if path == "/api/calibrate_offsets": + return { + "queued": SESSION.calibrate_offsets( + target_pose_name=data.get("target_pose", "stand"), + samples=int(data.get("samples", 100)), + ) + } + if path == "/api/startup": + return {"queued": SESSION.startup()} + if path == "/api/runtime/start": + return {"queued": SESSION.runtime_start(policy_path=data.get("policy_path"))} + if path == "/api/runtime/stop": + return {"queued": SESSION.runtime_stop()} + if path == "/api/cmd": + SESSION.set_command( + vx=float(data.get("vx", 0.0)), + vy=float(data.get("vy", 0.0)), + yaw=float(data.get("yaw", 0.0)), + ) + return {} + if path == "/api/estop": + SESSION.estop() + return {} + if path == "/api/reset_estop": + SESSION.reset_estop() + return {} + return None + + def _handle_sse(self): + self.send_response(200) + self.send_header("Content-Type", "text/event-stream") + self.send_header("Cache-Control", "no-cache") + self.send_header("Connection", "keep-alive") + self.send_header("Access-Control-Allow-Origin", "*") + self.end_headers() + + event_queue: "queue.Queue" = queue.Queue(maxsize=1024) + SESSION.add_listener(event_queue) + try: + initial = {"kind": "STATUS_FULL", **SESSION.get_status()} + self.wfile.write(f"data: {json.dumps(initial, ensure_ascii=False)}\n\n".encode()) + self.wfile.flush() + last_keepalive = time.time() + while True: + try: + event = event_queue.get(timeout=1.0) + self.wfile.write(f"data: {json.dumps(event, ensure_ascii=False)}\n\n".encode()) + self.wfile.flush() + except queue.Empty: + if time.time() - last_keepalive > 15: + self.wfile.write(b": keepalive\n\n") + self.wfile.flush() + last_keepalive = time.time() + except (BrokenPipeError, ConnectionResetError): + pass + finally: + SESSION.remove_listener(event_queue) + + +def main(): + parser = argparse.ArgumentParser() + parser.add_argument("--port", type=int, default=8080) + parser.add_argument("--host", default="0.0.0.0") + parser.add_argument("--config", default=str(Path(__file__).resolve().parents[1] / "config.yaml")) + parser.add_argument("--dry-run", action="store_true") + args = parser.parse_args() + + cfg_path = Path(args.config) + with open(cfg_path, "r", encoding="utf-8") as file_obj: + cfg = yaml.safe_load(file_obj) + + global SESSION + SESSION = RobotSession( + cfg=cfg, + cfg_path=cfg_path, + driver_factory_real=make_real_factory(), + driver_factory_dry=make_dry_factory(), + ) + + def _pulse(): + while True: + try: + SESSION._broadcast({"kind": "PULSE", **SESSION.get_status()}) + except Exception: + pass + time.sleep(1.0) + + threading.Thread(target=_pulse, daemon=True).start() + + httpd = ThreadingHTTPServer((args.host, args.port), Handler) + print(f"\n[Web] sim2real console -> http://{args.host}:{args.port}\n") + try: + httpd.serve_forever() + except KeyboardInterrupt: + print("\n[Web] Ctrl+C received, shutting down...") + finally: + try: + SESSION.estop() + except Exception: + pass + try: + SESSION._do_disconnect() + except Exception: + pass + httpd.server_close() + + +if __name__ == "__main__": + main() diff --git a/05_software/real/sim2real/web/session.py b/05_software/real/sim2real/web/session.py new file mode 100644 index 0000000..aefb91a --- /dev/null +++ b/05_software/real/sim2real/web/session.py @@ -0,0 +1,1141 @@ +"""Web-facing session state machine for current sim2real deployment.""" + +from __future__ import annotations + +import threading +import time +import traceback +from collections import deque +from dataclasses import asdict, dataclass, field +from enum import Enum +from pathlib import Path +from typing import Any, Callable, Dict, Optional + +import numpy as np + +from tools.logger import LogBundle + + +class Stage(str, Enum): + DISCONNECTED = "DISCONNECTED" + CONNECTING = "CONNECTING" + CONNECTED = "CONNECTED" + ENABLING = "ENABLING" + ENABLED = "ENABLED" + JOINT_TEST = "JOINT_TEST" + CALIBRATING = "CALIBRATING" + STARTING_UP = "STARTING_UP" + STAND_HOLD = "STAND_HOLD" + RUNTIME = "RUNTIME" + FAULTED = "FAULTED" + ESTOPPED = "ESTOPPED" + + +@dataclass +class SessionStatus: + stage: str = Stage.DISCONNECTED.value + detail: str = "" + last_event: str = "" + busy: bool = False + cmd: list = field(default_factory=lambda: [0.0, 0.0, 0.0]) + last_state: Optional[Dict[str, Any]] = None + log_dir: Optional[str] = None + fault_reason: Optional[str] = None + last_error: Optional[str] = None + last_traceback: Optional[str] = None + diagnostics: Dict[str, Any] = field(default_factory=dict) + + +class RobotSession: + JOINT_LABELS = LogBundle.JOINT_LABELS + DEFAULT_ACTION_SCALE = np.array( + [ + 0.125, 0.25, 0.25, + 0.125, 0.25, 0.25, + 0.125, 0.25, 0.25, + 0.125, 0.25, 0.25, + 5.0, 5.0, 5.0, 5.0, + ], + dtype=np.float32, + ) + + def __init__( + self, + cfg: Dict[str, Any], + cfg_path: Path, + driver_factory_real: Callable, + driver_factory_dry: Callable, + ): + self.cfg = cfg + self.cfg_path = cfg_path + self.driver_factory_real = driver_factory_real + self.driver_factory_dry = driver_factory_dry + + self.lock = threading.RLock() + self.status = SessionStatus() + + self.io = None + self.runner = None + self.guard = None + self.safety = None + self.initializer = None + self.stand_balance = None + self.logger = None + + self._stop_runtime = threading.Event() + self._cmd_lock = threading.Lock() + self._cmd = np.zeros(3, dtype=np.float32) + self._estop = False + self._busy_thread: Optional[threading.Thread] = None + self._stand_target = None + + self._stop_poll = threading.Event() + self._poll_thread: Optional[threading.Thread] = None + + self._event_listeners: list = [] + self._recent_events = deque(maxlen=300) + self._listener_lock = threading.Lock() + + self._last_runtime_ts: float = 0.0 + self._last_poll_ts: float = 0.0 + self._last_command_ts: float = 0.0 + self._runtime_loop_count: int = 0 + self._poll_error_count: int = 0 + self._api_error_count: int = 0 + self._disconnecting: bool = False + + def _policy_action_scale(self) -> np.ndarray: + values = self.cfg.get("policy", {}).get("action_scale", self.DEFAULT_ACTION_SCALE.tolist()) + action_scale = np.asarray(values, dtype=np.float32) + if action_scale.shape != (16,): + raise ValueError(f"policy.action_scale must be 16 values, got shape {action_scale.shape}") + return action_scale + + def _diag_snapshot(self) -> Dict[str, Any]: + return { + "runtime_active": bool(self.status.stage == Stage.RUNTIME.value and not self._stop_runtime.is_set()), + "runtime_loop_count": int(self._runtime_loop_count), + "last_runtime_age_s": round(time.time() - self._last_runtime_ts, 3) if self._last_runtime_ts else None, + "last_poll_age_s": round(time.time() - self._last_poll_ts, 3) if self._last_poll_ts else None, + "last_command_age_s": round(time.time() - self._last_command_ts, 3) if self._last_command_ts else None, + "poll_thread_alive": bool(self._poll_thread and self._poll_thread.is_alive()), + "busy_thread_alive": bool(self._busy_thread and self._busy_thread.is_alive()), + "estop": bool(self._estop), + "poll_error_count": int(self._poll_error_count), + "api_error_count": int(self._api_error_count), + "zero_cmd_suppression": ( + bool(getattr(self.runner, "enable_zero_cmd_suppression", False)) + if self.runner is not None + else None + ), + "policy_path": str(getattr(self.runner, "policy_path", "")) if self.runner is not None else None, + } + + def _policy_release_cfg(self) -> Dict[str, float]: + policy_cfg = self.cfg.get("policy", {}) + return { + "command_hold_s": max(float(policy_cfg.get("release_command_hold_s", 0.12)), 0.0), + "posture_max_err": max(float(policy_cfg.get("release_posture_max_err", 0.35)), 0.0), + "target_blend_s": max(float(policy_cfg.get("release_target_blend_s", 0.30)), 1e-3), + } + + def _compute_release_metrics(self, state: Dict[str, Any], hold_target: np.ndarray, cmd: np.ndarray) -> Dict[str, float]: + joint_pos = np.asarray(state["joint_pos"], dtype=np.float32) + default_pose = np.asarray(self.runner.default_dof_pos, dtype=np.float32) + hold_target = np.asarray(hold_target, dtype=np.float32) + planar_cmd, yaw_cmd = self.runner.command_activation_metrics(cmd) + return { + "planar_cmd": float(planar_cmd), + "yaw_cmd": float(yaw_cmd), + "max_hold_err": float(np.max(np.abs(joint_pos[:12] - hold_target[:12]))), + "max_default_err": float(np.max(np.abs(joint_pos[:12] - default_pose[:12]))), + "max_hold_default_gap": float(np.max(np.abs(hold_target[:12] - default_pose[:12]))), + } + + def _blend_runtime_target( + self, + hold_target: np.ndarray, + policy_target: np.ndarray, + release_alpha: float, + target_blend_s: float, + control_dt: float, + ) -> np.ndarray: + blend = min(1.0, release_alpha * (self.runner.command_release_s / max(target_blend_s, control_dt))) + return ((1.0 - blend) * hold_target + blend * policy_target).astype(np.float32) + + def _compute_target_error_metrics( + self, + state: Dict[str, Any], + hold_target: np.ndarray, + policy_target: np.ndarray, + ) -> Dict[str, float]: + joint_pos = np.asarray(state["joint_pos"], dtype=np.float32) + hold_target = np.asarray(hold_target, dtype=np.float32) + policy_target = np.asarray(policy_target, dtype=np.float32) + return { + "hold_target_max_err": float(np.max(np.abs(joint_pos[:12] - hold_target[:12]))), + "policy_target_max_err": float(np.max(np.abs(joint_pos[:12] - policy_target[:12]))), + "hold_policy_max_gap": float(np.max(np.abs(hold_target[:12] - policy_target[:12]))), + } + + def _update_diag_locked(self) -> None: + self.status.diagnostics = self._diag_snapshot() + + def note_api_error(self) -> None: + with self.lock: + self._api_error_count += 1 + self._update_diag_locked() + + def _set_fault(self, exc: Exception, tb: str) -> None: + error_text = f"{type(exc).__name__}: {exc}" + with self.lock: + self.status.fault_reason = error_text + self.status.last_error = error_text + self.status.last_traceback = tb + self._update_diag_locked() + + def get_status(self) -> Dict[str, Any]: + with self.lock: + self._update_diag_locked() + return asdict(self.status) + + def get_debug_snapshot(self) -> Dict[str, Any]: + return {"status": self.get_status(), "recent_events": list(self._recent_events)} + + def _set(self, **kwargs): + with self.lock: + for key, value in kwargs.items(): + setattr(self.status, key, value) + self._update_diag_locked() + snapshot = asdict(self.status) + self._broadcast({"kind": "STATUS", **snapshot}) + + def _set_stage(self, stage: Stage, detail: str = ""): + self._set(stage=stage.value, detail=detail) + + def _build_action_diag( + self, + *, + state: Dict[str, Any], + raw: np.ndarray, + scaled: np.ndarray, + tentative: np.ndarray, + cmd: np.ndarray, + zero_command: bool, + runtime_released: bool, + release_alpha: float, + safety_details: Optional[Dict[str, Any]] = None, + ) -> Dict[str, Any]: + details = dict(safety_details or {}) + joint_indices = list(details.get("joint_indices", [])) + joint_pos = np.asarray(state["joint_pos"], dtype=np.float32) + default_pose = np.asarray(self.runner.default_dof_pos, dtype=np.float32) + pos_err = tentative - joint_pos + leg_offset = tentative[:12] - default_pose[:12] + + diag: Dict[str, Any] = { + "joint_indices": joint_indices, + "joint_names": [self.JOINT_LABELS[i] for i in joint_indices if 0 <= i < len(self.JOINT_LABELS)], + "cmd": cmd.tolist(), + "zero_command": bool(zero_command), + "runtime_released": bool(runtime_released), + "release_alpha": float(release_alpha), + "max_raw": float(np.max(np.abs(raw))) if raw.size else 0.0, + "max_scaled": float(np.max(np.abs(scaled[:12]))) if scaled.size else 0.0, + "max_target": float(np.max(np.abs(tentative[:12]))) if tentative.size else 0.0, + } + + if joint_indices: + primary = int(joint_indices[0]) + diag.update( + { + "primary_joint_index": primary, + "primary_joint_name": self.JOINT_LABELS[primary], + "primary_target": float(tentative[primary]), + "primary_default": float(default_pose[primary]), + "primary_measured": float(joint_pos[primary]), + "primary_pos_err": float(pos_err[primary]), + "primary_raw": float(raw[primary]), + "primary_scaled": float(scaled[primary]), + } + ) + if primary < 12: + diag["primary_leg_offset"] = float(leg_offset[primary]) + + details.update(diag) + return details + + def add_listener(self, q: "queue.Queue"): + with self._listener_lock: + self._event_listeners.append(q) + for event in list(self._recent_events): + try: + q.put_nowait(event) + except Exception: + pass + + def remove_listener(self, q: "queue.Queue"): + with self._listener_lock: + if q in self._event_listeners: + self._event_listeners.remove(q) + + def _broadcast(self, event: Dict[str, Any]): + payload = dict(event) + payload["t"] = time.time() + self._recent_events.append(payload) + with self._listener_lock: + for listener in list(self._event_listeners): + try: + listener.put_nowait(payload) + except Exception: + pass + + def _run_async(self, fn, *args, **kwargs) -> bool: + with self.lock: + if self.status.busy: + return False + self.status.busy = True + self.status.fault_reason = None + self.status.last_error = None + self.status.last_traceback = None + self._update_diag_locked() + + def _wrap(): + try: + fn(*args, **kwargs) + except Exception as exc: + tb = traceback.format_exc() + print(f"\n[Background Task Error] {fn.__name__}") + print(tb) + self._set_fault(exc, tb) + self._broadcast({"kind": "BG_TASK_ERROR", "fn": fn.__name__, "error": str(exc), "traceback": tb}) + self._set_stage(Stage.FAULTED, detail=str(exc)) + try: + if self.io: + self.io.damping_brake() + except Exception: + pass + finally: + with self.lock: + self.status.busy = False + self._update_diag_locked() + self._broadcast({"kind": "BG_TASK_DONE", "fn": fn.__name__}) + + self._busy_thread = threading.Thread(target=_wrap, daemon=True) + self._busy_thread.start() + return True + + def connect(self, dry_run: bool = False): + return self._run_async(self._do_connect, dry_run) + + def _do_connect(self, dry_run: bool): + if self.status.stage != Stage.DISCONNECTED.value: + self._broadcast({"kind": "WARN", "msg": "already connected"}) + return + self._set_stage(Stage.CONNECTING, "connecting hardware") + + from interface.real_io import RealIO + from safety.runtime_guard import RuntimeGuard + from safety.safety_monitor import SafetyMonitor + from startup.pose_initializer import PoseInitializer + from startup.stand_balance import StandBalanceController + from tools.logger import LogBundle + + cfg = self.cfg + control_dt = 1.0 / float(cfg["control_freq"]) + driver_factory = self.driver_factory_dry() if dry_run else self.driver_factory_real() + + self.logger = LogBundle(cfg["log_dir"]) + self._set(log_dir=str(self.logger.dir)) + self.logger.event( + "CONFIG_LOADED", + config_path=str(self.cfg_path), + dry_run=dry_run, + control_freq=cfg["control_freq"], + motor_model=cfg["motor_model"], + ) + + self.io = RealIO( + driver_factory=driver_factory, + motor_model=cfg["motor_model"], + can1_port=cfg["can1_port"], + can2_port=cfg["can2_port"], + imu_lib_path=cfg.get("imu_lib_path"), + control_dt=control_dt, + kp_leg=cfg["controller"]["kp_leg"], + kd_leg=cfg["controller"]["kd_leg"], + kd_wheel=cfg["controller"]["kd_wheel"], + debug=cfg.get("debug", False), + ) + self.guard = RuntimeGuard( + max_ang_vel=cfg["safety"]["max_ang_vel"], + max_tilt_z=cfg["safety"]["max_tilt_z"], + imu_age_warn_ms=cfg["safety"].get("imu_age_warn_ms", 60.0), + imu_age_stop_ms=cfg["safety"].get("imu_age_stop_ms", 200.0), + ) + self.safety = SafetyMonitor( + max_target_offset=cfg["safety"]["max_target_offset"], + max_ang_vel=cfg["safety"]["max_ang_vel"], + max_tilt_z=cfg["safety"]["max_tilt_z"], + clip_to_brake=cfg["safety"]["clip_to_brake"], + ) + self.initializer = PoseInitializer( + self.io, + control_dt=control_dt, + transition_time_min=cfg["startup"].get("transition_time_min", 2.0), + transition_time_max=cfg["startup"].get("transition_time_max", 6.0), + transition_seconds_per_rad=cfg["startup"].get("transition_seconds_per_rad", 1.5), + hold_time=cfg["startup"]["hold_time"], + settle_pos_threshold=cfg["startup"]["settle_pos_threshold"], + settle_vel_threshold=cfg["startup"]["settle_vel_threshold"], + timeout_extra=cfg["startup"].get("timeout_extra", 3.0), + progress_log_interval=cfg["startup"]["progress_log_interval"], + ramp_kp_time=cfg["startup"].get("ramp_kp_time", 1.0), + soft_hold_duration=cfg["startup"].get("soft_hold_duration", 1.0), + max_dev_warn=cfg["startup"].get("max_dev_warn", 1.5), + max_dev_abort=cfg["startup"].get("max_dev_abort", 3.0), + ) + self.stand_balance = StandBalanceController(cfg.get("stand_balance", {}), control_dt=control_dt) + + class _WebEstop: + def __init__(self, owner): + self.owner = owner + + def is_estop_triggered(self): + return self.owner._estop + + self.initializer.attach(self.logger, self.guard, _WebEstop(self)) + self.io.connect(imu_timeout_ms=cfg.get("imu_start_timeout_ms", 8000)) + self.logger.event("CAN_IMU_CONNECTED", initial_gravity=self.io.imu.initial_gravity) + self._set_stage(Stage.CONNECTED, "hardware connected") + + def disconnect(self): + return self._run_async(self._do_disconnect) + + def _do_disconnect(self): + if self._disconnecting: + return + self._disconnecting = True + self._stop_runtime.set() + self._stop_state_poll() + time.sleep(0.05) + try: + if self.io: + self.io.damping_brake() + except Exception: + pass + try: + if self.io: + self.io.disconnect() + except Exception: + pass + if self.logger: + self.logger.event("HARDWARE_DISCONNECTED") + self.logger.close() + self.io = None + self.runner = None + self.logger = None + self._stand_target = None + self._set_stage(Stage.DISCONNECTED, "hardware disconnected") + self._disconnecting = False + + def enable_motors(self): + return self._run_async(self._do_enable) + + def _do_enable(self): + if self.status.stage not in (Stage.CONNECTED.value, Stage.STAND_HOLD.value, Stage.FAULTED.value): + return + self._set_stage(Stage.ENABLING, "enabling motors") + self.io.enable_motors() + self.logger.event("MOTORS_ENABLED") + time.sleep(0.5) + self._set_stage(Stage.ENABLED, "motors enabled") + self._start_state_poll() + + def _start_state_poll(self): + self._stop_poll.clear() + if self._poll_thread and self._poll_thread.is_alive(): + return + + def _poll_loop(): + while not self._stop_poll.is_set(): + if self.status.stage == Stage.RUNTIME.value: + time.sleep(0.2) + continue + try: + stage = self.status.stage + if stage == Stage.ENABLED.value: + self.io.hw.passive_poll() + + state = self.io.read_state() + if stage == Stage.STAND_HOLD.value and self.stand_balance is not None and self.stand_balance.enabled: + self._stand_target = self.stand_balance.compute_target(state, self._cmd) + self.io.hold_pose(self._stand_target, kp_scale=1.0) + elif stage == Stage.STAND_HOLD.value and self._stand_target is not None: + self.io.hold_pose(self._stand_target, kp_scale=1.0) + + motor_diag = state.get("motor_stale", {}) + self._last_poll_ts = time.time() + self._set( + last_state={ + "joint_pos": state["joint_pos"].tolist(), + "joint_vel": state["joint_vel"].tolist(), + "joint_torque": state["joint_torque"].tolist(), + "target": self._stand_target.tolist() if self._stand_target is not None else [0.0] * 16, + "raw": [0.0] * 16, + "gyro": state["imu_gyro"].tolist(), + "proj_gravity": state["projected_gravity"].tolist(), + "imu_age_ms": float(state["imu_age_ms"]), + "loop_dt_ms": 0.0, + "holdover_total": int(getattr(self.io.hw, "holdover_total", 0)), + "safety_level": 0, + "guard_level": 0, + "phase": "POLL", + "per_motor_stale": motor_diag.get("per_motor_stale", [0] * 16), + } + ) + except Exception as exc: + self._poll_error_count += 1 + self._broadcast({"kind": "POLL_ERROR", "error": str(exc), "traceback": traceback.format_exc()}) + time.sleep(0.2) + + self._poll_thread = threading.Thread(target=_poll_loop, daemon=True) + self._poll_thread.start() + + def _stop_state_poll(self): + self._stop_poll.set() + thread = self._poll_thread + if thread and thread.is_alive() and thread is not threading.current_thread(): + thread.join(timeout=0.5) + self._poll_thread = None + + def disable_motors(self): + return self._run_async(self._do_disable) + + def _do_disable(self): + self._stop_state_poll() + try: + self.io.damping_brake() + except Exception: + pass + time.sleep(0.05) + self.io.disable_motors() + self.logger.event("MOTORS_DISABLED") + self._set_stage(Stage.CONNECTED, "motors disabled") + + def startup(self): + return self._run_async(self._do_startup) + + def _do_startup(self): + from startup.pose_initializer import PoseInitFailed, STAND_POSE + + if self.status.stage != Stage.ENABLED.value: + raise RuntimeError("startup requires ENABLED") + + self._set_stage(Stage.STARTING_UP, detail="transition to stand pose") + try: + target = self.initializer.transition_to_stand_from_current(target_pose=STAND_POSE) + self._stand_target = target + if self.stand_balance is not None and self.stand_balance.enabled: + self.logger.event("STAND_BALANCE_BEGIN") + self.stand_balance.reset() + stable_deadline = time.perf_counter() + 6.0 + while time.perf_counter() < stable_deadline: + state = self.io.read_state() + self._stand_target = self.stand_balance.compute_target(state, np.zeros(3, dtype=np.float32)) + self.io.hold_pose(self._stand_target, kp_scale=1.0) + if self.stand_balance.is_stable(): + debug = self.stand_balance.last_debug + self.logger.event( + "STAND_BALANCE_STABLE", + roll_deg=float(np.degrees(debug.roll)), + pitch_deg=float(np.degrees(debug.pitch)), + ) + break + time.sleep(1.0 / float(self.cfg["control_freq"])) + self.logger.event("STAND_BALANCE_END") + self._set_stage(Stage.STAND_HOLD, detail="stand-balance hold active") + else: + self._set_stage(Stage.STAND_HOLD, detail="holding stand pose with PD") + except PoseInitFailed as exc: + self.logger.event("POSE_INIT_FAILED", error=str(exc)) + try: + self.io.damping_brake() + except Exception: + pass + self._set_stage(Stage.FAULTED, detail=str(exc)) + raise + + def runtime_start(self, policy_path: Optional[str] = None): + return self._run_async(self._do_runtime_start, policy_path) + + def _do_runtime_start(self, policy_path: Optional[str]): + from policy.policy_runner import PolicyRunner + from safety.runtime_guard import GuardLevel + from safety.safety_monitor import SafetyLevel + + if self.status.stage != Stage.STAND_HOLD.value: + raise RuntimeError("runtime start requires STAND_HOLD") + + sim2real_root = Path(__file__).resolve().parents[1] + resolved_policy = Path(policy_path) if policy_path else sim2real_root / "policies" / "model_rough.pt" + if not resolved_policy.exists(): + raise RuntimeError(f"policy not found: {resolved_policy}") + + self.runner = PolicyRunner( + resolved_policy, + enable_zero_cmd_suppression=self.cfg.get("policy", {}).get("enable_zero_cmd_suppression", True), + hold_zero_command_pose=self.cfg.get("policy", {}).get("hold_zero_command_pose", True), + command_release_s=self.cfg.get("policy", {}).get("command_release_s", 0.35), + action_scale=self._policy_action_scale(), + zero_cmd_use_yaw_rate=self.cfg.get("policy", {}).get("zero_cmd_use_yaw_rate", False), + ) + self.safety.reset() + require_active_command = self.cfg.get("policy", {}).get("require_active_command_to_release", True) + self.logger.event("POLICY_LOADED", path=str(resolved_policy)) + self._stop_state_poll() + + control_dt = 1.0 / float(self.cfg["control_freq"]) + target = self._stand_target + + self.logger.event("PRIME_BEGIN") + zero_cmd = np.zeros(3, dtype=np.float32) + for index in range(1): + if self.stand_balance is not None and self.stand_balance.enabled: + state = self.io.read_state() + target = self.stand_balance.compute_target(state, zero_cmd) + self.io.hold_pose(target, kp_scale=1.0) + else: + self.io.hold_pose(target, kp_scale=1.0) + state = self.io.read_state() + obs = self.io.get_obs_policy(state, zero_cmd, self.runner.default_dof_pos, self.runner.last_actions) + if index == 0: + self.runner.reset(prime_obs=obs) + time.sleep(control_dt) + self.logger.event("PRIME_END") + + state = self.io.read_state() + projected_gravity = state["projected_gravity"] + if abs(projected_gravity[0]) > 0.5 or abs(projected_gravity[1]) > 0.5: + error_message = ( + f"IMU frame mismatch or body tilt too large: " + f"gravity projection X={projected_gravity[0]:.2f}, Y={projected_gravity[1]:.2f}" + ) + self.logger.event("GUARD_STOP", phase="STARTUP", reason=error_message) + self._set_stage(Stage.FAULTED, error_message) + return + + self.logger.event("HISTORY_PRIMED", initial_obs=obs) + self._stop_runtime.clear() + self._runtime_loop_count = 0 + self._set_stage(Stage.RUNTIME, detail="50Hz policy loop") + self.logger.event("RUNTIME_BEGIN") + + next_exec = time.perf_counter() + log_every = int(self.cfg.get("log_every", 1)) + loop_count = 0 + last_status_push = 0.0 + runtime_released = not require_active_command + release_cfg = self._policy_release_cfg() + release_active_time = 0.0 + release_block_reason = "active command required" if require_active_command else "" + + while not self._stop_runtime.is_set(): + loop_t0 = time.perf_counter() + with self._cmd_lock: + cmd = self._cmd.copy() + + state = self.io.read_state() + obs = self.io.get_obs_policy(state, cmd, self.runner.default_dof_pos, self.runner.last_actions) + zero_command = self.runner._is_zero_command(cmd, state["imu_gyro"]) + if np.any(np.isnan(obs)) or np.any(np.isinf(obs)): + self.logger.event("OBS_NAN") + self.io.damping_brake() + self._set_stage(Stage.FAULTED, "observation NaN/Inf") + break + + if not runtime_released and zero_command: + raw = np.zeros(16, dtype=np.float32) + scaled = np.zeros(16, dtype=np.float32) + target_hold = self.stand_balance.compute_target(state, np.zeros(3, dtype=np.float32)) if self.stand_balance is not None and self.stand_balance.enabled else self.runner.default_dof_pos.copy() + actual_target = self.io.hold_pose(target_hold, kp_scale=1.0) + tentative = target_hold.astype(np.float32) + policy_target = self.runner.default_dof_pos.copy() + projected_gravity = state["projected_gravity"] + release_active_time = 0.0 + release_metrics = self._compute_release_metrics(state, target_hold, cmd) + target_metrics = self._compute_target_error_metrics(state, target_hold, policy_target) + release_block_reason = "zero command" + runtime_blend_ratio = 0.0 + else: + target_hold = self.stand_balance.compute_target(state, np.zeros(3, dtype=np.float32)) if self.stand_balance is not None and self.stand_balance.enabled else self.runner.default_dof_pos.copy() + release_metrics = self._compute_release_metrics(state, target_hold, cmd) + if not runtime_released: + release_active_time += control_dt if self.runner.is_command_active(cmd) else 0.0 + active_ready = release_active_time >= release_cfg["command_hold_s"] + posture_ready = release_metrics["max_hold_err"] <= release_cfg["posture_max_err"] + if active_ready and posture_ready: + runtime_released = True + self.logger.event( + "RUNTIME_COMMAND_RELEASED", + cmd=cmd.tolist(), + active_hold_s=release_active_time, + max_hold_err=release_metrics["max_hold_err"], + max_default_err=release_metrics["max_default_err"], + max_hold_default_gap=release_metrics["max_hold_default_gap"], + ) + else: + reasons = [] + if not active_ready: + reasons.append(f"cmd_hold<{release_cfg['command_hold_s']:.2f}s") + if not posture_ready: + reasons.append(f"hold_err>{release_cfg['posture_max_err']:.3f}") + release_block_reason = ",".join(reasons) + raw = np.zeros(16, dtype=np.float32) + scaled = np.zeros(16, dtype=np.float32) + actual_target = self.io.hold_pose(target_hold, kp_scale=1.0) + tentative = target_hold.astype(np.float32) + policy_target = self.runner.default_dof_pos.copy() + projected_gravity = state["projected_gravity"] + target_metrics = self._compute_target_error_metrics(state, target_hold, policy_target) + if loop_count % max(1, int(0.2 / control_dt)) == 0: + self.logger.event( + "RUNTIME_RELEASE_BLOCKED", + reason=release_block_reason, + cmd=cmd.tolist(), + active_hold_s=release_active_time, + max_hold_err=release_metrics["max_hold_err"], + max_default_err=release_metrics["max_default_err"], + max_hold_default_gap=release_metrics["max_hold_default_gap"], + ) + safety_decision = self.safety.check( + target_pose=tentative, + default_pose=self.runner.default_dof_pos, + imu_gyro=state["imu_gyro"], + projected_gravity=projected_gravity, + estop_triggered=self._estop, + ) + guard_decision = self.guard.check( + imu_gyro=state["imu_gyro"], + projected_gravity=projected_gravity, + imu_age_ms=float(state["imu_age_ms"]), + estop_triggered=self._estop, + extra_nan_arrays=(tentative,), + ) + loop_dt_ms = (time.perf_counter() - loop_t0) * 1000.0 + self._last_runtime_ts = time.time() + self._runtime_loop_count += 1 + motor_diag = state.get("motor_stale", {}) + if log_every and (loop_count % log_every == 0): + self.logger.state( + phase="RUNTIME", + joint_pos=state["joint_pos"], + joint_vel=state["joint_vel"], + joint_torque=state["joint_torque"], + target_pose=actual_target, + raw_action=raw, + gyro=state["imu_gyro"], + accel=state["imu_accel"], + quat=state["quat_wxyz"], + proj_gravity=projected_gravity, + command=cmd, + imu_age_ms=float(state["imu_age_ms"]), + loop_dt_ms=loop_dt_ms, + safety_level=int(safety_decision.level), + guard_level=int(guard_decision.level), + holdover=int(motor_diag.get("holdover_this_frame", 0)), + stale_max=int(motor_diag.get("stale_max", 0)), + fresh_count=int(motor_diag.get("fresh_count", 16)), + kp_leg_cmd=float(self.io.kp_leg), + kd_leg_cmd=float(self.io.kd_leg), + kd_wheel_cmd=float(self.io.kd_wheel), + runtime_release_alpha=0.0, + runtime_release_hold_s=release_active_time, + runtime_blend_ratio=0.0, + hold_target_max_err=target_metrics["hold_target_max_err"], + policy_target_max_err=target_metrics["policy_target_max_err"], + hold_policy_max_gap=target_metrics["hold_policy_max_gap"], + target_source="runtime_hold", + clip_primary_joint="", + safety_reason=f"release_blocked:{release_block_reason}", + guard_reason=guard_decision.reason, + ) + next_exec += control_dt + slack = next_exec - time.perf_counter() + if slack > 0: + coarse = slack - 0.002 + if coarse > 0: + time.sleep(coarse) + while time.perf_counter() < next_exec: + pass + elif slack < -control_dt: + self.logger.event("LOOP_OVERRUN", over_ms=-slack * 1000.0) + next_exec = time.perf_counter() + if time.time() - last_status_push > 0.2: + self._set( + last_state={ + "joint_pos": state["joint_pos"].tolist(), + "joint_vel": state["joint_vel"].tolist(), + "joint_torque": state["joint_torque"].tolist(), + "target": actual_target.tolist(), + "raw": raw.tolist(), + "gyro": state["imu_gyro"].tolist(), + "proj_gravity": projected_gravity.tolist(), + "imu_age_ms": float(state["imu_age_ms"]), + "loop_dt_ms": loop_dt_ms, + "holdover_total": int(self.io.hw.holdover_total), + "safety_level": int(safety_decision.level), + "guard_level": int(guard_decision.level), + "phase": "RUNTIME", + "cmd": cmd.tolist(), + "safety_reason": f"release_blocked:{release_block_reason}", + "guard_reason": guard_decision.reason, + "zero_command": bool(zero_command), + "runtime_released": False, + "release_alpha": 0.0, + "release_active_hold_s": release_active_time, + "release_max_hold_err": release_metrics["max_hold_err"], + } + ) + last_status_push = time.time() + loop_count += 1 + continue + scaled, raw = self.runner.step(obs) + if np.any(np.isnan(raw)) or np.any(np.isinf(raw)): + self.logger.event("ACTION_NAN") + self.io.damping_brake() + self._set_stage(Stage.FAULTED, "action NaN/Inf") + break + policy_target = (scaled + self.runner.default_dof_pos).astype(np.float32) + tentative = self._blend_runtime_target( + target_hold, + policy_target, + float(getattr(self.runner, "_command_release_alpha", 0.0)), + release_cfg["target_blend_s"], + control_dt, + ) + scaled = tentative - self.runner.default_dof_pos + target_metrics = self._compute_target_error_metrics(state, target_hold, policy_target) + runtime_blend_ratio = min( + 1.0, + float(getattr(self.runner, "_command_release_alpha", 0.0)) + * (self.runner.command_release_s / max(release_cfg["target_blend_s"], control_dt)), + ) + projected_gravity = state["projected_gravity"] + + guard_decision = self.guard.check( + imu_gyro=state["imu_gyro"], + projected_gravity=projected_gravity, + imu_age_ms=float(state["imu_age_ms"]), + estop_triggered=self._estop, + extra_nan_arrays=(raw, tentative), + ) + if guard_decision.level == GuardLevel.STOP: + self.logger.event("GUARD_STOP", phase="RUNTIME", reason=guard_decision.reason) + self.io.damping_brake() + self._set_stage(Stage.FAULTED, guard_decision.reason) + break + + safety_decision = self.safety.check( + target_pose=tentative, + default_pose=self.runner.default_dof_pos, + imu_gyro=state["imu_gyro"], + projected_gravity=projected_gravity, + estop_triggered=self._estop, + ) + if safety_decision.level == SafetyLevel.ESTOP: + self.logger.event("SAFETY_ESTOP", reason=safety_decision.message) + self.io.damping_brake() + self._set_stage(Stage.ESTOPPED, safety_decision.message) + break + if safety_decision.level == SafetyLevel.BRAKE: + safety_diag = self._build_action_diag( + state=state, + raw=raw, + scaled=scaled, + tentative=tentative, + cmd=cmd, + zero_command=zero_command, + runtime_released=runtime_released, + release_alpha=float(getattr(self.runner, "_command_release_alpha", 0.0)), + safety_details=safety_decision.details, + ) + self.logger.event( + "SAFETY_BRAKE", + reason=safety_decision.message, + details=safety_diag, + primary_joint=safety_diag.get("primary_joint_name"), + primary_offset=safety_diag.get("primary_leg_offset"), + primary_target=safety_diag.get("primary_target"), + primary_measured=safety_diag.get("primary_measured"), + primary_raw=safety_diag.get("primary_raw"), + primary_scaled=safety_diag.get("primary_scaled"), + cmd=cmd.tolist(), + release_alpha=float(getattr(self.runner, "_command_release_alpha", 0.0)), + ) + self.io.damping_brake() + self._set_stage(Stage.FAULTED, safety_decision.message) + break + if safety_decision.level == SafetyLevel.CLIP and safety_decision.clipped_target is not None: + scaled = safety_decision.clipped_target - self.runner.default_dof_pos + safety_diag = self._build_action_diag( + state=state, + raw=raw, + scaled=scaled, + tentative=tentative, + cmd=cmd, + zero_command=zero_command, + runtime_released=runtime_released, + release_alpha=float(getattr(self.runner, "_command_release_alpha", 0.0)), + safety_details=safety_decision.details, + ) + self.logger.event( + "SAFETY_CLIP", + reason=safety_decision.message, + details=safety_diag, + primary_joint=safety_diag.get("primary_joint_name"), + primary_offset=safety_diag.get("primary_leg_offset"), + primary_target=safety_diag.get("primary_target"), + primary_measured=safety_diag.get("primary_measured"), + primary_raw=safety_diag.get("primary_raw"), + primary_scaled=safety_diag.get("primary_scaled"), + max_raw=float(np.max(np.abs(raw))), + cmd=cmd.tolist(), + release_alpha=float(getattr(self.runner, "_command_release_alpha", 0.0)), + ) + + if runtime_released or not zero_command: + actual_target = self.io.send_actions(scaled, self.runner.default_dof_pos) + loop_dt_ms = (time.perf_counter() - loop_t0) * 1000.0 + self._last_runtime_ts = time.time() + self._runtime_loop_count += 1 + + motor_diag = state.get("motor_stale", {}) + if actual_target is None: + actual_target = tentative.astype(np.float32) + if log_every and (loop_count % log_every == 0): + self.logger.state( + phase="RUNTIME", + joint_pos=state["joint_pos"], + joint_vel=state["joint_vel"], + joint_torque=state["joint_torque"], + target_pose=actual_target, + raw_action=raw, + gyro=state["imu_gyro"], + accel=state["imu_accel"], + quat=state["quat_wxyz"], + proj_gravity=projected_gravity, + command=cmd, + imu_age_ms=float(state["imu_age_ms"]), + loop_dt_ms=loop_dt_ms, + safety_level=int(safety_decision.level), + guard_level=int(guard_decision.level), + holdover=int(motor_diag.get("holdover_this_frame", 0)), + stale_max=int(motor_diag.get("stale_max", 0)), + fresh_count=int(motor_diag.get("fresh_count", 16)), + kp_leg_cmd=float(self.io.kp_leg), + kd_leg_cmd=float(self.io.kd_leg), + kd_wheel_cmd=float(self.io.kd_wheel), + runtime_release_alpha=float(getattr(self.runner, "_command_release_alpha", 0.0)), + runtime_release_hold_s=release_active_time, + runtime_blend_ratio=runtime_blend_ratio, + hold_target_max_err=target_metrics["hold_target_max_err"], + policy_target_max_err=target_metrics["policy_target_max_err"], + hold_policy_max_gap=target_metrics["hold_policy_max_gap"], + target_source="runtime_blend" if runtime_blend_ratio < 0.999 else "runtime_policy", + clip_primary_joint=str((safety_decision.details or {}).get("primary_joint_name", "")), + clip_primary_target=float((safety_decision.details or {}).get("primary_target", 0.0) or 0.0), + clip_primary_measured=float((safety_decision.details or {}).get("primary_measured", 0.0) or 0.0), + clip_primary_default=float((safety_decision.details or {}).get("primary_default", 0.0) or 0.0), + clip_primary_pos_err=float((safety_decision.details or {}).get("primary_pos_err", 0.0) or 0.0), + clip_primary_raw=float((safety_decision.details or {}).get("primary_raw", 0.0) or 0.0), + clip_primary_scaled=float((safety_decision.details or {}).get("primary_scaled", 0.0) or 0.0), + safety_reason=( + f"{safety_decision.message};zero_cmd={int(zero_command)};" + f"released={int(runtime_released)};alpha={getattr(self.runner, '_command_release_alpha', 0.0):.2f};" + f"max_raw={float(np.max(np.abs(raw))):.2f};" + f"clip={((safety_decision.details or {}).get('joint_indices', []))}" + ), + guard_reason=guard_decision.reason, + ) + + next_exec += control_dt + slack = next_exec - time.perf_counter() + if slack > 0: + coarse = slack - 0.002 + if coarse > 0: + time.sleep(coarse) + while time.perf_counter() < next_exec: + pass + elif slack < -control_dt: + self.logger.event("LOOP_OVERRUN", over_ms=-slack * 1000.0) + next_exec = time.perf_counter() + + if time.time() - last_status_push > 0.2: + self._set( + last_state={ + "joint_pos": state["joint_pos"].tolist(), + "joint_vel": state["joint_vel"].tolist(), + "joint_torque": state["joint_torque"].tolist(), + "target": actual_target.tolist(), + "raw": raw.tolist(), + "gyro": state["imu_gyro"].tolist(), + "proj_gravity": projected_gravity.tolist(), + "imu_age_ms": float(state["imu_age_ms"]), + "loop_dt_ms": loop_dt_ms, + "holdover_total": int(self.io.hw.holdover_total), + "safety_level": int(safety_decision.level), + "guard_level": int(guard_decision.level), + "phase": "RUNTIME", + "cmd": cmd.tolist(), + "safety_reason": safety_decision.message, + "guard_reason": guard_decision.reason, + "zero_command": bool(zero_command), + "runtime_released": bool(runtime_released), + "release_alpha": float(getattr(self.runner, "_command_release_alpha", 0.0)), + "release_active_hold_s": release_active_time, + "release_max_hold_err": release_metrics["max_hold_err"], + "max_raw": float(np.max(np.abs(raw))), + "clip_joint_indices": (safety_decision.details or {}).get("joint_indices", []), + "clip_joint_names": (safety_decision.details or {}).get("joint_names", []), + "clip_primary_joint": (safety_decision.details or {}).get("primary_joint_name"), + "clip_primary_target": (safety_decision.details or {}).get("primary_target"), + "clip_primary_measured": (safety_decision.details or {}).get("primary_measured"), + "clip_primary_default": (safety_decision.details or {}).get("primary_default"), + "clip_primary_pos_err": (safety_decision.details or {}).get("primary_pos_err"), + "clip_primary_raw": (safety_decision.details or {}).get("primary_raw"), + "clip_primary_scaled": (safety_decision.details or {}).get("primary_scaled"), + "per_motor_stale": motor_diag.get("per_motor_stale", [0] * 16), + } + ) + last_status_push = time.time() + + loop_count += 1 + + if self.status.stage == Stage.RUNTIME.value: + self.logger.event("RUNTIME_STOP") + self._set_stage(Stage.STAND_HOLD, detail="runtime stopped, back to stand-balance hold") + self._start_state_poll() + + def runtime_stop(self): + self._stop_runtime.set() + return True + + def estop(self): + self._estop = True + try: + if self.io: + self.io.damping_brake() + except Exception: + pass + self._stop_runtime.set() + if self.logger: + self.logger.event("USER_ESTOP_WEB") + self._set_stage(Stage.ESTOPPED, detail="web emergency stop") + return True + + def reset_estop(self): + self._estop = False + self._set(detail="estop cleared") + return True + + def set_command(self, vx: float, vy: float, yaw: float): + with self._cmd_lock: + self._cmd[0] = float(vx) + self._cmd[1] = float(vy) + self._cmd[2] = float(yaw) + self._last_command_ts = time.time() + self._set(cmd=self._cmd.tolist()) + return True + + def test_motor(self, leg: str, joint: str, delta_rad: float, kp: float, kd: float, duration_s: float): + return self._run_async(self._do_test_motor, leg, joint, delta_rad, kp, kd, duration_s) + + def _do_test_motor(self, leg: str, joint: str, delta_rad: float, kp: float, kd: float, duration_s: float): + if self.io is None: + raise RuntimeError("hardware not connected") + if self.status.stage not in (Stage.ENABLED.value, Stage.STAND_HOLD.value, Stage.FAULTED.value): + raise RuntimeError("test motor requires ENABLED/STAND_HOLD/FAULTED") + + joint_key = (str(leg), str(joint)) + if joint_key not in self.io.hw.mapper.SIM_INDEX_MAP: + raise RuntimeError(f"unknown joint: {leg}_{joint}") + + idx = self.io.hw.mapper.SIM_INDEX_MAP[joint_key] + base_pose = self.io.read_measured_pose().astype(np.float32) + target_pose = base_pose.copy() + target_pose[idx] += float(delta_rad) + + prev_stage = self.status.stage + self._set_stage(Stage.JOINT_TEST, detail=f"testing {leg}_{joint}") + if self.logger: + self.logger.event( + "JOINT_TEST_BEGIN", + joint=f"{leg}_{joint}", + joint_index=idx, + delta_rad=float(delta_rad), + kp=float(kp), + kd=float(kd), + duration_s=float(duration_s), + start_pos=float(base_pose[idx]), + target_pos=float(target_pose[idx]), + ) + + self._stop_state_poll() + next_exec = time.perf_counter() + deadline = next_exec + max(float(duration_s), 0.1) + samples = [] + while time.perf_counter() < deadline: + state = self.io.read_state() + measured = float(state["joint_pos"][idx]) + error = float(target_pose[idx] - measured) + torque = float(state["joint_torque"][idx]) + vel = float(state["joint_vel"][idx]) + samples.append((measured, error, vel, torque)) + self.io.hw.send_control(target_pose, float(kp), float(kd), self.io.kd_wheel) + next_exec += 1.0 / float(self.cfg["control_freq"]) + slack = next_exec - time.perf_counter() + if slack > 0: + time.sleep(slack) + + self.io.hold_pose(base_pose, kp_scale=1.0) + final_state = self.io.read_state() + final_measured = float(final_state["joint_pos"][idx]) + if self.logger: + self.logger.event( + "JOINT_TEST_END", + joint=f"{leg}_{joint}", + joint_index=idx, + final_pos=final_measured, + final_err=float(target_pose[idx] - final_measured), + max_abs_err=float(max(abs(s[1]) for s in samples) if samples else 0.0), + max_abs_vel=float(max(abs(s[2]) for s in samples) if samples else 0.0), + max_abs_tau=float(max(abs(s[3]) for s in samples) if samples else 0.0), + ) + self._set(stage=prev_stage, detail=f"joint test {leg}_{joint} done") + if prev_stage in (Stage.ENABLED.value, Stage.STAND_HOLD.value): + self._start_state_poll() + + def list_logs(self): + log_root = Path(self.cfg.get("log_dir", "logs")) + if not log_root.exists(): + return [] + out = [] + for directory in sorted(log_root.iterdir(), reverse=True): + if not directory.is_dir(): + continue + state_path = directory / "state.csv" + events_path = directory / "events.jsonl" + out.append( + { + "id": directory.name, + "state_csv": state_path.exists(), + "events_jsonl": events_path.exists(), + "size_kb": ( + (state_path.stat().st_size + events_path.stat().st_size) // 1024 + if state_path.exists() and events_path.exists() + else 0 + ), + } + ) + return out diff --git a/05_software/real/sim2real/web/static/app.js b/05_software/real/sim2real/web/static/app.js new file mode 100644 index 0000000..1678ac5 --- /dev/null +++ b/05_software/real/sim2real/web/static/app.js @@ -0,0 +1,480 @@ +const SIM_JOINT_ORDER = [ + ["fl", "hip_abduction"], ["fl", "hip_pitch"], ["fl", "knee"], + ["fr", "hip_abduction"], ["fr", "hip_pitch"], ["fr", "knee"], + ["rl", "hip_abduction"], ["rl", "hip_pitch"], ["rl", "knee"], + ["rr", "hip_abduction"], ["rr", "hip_pitch"], ["rr", "knee"], + ["fl", "wheel"], ["fr", "wheel"], ["rl", "wheel"], ["rr", "wheel"], +]; + +const $ = (id) => document.getElementById(id); +const PLOTS = {}; +let CURRENT_STATUS = null; +let SSE_CONN = null; +let SSE_RECONNECT_TIMER = null; +let LAST_RENDER_TS = 0; + +async function api(path, body = null) { + const options = { method: body ? "POST" : "GET" }; + if (body) { + options.headers = { "Content-Type": "application/json" }; + options.body = JSON.stringify(body); + } + const response = await fetch(path, options); + const payload = await response.json().catch(() => ({})); + if (!response.ok) { + throw new Error(payload.error || `HTTP ${response.status}`); + } + return payload; +} + +function safeText(value, fallback = "--") { + return value === undefined || value === null || Number.isNaN(value) ? fallback : value; +} + +function appendEvent(ev) { + const el = $("events-log"); + if (!el) return; + const item = document.createElement("div"); + let cls = "ev-name"; + if (/ERROR|STOP|NAN/.test(ev.kind || "")) cls = "ev-stop"; + else if (/FAULT|BRAKE/.test(ev.kind || "")) cls = "ev-fault"; + else if (/DONE|CONNECTED|ENABLED|PRIMED/.test(ev.kind || "")) cls = "ev-ok"; + const t = ev.t ? new Date(ev.t * 1000).toLocaleTimeString() : new Date().toLocaleTimeString(); + const detail = Object.entries(ev) + .filter(([k]) => !["t", "kind"].includes(k)) + .slice(0, 6) + .map(([k, v]) => `${k}=${typeof v === "number" ? v.toFixed(3) : JSON.stringify(v).slice(0, 80)}`) + .join(" "); + item.innerHTML = `${t} ${ev.kind} ${detail}`; + el.appendChild(item); + while (el.children.length > 300) el.removeChild(el.firstChild); + el.scrollTop = el.scrollHeight; +} + +function setStage(stage, detail) { + const el = $("stage"); + if (!el) return; + el.textContent = stage + (detail ? ` · ${detail}` : ""); + el.className = "stage " + stage; +} + +function setButtonEnabled(id, enabled) { + const el = $(id); + if (!el) return; + el.disabled = !enabled; +} + +function updateButtons(status) { + if (!status) return; + const stage = status.stage || "DISCONNECTED"; + const busy = !!status.busy; + const runtime = stage === "RUNTIME"; + const connected = stage !== "DISCONNECTED" && stage !== "CONNECTING"; + const enabled = ["ENABLED", "STARTING_UP", "STAND_HOLD", "RUNTIME"].includes(stage); + const canStartup = stage === "ENABLED"; + const canRuntimeStart = stage === "STAND_HOLD"; + const canRuntimeStop = runtime; + + setButtonEnabled("btn-connect", !busy && stage === "DISCONNECTED"); + setButtonEnabled("btn-disconnect", !busy && connected); + setButtonEnabled("btn-enable", !busy && ["CONNECTED", "FAULTED"].includes(stage)); + setButtonEnabled("btn-disable", !busy && enabled); + setButtonEnabled("btn-startup", !busy && canStartup); + setButtonEnabled("btn-runtime-start", !busy && canRuntimeStart); + setButtonEnabled("btn-runtime-stop", !busy && canRuntimeStop); + setButtonEnabled("btn-reset-estop", !busy && stage === "ESTOPPED"); + setButtonEnabled("btn-estop", connected); +} + +function renderState(state) { + const el = $("state-summary"); + if (!el) return; + if (!state) { + el.innerHTML = '
STATUSNO DATA
'; + return; + } + const metric = (k, v, cls = "") => + `
${k}${v}
`; + const safetyText = ["NORMAL", "CLIP", "BRAKE", "ESTOP"][state.safety_level || 0]; + const guardText = ["NORMAL", "WARN", "STOP"][state.guard_level || 0] || "NORMAL"; + const imuCls = (state.imu_age_ms || 0) > 60 ? "bad" : (state.imu_age_ms || 0) > 30 ? "warn" : ""; + const dtCls = (state.loop_dt_ms || 0) > 25 ? "bad" : (state.loop_dt_ms || 0) > 22 ? "warn" : ""; + const gravityZ = state.proj_gravity?.[2] ?? -1; + const gravityCls = gravityZ > -0.5 ? "warn" : ""; + const rawMax = Math.max(...(state.raw || [0]).map((x) => Math.abs(x || 0))); + const trackingErr = Math.max( + ...(state.joint_pos || []).slice(0, 12).map((pos, i) => Math.abs(pos - ((state.target || [])[i] || 0))), + 0, + ); + el.innerHTML = [ + metric("phase", safeText(state.phase, "?")), + metric("imu_age", `${(state.imu_age_ms || 0).toFixed(1)} ms`, imuCls), + metric("loop_dt", `${(state.loop_dt_ms || 0).toFixed(1)} ms`, dtCls), + metric("safety", safetyText, state.safety_level >= 2 ? "bad" : state.safety_level === 1 ? "warn" : ""), + metric("guard", guardText, state.guard_level >= 2 ? "bad" : state.guard_level === 1 ? "warn" : ""), + metric("holdover", String(state.holdover_total || 0)), + metric("raw max", rawMax.toFixed(2)), + metric("grav_z", gravityZ.toFixed(3), gravityCls), + metric("track_err", trackingErr.toFixed(3), trackingErr > 0.5 ? "bad" : trackingErr > 0.2 ? "warn" : ""), + ].join(""); +} + +function renderDiagnostics(diag, state) { + if (!diag) return; + const setValue = (id, text, cls = "") => { + const el = $(id); + if (!el) return; + el.textContent = text; + el.className = "diag-value " + cls; + }; + setValue("diag-norm", "Aligned", "success"); + setValue("diag-latency", `${(state?.loop_dt_ms || 0).toFixed(1)} ms`, (state?.loop_dt_ms || 0) > 25 ? "danger" : (state?.loop_dt_ms || 0) > 22 ? "warning" : "success"); + const trackErr = Math.max( + ...(state?.joint_pos || []).slice(0, 12).map((pos, i) => Math.abs(pos - ((state?.target || [])[i] || 0))), + 0, + ); + setValue("diag-track-err", `${trackErr.toFixed(3)} rad`, trackErr > 0.5 ? "danger" : trackErr > 0.2 ? "warning" : "success"); + setValue("diag-runtime", diag.runtime_active ? "ACTIVE" : "IDLE", diag.runtime_active ? "success" : "warning"); + setValue("diag-runtime-age", diag.last_runtime_age_s == null ? "--" : `${diag.last_runtime_age_s.toFixed(2)} s`, diag.last_runtime_age_s != null && diag.last_runtime_age_s > 1.0 ? "danger" : "success"); + setValue("diag-poll-age", diag.last_poll_age_s == null ? "--" : `${diag.last_poll_age_s.toFixed(2)} s`, diag.last_poll_age_s != null && diag.last_poll_age_s > 1.0 ? "warning" : "success"); + setValue("diag-cmd-age", diag.last_command_age_s == null ? "--" : `${diag.last_command_age_s.toFixed(2)} s`); + setValue("diag-poll-errors", String(diag.poll_error_count || 0), (diag.poll_error_count || 0) > 0 ? "danger" : "success"); + setValue("diag-api-errors", String(diag.api_error_count || 0), (diag.api_error_count || 0) > 0 ? "warning" : "success"); + setValue("diag-suppression", String(diag.zero_cmd_suppression), diag.zero_cmd_suppression ? "warning" : "success"); + const pathEl = $("diag-policy"); + if (pathEl) pathEl.textContent = diag.policy_path || "--"; +} + +function renderFault(status) { + const faultBox = $("fault-box"); + const faultText = $("fault-text"); + const traceText = $("traceback-text"); + if (!faultBox || !faultText || !traceText) return; + if (!status.fault_reason && !status.last_error) { + faultBox.classList.add("hidden"); + faultText.textContent = ""; + traceText.textContent = ""; + return; + } + faultBox.classList.remove("hidden"); + faultText.textContent = status.fault_reason || status.last_error || ""; + traceText.textContent = status.last_traceback || ""; +} + +function applyStatus(status) { + if (!status) return; + CURRENT_STATUS = { ...(CURRENT_STATUS || {}), ...status }; + const merged = CURRENT_STATUS; + if (merged.stage) setStage(merged.stage, merged.detail || ""); + if (merged.busy !== undefined && $("busy")) $("busy").textContent = merged.busy ? " [BUSY]" : ""; + if (merged.log_dir && $("logdir")) $("logdir").textContent = merged.log_dir; + updateButtons(merged); + renderFault(merged); + if (merged.last_state !== undefined) { + const now = performance.now(); + if (now - LAST_RENDER_TS > 80) { + renderState(merged.last_state); + renderDiagnostics(merged.diagnostics || {}, merged.last_state); + if (window.viewer3d && window.viewer3d._isLoaded && merged.last_state.joint_pos) { + window.viewer3d.updateJoints(merged.last_state.joint_pos); + } + updateMotorsGrid(merged.last_state); + addPlotData(merged.last_state); + LAST_RENDER_TS = now; + } + } +} + +async function refreshDebug() { + try { + const debug = await api("/api/debug"); + if (debug.status) { + applyStatus(debug.status); + } + renderDiagnostics(debug.status?.diagnostics || {}, debug.status?.last_state || null); + renderFault(debug.status || {}); + const diagJson = $("debug-json"); + if (diagJson) diagJson.textContent = JSON.stringify(debug.status?.diagnostics || {}, null, 2); + } catch (err) { + appendEvent({ kind: "DEBUG_FETCH_ERROR", error: err.message }); + } +} + +function connectSSE() { + if (SSE_CONN) { + SSE_CONN.close(); + SSE_CONN = null; + } + if (SSE_RECONNECT_TIMER) { + clearTimeout(SSE_RECONNECT_TIMER); + SSE_RECONNECT_TIMER = null; + } + const es = new EventSource("/events"); + SSE_CONN = es; + es.onmessage = (event) => { + const ev = JSON.parse(event.data); + if (ev.kind === "STATUS_FULL" || ev.kind === "PULSE" || ev.kind === "STATUS") { + applyStatus(ev); + if (ev.fault_reason) appendEvent({ t: ev.t, kind: "FAULT_REASON", reason: ev.fault_reason }); + } else { + appendEvent(ev); + } + }; + es.onerror = () => { + if (SSE_CONN) { + SSE_CONN.close(); + SSE_CONN = null; + } + if (!SSE_RECONNECT_TIMER) { + SSE_RECONNECT_TIMER = setTimeout(() => { + SSE_RECONNECT_TIMER = null; + connectSSE(); + }, 1500); + } + }; +} + +window.jog = async (leg, joint, dir) => { + const delta = parseFloat($("jt-delta").value) * dir; + const kp = parseFloat($("jt-kp").value); + const kd = parseFloat($("jt-kd").value); + const duration = parseFloat($("jt-dur").value); + try { + await api("/api/test_motor", { leg, joint, delta_rad: delta, kp, kd, duration_s: duration }); + appendEvent({ kind: "JOG_SENT", leg, joint, delta }); + } catch (err) { + appendEvent({ kind: "JOG_ERROR", error: err.message, leg, joint }); + } +}; + +function initMotorsGrid() { + const grid = $("motors-grid"); + if (!grid) return; + const abbr = { hip_abduction: "H_ABD", hip_pitch: "H_PIT", knee: "KNEE", wheel: "WHEEL" }; + grid.innerHTML = SIM_JOINT_ORDER.map(([leg, joint], i) => ` +
+ ● + ${leg.toUpperCase()}_${abbr[joint]} + 0.00 + 0.00 + 0.00 +
+ + +
+
+ `).join(""); +} + +function updateMotorsGrid(state) { + if (!state || !state.joint_pos) return; + const positions = state.joint_pos; + const velocities = state.joint_vel || []; + const torques = state.joint_torque || []; + const stale = state.per_motor_stale || []; + for (let i = 0; i < 16; i += 1) { + const row = $("mi-" + i); + if (!row) continue; + const dot = $("ms-" + i); + if (dot) { + const count = stale[i] ?? 99; + if (count <= 0) { + dot.style.color = "#4ade80"; + dot.title = "online"; + } else if (count < 5) { + dot.style.color = "#facc15"; + dot.title = `stale(${count})`; + } else { + dot.style.color = "#ef4444"; + dot.title = `offline(${count})`; + } + } + row.children[2].textContent = (positions[i] || 0).toFixed(2); + row.children[3].textContent = (velocities[i] || 0).toFixed(2); + const tau = torques[i] || 0; + row.children[4].textContent = tau.toFixed(2); + row.children[4].style.color = Math.abs(tau) > 16.0 ? "var(--color-danger)" : ""; + row.children[4].style.fontWeight = Math.abs(tau) > 16.0 ? "bold" : ""; + } +} + +function initPlots() { + const colors12 = ["#ff453a", "#ff9f0a", "#ffd60a", "#32ade6", "#0a84ff", "#5e5ce6", "#ff375f", "#bf5af2", "#30d158", "#66d4cf", "#8e8e93", "#c7c7cc"]; + const specs = [ + { id: "plot-pos", title: "Leg Pos (12)", nCh: 12, colors: colors12 }, + { id: "plot-vel", title: "Wheel Vel (4)", nCh: 4, colors: ["#ff453a", "#32ade6", "#30d158", "#ffd60a"] }, + { id: "plot-imu", title: "IMU (gyro+gz)", nCh: 4, colors: ["#ff453a", "#30d158", "#0a84ff", "#ffd60a"] }, + { id: "plot-diag", title: "Diag (dt+age)", nCh: 2, colors: ["#ff453a", "#30d158"] }, + ]; + const maxPts = 150; + specs.forEach((spec) => { + const canvas = $(spec.id); + if (!canvas) return; + canvas.width = canvas.parentElement.clientWidth; + canvas.height = 80; + PLOTS[spec.id] = { + ctx: canvas.getContext("2d"), + title: spec.title, + nCh: spec.nCh, + colors: spec.colors, + data: Array.from({ length: spec.nCh }, () => new Array(maxPts).fill(0)), + yMin: Array(spec.nCh).fill(Infinity), + yMax: Array(spec.nCh).fill(-Infinity), + maxPts, + }; + }); +} + +function addPlotData(state) { + if (!state) return; + const channels = [ + ["plot-pos", (state.joint_pos || []).slice(0, 12)], + ["plot-vel", (state.joint_vel || []).slice(12, 16)], + ["plot-imu", [...(state.gyro || [0, 0, 0]), (state.proj_gravity || [0, 0, -1])[2]]], + ["plot-diag", [state.loop_dt_ms || 0, state.imu_age_ms || 0]], + ]; + channels.forEach(([id, values]) => { + const plot = PLOTS[id]; + if (!plot) return; + for (let i = 0; i < plot.nCh && i < values.length; i += 1) { + const data = plot.data[i]; + data.push(values[i]); + if (data.length > plot.maxPts) data.shift(); + if (values[i] < plot.yMin[i]) plot.yMin[i] = values[i]; + if (values[i] > plot.yMax[i]) plot.yMax[i] = values[i]; + } + drawPlot(plot); + }); +} + +function drawPlot(plot) { + const { ctx, data, colors, yMin, yMax, title, maxPts } = plot; + const canvas = ctx.canvas; + const width = canvas.width; + const height = canvas.height; + ctx.clearRect(0, 0, width, height); + ctx.fillStyle = "rgba(255,255,255,0.5)"; + ctx.font = "10px monospace"; + ctx.fillText(title, 4, 12); + const margin = { l: 30, r: 4, t: 16, b: 4 }; + const plotW = width - margin.l - margin.r; + const plotH = height - margin.t - margin.b; + if (plotW <= 0 || plotH <= 0) return; + for (let i = 0; i < data.length; i += 1) { + if (yMin[i] === Infinity) { + yMin[i] = -1; + yMax[i] = 1; + } + const curMin = Math.min(...data[i]); + const curMax = Math.max(...data[i]); + yMin[i] = yMin[i] * 0.99 + curMin * 0.01; + yMax[i] = yMax[i] * 0.99 + curMax * 0.01; + } + const globalMin = Math.min(...yMin); + const globalMax = Math.max(...yMax); + const range = globalMax - globalMin || 1; + data.forEach((series, i) => { + if (series.length < 2) return; + ctx.strokeStyle = colors[i] || "#8e8e93"; + ctx.lineWidth = 1.0; + ctx.beginPath(); + series.forEach((value, j) => { + const x = margin.l + (j / maxPts) * plotW; + const y = margin.t + plotH - ((value - globalMin) / range) * plotH; + if (j === 0) ctx.moveTo(x, y); + else ctx.lineTo(x, y); + }); + ctx.stroke(); + }); + ctx.fillStyle = "rgba(255,255,255,0.4)"; + ctx.font = "9px monospace"; + ctx.fillText(globalMax.toFixed(1), 2, margin.t + 8); + ctx.fillText(globalMin.toFixed(1), 2, margin.t + plotH - 2); +} + +async function refreshLogs() { + try { + const result = await api("/api/logs"); + const tbody = document.querySelector("#logs-table tbody"); + if (!tbody) return; + tbody.innerHTML = result.sessions.map((s) => ` + + ${s.id.slice(-8)} + ${s.state_csv ? `CSV` : "—"} + ${s.events_jsonl ? `JSONL` : "—"} + ${s.size_kb} KB + + `).join(""); + } catch (err) { + appendEvent({ kind: "LOG_REFRESH_ERROR", error: err.message }); + } +} + +let cmdTimer = null; +function sendCmd() { + if (cmdTimer) return; + cmdTimer = setTimeout(() => { + cmdTimer = null; + api("/api/cmd", { + vx: parseFloat($("cmd-vx").value), + vy: parseFloat($("cmd-vy").value), + yaw: parseFloat($("cmd-yaw").value), + }).catch((err) => appendEvent({ kind: "CMD_ERROR", error: err.message })); + }, 50); +} + +function bind() { + $("btn-connect").onclick = () => api("/api/connect", { dry_run: $("dry-run").checked }).catch((err) => appendEvent({ kind: "CONNECT_ERROR", error: err.message })); + $("btn-disconnect").onclick = () => api("/api/disconnect", {}).catch((err) => appendEvent({ kind: "DISCONNECT_ERROR", error: err.message })); + $("btn-enable").onclick = () => api("/api/enable", {}).catch((err) => appendEvent({ kind: "ENABLE_ERROR", error: err.message })); + $("btn-disable").onclick = () => api("/api/disable", {}).catch((err) => appendEvent({ kind: "DISABLE_ERROR", error: err.message })); + $("btn-startup").onclick = () => api("/api/startup", {}).catch((err) => appendEvent({ kind: "STARTUP_ERROR", error: err.message })); + $("btn-runtime-start").onclick = () => api("/api/runtime/start", { policy_path: $("policy-path").value || null }).catch((err) => appendEvent({ kind: "RUNTIME_START_ERROR", error: err.message })); + $("btn-runtime-stop").onclick = () => api("/api/runtime/stop", {}).catch((err) => appendEvent({ kind: "RUNTIME_STOP_ERROR", error: err.message })); + $("btn-estop").onclick = () => api("/api/estop", {}).catch((err) => appendEvent({ kind: "ESTOP_ERROR", error: err.message })); + $("btn-reset-estop").onclick = () => api("/api/reset_estop", {}).catch((err) => appendEvent({ kind: "RESET_ESTOP_ERROR", error: err.message })); + $("btn-refresh-debug").onclick = () => refreshDebug(); + + ["vx", "vy", "yaw"].forEach((key) => { + const el = $("cmd-" + key); + el.oninput = () => { + $("cmd-" + key + "-v").textContent = parseFloat(el.value).toFixed(2); + sendCmd(); + }; + }); + $("btn-cmd-zero").onclick = () => { + ["vx", "vy", "yaw"].forEach((key) => { + const el = $("cmd-" + key); + el.value = 0; + $("cmd-" + key + "-v").textContent = "0.00"; + }); + sendCmd(); + }; + + const jtSlider = $("jt-delta"); + jtSlider.oninput = () => { $("jt-delta-v").textContent = parseFloat(jtSlider.value).toFixed(2); }; + + $("btn-show-logs").onclick = () => { + refreshLogs(); + $("logs-modal").classList.remove("hidden"); + }; + $("btn-close-logs").onclick = () => $("logs-modal").classList.add("hidden"); +} + +window.addEventListener("DOMContentLoaded", () => { + initMotorsGrid(); + bind(); + initPlots(); + connectSSE(); + refreshLogs(); + refreshDebug(); + updateButtons({ stage: "DISCONNECTED", busy: false }); + setInterval(refreshLogs, 10000); + setInterval(refreshDebug, 5000); +}); + +window.addEventListener("resize", () => { + Object.values(PLOTS).forEach((plot) => { + plot.ctx.canvas.width = plot.ctx.canvas.parentElement.clientWidth; + }); +}); diff --git a/05_software/real/sim2real/web/static/index.html b/05_software/real/sim2real/web/static/index.html new file mode 100644 index 0000000..ad506f5 --- /dev/null +++ b/05_software/real/sim2real/web/static/index.html @@ -0,0 +1,190 @@ + + + + + + sim2real 控制台 + + + + +
+ +
加载中...
+
+ +
+
+

sim2real

+ DISCONNECTED + + +
+
+ + + +
+ + +
+
+ + + +
+
+ +
+
+

控制流程

+
+ +
+ +
+ + +
+
+
+
+ +
+

实时状态

+
+
+ +
+
+

Motors / Jog Test

+ POS | VEL | TAU +
+
+ Kp + Kd + Time + Δ(rad) + 0.10 +
+
+
+
+ +
+
+
+

Diagnostics

+ +
+
Obs NormalizationAligned
+
Control Latency-- ms
+
Tracking Error-- rad
+
Runtime--
+
Runtime Age--
+
Poll Age--
+
Cmd Age--
+
Poll Errors0
+
API Errors0
+
Zero-Cmd Suppression--
+
Policy--
+
+ + + +
+

Command

+
+
+ vx + + 0.00 +
+
+ vy + + 0.00 +
+
+ yaw + + 0.00 +
+ +
+
+ +
+

事件流

+
+
+ +
+

Debug JSON

+ 

+    

+
+    

+      
实时曲线

+      

+        
+        
+        
+        
+      

+    

+  

+
+  
+
+  
+  
+
+  
+  
+
+
diff --git a/05_software/real/sim2real/web/static/style.css b/05_software/real/sim2real/web/static/style.css
new file mode 100644
index 0000000..85b3abf
--- /dev/null
+++ b/05_software/real/sim2real/web/static/style.css
@@ -0,0 +1,394 @@
+/* Apple Glass Design System for sim2real */
+:root {
+  --bg-primary: #000000;
+  --glass-bg: rgba(20, 20, 22, 0.65);
+  --glass-border: rgba(255, 255, 255, 0.12);
+  --glass-shadow: 0 8px 32px rgba(0, 0, 0, 0.25);
+  --text-primary: #ffffff;
+  --text-secondary: #ebebf5;
+  --text-tertiary: #8e8e93;
+  --accent: #0a84ff;
+  --accent-hover: #409cff;
+  --success: #30d158;
+  --warning: #ffd60a;
+  --danger: #ff453a;
+  --blur-amount: 24px;
+  --saturation: 180%;
+  --spring: cubic-bezier(0.4, 0, 0.2, 1);
+  --panel-radius: 16px;
+  --font-family: -apple-system, BlinkMacSystemFont, 'SF Pro Display', 'PingFang SC', sans-serif;
+}
+
+[data-theme="light"] {
+  --bg-primary: #f5f5f7;
+  --glass-bg: rgba(245, 245, 245, 0.75);
+  --glass-border: rgba(0, 0, 0, 0.15);
+  --glass-shadow: 0 8px 32px rgba(0, 0, 0, 0.12);
+  --text-primary: #1d1d1f;
+  --text-secondary: #424245;
+  --text-tertiary: #86868b;
+}
+
+* {
+  box-sizing: border-box;
+  margin: 0;
+  padding: 0;
+}
+
+body {
+  font-family: var(--font-family);
+  overflow: hidden;
+  background: var(--bg-primary);
+  color: var(--text-primary);
+  -webkit-font-smoothing: antialiased;
+  transition: background 0.3s var(--spring);
+}
+
+/* 3D Canvas Background */
+#canvas-container {
+  position: fixed;
+  top: 0; left: 0; right: 0; bottom: 0;
+  z-index: 0;
+  background: radial-gradient(circle at center, #1a1a24 0%, #000000 100%);
+}
+#viewer-canvas {
+  width: 100%;
+  height: 100%;
+  display: block;
+  cursor: grab;
+}
+#viewer-canvas:active {
+  cursor: grabbing;
+}
+.viewer-overlay {
+  position: absolute;
+  top: 50%; left: 50%;
+  transform: translate(-50%, -50%);
+  color: var(--text-tertiary);
+  font-size: 14px;
+  pointer-events: none;
+}
+
+.viewer-count-indicator {
+  position: fixed;
+  bottom: 20px;
+  right: 20px;
+  font-size: 11px;
+  color: var(--text-tertiary);
+  z-index: 10;
+  font-family: monospace;
+}
+
+/* Glass Panels */
+.glass-panel {
+  background: var(--glass-bg);
+  backdrop-filter: blur(var(--blur-amount)) saturate(var(--saturation));
+  -webkit-backdrop-filter: blur(var(--blur-amount)) saturate(var(--saturation));
+  border: 0.5px solid var(--glass-border);
+  box-shadow: var(--glass-shadow);
+  z-index: 50;
+}
+
+/* Top Bar */
+.top-bar {
+  position: fixed;
+  top: 16px;
+  left: 50%;
+  transform: translateX(-50%);
+  display: flex;
+  align-items: center;
+  justify-content: space-between;
+  padding: 8px 16px;
+  border-radius: 24px;
+  width: 96%;
+  max-width: 1400px;
+  gap: 16px;
+}
+
+.top-bar-left, .top-bar-center, .top-bar-right {
+  display: flex;
+  align-items: center;
+  gap: 12px;
+}
+.top-bar-center {
+  flex: 1;
+  justify-content: center;
+}
+
+.top-bar h1 {
+  font-size: 16px;
+  font-weight: 600;
+  margin: 0;
+  background: -webkit-linear-gradient(45deg, #fff, #8e8e93);
+  -webkit-background-clip: text;
+  -webkit-text-fill-color: transparent;
+}
+
+.divider {
+  width: 1px;
+  height: 24px;
+  background: var(--glass-border);
+  margin: 0 4px;
+}
+
+/* Side Panels */
+.side-panel {
+  position: fixed;
+  top: 80px;
+  bottom: 20px;
+  width: 340px;
+  border-radius: var(--panel-radius);
+  display: flex;
+  flex-direction: column;
+  overflow: hidden;
+}
+
+.left-panel { left: 2%; }
+.right-panel { right: 2%; }
+
+.panel-section {
+  padding: 16px;
+  border-bottom: 0.5px solid var(--glass-border);
+  display: flex;
+  flex-direction: column;
+}
+.panel-section:last-child {
+  border-bottom: none;
+}
+.flex-1 { flex: 1; min-height: 0; }
+
+.panel-title {
+  font-size: 12px;
+  font-weight: 700;
+  color: var(--text-tertiary);
+  text-transform: uppercase;
+  letter-spacing: 0.5px;
+  margin-bottom: 12px;
+}
+.panel-title-row {
+  display: flex; justify-content: space-between; align-items: center;
+}
+
+/* Typography & Badges */
+.stage {
+  padding: 4px 10px;
+  border-radius: 12px;
+  font-size: 11px;
+  font-weight: 700;
+  text-transform: uppercase;
+  letter-spacing: 0.5px;
+  background: rgba(255,255,255,0.1);
+  color: var(--text-secondary);
+}
+.stage.DISCONNECTED { background: rgba(142,142,147,0.3); }
+.stage.CONNECTED { background: rgba(10,132,255,0.3); color: #82c4ff; }
+.stage.ENABLED { background: rgba(48,209,88,0.3); color: #8deda7; }
+.stage.FAULTED { background: rgba(255,69,58,0.3); color: #ff8b86; }
+.stage.ESTOPPED { background: rgba(255,69,58,0.5); color: #ff8b86; box-shadow: 0 0 8px rgba(255,69,58,0.4); }
+
+/* Buttons */
+.btn {
+  background: rgba(255, 255, 255, 0.08);
+  border: 1px solid rgba(255, 255, 255, 0.1);
+  border-radius: 8px;
+  color: var(--text-primary);
+  font-size: 12px;
+  font-weight: 500;
+  padding: 6px 12px;
+  cursor: pointer;
+  transition: all 0.2s var(--spring);
+  font-family: inherit;
+  display: inline-flex;
+  align-items: center;
+  justify-content: center;
+}
+.btn:hover:not(:disabled) {
+  background: rgba(255, 255, 255, 0.15);
+  transform: translateY(-1px);
+}
+.btn:active:not(:disabled) {
+  transform: translateY(1px);
+}
+.btn:disabled {
+  opacity: 0.5;
+  cursor: not-allowed;
+}
+.btn-primary { background: var(--accent); border-color: var(--accent); color: white; }
+.btn-primary:hover:not(:disabled) { background: var(--accent-hover); }
+.btn-success { background: rgba(48,209,88,0.8); border-color: transparent; color: white; }
+.btn-warning { background: rgba(255,214,10,0.8); border-color: transparent; color: black; }
+.btn-danger { background: rgba(255,69,58,0.8); border-color: transparent; color: white; }
+.btn-icon { width: 28px; height: 28px; padding: 0; border-radius: 50%; }
+.full-width { width: 100%; }
+.mt-2 { margin-top: 8px; }
+
+.btn-group-vertical {
+  display: flex; flex-direction: column; gap: 8px;
+}
+.btn-row {
+  display: flex; gap: 8px;
+}
+
+/* Inputs */
+.glass-input, .glass-select {
+  background: rgba(0,0,0,0.2);
+  border: 1px solid var(--glass-border);
+  border-radius: 6px;
+  padding: 6px 10px;
+  color: var(--text-primary);
+  font-size: 12px;
+  font-family: inherit;
+  outline: none;
+  transition: border-color 0.2s;
+}
+.glass-input:focus, .glass-select:focus {
+  border-color: var(--accent);
+}
+.glass-input.small { width: 60px; }
+.glass-input.mini { width: 45px; padding: 4px 6px; }
+
+.control-row {
+  display: flex; align-items: center; gap: 8px; margin-bottom: 8px;
+}
+.label { font-size: 11px; color: var(--text-tertiary); }
+
+/* Toggle Switch */
+.toggle-switch {
+  display: flex; align-items: center; gap: 8px; cursor: pointer;
+}
+.toggle-switch input { display: none; }
+.toggle-switch .slider {
+  position: relative; width: 32px; height: 18px;
+  background: rgba(255,255,255,0.2); border-radius: 18px;
+  transition: 0.3s;
+}
+.toggle-switch .slider::before {
+  content: ""; position: absolute;
+  width: 14px; height: 14px; border-radius: 50%;
+  background: white; top: 2px; left: 2px; transition: 0.3s;
+}
+.toggle-switch input:checked + .slider { background: var(--accent); }
+.toggle-switch input:checked + .slider::before { transform: translateX(14px); }
+.toggle-switch .label { font-size: 12px; color: var(--text-secondary); }
+
+/* Range Sliders */
+.slider-row {
+  display: flex; align-items: center; gap: 8px; margin-bottom: 8px;
+}
+.slider-label {
+  font-size: 12px; width: 30px; color: var(--text-secondary); font-family: monospace;
+}
+.slider-val {
+  font-size: 12px; width: 36px; text-align: right; color: var(--accent); font-family: monospace;
+}
+.glass-slider {
+  flex: 1; -webkit-appearance: none; height: 4px; border-radius: 2px;
+  background: rgba(255,255,255,0.2); outline: none;
+}
+.glass-slider::-webkit-slider-thumb {
+  -webkit-appearance: none; width: 14px; height: 14px;
+  border-radius: 50%; background: white; cursor: pointer;
+  box-shadow: 0 2px 4px rgba(0,0,0,0.5);
+}
+.glass-slider:active::-webkit-slider-thumb { transform: scale(1.2); }
+
+/* Motors List (Jog & Status) */
+.motors-grid-list {
+  display: flex; flex-direction: column; gap: 2px; overflow-y: auto; padding-right: 4px;
+}
+.motor-row {
+  display: flex; align-items: center; justify-content: space-between;
+  padding: 2px 6px; background: rgba(0,0,0,0.25); border-radius: 6px;
+  border: 1px solid rgba(255,255,255,0.03);
+}
+.motor-row .name { font-size: 11px; color: var(--text-secondary); width: 65px; font-weight: 500; font-family: monospace; }
+.motor-row .m-status { font-size: 8px; color: #ef4444; flex-shrink: 0; width: 12px; text-align: center; transition: color 0.3s; }
+.motor-row .val { font-size: 10px; font-family: monospace; text-align: right; width: 35px; }
+.motor-row .val.pos { color: #0a84ff; }
+.motor-row .val.vel { color: #30d158; }
+.motor-row .val.tau { color: #ff9f0a; }
+
+.m-jog { display: flex; gap: 2px; }
+.btn-jog { 
+  background: rgba(255,255,255,0.1); border: none; border-radius: 4px; 
+  color: white; font-family: monospace; font-size: 11px; padding: 2px 6px;
+  cursor: pointer; min-width: 24px; text-align: center;
+}
+.btn-jog:hover { background: rgba(255,255,255,0.25); }
+
+/* State Grid */
+.state-grid {
+  display: grid; grid-template-columns: 1fr 1fr; gap: 6px;
+  overflow-y: auto;
+}
+.state-item {
+  display: flex; justify-content: space-between; align-items: center;
+  padding: 4px 6px; background: rgba(0,0,0,0.2); border-radius: 4px;
+}
+.state-item .k { font-size: 10px; color: var(--text-tertiary); text-transform: uppercase; }
+.state-item .v { font-size: 11px; font-family: monospace; color: var(--text-primary); }
+.state-item .v.warn { color: var(--warning); }
+.state-item .v.bad { color: var(--danger); }
+
+/* ==== Plots & Logs ==== */
+.log-section { flex: 1; display: flex; flex-direction: column; min-height: 150px; }
+.log {
+  flex: 1; background: rgba(0,0,0,0.4); border-radius: 6px; padding: 8px;
+  font-family: monospace; font-size: 11px; overflow-y: auto; color: var(--text-secondary);
+  border: 1px solid rgba(255,255,255,0.05);
+}
+.log div { margin-bottom: 2px; line-height: 1.3; }
+.plots-section { margin-top: auto; }
+.plots-container {
+  display: flex; flex-direction: column; gap: 4px; overflow-y: auto; padding-right: 4px;
+}
+.plots-container canvas {
+  width: 100% !important; height: 50px !important; background: rgba(0,0,0,0.2); border-radius: 4px;
+}
+
+/* ==== Diagnostics ==== */
+.diag-row {
+  display: flex; justify-content: space-between; align-items: center;
+  padding: 4px 6px; background: rgba(0,0,0,0.2); border-radius: 4px;
+  margin-bottom: 4px; font-family: monospace; font-size: 12px;
+}
+.diag-label { color: var(--text-secondary); }
+.diag-value { color: var(--text-primary); font-weight: bold; }
+.diag-value.success { color: var(--color-success); }
+.diag-value.warning { color: var(--color-warning); }
+.diag-value.danger { color: var(--color-danger); }
+.plots-container::-webkit-scrollbar { width: 4px; }
+.plots-container::-webkit-scrollbar-thumb { background: rgba(255,255,255,0.2); border-radius: 2px; }
+
+/* Modal & Floating BTN */
+.floating-btn {
+  position: fixed; bottom: 20px; left: 20px; width: 40px; height: 40px;
+  border-radius: 50%; font-size: 18px; z-index: 100;
+  box-shadow: var(--glass-shadow);
+}
+.glass-modal {
+  position: fixed; top: 0; left: 0; right: 0; bottom: 0;
+  background: rgba(0,0,0,0.5); backdrop-filter: blur(4px);
+  display: flex; align-items: center; justify-content: center;
+  z-index: 1000; transition: opacity 0.3s;
+}
+.glass-modal.hidden { opacity: 0; pointer-events: none; }
+.modal-content {
+  width: 80%; max-width: 600px; max-height: 80vh;
+  border-radius: var(--panel-radius); display: flex; flex-direction: column;
+}
+.modal-header {
+  padding: 16px; border-bottom: 0.5px solid var(--glass-border);
+  display: flex; justify-content: space-between; align-items: center;
+}
+.btn-close {
+  background: transparent; border: none; color: var(--text-tertiary);
+  font-size: 20px; cursor: pointer;
+}
+.btn-close:hover { color: var(--text-primary); }
+.modal-body { padding: 16px; overflow-y: auto; }
+table { width: 100%; border-collapse: collapse; font-size: 12px; }
+table th { color: var(--text-tertiary); text-align: left; padding: 8px; border-bottom: 1px solid var(--glass-border); }
+table td { padding: 8px; border-bottom: 1px solid rgba(255,255,255,0.05); }
+table a { color: var(--accent); text-decoration: none; }
+table a:hover { text-decoration: underline; }
diff --git a/05_software/real/sim2real/web/static/viewer/MJCFAdapter.js b/05_software/real/sim2real/web/static/viewer/MJCFAdapter.js
new file mode 100644
index 0000000..8140e14
--- /dev/null
+++ b/05_software/real/sim2real/web/static/viewer/MJCFAdapter.js
@@ -0,0 +1,2090 @@
+/**
+ * MJCF Adapter
+ * Parses MJCF XML and converts to unified model
+ */
+import { UnifiedRobotModel, Link, Joint, JointLimits, VisualGeometry, CollisionGeometry, InertialProperties, GeometryType, Constraint } from './UnifiedRobotModel.js';
+import * as THREE from 'three';
+import { loadMeshFile, ensureMeshHasPhongMaterial, getLoaders } from './MeshLoader.js';
+
+export class MJCFAdapter {
+    /**
+     * Process include tags in MJCF XML
+     * Replaces  with the content of the referenced file
+     * @param {string} xmlContent - MJCF XML content
+     * @param {Map} fileMap - File map for loading included files
+     * @param {string} basePath - Base path for resolving relative paths
+     * @returns {Promise} Processed XML content
+     */
+    static async processIncludes(xmlContent, fileMap = null, basePath = null) {
+        const parser = new DOMParser();
+        const doc = parser.parseFromString(xmlContent, 'text/xml');
+
+        // Check for parse errors
+        const parseError = doc.querySelector('parsererror');
+        if (parseError) {
+            // If there's a parse error, return original content
+            console.warn('Initial XML parse error, skipping include processing:', parseError.textContent);
+            return xmlContent;
+        }
+
+        // Find all include elements
+        const includes = doc.querySelectorAll('include');
+
+        if (includes.length === 0) {
+            return xmlContent;
+        }
+
+        console.log(`Processing ${includes.length} include tag(s)...`);
+
+        // Process each include tag
+        for (const includeEl of includes) {
+            const filePath = includeEl.getAttribute('file');
+
+            if (!filePath) {
+                console.warn('Include tag missing file attribute');
+                includeEl.remove();
+                continue;
+            }
+
+            // Try to find the included file in fileMap
+            let includedContent = null;
+
+            if (fileMap) {
+                // Try different path variations
+                const pathVariations = [
+                    filePath,
+                    basePath ? basePath + '/' + filePath : filePath,
+                    filePath.startsWith('/') ? filePath : '/' + filePath
+                ];
+
+                for (const path of pathVariations) {
+                    // Try exact match first
+                    if (fileMap.has(path)) {
+                        const file = fileMap.get(path);
+                        try {
+                            includedContent = await file.text();
+                            console.log(`Found included file: ${path}`);
+                            break;
+                        } catch (e) {
+                            console.warn(`Failed to read included file ${path}:`, e);
+                        }
+                    }
+
+                    // Try case-insensitive match
+                    for (const [key, value] of fileMap) {
+                        if (key.toLowerCase() === path.toLowerCase()) {
+                            try {
+                                includedContent = await value.text();
+                                console.log(`Found included file (case-insensitive): ${key}`);
+                                break;
+                            } catch (e) {
+                                console.warn(`Failed to read included file ${key}:`, e);
+                            }
+                        }
+                    }
+                    if (includedContent) break;
+                }
+            }
+
+            if (!includedContent) {
+                console.warn(`Could not find included file: ${filePath}`);
+                includeEl.remove();
+                continue;
+            }
+
+            // Parse the included content
+            const includedDoc = parser.parseFromString(includedContent, 'text/xml');
+            const includedParseError = includedDoc.querySelector('parsererror');
+
+            if (includedParseError) {
+                console.warn(`Failed to parse included file ${filePath}:`, includedParseError.textContent);
+                includeEl.remove();
+                continue;
+            }
+
+            // Get the mujoco root element from included file
+            const includedRoot = includedDoc.querySelector('mujoco');
+
+            if (!includedRoot) {
+                console.warn(`Included file ${filePath} has no mujoco root element`);
+                includeEl.remove();
+                continue;
+            }
+
+            // Move all child elements from included mujoco to current document
+            // Insert them before the include element
+            const childNodes = Array.from(includedRoot.childNodes);
+
+            for (const child of childNodes) {
+                // Skip text nodes and comment nodes
+                if (child.nodeType === Node.TEXT_NODE ||
+                    (child.nodeType === Node.COMMENT_NODE) ||
+                    (child.nodeType === Node.PROCESSING_INSTRUCTION_NODE)) {
+                    continue;
+                }
+
+                // Clone the node to avoid removing from included doc
+                const importedNode = doc.importNode(child, true);
+
+                // Insert before the include element
+                includeEl.parentNode.insertBefore(importedNode, includeEl);
+            }
+
+            console.log(`Successfully included content from: ${filePath}`);
+
+            // Remove the include element
+            includeEl.remove();
+        }
+
+        // Serialize the modified document back to string
+        const serializer = new XMLSerializer();
+        return serializer.serializeToString(doc);
+    }
+
+    /**
+     * Parse MJCF XML content and convert to unified model
+     * @param {string} xmlContent - MJCF XML content
+     * @param {Map} fileMap - File map (optional), for loading mesh files
+     * @param {string} basePath - Base path for resolving relative include paths (optional)
+     * @returns {Promise}
+     */
+    static async parse(xmlContent, fileMap = null, basePath = null) {
+        // Process include tags first
+        const processedContent = await this.processIncludes(xmlContent, fileMap, basePath);
+
+        const parser = new DOMParser();
+        const doc = parser.parseFromString(processedContent, 'text/xml');
+
+        // Check parse errors
+        const parseError = doc.querySelector('parsererror');
+        if (parseError) {
+            throw new Error('MJCF XML parsing failed: ' + parseError.textContent);
+        }
+
+        const model = new UnifiedRobotModel();
+        model.name = 'mujoco_model';
+
+        // Parse default values and class definitions in default tags first
+        // (needed for mesh scale inheritance)
+        const { classDefaults, rootDefaults } = this.parseDefaults(doc);
+
+        // Parse mesh definitions in asset tags (build mesh name to file path mapping)
+        // Pass classDefaults and rootDefaults to inherit mesh scale
+        const meshMap = this.parseAssets(doc, classDefaults, rootDefaults);
+
+        // Parse material definitions in material tags
+        const materialMap = this.parseMaterials(doc);
+
+        // Get worldbody (root node)
+        const worldbody = doc.querySelector('worldbody');
+        if (!worldbody) {
+            throw new Error('MJCF file missing worldbody element');
+        }
+
+        // Parse geoms directly in worldbody (not inside any body element)
+        // These geoms belong to a special "worldbody" link
+        const worldbodyGeoms = worldbody.querySelectorAll(':scope > geom');
+        if (worldbodyGeoms.length > 0) {
+            const worldbodyLink = new Link('worldbody');
+            worldbodyLink.userData.isWorldbody = true;
+            const seenMeshes = new Set();
+
+            worldbodyGeoms.forEach((geomEl, geomIndex) => {
+                // Get inherited properties from default class
+                const inheritedProps = this.getGeomInheritedProperties(geomEl, classDefaults, rootDefaults);
+
+                const group = geomEl.getAttribute('group');
+                // Use inherited group if not explicitly defined
+                const groupNum = group !== null ? parseInt(group) : 
+                    (inheritedProps.group !== null ? inheritedProps.group : 0);
+                const geomName = (geomEl.getAttribute('name') || '').toLowerCase();
+                const hasRgba = geomEl.hasAttribute('rgba') || inheritedProps.rgba !== null;
+                const meshRef = geomEl.getAttribute('mesh');
+                
+                // Use inherited contype/conaffinity if not explicitly defined
+                const contype = geomEl.getAttribute('contype');
+                const conaffinity = geomEl.getAttribute('conaffinity');
+                const density = geomEl.getAttribute('density');
+                const contypeNum = contype !== null ? parseInt(contype) : 
+                    (inheritedProps.contype !== null ? inheritedProps.contype : null);
+                const conaffinityNum = conaffinity !== null ? parseInt(conaffinity) : 
+                    (inheritedProps.conaffinity !== null ? inheritedProps.conaffinity : null);
+                const densityNum = density !== null ? parseFloat(density) : 
+                    (inheritedProps.density !== null ? inheritedProps.density : null);
+
+                // Determine if collision or visual (same logic as in parseBodies)
+                let isCollisionGeom = false;
+                if (!meshRef) {
+                    isCollisionGeom = true;
+                } else {
+                    if (contypeNum === 0 && conaffinityNum === 0) {
+                        isCollisionGeom = false;
+                    } else if (groupNum === 3) {
+                        // group=3 is collision in MuJoCo convention
+                        isCollisionGeom = true;
+                    } else if (groupNum === 2 || groupNum === 1) {
+                        // group=1,2 are visual
+                        isCollisionGeom = false;
+                    } else if (geomName.includes('collision')) {
+                        isCollisionGeom = true;
+                    } else if (seenMeshes.has(meshRef)) {
+                        if (hasRgba || (contypeNum === 0 && conaffinityNum === 0)) {
+                            return; // Skip duplicate visual
+                        } else {
+                            isCollisionGeom = true;
+                        }
+                    } else if (densityNum === 0 && groupNum === 1) {
+                        isCollisionGeom = false;
+                    } else if (hasRgba) {
+                        isCollisionGeom = false;
+                    } else {
+                        isCollisionGeom = false;
+                    }
+                }
+
+                const geom = this.parseGeom(geomEl, meshMap);
+                if (geom) {
+                    if (isCollisionGeom) {
+                        const collision = new CollisionGeometry();
+                        collision.geometry = geom;
+                        collision.name = geomEl.getAttribute('name') || `worldbody_collision_${geomIndex}`;
+                        collision.origin = this.parseOrigin(geomEl);
+                        worldbodyLink.collisions.push(collision);
+                    } else {
+                        if (meshRef) {
+                            seenMeshes.add(meshRef);
+                        }
+                        const visual = new VisualGeometry();
+                        visual.geometry = geom;
+                        visual.name = geomEl.getAttribute('name') || `worldbody_geom_${geomIndex}`;
+                        visual.origin = this.parseOrigin(geomEl);
+
+                        // Parse rgba (priority: explicit > inherited)
+                        let rgba = null;
+                        if (geomEl.hasAttribute('rgba')) {
+                            const rgbaStr = geomEl.getAttribute('rgba');
+                            const rgbaVals = rgbaStr.split(' ').map(parseFloat);
+                            if (rgbaVals.length >= 3) {
+                                rgba = {
+                                    r: rgbaVals[0],
+                                    g: rgbaVals[1],
+                                    b: rgbaVals[2],
+                                    a: rgbaVals.length >= 4 ? rgbaVals[3] : 1.0
+                                };
+                            }
+                        } else if (inheritedProps.rgba) {
+                            rgba = inheritedProps.rgba;
+                        }
+
+                        visual.userData = {
+                            group: groupNum,
+                            hasRgba: hasRgba || !!rgba,
+                            rgba: rgba,
+                            meshRef: meshRef,
+                            geomType: geomEl.getAttribute('type') || (meshRef ? 'mesh' : 'box')
+                        };
+                        worldbodyLink.visuals.push(visual);
+                    }
+                }
+            });
+
+            // Only add worldbody link if it has geometries
+            if (worldbodyLink.visuals.length > 0 || worldbodyLink.collisions.length > 0) {
+                model.addLink(worldbodyLink);
+            }
+        }
+
+        // Parse all bodies (links), pass meshMap, materialMap, classDefaults and rootDefaults
+        const bodyMap = new Map();
+        this.parseBodies(worldbody, null, bodyMap, model, null, meshMap, null, materialMap, classDefaults, rootDefaults);
+
+        // Parse all joints
+        this.parseJoints(worldbody, bodyMap, model, null, classDefaults);
+
+        // Parse equality constraints (closed-chain constraints for parallel mechanisms)
+        this.parseEquality(doc, model);
+
+        // Find root body
+        // Priority: worldbody link > bodies without parent joints > first link
+        const worldbodyLink = model.links.get('worldbody');
+        if (worldbodyLink) {
+            model.rootLink = 'worldbody';
+        } else {
+            const rootBodies = Array.from(model.links.keys()).filter(
+                name => !Array.from(model.joints.values()).some(j => j.child === name)
+            );
+            if (rootBodies.length > 0) {
+                model.rootLink = rootBodies[0];
+            } else if (model.links.size > 0) {
+                model.rootLink = Array.from(model.links.keys())[0];
+            }
+        }
+
+        // Create Three.js objects (asynchronously load mesh files)
+        await this.createThreeObject(model, fileMap, meshMap);
+
+        return model;
+    }
+
+    /**
+     * Parse mesh definitions in asset tags
+     * @param {Document} doc - XML document
+     * @param {Map} classDefaults - Class default properties map (optional)
+     * @param {object} rootDefaults - Root default properties (optional)
+     * @returns {Map} Mapping from mesh names to mesh data
+     * Mesh data can be: { type: 'file', path: string, scale: [x,y,z] } or { type: 'vertex', vertices: Float32Array, scale: [x,y,z] }
+     */
+    static parseAssets(doc, classDefaults = null, rootDefaults = null) {
+        const meshMap = new Map();
+        const asset = doc.querySelector('asset');
+        if (!asset) {
+            return meshMap;
+        }
+
+        const meshes = asset.querySelectorAll('mesh');
+        meshes.forEach((meshEl, index) => {
+            let name = meshEl.getAttribute('name');
+            const file = meshEl.getAttribute('file');
+            const vertex = meshEl.getAttribute('vertex');
+            const scale = meshEl.getAttribute('scale');
+            const meshClass = meshEl.getAttribute('class');
+
+            // Parse scale (priority: direct attribute > class inheritance > root defaults > [1,1,1])
+            let scaleVec = [1, 1, 1];
+            
+            // First check direct scale attribute
+            if (scale) {
+                const scaleValues = scale.trim().split(/\s+/).map(parseFloat);
+                if (scaleValues.length === 1) {
+                    scaleVec = [scaleValues[0], scaleValues[0], scaleValues[0]];
+                } else if (scaleValues.length === 3) {
+                    scaleVec = scaleValues;
+                }
+            } else if (meshClass && classDefaults && classDefaults.has(meshClass)) {
+                // Try to inherit scale from class defaults
+                const classDefault = classDefaults.get(meshClass);
+                if (classDefault.mesh && classDefault.mesh.scale) {
+                    scaleVec = classDefault.mesh.scale;
+                }
+            } else if (rootDefaults && rootDefaults.mesh && rootDefaults.mesh.scale) {
+                // Fall back to root defaults (e.g., robotis_op3)
+                scaleVec = rootDefaults.mesh.scale;
+            }
+
+            // If has vertex attribute, it's an inline-defined mesh
+            if (vertex) {
+                if (!name) {
+                    name = `inline_mesh_${index}`;
+                }
+
+                // Parse vertex data
+                const vertexValues = vertex.trim().split(/\s+/).map(parseFloat);
+                const vertices = new Float32Array(vertexValues);
+
+                meshMap.set(name, {
+                    type: 'vertex',
+                    vertices: vertices,
+                    scale: scaleVec
+                });
+            }
+            // If has file attribute, it's an external file
+            else if (file) {
+                // If no name, extract filename from file (remove path and extension)
+                if (!name) {
+                    // Extract filename from path: "path/to/wheel.stl" -> "wheel"
+                    const fileName = file.split('/').pop().split('\\').pop(); // Support / and \ path separators
+                    name = fileName.split('.')[0]; // Remove extension
+                }
+
+                meshMap.set(name, {
+                    type: 'file',
+                    path: file,
+                    scale: scaleVec
+                });
+            } else {
+                console.warn('MJCF mesh element missing file or vertex attribute, skipping');
+                return;
+            }
+        });
+
+        return meshMap;
+    }
+
+    /**
+     * Parse material definitions in asset tags
+     * @param {Document} doc - XML document
+     * @returns {Map} Mapping from material names to material properties
+     */
+    static parseMaterials(doc) {
+        const materialMap = new Map();
+        const asset = doc.querySelector('asset');
+        if (!asset) {
+            return materialMap;
+        }
+
+        const materials = asset.querySelectorAll('material');
+        materials.forEach((matEl) => {
+            const name = matEl.getAttribute('name');
+            if (!name) return;
+
+            const material = {};
+
+            // Parse rgba
+            const rgba = matEl.getAttribute('rgba');
+            if (rgba) {
+                const vals = rgba.split(' ').map(parseFloat);
+                if (vals.length >= 3) {
+                    material.rgba = {
+                        r: vals[0],
+                        g: vals[1],
+                        b: vals[2],
+                        a: vals.length >= 4 ? vals[3] : 1.0
+                    };
+                }
+            }
+
+            // Parse other material properties
+            const specular = matEl.getAttribute('specular');
+            if (specular) {
+                const vals = specular.split(' ').map(parseFloat);
+                material.specular = vals[0] || 0.5;
+            }
+
+            const shininess = matEl.getAttribute('shininess');
+            if (shininess) {
+                material.shininess = parseFloat(shininess);
+            }
+
+            materialMap.set(name, material);
+        });
+
+        return materialMap;
+    }
+
+    /**
+     * Parse default values and class definitions in default tags
+     * @param {Document} doc - XML document
+     * @returns {object} Object containing classDefaults Map and rootDefaults object
+     */
+    static parseDefaults(doc) {
+        const classDefaults = new Map();
+        let rootDefaults = {};
+
+        // Recursively parse default tags
+        const parseDefaultElement = (defaultEl, parentDefaults = {}) => {
+            const className = defaultEl.getAttribute('class');
+
+            // Start from parent defaults, deep copy to avoid reference issues
+            const defaults = JSON.parse(JSON.stringify(parentDefaults || {}));
+
+            // Parse mesh default values
+            const meshEl = defaultEl.querySelector(':scope > mesh');
+            if (meshEl) {
+                if (!defaults.mesh) {
+                    defaults.mesh = {};
+                }
+
+                // Parse scale
+                const scale = meshEl.getAttribute('scale');
+                if (scale) {
+                    const scaleVals = scale.trim().split(/\s+/).map(parseFloat);
+                    if (scaleVals.length === 1) {
+                        defaults.mesh.scale = [scaleVals[0], scaleVals[0], scaleVals[0]];
+                    } else if (scaleVals.length === 3) {
+                        defaults.mesh.scale = scaleVals;
+                    }
+                }
+            }
+
+            // Parse joint default values
+            const jointEl = defaultEl.querySelector(':scope > joint');
+            if (jointEl) {
+                // If parent has joint defaults, inherit first
+                if (!defaults.joint) {
+                    defaults.joint = {};
+                }
+
+                // Parse axis (if axis defined, completely replace parent axis)
+                const axis = jointEl.getAttribute('axis');
+                if (axis) {
+                    const axisVals = axis.split(' ').map(parseFloat);
+                    defaults.joint.axis = [axisVals[0] || 0, axisVals[1] || 0, axisVals[2] || 0];
+                }
+
+                // Parse range
+                const range = jointEl.getAttribute('range');
+                if (range) {
+                    const rangeVals = range.split(' ').map(parseFloat);
+                    defaults.joint.range = rangeVals;
+                }
+
+                // Parse damping
+                const damping = jointEl.getAttribute('damping');
+                if (damping) {
+                    defaults.joint.damping = parseFloat(damping);
+                }
+            }
+
+            // Parse geom default values
+            const geomEl = defaultEl.querySelector(':scope > geom');
+            if (geomEl) {
+                if (!defaults.geom) {
+                    defaults.geom = {};
+                }
+
+                // Parse contype
+                const contype = geomEl.getAttribute('contype');
+                if (contype !== null) {
+                    defaults.geom.contype = parseInt(contype);
+                }
+
+                // Parse conaffinity
+                const conaffinity = geomEl.getAttribute('conaffinity');
+                if (conaffinity !== null) {
+                    defaults.geom.conaffinity = parseInt(conaffinity);
+                }
+
+                // Parse group
+                const group = geomEl.getAttribute('group');
+                if (group !== null) {
+                    defaults.geom.group = parseInt(group);
+                }
+
+                // Parse rgba
+                const rgba = geomEl.getAttribute('rgba');
+                if (rgba) {
+                    const rgbaVals = rgba.split(' ').map(parseFloat);
+                    if (rgbaVals.length >= 3) {
+                        defaults.geom.rgba = {
+                            r: rgbaVals[0],
+                            g: rgbaVals[1],
+                            b: rgbaVals[2],
+                            a: rgbaVals.length >= 4 ? rgbaVals[3] : 1.0
+                        };
+                    }
+                }
+
+                // Parse material
+                const material = geomEl.getAttribute('material');
+                if (material) {
+                    defaults.geom.material = material;
+                }
+
+                // Parse type
+                const type = geomEl.getAttribute('type');
+                if (type) {
+                    defaults.geom.type = type;
+                }
+
+                // Parse density
+                const density = geomEl.getAttribute('density');
+                if (density !== null) {
+                    defaults.geom.density = parseFloat(density);
+                }
+            }
+
+            // If has class name, save to class map
+            if (className) {
+                classDefaults.set(className, defaults);
+            } else {
+                // No class name means this is a root default (inherits to all)
+                // Store the final computed defaults as rootDefaults
+                Object.assign(rootDefaults, defaults);
+            }
+
+            // Recursively process nested default tags
+            const nestedDefaults = defaultEl.querySelectorAll(':scope > default');
+            nestedDefaults.forEach(nested => {
+                parseDefaultElement(nested, defaults);
+            });
+        };
+
+        // Start parsing from root default tags
+        const rootDefaultElements = doc.querySelectorAll('mujoco > default');
+        rootDefaultElements.forEach(defaultEl => {
+            parseDefaultElement(defaultEl);
+        });
+
+        return { classDefaults, rootDefaults };
+    }
+
+    /**
+     * Get inherited geom properties from default class
+     * @param {Element} geomEl - geom element
+     * @param {Map} classDefaults - Class default properties map
+     * @param {object} rootDefaults - Root default properties
+     * @returns {object} Inherited properties object
+     */
+    static getGeomInheritedProperties(geomEl, classDefaults, rootDefaults) {
+        const inherited = {
+            contype: null,
+            conaffinity: null,
+            group: null,
+            rgba: null,
+            material: null,
+            type: null,
+            density: null
+        };
+
+        // First apply root defaults
+        if (rootDefaults && rootDefaults.geom) {
+            Object.assign(inherited, rootDefaults.geom);
+        }
+
+        // Then apply class defaults (if geom has class attribute)
+        const className = geomEl.getAttribute('class');
+        if (className && classDefaults && classDefaults.has(className)) {
+            const classDefault = classDefaults.get(className);
+            if (classDefault.geom) {
+                Object.assign(inherited, classDefault.geom);
+            }
+        }
+
+        return inherited;
+    }
+
+    /**
+     * Recursively parse body elements, record parent-child relationships
+     */
+    static parseBodies(element, parentName, bodyMap, model, parentLinkRef = null, meshMap = null, stats = null, materialMap = null, classDefaults = null, rootDefaults = null) {
+        // Initialize stats object (only on root call)
+        if (!stats) {
+            stats = { totalGeoms: 0, skippedCollisionGeoms: 0, visualGeoms: 0 };
+        }
+
+        const bodies = element.querySelectorAll(':scope > body');
+
+        bodies.forEach(bodyEl => {
+            const linkName = bodyEl.getAttribute('name') || `body_${bodyMap.size}`;
+            const link = new Link(linkName);
+
+            // Record parent link relationship (for building hierarchy later)
+            if (parentName) {
+                link.userData.parentName = parentName;
+            }
+
+            // Parse body's pos and quat (body's own position)
+            const bodyOrigin = this.parseOrigin(bodyEl);
+            link.userData.bodyOrigin = bodyOrigin;
+
+            // Parse geometries (geom)
+            const geoms = bodyEl.querySelectorAll(':scope > geom');
+            const seenMeshes = new Set(); // Track added meshes to avoid duplicates
+
+            geoms.forEach((geomEl, geomIndex) => {
+                stats.totalGeoms++;
+
+                // Get inherited properties from default class
+                const inheritedProps = this.getGeomInheritedProperties(geomEl, classDefaults, rootDefaults);
+
+                const group = geomEl.getAttribute('group');
+                // Use inherited group if not explicitly defined
+                const groupNum = group !== null ? parseInt(group) : 
+                    (inheritedProps.group !== null ? inheritedProps.group : 0);
+                const geomName = (geomEl.getAttribute('name') || '').toLowerCase();
+                const hasRgba = geomEl.hasAttribute('rgba') || inheritedProps.rgba !== null;
+                const meshRef = geomEl.getAttribute('mesh');
+                // Use inherited type if not explicitly defined
+                const geomType = geomEl.getAttribute('type') || inheritedProps.type || (meshRef ? 'mesh' : 'box');
+
+                // Check collision-related attributes (use inherited if not explicitly defined)
+                const contype = geomEl.getAttribute('contype');
+                const conaffinity = geomEl.getAttribute('conaffinity');
+                const density = geomEl.getAttribute('density');
+                const contypeNum = contype !== null ? parseInt(contype) : 
+                    (inheritedProps.contype !== null ? inheritedProps.contype : null);
+                const conaffinityNum = conaffinity !== null ? parseInt(conaffinity) : 
+                    (inheritedProps.conaffinity !== null ? inheritedProps.conaffinity : null);
+                const densityNum = density !== null ? parseFloat(density) : 
+                    (inheritedProps.density !== null ? inheritedProps.density : null);
+
+                // Determine geom type: visual or collision
+                let isCollisionGeom = false;
+                let skipReason = '';
+
+                // [Key Strategy]: Distinguish visual and collision geoms
+                // Basic geometries (box, cylinder, sphere) are usually simplified shapes for collision
+                if (!meshRef) {
+                    // No mesh reference, basic geometry, treat as collision
+                    isCollisionGeom = true;
+                } else {
+                    // Has mesh reference, check if should be collision
+
+                    // Strategy 1: Explicitly disabled collision (contype="0" conaffinity="0") = visual only
+                    if (contypeNum === 0 && conaffinityNum === 0) {
+                        // This is explicitly marked as visual-only (no collision)
+                        isCollisionGeom = false;
+                    }
+                    // Strategy 2: group=2 is visual, group=3 is collision
+                    // MuJoCo convention: group 0=default, 1=visual1, 2=visual2, 3=collision
+                    else if (groupNum === 3) {
+                        isCollisionGeom = true;
+                    } else if (groupNum === 2 || groupNum === 1) {
+                        isCollisionGeom = false;
+                    }
+                    // Strategy 3: Name contains collision (indicates collision-specific)
+                    else if (geomName.includes('collision')) {
+                        isCollisionGeom = true;
+                    }
+                    // Strategy 4: If same mesh already added as visual
+                    else if (seenMeshes.has(meshRef)) {
+                        // If current geom also has visual markers (rgba or contype="0"), skip duplicate visual
+                        if (hasRgba || (contypeNum === 0 && conaffinityNum === 0)) {
+                            stats.skippedCollisionGeoms++;
+                            return;
+                        } else {
+                            // Same mesh, but current geom has no visual markers - treat as collision
+                            isCollisionGeom = true;
+                        }
+                    }
+                    // Strategy 5: If density="0" and group="1", likely visual-only (common pattern in MJCF)
+                    else if (densityNum === 0 && groupNum === 1) {
+                        // This pattern (density="0" group="1") is often used for visual-only geoms
+                        isCollisionGeom = false;
+                    }
+                    // Strategy 6: Default: if has rgba, treat as visual
+                    else if (hasRgba) {
+                        isCollisionGeom = false;
+                    }
+                    // Strategy 7: Default for mesh: treat as visual (for display purposes)
+                    else {
+                        // No explicit markers, but it's a mesh - default to visual for display
+                        // (collision might be handled by a separate geom with same mesh)
+                        isCollisionGeom = false;
+                    }
+                }
+
+                const geom = this.parseGeom(geomEl, meshMap);
+                if (geom) {
+                    if (isCollisionGeom) {
+                        // Add to collision list
+                        const collision = new CollisionGeometry();
+                        collision.geometry = geom;
+                        collision.name = geomEl.getAttribute('name') || `collision_${geomIndex}`;
+                        collision.origin = this.parseOrigin(geomEl);
+                        link.collisions.push(collision);
+                    } else {
+                    // Add to visual list
+                        stats.visualGeoms++;
+
+                        // Record added mesh
+                        if (meshRef) {
+                            seenMeshes.add(meshRef);
+                        }
+
+                        const visual = new VisualGeometry();
+                        visual.geometry = geom;
+                        visual.name = geomEl.getAttribute('name') || `geom_${geomIndex}`;
+                        visual.origin = this.parseOrigin(geomEl);
+
+                        // Parse MJCF rgba color (priority: geom rgba > inherited rgba > material rgba)
+                        let rgba = null;
+                        let materialName = null;
+
+                        // 1. First check geom's own rgba, then inherited rgba
+                        if (geomEl.hasAttribute('rgba')) {
+                            const rgbaStr = geomEl.getAttribute('rgba');
+                            const rgbaVals = rgbaStr.split(' ').map(parseFloat);
+                            if (rgbaVals.length >= 3) {
+                                rgba = {
+                                    r: rgbaVals[0],
+                                    g: rgbaVals[1],
+                                    b: rgbaVals[2],
+                                    a: rgbaVals.length >= 4 ? rgbaVals[3] : 1.0
+                                };
+                            }
+                        }
+
+                        // 2. If geom has no explicit rgba, check inherited rgba
+                        if (!rgba && inheritedProps.rgba) {
+                            rgba = inheritedProps.rgba;
+                        }
+
+                        // 3. If still no rgba, check if references material (explicit or inherited)
+                        if (!rgba && materialMap) {
+                            materialName = geomEl.getAttribute('material') || inheritedProps.material;
+                            if (materialName && materialMap.has(materialName)) {
+                                const mat = materialMap.get(materialName);
+                                if (mat.rgba) {
+                                    rgba = mat.rgba;
+                                }
+                            }
+                        }
+
+                        visual.userData = {
+                            group: groupNum,
+                            hasRgba: hasRgba || !!rgba,
+                            rgba: rgba,
+                            materialName: materialName,
+                            meshRef: meshRef,
+                            geomType: geomType
+                        };
+                        link.visuals.push(visual);
+                    }
+                }
+            });
+
+            // Parse inertial properties
+            const inertialEl = bodyEl.querySelector(':scope > inertial');
+            if (inertialEl) {
+                link.inertial = this.parseInertial(inertialEl);
+            }
+
+            model.addLink(link);
+            bodyMap.set(linkName, { link, element: bodyEl, parentName });
+
+            // Recursively parse child bodies
+            this.parseBodies(bodyEl, linkName, bodyMap, model, link, meshMap, stats, materialMap, classDefaults, rootDefaults);
+        });
+    }
+
+    /**
+     * Parse geom element
+     * @param {Element} geomEl - geom element
+     * @param {Map} meshMap - Mapping from mesh names to file paths
+     */
+    static parseGeom(geomEl, meshMap = null) {
+        // In MJCF, if geom has mesh attribute, type should be mesh
+        const meshAttr = geomEl.getAttribute('mesh');
+        let type = geomEl.getAttribute('type');
+
+        // If has mesh attribute but no explicit type declaration, auto-set to mesh
+        if (meshAttr && !type) {
+            type = 'mesh';
+        }
+
+        // If no type attribute and no mesh attribute, default to sphere
+        if (!type) {
+            type = 'sphere';
+        }
+
+        const geometry = new GeometryType(type);
+
+        switch (type) {
+            case 'box':
+                const size = geomEl.getAttribute('size');
+                if (size) {
+                    const sizes = size.split(' ').map(parseFloat);
+                    // MJCF size is half-size, multiply by 2 to convert to full size
+                    geometry.size = sizes.length === 1
+                        ? { x: sizes[0] * 2, y: sizes[0] * 2, z: sizes[0] * 2 }
+                        : { x: (sizes[0] || 0.05) * 2, y: (sizes[1] || 0.05) * 2, z: (sizes[2] || 0.05) * 2 };
+                } else {
+                    geometry.size = { x: 0.1, y: 0.1, z: 0.1 };
+                }
+                break;
+
+            case 'sphere':
+                // MJCF sphere size is radius
+                const radius = parseFloat(geomEl.getAttribute('size') || '0.1');
+                geometry.size = { radius };
+                break;
+
+            case 'cylinder':
+            case 'capsule':
+                // Handle fromto attribute for capsule/cylinder
+                const fromto = geomEl.getAttribute('fromto');
+                const radiusAttr = geomEl.getAttribute('size');
+                
+                if (fromto) {
+                    const ft = fromto.split(' ').map(parseFloat);
+                    if (ft.length >= 6) {
+                        const p1 = new THREE.Vector3(ft[0], ft[1], ft[2]);
+                        const p2 = new THREE.Vector3(ft[3], ft[4], ft[5]);
+                        const center = new THREE.Vector3().addVectors(p1, p2).multiplyScalar(0.5);
+                        const height = p1.distanceTo(p2);
+                        
+                        // Calculate rotation to align cylinder/capsule with the fromto vector
+                        const direction = new THREE.Vector3().subVectors(p2, p1).normalize();
+                        const defaultDir = new THREE.Vector3(0, 1, 0); // Default cylinder axis is Y
+                        const quaternion = new THREE.Quaternion().setFromUnitVectors(defaultDir, direction);
+                        const euler = new THREE.Euler().setFromQuaternion(quaternion);
+                        
+                        // Store fromto data
+                        geometry.fromto = {
+                            p1: [ft[0], ft[1], ft[2]],
+                            p2: [ft[3], ft[4], ft[5]],
+                            center: [center.x, center.y, center.z],
+                            height: height,
+                            rpy: [euler.x, euler.y, euler.z]
+                        };
+                        
+                        // Parse radius - for fromto, size is just radius
+                        const radiusVal = parseFloat(radiusAttr || '0.01');
+                        geometry.size = { radius: radiusVal, height: height };
+                    }
+                } else if (radiusAttr) {
+                    const radii = radiusAttr.split(' ').map(parseFloat);
+                    // MJCF cylinder/capsule size is [radius, half-height], height needs to be multiplied by 2
+                    geometry.size = {
+                        radius: radii[0] || 0.1,
+                        height: (radii[1] || 0.1) * 2  // Multiply by 2 to get full height
+                    };
+                } else {
+                    geometry.size = { radius: 0.01, height: 0.1 };
+                }
+                break;
+
+            case 'mesh':
+                let meshRef = geomEl.getAttribute('mesh');
+                // If meshMap exists, try to find data corresponding to mesh name
+                if (meshMap && meshMap.has(meshRef)) {
+                    const meshData = meshMap.get(meshRef);
+                    if (meshData.type === 'file') {
+                        // External file mesh
+                        geometry.filename = meshData.path;
+                        // Apply mesh scale from asset definition (class inheritance)
+                        if (meshData.scale) {
+                            geometry.meshScale = meshData.scale;
+                        }
+                    } else if (meshData.type === 'vertex') {
+                        // Inline vertex mesh, store vertex data
+                        geometry.inlineVertices = meshData.vertices;
+                        geometry.inlineScale = meshData.scale;
+                    }
+                } else {
+                    // Otherwise directly use mesh attribute value (may be file path)
+                    geometry.filename = meshRef;
+                    if (meshMap && meshMap.size > 0) {
+                        console.warn(`⚠️ mesh "${meshRef}" not defined in assets`);
+                    }
+                }
+                geometry.size = null;
+                break;
+        }
+
+        return geometry;
+    }
+
+    /**
+     * Parse origin attribute (pos + quat or xyz + rpy)
+     */
+    static parseOrigin(element) {
+        const origin = { xyz: [0, 0, 0], rpy: [0, 0, 0] };
+
+        // Check pos attribute
+        const pos = element.getAttribute('pos');
+        if (pos) {
+            const xyz = pos.split(' ').map(parseFloat);
+            origin.xyz = [xyz[0] || 0, xyz[1] || 0, xyz[2] || 0];
+        }
+
+        // Check quat attribute (quaternion, needs to be converted to rpy)
+        const quat = element.getAttribute('quat');
+        if (quat) {
+            const q = quat.split(' ').map(parseFloat);
+            // MJCF uses wxyz order
+            const qw = q[0], qx = q[1], qy = q[2], qz = q[3];
+
+            // Save original quaternion (for inertia visualization)
+            origin.quat = { w: qw, x: qx, y: qy, z: qz };
+
+            // Convert to Euler angles
+            origin.rpy = this.quaternionToEuler(qw, qx, qy, qz);
+        } else {
+            // Check euler attribute
+            const euler = element.getAttribute('euler');
+            if (euler) {
+                const rpy = euler.split(' ').map(parseFloat);
+                origin.rpy = [rpy[0] || 0, rpy[1] || 0, rpy[2] || 0];
+            }
+        }
+
+        return origin;
+    }
+
+    /**
+     * Convert quaternion to Euler angles (simplified version)
+     */
+    static quaternionToEuler(w, x, y, z) {
+        // Normalize quaternion first (MJCF may use non-normalized quaternions)
+        const norm = Math.sqrt(w * w + x * x + y * y + z * z);
+        if (norm > 0) {
+            w = w / norm;
+            x = x / norm;
+            y = y / norm;
+            z = z / norm;
+        }
+
+        // Simplified conversion (using standard formula)
+        const sinr_cosp = 2 * (w * x + y * z);
+        const cosr_cosp = 1 - 2 * (x * x + y * y);
+        const roll = Math.atan2(sinr_cosp, cosr_cosp);
+
+        const sinp = 2 * (w * y - z * x);
+        const pitch = Math.abs(sinp) >= 1
+            ? Math.sign(sinp) * Math.PI / 2
+            : Math.asin(sinp);
+
+        const siny_cosp = 2 * (w * z + x * y);
+        const cosy_cosp = 1 - 2 * (y * y + z * z);
+        const yaw = Math.atan2(siny_cosp, cosy_cosp);
+
+        return [roll, pitch, yaw];
+    }
+
+    /**
+     * Parse inertial element
+     *
+     * MJCF inertia is defined in inertial frame, needs:
+     * 1. Transform to body frame via quat rotation
+     * 2. Then perform MJCF to Three.js coordinate system conversion
+     */
+    static parseInertial(inertialEl) {
+        const inertial = new InertialProperties();
+
+        const mass = inertialEl.getAttribute('mass');
+        if (mass) inertial.mass = parseFloat(mass);
+
+        const origin = this.parseOrigin(inertialEl);
+        inertial.origin = origin;
+
+        // Parse inertia matrix
+        const diaginertia = inertialEl.getAttribute('diaginertia');
+        const fullinertia = inertialEl.getAttribute('fullinertia');
+
+        let mjcf_ixx = 0, mjcf_iyy = 0, mjcf_izz = 0;
+        let mjcf_ixy = 0, mjcf_ixz = 0, mjcf_iyz = 0;
+
+        if (diaginertia) {
+            const values = diaginertia.split(' ').map(parseFloat);
+            mjcf_ixx = values[0] || 0;
+            mjcf_iyy = values[1] || 0;
+            mjcf_izz = values[2] || 0;
+        }
+
+        if (fullinertia) {
+            const values = fullinertia.split(' ').map(parseFloat);
+            mjcf_ixx = values[0] || 0;
+            mjcf_iyy = values[1] || 0;
+            mjcf_izz = values[2] || 0;
+            mjcf_ixy = values[3] || 0;
+            mjcf_ixz = values[4] || 0;
+            mjcf_iyz = values[5] || 0;
+        }
+
+        // Save original diagonal inertia values (for visualization)
+        // These are principal moments of inertia in inertial frame
+        inertial.diagonalInertia = {
+            ixx: mjcf_ixx,
+            iyy: mjcf_iyy,
+            izz: mjcf_izz
+        };
+
+        // If quat exists, need to rotate inertia tensor
+        if (origin.quat) {
+            const rotated = this.rotateInertiaTensor(
+                mjcf_ixx, mjcf_iyy, mjcf_izz,
+                mjcf_ixy, mjcf_ixz, mjcf_iyz,
+                origin.quat
+            );
+            mjcf_ixx = rotated.ixx;
+            mjcf_iyy = rotated.iyy;
+            mjcf_izz = rotated.izz;
+            mjcf_ixy = rotated.ixy;
+            mjcf_ixz = rotated.ixz;
+            mjcf_iyz = rotated.iyz;
+        }
+
+        // Coordinate system conversion: MJCF -> Three.js
+        // On top of quat rotation, need to rotate 180 degrees around Y-axis (split into two 90-degree rotations)
+        // This is the correct transformation from MJCF coordinate system (X-forward, Y-left, Z-up) to Three.js coordinate system (X-right, Y-up, Z-forward)
+        const coordRotated1 = this.rotateInertiaAroundAxis(
+            mjcf_ixx, mjcf_iyy, mjcf_izz,
+            mjcf_ixy, mjcf_ixz, mjcf_iyz,
+            'Y', 90
+        );
+
+        const coordRotated2 = this.rotateInertiaAroundAxis(
+            coordRotated1.ixx, coordRotated1.iyy, coordRotated1.izz,
+            coordRotated1.ixy, coordRotated1.ixz, coordRotated1.iyz,
+            'Y', 90
+        );
+
+        inertial.ixx = coordRotated2.ixx;
+        inertial.iyy = coordRotated2.iyy;
+        inertial.izz = coordRotated2.izz;
+        inertial.ixy = coordRotated2.ixy;
+        inertial.ixz = coordRotated2.ixz;
+        inertial.iyz = coordRotated2.iyz;
+
+        return inertial;
+    }
+
+    /**
+     * Rotate inertia tensor around specified axis
+     * @param {string} axis - 'X', 'Y', or 'Z'
+     * @param {number} degrees - Rotation angle (degrees)
+     */
+    static rotateInertiaAroundAxis(ixx, iyy, izz, ixy, ixz, iyz, axis, degrees) {
+        const rad = degrees * Math.PI / 180;
+        const c = Math.cos(rad);
+        const s = Math.sin(rad);
+
+        let R;
+        if (axis === 'X') {
+            R = [
+                [1, 0, 0],
+                [0, c, -s],
+                [0, s, c]
+            ];
+        } else if (axis === 'Y') {
+            R = [
+                [c, 0, s],
+                [0, 1, 0],
+                [-s, 0, c]
+            ];
+        } else if (axis === 'Z') {
+            R = [
+                [c, -s, 0],
+                [s, c, 0],
+                [0, 0, 1]
+            ];
+        }
+
+        // Inertia matrix
+        const I = [
+            [ixx, ixy, ixz],
+            [ixy, iyy, iyz],
+            [ixz, iyz, izz]
+        ];
+
+        // Calculate R * I
+        const RI = [
+            [0, 0, 0],
+            [0, 0, 0],
+            [0, 0, 0]
+        ];
+
+        for (let i = 0; i < 3; i++) {
+            for (let j = 0; j < 3; j++) {
+                for (let k = 0; k < 3; k++) {
+                    RI[i][j] += R[i][k] * I[k][j];
+                }
+            }
+        }
+
+        // Calculate (R * I) * R^T
+        const result = [
+            [0, 0, 0],
+            [0, 0, 0],
+            [0, 0, 0]
+        ];
+
+        for (let i = 0; i < 3; i++) {
+            for (let j = 0; j < 3; j++) {
+                for (let k = 0; k < 3; k++) {
+                    result[i][j] += RI[i][k] * R[j][k]; // R^T[k][j] = R[j][k]
+                }
+            }
+        }
+
+        return {
+            ixx: result[0][0],
+            iyy: result[1][1],
+            izz: result[2][2],
+            ixy: result[0][1],
+            ixz: result[0][2],
+            iyz: result[1][2]
+        };
+    }
+
+    /**
+     * Rotate inertia tensor: I_rotated = R * I * R^T
+     */
+    static rotateInertiaTensor(ixx, iyy, izz, ixy, ixz, iyz, quat) {
+        const {w, x, y, z} = quat;
+
+        // Build rotation matrix R (from quaternion)
+        const r11 = 1 - 2*(y*y + z*z);
+        const r12 = 2*(x*y - w*z);
+        const r13 = 2*(x*z + w*y);
+        const r21 = 2*(x*y + w*z);
+        const r22 = 1 - 2*(x*x + z*z);
+        const r23 = 2*(y*z - w*x);
+        const r31 = 2*(x*z - w*y);
+        const r32 = 2*(y*z + w*x);
+        const r33 = 1 - 2*(x*x + y*y);
+
+        // Inertia matrix
+        const I = [
+            [ixx, ixy, ixz],
+            [ixy, iyy, iyz],
+            [ixz, iyz, izz]
+        ];
+
+        // Calculate R * I
+        const RI = [
+            [0, 0, 0],
+            [0, 0, 0],
+            [0, 0, 0]
+        ];
+
+        RI[0][0] = r11*I[0][0] + r12*I[1][0] + r13*I[2][0];
+        RI[0][1] = r11*I[0][1] + r12*I[1][1] + r13*I[2][1];
+        RI[0][2] = r11*I[0][2] + r12*I[1][2] + r13*I[2][2];
+
+        RI[1][0] = r21*I[0][0] + r22*I[1][0] + r23*I[2][0];
+        RI[1][1] = r21*I[0][1] + r22*I[1][1] + r23*I[2][1];
+        RI[1][2] = r21*I[0][2] + r22*I[1][2] + r23*I[2][2];
+
+        RI[2][0] = r31*I[0][0] + r32*I[1][0] + r33*I[2][0];
+        RI[2][1] = r31*I[0][1] + r32*I[1][1] + r33*I[2][1];
+        RI[2][2] = r31*I[0][2] + r32*I[1][2] + r33*I[2][2];
+
+        // Calculate (R * I) * R^T
+        const result = {
+            ixx: RI[0][0]*r11 + RI[0][1]*r12 + RI[0][2]*r13,
+            iyy: RI[1][0]*r21 + RI[1][1]*r22 + RI[1][2]*r23,
+            izz: RI[2][0]*r31 + RI[2][1]*r32 + RI[2][2]*r33,
+            ixy: RI[0][0]*r21 + RI[0][1]*r22 + RI[0][2]*r23,
+            ixz: RI[0][0]*r31 + RI[0][1]*r32 + RI[0][2]*r33,
+            iyz: RI[1][0]*r31 + RI[1][1]*r32 + RI[1][2]*r33
+        };
+
+        return result;
+    }
+
+    /**
+     * Parse joint element
+     */
+    static parseJoints(element, bodyMap, model, parentBodyName = null, defaultsMap = null) {
+        const joints = element.querySelectorAll(':scope > joint');
+
+        joints.forEach(jointEl => {
+            const jointName = jointEl.getAttribute('name') || `joint_${model.joints.size}`;
+            const jointType = jointEl.getAttribute('type') || 'hinge';
+
+            // Map MJCF joint types to URDF types
+            let urdfType = 'revolute';
+            if (jointType === 'slide') urdfType = 'prismatic';
+            else if (jointType === 'free') urdfType = 'continuous';
+            else if (jointType === 'ball' || jointType === 'hinge') urdfType = 'revolute';
+
+            const joint = new Joint(jointName, urdfType);
+
+            // Joint types that don't require axis attribute
+            const jointTypesWithoutAxis = ['free', 'ball'];
+            const requiresAxis = !jointTypesWithoutAxis.includes(jointType);
+
+            // [Critical fix] In MJCF, joint is defined inside body, representing the connection relationship between this body and its parent body
+            // So: parent is parent body, child is current body
+            const currentBody = jointEl.parentElement;
+            const currentBodyName = currentBody.getAttribute('name');
+
+            // parent is the passed parent body name (or worldbody)
+            if (parentBodyName) {
+                joint.parent = parentBodyName;
+            } else {
+                // If no parent body, parent is worldbody
+                joint.parent = 'worldbody';
+            }
+
+            // child is current body
+            if (currentBodyName) {
+                joint.child = currentBodyName;
+            }
+
+
+            // [Important] Parse axis, consider class inheritance
+            let axisVals = null;
+            let axisSource = '';
+
+            // First try to get axis from joint element itself
+            const axis = jointEl.getAttribute('axis');
+            if (axis) {
+                axisVals = axis.split(' ').map(parseFloat);
+                axisSource = 'directly defined';
+            } else {
+                // If not, inherit from class or childclass
+                let className = jointEl.getAttribute('class');
+
+                // If joint has no class, check parent body's childclass
+                if (!className) {
+                    className = currentBody.getAttribute('childclass');
+                }
+
+                if (className && defaultsMap) {
+                    const defaults = defaultsMap.get(className);
+                    if (defaults && defaults.joint && defaults.joint.axis) {
+                        axisVals = defaults.joint.axis;
+                        axisSource = `inherited from class="${className}"`;
+                    }
+                }
+
+                // Only warn if axis is required for this joint type
+                if (!axisVals && requiresAxis) {
+                    console.warn(`  ⚠️ Joint "${jointName}" (type="${jointType}") has no axis attribute (class="${className || 'none'}")`);
+                }
+            }
+
+            // Set axis
+            if (axisVals) {
+                joint.axis = { xyz: [axisVals[0] || 0, axisVals[1] || 0, axisVals[2] || 0] };
+            }
+
+            // [Important] Parse limits, consider class inheritance
+            let rangeVals = null;
+
+            // First try to get range from joint element itself
+            const range = jointEl.getAttribute('range');
+            if (range) {
+                rangeVals = range.split(' ').map(parseFloat);
+            } else {
+                // If not, inherit from class or childclass
+                let className = jointEl.getAttribute('class');
+
+                // If joint has no class, check parent body's childclass
+                if (!className) {
+                    className = currentBody.getAttribute('childclass');
+                }
+
+                if (className && defaultsMap) {
+                    const defaults = defaultsMap.get(className);
+                    if (defaults && defaults.joint && defaults.joint.range) {
+                        rangeVals = defaults.joint.range;
+                    }
+                }
+            }
+
+            // Set limits
+            if (rangeVals && rangeVals.length >= 2) {
+                const limits = new JointLimits();
+                limits.lower = rangeVals[0];
+                limits.upper = rangeVals[1];
+                joint.limits = limits;
+            }
+            // If no range definition, joint.limits remains null (indicating unlimited/continuous)
+
+            // Parse joint's own origin (if any)
+            // joint's pos defines the offset of joint in this body's coordinate system
+            joint.origin = this.parseOrigin(jointEl);
+
+            model.addJoint(joint);
+        });
+
+        // Process freejoint elements (free-floating joints)
+        const freejoints = element.querySelectorAll(':scope > freejoint');
+        freejoints.forEach((freejointEl, index) => {
+            const freejointName = freejointEl.getAttribute('name') || `freejoint_${model.joints.size}`;
+            
+            // Create a 'free' type joint (maps to continuous/floating in URDF terms)
+            const joint = new Joint(freejointName, 'continuous');
+            joint.type = 'free'; // Mark as free joint type
+            
+            // Get parent body
+            const currentBody = freejointEl.parentElement;
+            const currentBodyName = currentBody.getAttribute('name');
+            
+            // Parent is worldbody for freejoints
+            if (parentBodyName) {
+                joint.parent = parentBodyName;
+            } else {
+                joint.parent = 'worldbody';
+            }
+            
+            // Child is current body
+            if (currentBodyName) {
+                joint.child = currentBodyName;
+            }
+            
+            // Parse origin
+            joint.origin = this.parseOrigin(freejointEl);
+            
+            model.addJoint(joint);
+        });
+
+        // Recursively process child bodies
+        // Find direct child bodies (use :scope > body to ensure only direct children are selected)
+        const bodies = element.querySelectorAll(':scope > body');
+        const currentElementName = element.getAttribute('name'); // Name of current body or worldbody
+
+        bodies.forEach(body => {
+            // Child body's parent body name is current element's name
+            // Note: worldbody has no name attribute, so first level body's parent is null or 'worldbody'
+            this.parseJoints(body, bodyMap, model, currentElementName || 'worldbody', defaultsMap);
+        });
+    }
+
+    /**
+     * Parse equality constraints (closed-chain constraints for parallel mechanisms)
+     */
+    static parseEquality(doc, model) {
+        const equality = doc.querySelector('equality');
+        if (!equality) {
+            return; // No equality tag, skip
+        }
+
+        // Parse connect constraints (connect two bodies)
+        const connects = equality.querySelectorAll('connect');
+        connects.forEach((connectEl, index) => {
+            const name = connectEl.getAttribute('name') || `connect_${index}`;
+            const constraint = new Constraint(name, 'connect');
+
+            constraint.body1 = connectEl.getAttribute('body1');
+            constraint.body2 = connectEl.getAttribute('body2');
+
+            const anchor = connectEl.getAttribute('anchor');
+            if (anchor) {
+                constraint.anchor = anchor.trim().split(/\s+/).map(parseFloat);
+            }
+
+            const torquescale = connectEl.getAttribute('torquescale');
+            if (torquescale) {
+                constraint.torquescale = parseFloat(torquescale);
+            }
+
+            constraint.userData = {
+                body1: constraint.body1,
+                body2: constraint.body2,
+                anchor: constraint.anchor
+            };
+
+            model.addConstraint(constraint);
+        });
+
+        // Parse weld constraints (weld two bodies)
+        const welds = equality.querySelectorAll('weld');
+        welds.forEach((weldEl, index) => {
+            const name = weldEl.getAttribute('name') || `weld_${index}`;
+            const constraint = new Constraint(name, 'weld');
+
+            constraint.body1 = weldEl.getAttribute('body1');
+            constraint.body2 = weldEl.getAttribute('body2');
+
+            const anchor = weldEl.getAttribute('anchor');
+            if (anchor) {
+                constraint.anchor = anchor.trim().split(/\s+/).map(parseFloat);
+            }
+
+            const torquescale = weldEl.getAttribute('torquescale');
+            if (torquescale) {
+                constraint.torquescale = parseFloat(torquescale);
+            }
+
+            constraint.userData = {
+                body1: constraint.body1,
+                body2: constraint.body2,
+                anchor: constraint.anchor
+            };
+
+            model.addConstraint(constraint);
+        });
+
+        // Parse joint constraints (joint coupling)
+        const joints = equality.querySelectorAll('joint');
+        joints.forEach((jointEl, index) => {
+            const name = jointEl.getAttribute('name') || `joint_constraint_${index}`;
+            const constraint = new Constraint(name, 'joint');
+
+            constraint.joint1 = jointEl.getAttribute('joint1');
+            constraint.joint2 = jointEl.getAttribute('joint2');
+
+            const polycoef = jointEl.getAttribute('polycoef');
+            if (polycoef) {
+                constraint.polycoef = polycoef.trim().split(/\s+/).map(parseFloat);
+            } else {
+                constraint.polycoef = [0, 1]; // Default 1:1
+            }
+
+            constraint.userData = {
+                joint1: constraint.joint1,
+                joint2: constraint.joint2,
+                polycoef: constraint.polycoef
+            };
+
+            model.addConstraint(constraint);
+        });
+
+        // Parse distance constraints
+        const distances = equality.querySelectorAll('distance');
+        distances.forEach((distanceEl, index) => {
+            const name = distanceEl.getAttribute('name') || `distance_${index}`;
+            const constraint = new Constraint(name, 'distance');
+
+            constraint.body1 = distanceEl.getAttribute('body1');
+            constraint.body2 = distanceEl.getAttribute('body2');
+
+            constraint.userData = {
+                body1: constraint.body1,
+                body2: constraint.body2
+            };
+
+            model.addConstraint(constraint);
+        });
+    }
+
+    /**
+     * Create Three.js objects (recursively build hierarchy)
+     * @param {UnifiedRobotModel} model
+     * @param {Map} fileMap - File map for loading mesh files
+     * @param {Map} meshMap - Mesh name to file path mapping (optional)
+     */
+    static async createThreeObject(model, fileMap = null, meshMap = null) {
+        // Preload loaders
+        await getLoaders();
+
+        const rootGroup = new THREE.Group();
+        rootGroup.name = model.name;
+
+        // Create Three.js objects for all links (but don't add to scene yet)
+        const linkObjects = new Map();
+
+        // Collect all unique mesh file paths (only need visual, as MJCF doesn't create collision separately)
+        const uniqueMeshFiles = new Set();
+        for (const [name, link] of model.links) {
+            for (const visual of link.visuals) {
+                if (visual.geometry.type === 'mesh' && visual.geometry.filename) {
+                    uniqueMeshFiles.add(visual.geometry.filename);
+                }
+            }
+        }
+
+        // Load all unique mesh files in parallel
+        const meshPromises = Array.from(uniqueMeshFiles).map(filename =>
+            this.loadMeshFile(filename, fileMap).catch(err => {
+                console.error(`Failed to load mesh: ${filename}`, err);
+                return null;
+            })
+        );
+
+        // Wait for all mesh loading to complete
+        const meshResults = await Promise.all(meshPromises);
+        const meshCache = new Map();
+
+        // Build mesh cache (filename -> geometry)
+        let index = 0;
+        for (const filename of uniqueMeshFiles) {
+            const result = meshResults[index++];
+            meshCache.set(filename, result);
+        }
+
+        // Create link groups
+        let totalVisuals = 0;
+        for (const [name, link] of model.links) {
+            const linkGroup = new THREE.Group();
+            linkGroup.name = name;
+            linkGroup.isURDFLink = true; // Mark as link for JointDragControls recognition
+            linkGroup.type = 'URDFLink'; // Set type
+
+            // [Critical] Do not apply body.pos on linkGroup!
+            // body.pos should be applied on the jointGroup that connects it
+            // linkGroup only needs to contain geometry, position is determined by jointGroup
+
+            let linkVisualCount = 0;
+            let linkCollisionCount = 0;
+
+            // Create visual geometry
+            for (const visual of link.visuals) {
+                const mesh = await this.createGeometryMesh(visual.geometry, fileMap, meshCache);
+                if (mesh) {
+                    // Apply origin transformation
+                    // Check if this geom has fromto data (for capsule/cylinder)
+                    if (visual.geometry && visual.geometry.fromto) {
+                        // Use fromto center position
+                        mesh.position.set(...visual.geometry.fromto.center);
+                        // Apply fromto rotation plus any explicit rotation
+                        const fromtoRpy = visual.geometry.fromto.rpy;
+                        mesh.rotation.set(
+                            fromtoRpy[0] + visual.origin.rpy[0],
+                            fromtoRpy[1] + visual.origin.rpy[1],
+                            fromtoRpy[2] + visual.origin.rpy[2]
+                        );
+                    } else {
+                        mesh.position.set(...visual.origin.xyz);
+                        mesh.rotation.set(...visual.origin.rpy);
+                    }
+                    mesh.name = visual.name || 'visual';
+
+                    // If MJCF defines rgba color, apply to mesh
+                    if (visual.userData && visual.userData.rgba) {
+                        const rgba = visual.userData.rgba;
+                        const color = new THREE.Color(rgba.r, rgba.g, rgba.b);
+
+                        mesh.traverse((child) => {
+                            if (child.isMesh && child.material) {
+                                // Handle material arrays and single materials
+                                if (Array.isArray(child.material)) {
+                                    child.material = child.material.map(mat => {
+                                        const clonedMat = mat.clone();
+                                        clonedMat.color = color;
+                                        if (rgba.a < 1.0) {
+                                            clonedMat.transparent = true;
+                                            clonedMat.opacity = rgba.a;
+                                        }
+                                        // Save original properties before enhancing (for lighting toggle)
+                                        if (clonedMat.isMeshPhongMaterial || clonedMat.isMeshStandardMaterial) {
+                                            if (clonedMat.userData.originalShininess === undefined) {
+                                                clonedMat.userData.originalShininess = clonedMat.shininess !== undefined ? clonedMat.shininess : 30;
+                                                // Save original specular - if material had no specular, save null
+                                                if (!clonedMat.specular) {
+                                                    clonedMat.userData.originalSpecular = null;
+                                                } else if (clonedMat.specular.isColor) {
+                                                    const spec = clonedMat.specular;
+                                                    if (spec.r < 0.1 && spec.g < 0.1 && spec.b < 0.1) {
+                                                        clonedMat.userData.originalSpecular = null; // Likely default
+                                                    } else {
+                                                        clonedMat.userData.originalSpecular = spec.clone();
+                                                    }
+                                                } else if (typeof clonedMat.specular === 'number') {
+                                                    if (clonedMat.specular === 0x111111 || clonedMat.specular < 0x111111) {
+                                                        clonedMat.userData.originalSpecular = null;
+                                                    } else {
+                                                        clonedMat.userData.originalSpecular = new THREE.Color(clonedMat.specular);
+                                                    }
+                                                } else {
+                                                    clonedMat.userData.originalSpecular = null;
+                                                }
+                                            }
+                                            // Enhance material for better lighting (MuJoCo style) - default enabled
+                                            if (clonedMat.shininess === undefined || clonedMat.shininess < 50) {
+                                                clonedMat.shininess = 50;
+                                            }
+                                            if (!clonedMat.specular ||
+                                                (clonedMat.specular.isColor && clonedMat.specular.r < 0.2) ||
+                                                (typeof clonedMat.specular === 'number' && clonedMat.specular < 0x333333)) {
+                                                clonedMat.specular = new THREE.Color(0.3, 0.3, 0.3);
+                                            }
+                                        }
+                                        return clonedMat;
+                                    });
+                                } else {
+                                    // Clone material to avoid affecting other instances
+                                    child.material = child.material.clone();
+                                    child.material.color = color;
+                                    if (rgba.a < 1.0) {
+                                        child.material.transparent = true;
+                                        child.material.opacity = rgba.a;
+                                    }
+                                    // Save original properties before enhancing (for lighting toggle)
+                                    if (child.material.isMeshPhongMaterial || child.material.isMeshStandardMaterial) {
+                                        if (child.material.userData.originalShininess === undefined) {
+                                            child.material.userData.originalShininess = child.material.shininess !== undefined ? child.material.shininess : 30;
+                                            // Save original specular - if material had no specular, save null
+                                            if (!child.material.specular) {
+                                                child.material.userData.originalSpecular = null;
+                                            } else if (child.material.specular.isColor) {
+                                                const spec = child.material.specular;
+                                                if (spec.r < 0.1 && spec.g < 0.1 && spec.b < 0.1) {
+                                                    child.material.userData.originalSpecular = null; // Likely default
+                                                } else {
+                                                    child.material.userData.originalSpecular = spec.clone();
+                                                }
+                                            } else if (typeof child.material.specular === 'number') {
+                                                if (child.material.specular === 0x111111 || child.material.specular < 0x111111) {
+                                                    child.material.userData.originalSpecular = null;
+                                                } else {
+                                                    child.material.userData.originalSpecular = new THREE.Color(child.material.specular);
+                                                }
+                                            } else {
+                                                child.material.userData.originalSpecular = null;
+                                            }
+                                        }
+                                        // Enhance material for better lighting (MuJoCo style) - default enabled
+                                        if (child.material.shininess === undefined || child.material.shininess < 50) {
+                                            child.material.shininess = 50;
+                                        }
+                                        if (!child.material.specular ||
+                                            (child.material.specular.isColor && child.material.specular.r < 0.2) ||
+                                            (typeof child.material.specular === 'number' && child.material.specular < 0x333333)) {
+                                            child.material.specular = new THREE.Color(0.3, 0.3, 0.3);
+                                        }
+                                    }
+                                }
+                            }
+                        });
+                    }
+
+                    linkGroup.add(mesh);
+                    visual.threeObject = mesh;
+                    totalVisuals++;
+                    linkVisualCount++;
+                }
+            }
+
+            // Create collision geometry
+            for (const collision of link.collisions) {
+                const mesh = await this.createGeometryMesh(collision.geometry, fileMap, meshCache);
+                if (mesh) {
+                    // Apply origin transformation
+                    // Check if this geom has fromto data (for capsule/cylinder)
+                    if (collision.geometry && collision.geometry.fromto) {
+                        // Use fromto center position
+                        mesh.position.set(...collision.geometry.fromto.center);
+                        // Apply fromto rotation plus any explicit rotation
+                        const fromtoRpy = collision.geometry.fromto.rpy;
+                        mesh.rotation.set(
+                            fromtoRpy[0] + collision.origin.rpy[0],
+                            fromtoRpy[1] + collision.origin.rpy[1],
+                            fromtoRpy[2] + collision.origin.rpy[2]
+                        );
+                    } else {
+                        mesh.position.set(...collision.origin.xyz);
+                        mesh.rotation.set(...collision.origin.rpy);
+                    }
+                    mesh.name = collision.name || 'collision';
+
+                    // Create collision body container (similar to URDF handling)
+                    const colliderGroup = new THREE.Group();
+                    colliderGroup.name = `${name}_collider_${linkCollisionCount}`;
+                    colliderGroup.isURDFCollider = true; // Mark as collision body
+                    colliderGroup.add(mesh);
+
+                    linkGroup.add(colliderGroup);
+                    collision.threeObject = colliderGroup;
+                    linkCollisionCount++;
+                }
+            }
+
+            link.threeObject = linkGroup;
+            linkObjects.set(name, linkGroup);
+        }
+
+
+        // Build hierarchy based on body parent-child relationships (MJCF bodies are nested)
+        const bodyMap = new Map();
+        for (const [name, link] of model.links) {
+            bodyMap.set(name, { link, parentName: link.userData.parentName });
+        }
+
+        // Find root body (body without parent)
+        const rootLinks = Array.from(model.links.keys()).filter(
+            name => !bodyMap.get(name).parentName
+        );
+
+        // Recursively build hierarchy
+        function buildHierarchy(linkName, parentGroup) {
+            const linkGroup = linkObjects.get(linkName);
+            if (!linkGroup) return;
+
+            // Add current link to parent group
+            parentGroup.add(linkGroup);
+
+            // Find all joints with this link as parent
+            const childJoints = Array.from(model.joints.values()).filter(
+                j => j.parent === linkName && j.child
+            );
+
+            // Process child joints and child bodies
+            childJoints.forEach(joint => {
+                const childLinkName = joint.child;
+                if (!childLinkName) return;
+
+                // Get child link's body origin (in MJCF, body.pos defines connection position)
+                const childLink = model.links.get(childLinkName);
+                const bodyOrigin = childLink.userData.bodyOrigin || { xyz: [0, 0, 0], rpy: [0, 0, 0] };
+
+                // Create joint transformation group
+                const jointGroup = new THREE.Group();
+                jointGroup.name = joint.name || `joint_${childLinkName}`;
+                jointGroup.isURDFJoint = true; // Mark as joint for JointDragControls recognition
+                jointGroup.type = 'URDFJoint'; // Set type
+                jointGroup.jointType = joint.type; // Set joint type
+
+                // Store joint axis information (for JointDragControls use)
+                if (joint.axis && joint.axis.xyz) {
+                    const mjcfAxis = joint.axis.xyz;
+                    jointGroup.axis = new THREE.Vector3(mjcfAxis[0], mjcfAxis[1], mjcfAxis[2]).normalize();
+                } else {
+                    // If no axis defined, use default value (0, 1, 0)
+                    jointGroup.axis = new THREE.Vector3(0, 1, 0);
+                }
+
+                // [Critical] Apply body.pos + joint.pos as jointGroup position
+                // body.pos defines body position relative to parent body (i.e., connection position)
+                // joint.pos defines joint offset in body coordinate system (usually 0)
+                jointGroup.position.set(
+                    bodyOrigin.xyz[0] + joint.origin.xyz[0],
+                    bodyOrigin.xyz[1] + joint.origin.xyz[1],
+                    bodyOrigin.xyz[2] + joint.origin.xyz[2]
+                );
+                jointGroup.rotation.set(...bodyOrigin.rpy);
+
+                // Recursively build child link
+                buildHierarchy(childLinkName, jointGroup);
+
+                linkGroup.add(jointGroup);
+                joint.threeObject = jointGroup;
+            });
+
+            // Process direct child bodies (find via bodyMap)
+            for (const [childName, bodyData] of bodyMap.entries()) {
+                if (bodyData.parentName === linkName) {
+                    // Check if joint connection already exists
+                    const hasJoint = Array.from(model.joints.values()).some(
+                        j => j.parent === linkName && j.child === childName
+                    );
+                    if (!hasJoint) {
+                        // If no joint, create fixed connection group to apply body position and rotation
+                        const childLink = model.links.get(childName);
+                        const childBodyOrigin = childLink.userData.bodyOrigin || { xyz: [0, 0, 0], rpy: [0, 0, 0] };
+
+                        // Mark this as fixed-connected child body (for structure graph display)
+                        childLink.userData.isFixedConnection = true;
+
+                        // Create fixed connection group
+                        const fixedGroup = new THREE.Group();
+                        fixedGroup.position.set(...childBodyOrigin.xyz);
+                        fixedGroup.rotation.set(...childBodyOrigin.rpy);
+
+                        // Recursively build child body and add to fixed group
+                        buildHierarchy(childName, fixedGroup);
+
+                        linkGroup.add(fixedGroup);
+                    }
+                }
+            }
+        }
+
+        // Start building from root link
+        if (rootLinks.length > 0) {
+            rootLinks.forEach(rootName => {
+                // Root link needs to apply its own body.pos (because it has no parent joint)
+                const rootLink = model.links.get(rootName);
+                const rootLinkGroup = linkObjects.get(rootName);
+                if (rootLink.userData.bodyOrigin) {
+                    rootLinkGroup.position.set(...rootLink.userData.bodyOrigin.xyz);
+                    rootLinkGroup.rotation.set(...rootLink.userData.bodyOrigin.rpy);
+                }
+                buildHierarchy(rootName, rootGroup);
+            });
+        } else if (model.links.size > 0) {
+            // If no root link found, use first link
+            const firstLink = Array.from(model.links.keys())[0];
+            const firstLinkObj = model.links.get(firstLink);
+            const firstLinkGroup = linkObjects.get(firstLink);
+            if (firstLinkObj.userData.bodyOrigin) {
+                firstLinkGroup.position.set(...firstLinkObj.userData.bodyOrigin.xyz);
+                firstLinkGroup.rotation.set(...firstLinkObj.userData.bodyOrigin.rpy);
+            }
+            buildHierarchy(firstLink, rootGroup);
+        }
+
+        model.threeObject = rootGroup;
+
+        // Mark model type as MJCF (also set on model)
+        if (!rootGroup.userData) rootGroup.userData = {};
+        rootGroup.userData.type = 'mjcf';
+
+        if (!model.userData) model.userData = {};
+        model.userData.type = 'mjcf';
+    }
+
+    /**
+     * Create Three.js Mesh based on geometry type
+     * @param {GeometryType} geometry
+     * @param {Map} fileMap - File map for loading mesh files
+     * @param {Map} meshCache - Cache of loaded meshes (optional)
+     * @returns {Promise}
+     */
+    static async createGeometryMesh(geometry, fileMap = null, meshCache = null) {
+        let threeGeometry = null;
+
+        switch (geometry.type) {
+            case 'box':
+                if (geometry.size) {
+                    threeGeometry = new THREE.BoxGeometry(
+                        geometry.size.x,
+                        geometry.size.y,
+                        geometry.size.z
+                    );
+                }
+                break;
+
+            case 'sphere':
+                if (geometry.size && geometry.size.radius) {
+                    threeGeometry = new THREE.SphereGeometry(geometry.size.radius, 32, 32);
+                }
+                break;
+
+            case 'cylinder':
+                if (geometry.size) {
+                    // Three.js CylinderGeometry defaults to Y-axis
+                    threeGeometry = new THREE.CylinderGeometry(
+                        geometry.size.radius,
+                        geometry.size.radius,
+                        geometry.size.height,
+                        32
+                    );
+                    // MJCF cylinder defaults to Z-axis, Three.js Cylinder is Y-axis aligned
+                    // Rotate to align with Z-axis
+                    threeGeometry.rotateX(Math.PI / 2);
+                    
+                    // If fromto is defined, the mesh will be positioned and rotated by fromto data
+                    // in the calling code
+                }
+                break;
+
+            case 'capsule':
+                if (geometry.size) {
+                    // Three.js doesn't have native CapsuleGeometry in older versions
+                    // Use a combination of cylinder and spheres, or CapsuleGeometry if available
+                    const { radius, height } = geometry.size;
+                    
+                    // Check if CapsuleGeometry is available (Three.js r133+)
+                    if (typeof THREE.CapsuleGeometry !== 'undefined') {
+                        threeGeometry = new THREE.CapsuleGeometry(radius, height, 4, 16);
+                        // CapsuleGeometry is Y-axis aligned, MJCF capsule is Z-axis aligned
+                        threeGeometry.rotateX(Math.PI / 2);
+                    } else {
+                        // Fallback: create a cylinder with sphere caps
+                        const cylinderHeight = Math.max(0, height - 2 * radius);
+                        const cylinder = new THREE.CylinderGeometry(radius, radius, cylinderHeight, 16);
+                        cylinder.rotateX(Math.PI / 2); // Align with Z-axis
+                        threeGeometry = cylinder;
+                    }
+                }
+                break;
+
+            case 'mesh':
+                // Load mesh file
+                if (geometry.filename) {
+                    let cachedMesh = null;
+
+                    // If already cached, get it
+                    if (meshCache && meshCache.has(geometry.filename)) {
+                        cachedMesh = meshCache.get(geometry.filename);
+                    } else if (fileMap) {
+                        cachedMesh = await this.loadMeshFile(geometry.filename, fileMap);
+                    }
+
+                    if (!cachedMesh) {
+                        console.error(`❌ Cannot load mesh file: ${geometry.filename}`);
+                        return null;
+                    }
+
+                    // loadMeshFile may return Group/Scene (OBJ/DAE/GLTF) or BufferGeometry (STL)
+                    // If Group/Scene, need to clone (because Three.js objects can only have one parent)
+                    if (cachedMesh.isGroup || cachedMesh.isObject3D) {
+                        threeGeometry = cachedMesh.clone(true); // Deep clone (including materials)
+
+                        // Apply mesh scale from MJCF class inheritance (e.g., scale="0.001 0.001 0.001")
+                        if (geometry.meshScale) {
+                            const [sx, sy, sz] = geometry.meshScale;
+                            threeGeometry.scale.set(sx, sy, sz);
+                        }
+
+                        // Check cloned mesh material situation
+                        let meshCount = 0;
+                        let materialCount = 0;
+                        threeGeometry.traverse((child) => {
+                            if (child.isMesh) {
+                                meshCount++;
+                                if (child.material) {
+                                    materialCount++;
+                                }
+                            }
+                        });
+
+                        // Ensure mesh uses lighting-compatible material
+                        ensureMeshHasPhongMaterial(threeGeometry);
+                        return threeGeometry;
+                    }
+                    // If BufferGeometry (e.g., STL), create a mesh and apply scale
+                    if (geometry.meshScale) {
+                        const [sx, sy, sz] = geometry.meshScale;
+                        // Scale the geometry directly
+                        threeGeometry = cachedMesh.clone();
+                        threeGeometry.scale(sx, sy, sz);
+                    } else {
+                        threeGeometry = cachedMesh;
+                    }
+                } else {
+                    console.warn('⚠️ Mesh type geometry missing filename');
+                    return null;
+                }
+                break;
+        }
+
+        if (!threeGeometry) return null;
+
+        // Create default material for BufferGeometry (basic geometries: box, sphere, cylinder, stl, etc.)
+        // Enhanced for better lighting (MuJoCo style) with reflections
+        const envMap = typeof window !== 'undefined' && window.app?.sceneManager?.environmentManager?.getEnvironmentMap();
+        const material = new THREE.MeshPhongMaterial({
+            color: 0xf0f0f0,  // Near white
+            shininess: 50,  // Increased for better highlights
+            specular: new THREE.Color(0.3, 0.3, 0.3),  // Enhanced specular reflection
+            envMap: envMap || null,
+            reflectivity: envMap ? 0.3 : 0
+        });
+        // Save original properties for lighting toggle
+        material.userData.originalShininess = 30;
+        material.userData.originalSpecular = null; // New material, no original specular
+        return new THREE.Mesh(threeGeometry, material);
+    }
+
+    /**
+     * Load mesh file from fileMap (using universal loader)
+     */
+    static async loadMeshFile(meshPath, fileMap) {
+        return loadMeshFile(meshPath, fileMap);
+    }
+
+    /**
+     * Set joint angle
+     */
+    static setJointAngle(joint, angle) {
+        joint.currentValue = angle;
+
+        if (joint.threeObject) {
+            // Rotate based on joint type and axis
+            if (joint.type === 'revolute' || joint.type === 'continuous') {
+                // Use axis stored on threeObject (already converted), if not available convert from joint.axis
+                let axis;
+                if (joint.threeObject.axis) {
+                    axis = joint.threeObject.axis.clone().normalize();
+                } else if (joint.axis && joint.axis.xyz) {
+                    // If no pre-stored axis, need coordinate system conversion
+                    const mjcfAxis = joint.axis.xyz;
+                    axis = new THREE.Vector3(mjcfAxis[0], mjcfAxis[2], -mjcfAxis[1]).normalize();
+                } else {
+                    console.warn('Joint has no axis definition:', joint.name);
+                    return;
+                }
+
+                // Save initial rotation (only save on first call)
+                if (!joint.threeObject.userData.initialQuaternion) {
+                    joint.threeObject.userData.initialQuaternion = joint.threeObject.quaternion.clone();
+                }
+
+                // Set rotation using quaternion: initial rotation * joint rotation
+                const rotationQuat = new THREE.Quaternion();
+                rotationQuat.setFromAxisAngle(axis, angle);
+
+                // Combine rotations: apply initial rotation first, then joint rotation
+                joint.threeObject.quaternion.copy(joint.threeObject.userData.initialQuaternion);
+                joint.threeObject.quaternion.multiply(rotationQuat);
+
+                // Update matrix
+                joint.threeObject.updateMatrixWorld(true);
+            } else if (joint.type === 'prismatic') {
+                // Use axis stored on threeObject (already converted) or convert from joint.axis
+                let axis;
+                if (joint.threeObject.axis) {
+                    axis = joint.threeObject.axis.clone().normalize();
+                } else if (joint.axis && joint.axis.xyz) {
+                    // If no pre-stored axis, need coordinate system conversion
+                    const mjcfAxis = joint.axis.xyz;
+                    axis = new THREE.Vector3(mjcfAxis[0], mjcfAxis[2], -mjcfAxis[1]).normalize();
+                } else {
+                    console.warn('Joint has no axis definition:', joint.name);
+                    return;
+                }
+
+                // Save initial position (only save on first call)
+                if (!joint.threeObject.userData.initialPosition) {
+                    joint.threeObject.userData.initialPosition = joint.threeObject.position.clone();
+                }
+
+                // Translate joint: initial position + move along axis
+                joint.threeObject.position.copy(joint.threeObject.userData.initialPosition);
+                joint.threeObject.position.addScaledVector(axis, angle);
+
+                // Update matrix
+                joint.threeObject.updateMatrixWorld(true);
+            }
+        }
+    }
+}
+
diff --git a/05_software/real/sim2real/web/static/viewer/MeshLoader.js b/05_software/real/sim2real/web/static/viewer/MeshLoader.js
new file mode 100644
index 0000000..7505935
--- /dev/null
+++ b/05_software/real/sim2real/web/static/viewer/MeshLoader.js
@@ -0,0 +1,106 @@
+/**
+ * Adapted MeshLoader for sim2real web console.
+ * Supports both fileMap-based loading (original robot_viewer API) and URL-based
+ * fetching from the sim2real HTTP server at /meshes/.STL.
+ *
+ * Uses importmap-resolved Three.js via CDN (no bundler).
+ */
+import * as THREE from 'three';
+import { STLLoader } from 'three/examples/jsm/loaders/STLLoader.js';
+
+const _stlLoader = new STLLoader();
+
+let loadersCache = null;
+async function getLoaders() {
+    if (!loadersCache) {
+        loadersCache = { STLLoader: _stlLoader };
+    }
+    return loadersCache;
+}
+
+function normalizePath(path) {
+    if (!path) return '';
+    return path.replace(/\\/g, '/').replace(/^\/+/, '').replace(/\/+/g, '/');
+}
+
+/**
+ * Load mesh from URL (sim2real server) or fileMap (robot_viewer compatibility).
+ * @param {string} meshPath - e.g. "fl_hip_abduction_Link.STL"
+ * @param {Map|null} fileMap - optional File map (compat with MJCFAdapter)
+ * @param {string|null} meshBaseUrl - e.g. "/meshes/" for URL-based loading
+ * @returns {Promise}
+ */
+export async function loadMeshFile(meshPath, fileMap = null, meshBaseUrl = null) {
+    const fileName = normalizePath(meshPath).split('/').pop();
+
+    // Strategy 1: try fileMap (robot_viewer compatibility)
+    if (fileMap) {
+        for (const [key, file] of fileMap.entries()) {
+            if (typeof key === 'string' && key.toLowerCase().endsWith(fileName.toLowerCase())) {
+                try {
+                    const url = URL.createObjectURL(file);
+                    const geom = await new Promise((resolve, reject) => {
+                        _stlLoader.load(url, resolve, undefined, reject);
+                    });
+                    URL.revokeObjectURL(url);
+                    console.log('[MeshLoader] loaded from fileMap:', fileName);
+                    return geom;
+                } catch (e) {
+                    URL.revokeObjectURL(url);
+                    console.warn('[MeshLoader] fileMap load failed:', fileName, e);
+                }
+            }
+        }
+    }
+
+    // Strategy 2: try URL-based loading from sim2real server
+    const baseUrl = meshBaseUrl || '/meshes/';
+    const url = baseUrl + fileName;
+    try {
+        console.log('[MeshLoader] fetching:', url);
+        const resp = await fetch(url);
+        if (!resp.ok) {
+            console.warn('[MeshLoader] 404:', url);
+            return null;
+        }
+        const arrayBuf = await resp.arrayBuffer();
+        const blobUrl = URL.createObjectURL(new Blob([arrayBuf]));
+        const geom = await new Promise((resolve, reject) => {
+            _stlLoader.load(blobUrl, resolve, undefined, reject);
+        });
+        URL.revokeObjectURL(blobUrl);
+        console.log('[MeshLoader] loaded from URL:', fileName);
+        return geom;
+    } catch (e) {
+        console.warn('[MeshLoader] URL load failed:', url, e);
+    }
+    return null;
+}
+
+export function ensureMeshHasPhongMaterial(meshObject) {
+    meshObject.traverse((child) => {
+        if (child.isMesh && child.material) {
+            const materials = Array.isArray(child.material) ? child.material : [child.material];
+            materials.forEach((mat, i) => {
+                if (!mat) return;
+                if (mat.type === 'MeshBasicMaterial' || mat.type === 'MeshLambertMaterial') {
+                    const nm = new THREE.MeshPhongMaterial({
+                        color: mat.color, map: mat.map,
+                        transparent: mat.transparent, opacity: mat.opacity, side: mat.side,
+                        shininess: 50, specular: new THREE.Color(0.3, 0.3, 0.3),
+                    });
+                    if (nm.map) nm.map.colorSpace = THREE.SRGBColorSpace;
+                    materials[i] = nm;
+                } else if (mat.isMeshPhongMaterial || mat.isMeshStandardMaterial) {
+                    if (mat.shininess === undefined || mat.shininess < 50) mat.shininess = 50;
+                    if (!mat.specular) mat.specular = new THREE.Color(0.3, 0.3, 0.3);
+                    mat.needsUpdate = true;
+                }
+            });
+            if (Array.isArray(child.material)) child.material = materials;
+            else if (materials.length === 1) child.material = materials[0];
+        }
+    });
+}
+
+export { getLoaders };
diff --git a/05_software/real/sim2real/web/static/viewer/RobotViewer3D.js b/05_software/real/sim2real/web/static/viewer/RobotViewer3D.js
new file mode 100644
index 0000000..3189de9
--- /dev/null
+++ b/05_software/real/sim2real/web/static/viewer/RobotViewer3D.js
@@ -0,0 +1,181 @@
+/**
+ * RobotViewer3D — sim2real 3D 可视化（基于 robot_viewer 的 MJCFAdapter + Three.js）
+ *
+ * 加载 wheelleg.xml → MJCFAdapter.parse → Three.js 场景树
+ * 建立 jointName → THREE.Object3D 映射，通过 updateJoints(pos16) 实时更新。
+ * 支持 OrbitControls 旋转/缩放/平移。
+ *
+ * 用法：
+ *   const viewer = new RobotViewer3D(canvasElement);
+ *   await viewer.load('/mjcf/wheelleg.xml');
+ *   viewer.updateJoints(jointPositions16);
+ */
+import * as THREE from 'three';
+import { OrbitControls } from 'three/examples/jsm/controls/OrbitControls.js';
+import { MJCFAdapter } from './MJCFAdapter.js';
+import { STLLoader } from 'three/examples/jsm/loaders/STLLoader.js';
+
+// 16 关节的标准顺序（与 motor_mapping.py:SIM_JOINT_ORDER 对齐）
+const JOINT_ORDER = [
+    'fl_hip_abduction_joint', 'fl_hip_pitch_joint', 'fl_knee_joint',
+    'fr_hip_abduction_joint', 'fr_hip_pitch_joint', 'fr_knee_joint',
+    'rl_hip_abduction_joint', 'rl_hip_pitch_joint', 'rl_knee_joint',
+    'rr_hip_abduction_joint', 'rr_hip_pitch_joint', 'rr_knee_joint',
+    'fl_wheel_joint', 'fr_wheel_joint', 'rl_wheel_joint', 'rr_wheel_joint',
+];
+
+// MJCF → Three.js 坐标轴转换：让 MJCF 的 Z 轴(向上) 映射到 Three.js 的 Y 轴(向上)
+const MJCF_TO_THREE = new THREE.Matrix4().makeRotationX(-Math.PI / 2);
+// 或直接用 euler: (0, PI, 0)
+
+export class RobotViewer3D {
+    /**
+     * @param {HTMLCanvasElement} canvas
+     * @param {object} [opts]
+     * @param {string} [opts.meshBaseUrl='/meshes/']  STL mesh 文件的 HTTP 路径前缀
+     * @param {string} [opts.mjcfUrl='/mjcf/wheelleg.xml']
+     * @param {string} [opts.backgroundColor='#1a1d24']
+     */
+    constructor(canvas, opts = {}) {
+        this.canvas = canvas;
+        this.meshBaseUrl = opts.meshBaseUrl || '/meshes/';
+        this.mjcfUrl = opts.mjcfUrl || '/mjcf/wheelleg.xml';
+
+        // Three.js 核心
+        const w = canvas.clientWidth, h = canvas.clientHeight;
+        this.scene = new THREE.Scene();
+        // 移除背景色，使用透明背景，由 CSS 控制
+        // this.scene.background = new THREE.Color(opts.backgroundColor || '#1a1d24');
+
+        this.camera = new THREE.PerspectiveCamera(55, w / h, 0.05, 50);
+        this.camera.position.set(0.5, 0.35, 0.65);
+        this.camera.lookAt(0.2, 0, 0);
+
+        this.renderer = new THREE.WebGLRenderer({ canvas, antialias: true, alpha: true });
+        this.renderer.setSize(w, h);
+        this.renderer.setPixelRatio(Math.min(window.devicePixelRatio, 2));
+        this.renderer.shadowMap.enabled = true;
+
+        // OrbitControls
+        this.controls = new OrbitControls(this.camera, canvas);
+        this.controls.target.set(0.15, 0.08, 0.0);
+        this.controls.enableDamping = true;
+        this.controls.dampingFactor = 0.12;
+        this.controls.update();
+
+        // 灯光
+        this._setupLights();
+
+        // 地面
+        const grid = new THREE.GridHelper(2, 20, 0x444444, 0x222222);
+        grid.position.y = -0.35;
+        this.scene.add(grid);
+
+        // 状态
+        this.model = null;
+        this.rootGroup = null;
+        this.jointMap = new Map();           // jointName → { joint, group }
+        this._isLoaded = false;
+        this._rafId = null;
+        this._stlCache = new Map();          // filename → BufferGeometry
+    }
+
+    _setupLights() {
+        const ambient = new THREE.AmbientLight(0x606060, 1.5);
+        this.scene.add(ambient);
+
+        const dir1 = new THREE.DirectionalLight(0xffffff, 2.5);
+        dir1.position.set(2, 3, 2);
+        this.scene.add(dir1);
+
+        const dir2 = new THREE.DirectionalLight(0x8899cc, 1.0);
+        dir2.position.set(-1, 1, -1);
+        this.scene.add(dir2);
+
+        const hemi = new THREE.HemisphereLight(0x8899cc, 0x334455, 1.2);
+        this.scene.add(hemi);
+    }
+
+    // ---- 加载模型 ----
+    async load(mjcfUrlOverride) {
+        const url = mjcfUrlOverride || this.mjcfUrl;
+        console.log('[RobotViewer3D] loading MJCF:', url);
+        const resp = await fetch(url);
+        if (!resp.ok) throw new Error(`MJCF 404: ${url}`);
+        const xmlText = await resp.text();
+
+        // 用 MJCFAdapter 解析 → UnifiedRobotModel
+        // fileMap 为空时不传；MeshLoader 会自动 fallback 到 URL 加载
+        const model = await MJCFAdapter.parse(xmlText, null);
+        this.model = model;
+        console.log('[RobotViewer3D] parsed:', model.links.size, 'links,', model.joints.size, 'joints');
+
+        // 取 rootGroup（MJCFAdapter.createThreeObject 已构建完整 hierarchy）
+        this.rootGroup = model.threeObject;
+        // 坐标轴转换：MJCF → Three.js
+        this.rootGroup.applyMatrix4(MJCF_TO_THREE);
+        this.scene.add(this.rootGroup);
+
+        // 遍历 joints，建立索引
+        this.jointMap.clear();
+        for (const [jointName, joint] of model.joints) {
+            if (joint.threeObject) {
+                this.jointMap.set(jointName, joint);
+            }
+        }
+        // 已建立映射的关节列表
+        const mapped = Array.from(this.jointMap.keys()).sort();
+        console.log('[RobotViewer3D] joint map:', mapped.length, 'joints');
+
+        this._isLoaded = true;
+        this._startRenderLoop();
+    }
+
+    // ---- 渲染循环（按需 + 持续） ----
+    _startRenderLoop() {
+        if (this._rafId) return;
+        const loop = () => {
+            this.controls.update();
+            this.renderer.render(this.scene, this.camera);
+            this._rafId = requestAnimationFrame(loop);
+        };
+        loop();
+    }
+
+    // ---- 实时更新关节角度 ----
+    /**
+     * @param {Float64Array|number[]} pos16 — 16 关节角度 (rad)，顺序同 SIM_JOINT_ORDER
+     *   索引 0-11: 腿关节 (fl_abd,fl_pitch,fl_knee,fr...,rl...,rr...)
+     *   索引 12-15: 轮子关节 (fl_wheel,fr_wheel,rl_wheel,rr_wheel)
+     */
+    updateJoints(pos16) {
+        if (!this._isLoaded) return;
+        for (let i = 0; i < JOINT_ORDER.length && i < pos16.length; i++) {
+            const name = JOINT_ORDER[i];
+            const joint = this.jointMap.get(name);
+            if (joint) {
+                MJCFAdapter.setJointAngle(joint, pos16[i]);
+            }
+        }
+    }
+
+    // ---- 重置相机 ----
+    resetCamera() {
+        this.camera.position.set(0.5, 0.35, 0.65);
+        this.controls.target.set(0.15, 0.08, 0.0);
+        this.controls.update();
+    }
+
+    // ---- 调整大小 ----
+    resize() {
+        const w = this.canvas.clientWidth, h = this.canvas.clientHeight;
+        this.camera.aspect = w / h;
+        this.camera.updateProjectionMatrix();
+        this.renderer.setSize(w, h);
+    }
+
+    dispose() {
+        if (this._rafId) cancelAnimationFrame(this._rafId);
+        this.renderer.dispose();
+    }
+}
diff --git a/05_software/real/sim2real/web/static/viewer/UnifiedRobotModel.js b/05_software/real/sim2real/web/static/viewer/UnifiedRobotModel.js
new file mode 100644
index 0000000..1ef44f8
--- /dev/null
+++ b/05_software/real/sim2real/web/static/viewer/UnifiedRobotModel.js
@@ -0,0 +1,181 @@
+/**
+ * Unified robot model data interface
+ * All formats (URDF, MJCF, USD) are converted to this unified format
+ */
+export class UnifiedRobotModel {
+    constructor() {
+        this.name = '';
+        this.links = new Map(); // Map
+        this.joints = new Map(); // Map
+        this.materials = new Map(); // Map
+        this.constraints = new Map(); // Map - for parallel mechanism constraints
+        this.rootLink = null; // Root link name
+        this.threeObject = null; // Three.js object (if available)
+    }
+
+    addLink(link) {
+        this.links.set(link.name, link);
+    }
+
+    addJoint(joint) {
+        this.joints.set(joint.name, joint);
+    }
+
+    addConstraint(constraint) {
+        this.constraints.set(constraint.name, constraint);
+    }
+
+    getLink(name) {
+        return this.links.get(name);
+    }
+
+    getJoint(name) {
+        return this.joints.get(name);
+    }
+
+    getConstraint(name) {
+        return this.constraints.get(name);
+    }
+}
+
+/**
+ * Link interface
+ */
+export class Link {
+    constructor(name) {
+        this.name = name;
+        this.visuals = []; // VisualGeometry[]
+        this.collisions = []; // CollisionGeometry[]
+        this.inertial = null; // InertialProperties
+        this.threeObject = null; // Three.js object
+        this.userData = {}; // User-defined data (for adapters to store additional information)
+    }
+}
+
+/**
+ * VisualGeometry interface
+ */
+export class VisualGeometry {
+    constructor() {
+        this.name = '';
+        this.origin = { xyz: [0, 0, 0], rpy: [0, 0, 0] };
+        this.geometry = null; // GeometryType
+        this.material = null; // Material
+        this.threeObject = null; // Three.js Mesh
+    }
+}
+
+/**
+ * CollisionGeometry interface
+ */
+export class CollisionGeometry {
+    constructor() {
+        this.name = '';
+        this.origin = { xyz: [0, 0, 0], rpy: [0, 0, 0] };
+        this.geometry = null; // GeometryType
+        this.threeObject = null; // Three.js Mesh
+    }
+}
+
+/**
+ * GeometryType interface
+ */
+export class GeometryType {
+    constructor(type) {
+        this.type = type; // 'box' | 'sphere' | 'cylinder' | 'mesh'
+        this.size = null; // Size parameters (varies by type)
+        this.filename = null; // Mesh file path (if mesh type)
+    }
+
+    clone() {
+        const cloned = new GeometryType(this.type);
+        cloned.size = this.size ? { ...this.size } : null;
+        cloned.filename = this.filename;
+        return cloned;
+    }
+}
+
+/**
+ * InertialProperties interface
+ */
+export class InertialProperties {
+    constructor() {
+        this.mass = 0;
+        this.origin = { xyz: [0, 0, 0], rpy: [0, 0, 0] };
+        this.ixx = 0;
+        this.iyy = 0;
+        this.izz = 0;
+        this.ixy = 0;
+        this.ixz = 0;
+        this.iyz = 0;
+    }
+}
+
+/**
+ * Joint interface
+ */
+export class Joint {
+    constructor(name, type) {
+        this.name = name;
+        this.type = type; // 'revolute' | 'prismatic' | 'fixed' | 'continuous'
+        this.parent = null; // Parent link name
+        this.child = null; // Child link name
+        this.origin = { xyz: [0, 0, 0], rpy: [0, 0, 0] };
+        this.axis = { xyz: [0, 0, 1] }; // Default z-axis
+        this.limits = null; // JointLimits
+        this.currentValue = 0; // Current joint value
+        this.threeObject = null; // Three.js object (if available)
+    }
+}
+
+/**
+ * JointLimits interface
+ */
+export class JointLimits {
+    constructor() {
+        this.lower = -Math.PI;
+        this.upper = Math.PI;
+        this.effort = null;
+        this.velocity = null;
+    }
+}
+
+/**
+ * Material interface
+ */
+export class Material {
+    constructor(name) {
+        this.name = name;
+        this.color = { r: 0.8, g: 0.8, b: 0.8 };
+        this.texture = null;
+    }
+}
+
+/**
+ * Constraint interface - for describing closed-chain constraints of parallel mechanisms
+ * Supports MuJoCo equality constraint types
+ */
+export class Constraint {
+    constructor(name, type) {
+        this.name = name;
+        this.type = type; // 'connect' | 'weld' | 'joint' | 'tendon' | 'distance'
+
+        // Constraint objects (may be body, geom, joint, etc. depending on type)
+        this.body1 = null;
+        this.body2 = null;
+        this.anchor = null; // Connection point coordinates
+        this.torquescale = null; // Torque scale
+
+        // Joint constraint specific properties
+        this.joint1 = null;
+        this.joint2 = null;
+        this.polycoef = null; // Polynomial coefficients [a0, a1, a2, a3, a4]
+
+        // Visualization object
+        this.threeObject = null; // Three.js object for displaying constraint
+
+        // Original data (for debugging)
+        this.userData = {};
+    }
+}
+
diff --git a/05_software/train/README.md b/05_software/train/README.md
new file mode 100644
index 0000000..92d2fab
--- /dev/null
+++ b/05_software/train/README.md
@@ -0,0 +1,15 @@
+# 第一代强化学习与仿真工程
+
+`rc_mjlab/` 是 16DOF 轮足机器人的第一代自包含训练与仿真工程。
+
+## 内容
+
+- `src/robot`：Flat、Rough、Crawl 训练任务和自定义 MDP
+- `mjcf`：轮足机器人 MuJoCo 模型和网格
+- `mujoco_sim`：不依赖策略的独立 MuJoCo/MPC 调试工具
+- `sim2sim`：策略加载、交互控制和比赛地形验证
+- `mjlab`：固定版本的本地训练框架依赖
+- `model_rough.pt`、`model_crawl.pt`：对应的早期策略权重
+- `pyproject.toml`、`uv.lock`：Python 环境与依赖锁定
+
+工程命令和任务说明见 [`rc_mjlab/README.md`](rc_mjlab/README.md)，本地依赖来源见 [`rc_mjlab/DEPENDENCIES.md`](rc_mjlab/DEPENDENCIES.md)。
diff --git a/05_software/train/rc_mjlab/DEPENDENCIES.md b/05_software/train/rc_mjlab/DEPENDENCIES.md
new file mode 100644
index 0000000..7e22564
--- /dev/null
+++ b/05_software/train/rc_mjlab/DEPENDENCIES.md
@@ -0,0 +1,44 @@
+# 依赖说明
+
+## Python 环境
+
+- Python `>=3.10`
+- `uv` 依赖管理
+- MuJoCo development wheel
+- `mjlab[cu128]`
+- PyTorch CUDA 12.8 环境
+- `pynput`
+
+精确解析结果保存在 `uv.lock`。项目使用本地可编辑 `mjlab`：
+
+```toml
+[tool.uv.sources]
+mjlab = { path = "mjlab", editable = true }
+```
+
+## mjlab 来源
+
+- 上游仓库：`https://github.com/mujocolab/mjlab.git`
+- 基准提交：`0040979763ab43bc1220812c9de4bc74e2631f42`
+- 基准日期：`2026-04-28`
+- 上游许可证：Apache-2.0，许可证文件保留在 `mjlab/LICENSE`
+
+早期工程在该基准上保留了 3 处本地修改：
+
+1. `mjlab/pyproject.toml`：增加清华 PyPI 镜像。
+2. `mjlab/src/mjlab/envs/mdp/dr/actuator.py`：让 effort limit 随机化支持轮子使用的 velocity/motor actuator。
+3. `mjlab/src/mjlab/scene/scene.py`：通过 XML 字符串加载场景，以适配当时的场景组合方式。
+
+本次归档保留修改后的完整工作树，但不包含上游 `.git`、本地 `.venv`、缓存和生成日志。
+
+## 基本入口
+
+在 `05_software/train/rc_mjlab` 下执行：
+
+```bash
+uv sync
+uv run train Robot-Flat-v0
+uv run play Robot-Rough-v0
+```
+
+GPU、CUDA、MuJoCo development wheel 和驱动版本必须满足 `pyproject.toml` 与 `uv.lock` 的约束。
diff --git a/05_software/train/rc_mjlab/README.md b/05_software/train/rc_mjlab/README.md
new file mode 100644
index 0000000..87d90da
--- /dev/null
+++ b/05_software/train/rc_mjlab/README.md
@@ -0,0 +1,222 @@
+# rc_mjlab
+
+基于 [mjlab](https://github.com/google-deepmind/mjlab) 框架的四轮腿混合机器人强化学习训练与部署部署项目，面向机器人竞赛场景（如越障、匍匐、斜坡、台阶等复合任务）。
+
+---
+
+## 🛠️ 项目简介
+
+本项目针对一台 **4 腿 × 3 关节 + 4 驱动轮（轮腿混合）** 的移动机器人，在 MuJoCo 物理引擎中利用 PPO 算法进行多任务运动控制策略训练。
+
+系统设计特点包括：
+1. **高保真动力学步进**：物理仿真计算步长设为 **`2ms` (0.002s)**，为碰撞、地面力学传递提供极高的解算频宽与稳定性。
+2. **50Hz 控制决策循环**：通过在环境中设置 `decimation = 10`，策略决策周期为 `20ms` ($0.002\text{s} \times 10 = 0.02\text{s}$)，即控制决策频率为 **`50Hz`**，完全对齐真机控制周期。
+3. **混合滤波执行器**：
+   - 腿部 12 个位置控制关节采用位置 PD 伺服（$K_p=40, K_d=1$），并叠加截止频率为 **`5Hz`** 的低通滤波器进行动作平滑，减小高频机械抖动。
+   - 轮部 4 个速度驱动关节采用阻尼速度伺服（$K_d=0.5$），叠加截止频率为 **`15Hz`** 的低通速度滤波器，保证转速响应的灵敏度。
+4. **大规模并行加速**：利用 GPU 并行（通过 Warp 和 MuJoCo GPU 物理管线），支持最多 $4096$ 环境同时训练，并包含对动作变化率、关节加速度的惩罚项以平抑噪声。
+
+---
+
+## 📦 项目结构
+
+```
+rc_mjlab/
+├── src/robot/                    # RL 训练任务包（主体代码）
+│   ├── __init__.py               # 任务注册（Robot-Flat-v0 / Robot-Rough-v0 / Robot-Crawl-v0）
+│   ├── robot_cfg.py              # 机器人物理参数（PD 增益、执行器上限、碰撞属性）
+│   ├── config/
+│   │   ├── env_cfgs.py           # 三套环境完整配置（观测、奖励、事件、地形、终止条件）
+│   │   └── rl_cfg.py             # PPO 超参数（网络结构、学习率、折扣因子等）
+│   ├── mdp/
+│   │   ├── rewards.py            # 自定义奖励函数（速度追踪、姿态约束、接触、越障反射惩罚等）
+│   │   ├── curriculums.py        # 地形关卡课程（严格速度约束版）+ 自适应速度范围
+│   │   ├── lowpass_actions.py    # 低通滤波动作包装（腿 5 Hz / 轮 15 Hz IIR 滤波）
+│   │   ├── disturbances.py       # 持续外力扰动（一阶低通滤波平滑随机外力/扭矩）
+│   │   ├── mode_command.py       # 离散步态模式命令（保留扩展用）
+│   │   └── only_positive_rewards.py  # HIMLoco 风格：每步总奖励截断为 ≥ 0，防止消极逃避
+│   └── terrains/
+│       └── competition_terrains.py   # 竞赛自定义地形（高墙障碍、低杆障碍）
+├── sim2sim/                      # Sim2Sim 物理部署与高精度交互回放工具
+│   ├── nav_sim2sim.py            # 主程序：2D Pygame 交互面板 + 全自动多地形导航追踪
+│   ├── sim2sim.py                # 简易版键盘调试工具
+│   ├── interface/
+│   │   └── mujoco_io.py          # MuJoCo 输入输出与传感器、低通滤波器接口
+│   ├── tools/
+│   │   └── math_utils.py         # 姿态重力等数学转换
+│   ├── policy/                   # 保存的 pt 策略权重
+│   └── terrain/
+│       └── scene_terrain.xml     # 完整越障比赛场地的物理 XML 定义
+├── mjcf/
+│   ├── wheelleg.xml              # 机器人 MuJoCo 模型（含网格引用）
+│   ├── scene.xml                 # mjlab 场景入口文件
+│   └── meshes/                   # STL/OBJ 碰撞与外观网格
+├── mujoco_sim/                   # 独立 MPC 仿真调试工具（不依赖 RL 训练）
+├── logs/                         # 训练日志（rsl_rl 格式，按任务名/日期/checkpoint 归档）
+├── pyproject.toml                # 项目依赖（uv 管理，含清华镜像源加速）
+└── uv.lock                       # 精确依赖锁定文件
+```
+
+---
+
+## 🚀 常用命令
+
+### 1. 训练与回放
+
+```bash
+# 运行平地基础训练 (Robot-Flat-v0)
+uv run train Robot-Flat-v0
+
+# 运行多障碍复杂地形训练 (Robot-Rough-v0)，可从 Flat 的Checkpoint热启动
+uv run train Robot-Rough-v0 --agent.resume True --agent.experiment-name robot_flat
+
+# 运行爬坡与匍匐限高任务 (Robot-Crawl-v0)
+uv run train Robot-Crawl-v0
+
+# 使用默认 20 个并行环境回放最新 checkpoint 效果
+uv run play Robot-Rough-v0
+```
+
+### 2. 交互式 Sim2Sim 自动导航仪表盘
+
+我们提供了一个强大的 GUI 交互和全自动障碍赛追踪平台，位于 `sim2sim` 目录下：
+
+```bash
+# 启动 2D 交互导航平台
+cd sim2sim
+uv run python nav_sim2sim.py
+```
+
+---
+
+## 🖥️ 交互式自动导航平台 (sim2sim/nav_sim2sim.py)
+
+该平台包含一个 **Pygame 2D HUD 监控面板** 和一个 **实时 MuJoCo 3D 渲染器**，支持对仿真参数和任务执行的精细控制。
+
+### 1. 按钮面板分区与布局
+
+面板在垂直方向进行了高紧凑性排版，避免控件重叠，并在底端留有安全间距：
+* **【预设任务列表】**（按物理穿越顺序排列）：
+  - **S形绕杆 (Slalom)**：绕过红蓝两色障碍杆路径。
+  - **限高下蹲 (Crawl)**：降低机身高度穿过低杆障碍。
+  - **砂砾碎石 (Gravel)**：平稳低速通过多颗粒非结构碎石坑。
+  - **高墙越障 (Wall)**：高速度冲向障碍高墙，利用前轮攀爬反射爬越。
+  - **台阶攀爬 (Stairs)**：攀越分段式台阶。
+  - **斜坡木桥 (Bridge)**：穿过A坡并稳健从B坡落地。
+  - **障碍赛大满贯 (Grand)**：**科技紫**圆角高亮按钮。点击后，机器人将以**顺时针**方向，自动、连贯且闭环地一次性穿越上述全部 6 个核心比赛障碍，并在木桥落地后，通过安全通道直角返航至起终点。
+* **【系统与视图控制】**：
+  - **清除与停止 (Stop)**：一键紧急停止并重置当前目标航点。
+  - **视角居中 (Center)**：一键锁定相机随机器人机身移动。
+  - **物理流速三联排 (倍速- / 标准 / 倍速+)**：在不破坏物理计算数值稳定性的前提下，实现对仿真总体时间的平滑加速与慢放（支持 `0.2x` ~ `5.0x`，可随时点击“标准”一键归位 `1.0x`）。
+* **【目标微调与命令终端】**：
+  - 拥有高精度航点微调发令键。
+  - 底部命令行支持输入 `speed <倍率>` 更改仿真速度，或者输入 `grand` 直接开启大满贯。
+
+---
+
+## 📊 机器人系统规格参数
+
+### 1. 机器人本体参数
+
+| 参数项 | 基准数值 | 说明 |
+|---|---|---|
+| **物理步长 ($dt_{physics}$)** | `0.002s` (2ms) | 底层 MuJoCo 求解器步长，物理精度极高 |
+| **控制决策频率 ($Freq_{ctrl}$)** | `50Hz` (20ms) | $decimation = 10$，环境每 10 个子步进行一次交互决策 |
+| **单轮仿真时长** | `30.0s` | 最大决策步数上限为 $30.0 / 0.02 = 1500$ 步 |
+| **腿部控制** | 位置 PD 伺服 | 目标关节角限幅 ±0.25 rad，叠加 **5Hz** 低通滤波器 |
+| **轮部控制** | 阻尼速度伺服 | 目标速度限幅 ±10.0 rad/s，叠加 **15Hz** 低通滤波器 |
+| **结构形式** | 4腿 × 3关节 + 4轮 | 腿：hip abduction, hip pitch, knee；轮半径 0.1m，左右轮距 0.32m |
+| **关节扭矩上限** | 17.0 Nm | 关节最大输出力矩（训练时含 80%~100% 随机缩放） |
+| **最大关节角速度** | 13.0 rad/s | 关节最大运动速度限制 |
+
+### 2. 状态观测空间 (Actor Obs, 53维)
+
+网络输入包含 $6$ 步历史数据，并在训练时注入均匀高斯噪声以提升泛化能力：
+
+| 观测项目 | 维度 | 缩放比例 | 噪声范围 |
+|---|---|---|---|
+| 基座角速度 (ang_vel) | 3 | 0.25 | $[-0.2, 0.2]$ rad/s |
+| 投影重力向量 (projected_gravity) | 3 | 1.0 | $[-0.05, 0.05]$ |
+| 指令速度 (vx, vy, wz/heading) | 3 | 1.0 | — |
+| 腿部关节相对角度 (joint_pos_rel) | 12 | 1.0 | $[-0.01, 0.01]$ rad |
+| 腿部关节角速度 (joint_vel) | 12 | 0.05 | $[-1.5, 1.5]$ rad/s |
+| 轮子角速度 (wheel_vel) | 4 | 0.05 | $[-1.0, 1.0]$ rad/s |
+| 上一步动作缓存 (last_actions) | 16 | 1.0 | — |
+
+> **Critic 附加观测**：包含高精度基座物理线速度、轮地实际接触状态、以及 $1.6\text{m} \times 1.0\text{m}$ 分辨率为 $0.08\text{m}$ 的高度雷达扫描网格，提供大范围越障感知。
+
+---
+
+## ⚖️ 奖惩体系设计 (Robot-Rough-v0)
+
+复杂地形任务采用 **“仅正奖励截断”** 机制（即每步累加的总奖励若小于0则强制截断为0），防止机器人在困难关卡早期选择倒下自杀来规避负惩罚。
+
+### 1. 运动追踪与状态惩罚
+
+| 奖励/惩罚项 | 权重 (Weight) | 适用函数 / 物理意义 |
+|---|---|---|
+| **track_lin_vel** | `+4.5` | L1 范数水平线速度跟踪奖励，平缓高速漂移 |
+| **track_ang_vel** | `+2.0` | 偏航角速度指数跟踪奖励 |
+| **stand_still** | `-2.0` | 当速度指令为 0 时，严厉惩罚关节多余晃动，保持稳立 |
+| **joint_pos_penalty** | `-0.8` | 当速度指令为 0 时，惩罚关节角度偏离初始对齐姿态，维持高刚度 |
+| **roll_penalty** | `-1.0` | 机身横滚角 (Roll) 倾斜惩罚，抑制左右倾倒抖动 |
+| **pitch_penalty** | `-1.5` | 俯仰角 (Pitch) 死区惩罚，限制仰角不超过 29 度，抑制越障瞬间前轮翘头和后翻 |
+| **base_height_l2** | `-0.5` | 机身高度偏离 0.36m 惩罚（基于高度扫描均值，允许自适应高低） |
+
+### 2. 能量正则与平滑惩罚 (平抑高频抖动)
+
+| 奖励/惩罚项 | 权重 (Weight) | 适用函数 / 物理意义 |
+|---|---|---|
+| **action_rate_curriculum** | `-0.005` | 动作变化率 L2 惩罚，迫使连续两个决策步的输出动作变化平滑 |
+| **joint_torques** | `-1.0e-4` | 关节输出扭矩 L2 正则，降低电机总发热和冲击性载荷 |
+| **leg_joint_acc_l2** | `-2.5e-7` | 限制腿部 12 关节**角加速度**，直接抑制关节高频电磁和机械震荡 |
+| **wheel_joint_acc_l2** | `-2.5e-9` | 限制 4 个驱动轮的**角加速度**，平缓轮速切换，降低打滑振荡 |
+| **joint_pos_limits** | `-0.2` | 极度接近关节极限限位阻挡时的硬惩罚 |
+
+### 3. 接触反射与安全约束
+
+| 奖励/惩罚项 | 权重 (Weight) | 适用函数 / 物理意义 |
+|---|---|---|
+| **feet_contact_without_cmd** | `+0.1` | 当速度指令为 0 时，鼓励四轮保持稳定接地的正向收益 |
+| **body_collision** | `-1.0` | 腿部连杆（大腿、小腿）触地碰撞惩罚，迫使抬腿跨越障碍 |
+| **base_collision** | `-5.0` | 机身/底盘硬撞障碍物时的严厉惩罚，逼迫机器人学会抬起前轮支撑攀爬 |
+| **is_terminated** | `0.0` | 关闭越障任务的提早终止，允许机器人跌倒后自行挣扎起立，提高生存极限 |
+
+---
+
+## 🌀 域随机化 (Domain Randomization)
+
+为了使训练的控制策略具有卓越的零样本真机部署能力，在环境重置及仿真运行中注入了高强度的域随机化参数：
+
+| 随机化项目 | 扰动操作 | 随机范围 |
+|---|---|---|
+| **机身质心偏移 (base_com)** | 加法 | X, Y, Z 三轴分别随机偏置 `[-0.05, 0.05]` 米 |
+| **角度传感器零偏 (encoder_bias)** | 加法 | 关节传感器绝对偏置 `[-0.015, 0.015]` rad (约 $\pm 0.85^{\circ}$) |
+| **几何表面摩擦力 (body_friction)** | 绝对值 | 地面及机器人碰撞几何体摩擦力在 `[0.3, 1.2]` 均匀随机 |
+| **关节摩擦阻尼 (joint_friction)** | 乘法 | 所有旋转轴关节运动阻尼摩擦在原值的 `[0.7, 1.3]` 倍间随机 |
+| **关节传动刚度 (actuator_stiffness)** | 乘法 | Kp 刚度系数在原值的 `[0.9, 1.1]` 对数均匀范围内随机缩放 |
+| **关节传动阻尼 (actuator_damping)** | 乘法 | Kd 阻尼系数在原值的 `[0.9, 1.1]` 对数均匀范围内随机缩放 |
+| **力矩输出上限 (actuator_effort_limit)**| 乘法 | 最大输出扭矩极限随机在原值的 `[0.8, 1.0]` 倍均匀缩放 |
+| **负载质量 (payload_mass)** | 加法 | 在机身处添加载荷质量，扰动范围在 `[-1.0, 3.0]` kg |
+| **瞬时侧向推撞 (push_robot)** | 脉冲 | 每隔 `[5.0, 10.0]` 秒，瞬间施加 X/Y 轴 `[-0.5, 0.5]` m/s 冲击速度 |
+| **一阶低通持续风阻 (continuous_disturbance)** | 连续 | 机身持续叠加随机外力（±15N）与力矩（±10Nm），低通周期 0.5s |
+
+---
+
+## 🏆 多地形关卡难度控制 (Robot-Rough-v0)
+
+共有 8 种子地形按照比例混合，通过自适应升级距离控制关卡难度的推进：
+
+| 地形名称 | 混合比例 (Proportion) | 最大配置难度 |
+|---|---|---|
+| **平地 (flat)** | `5%` | 作为初始安定性恢复区域 |
+| **金字塔台阶 (pyramid_stairs)** | `25%` | 最大阶梯高度上限 `0.30` 米，级宽 0.30m |
+| **倒金字塔台阶 (pyramid_stairs_inv)** | `10%` | 最大倒台阶高度上限 `0.30` 米，级宽 0.30m |
+| **随机高度网格 (random_grid)** | `10%` | 最大网格方块起伏上限 `0.30` 米 |
+| **随机粗糙地形 (random_rough)** | `5%` | 地表最大颗粒随机噪声起伏 `0.06` 米 |
+| **柏林噪声地形 (perlin_noise)** | `5%` | 大范围高平缓起伏最大高度 `0.06` 米 |
+| **越障高墙地形 (rc_wall)** | `25%` | 自定义跳跃垂直高墙，最大墙高上限 `0.45` 米 |
+| **平台斜坡地形 (sloped_terrain)** | `15%` | 最大坡度限制 `0.325` (约 $18.5^{\circ}$) |
+
+> **地形升级规则**：当机器人朝指令方向行进距离超过当前地块的一半（4米），且实际行进距离大于速度指令对应期望距离的 45% 时，该环境关卡等级 +1。  
+> **地形降级规则**：当指令速度大于 0.1m/s 但实际行进距离小于期望距离的 25%，或者实际移动不足 2.0米时，环境难度等级 -1。
diff --git a/05_software/train/rc_mjlab/mjcf/meshes/base_link.STL b/05_software/train/rc_mjlab/mjcf/meshes/base_link.STL
new file mode 100644
index 0000000..015035f
Binary files /dev/null and b/05_software/train/rc_mjlab/mjcf/meshes/base_link.STL differ
diff --git a/05_software/train/rc_mjlab/mjcf/meshes/fl_hip_abduction_Link.STL b/05_software/train/rc_mjlab/mjcf/meshes/fl_hip_abduction_Link.STL
new file mode 100644
index 0000000..971a78e
Binary files /dev/null and b/05_software/train/rc_mjlab/mjcf/meshes/fl_hip_abduction_Link.STL differ
diff --git a/05_software/train/rc_mjlab/mjcf/meshes/fl_hip_pitch_Link.STL b/05_software/train/rc_mjlab/mjcf/meshes/fl_hip_pitch_Link.STL
new file mode 100644
index 0000000..e1803e0
Binary files /dev/null and b/05_software/train/rc_mjlab/mjcf/meshes/fl_hip_pitch_Link.STL differ
diff --git a/05_software/train/rc_mjlab/mjcf/meshes/fl_knee_Link.STL b/05_software/train/rc_mjlab/mjcf/meshes/fl_knee_Link.STL
new file mode 100644
index 0000000..cc5f40c
Binary files /dev/null and b/05_software/train/rc_mjlab/mjcf/meshes/fl_knee_Link.STL differ
diff --git a/05_software/train/rc_mjlab/mjcf/meshes/fl_wheel_Link.STL b/05_software/train/rc_mjlab/mjcf/meshes/fl_wheel_Link.STL
new file mode 100644
index 0000000..c3e124b
Binary files /dev/null and b/05_software/train/rc_mjlab/mjcf/meshes/fl_wheel_Link.STL differ
diff --git a/05_software/train/rc_mjlab/mjcf/meshes/fr_hip_abduction_Link.STL b/05_software/train/rc_mjlab/mjcf/meshes/fr_hip_abduction_Link.STL
new file mode 100644
index 0000000..8a15920
Binary files /dev/null and b/05_software/train/rc_mjlab/mjcf/meshes/fr_hip_abduction_Link.STL differ
diff --git a/05_software/train/rc_mjlab/mjcf/meshes/fr_hip_pitch_Link.STL b/05_software/train/rc_mjlab/mjcf/meshes/fr_hip_pitch_Link.STL
new file mode 100644
index 0000000..909e5ae
Binary files /dev/null and b/05_software/train/rc_mjlab/mjcf/meshes/fr_hip_pitch_Link.STL differ
diff --git a/05_software/train/rc_mjlab/mjcf/meshes/fr_knee_Link.STL b/05_software/train/rc_mjlab/mjcf/meshes/fr_knee_Link.STL
new file mode 100644
index 0000000..802f2eb
Binary files /dev/null and b/05_software/train/rc_mjlab/mjcf/meshes/fr_knee_Link.STL differ
diff --git a/05_software/train/rc_mjlab/mjcf/meshes/fr_wheel_Link.STL b/05_software/train/rc_mjlab/mjcf/meshes/fr_wheel_Link.STL
new file mode 100644
index 0000000..9c17db1
Binary files /dev/null and b/05_software/train/rc_mjlab/mjcf/meshes/fr_wheel_Link.STL differ
diff --git a/05_software/train/rc_mjlab/mjcf/meshes/rl_hip_abduction_Link.STL b/05_software/train/rc_mjlab/mjcf/meshes/rl_hip_abduction_Link.STL
new file mode 100644
index 0000000..0680c00
Binary files /dev/null and b/05_software/train/rc_mjlab/mjcf/meshes/rl_hip_abduction_Link.STL differ
diff --git a/05_software/train/rc_mjlab/mjcf/meshes/rl_hip_pitch_Link.STL b/05_software/train/rc_mjlab/mjcf/meshes/rl_hip_pitch_Link.STL
new file mode 100644
index 0000000..ab8ffa7
Binary files /dev/null and b/05_software/train/rc_mjlab/mjcf/meshes/rl_hip_pitch_Link.STL differ
diff --git a/05_software/train/rc_mjlab/mjcf/meshes/rl_knee_Link.STL b/05_software/train/rc_mjlab/mjcf/meshes/rl_knee_Link.STL
new file mode 100644
index 0000000..5d64c75
Binary files /dev/null and b/05_software/train/rc_mjlab/mjcf/meshes/rl_knee_Link.STL differ
diff --git a/05_software/train/rc_mjlab/mjcf/meshes/rl_wheel_Link.STL b/05_software/train/rc_mjlab/mjcf/meshes/rl_wheel_Link.STL
new file mode 100644
index 0000000..5bab538
Binary files /dev/null and b/05_software/train/rc_mjlab/mjcf/meshes/rl_wheel_Link.STL differ
diff --git a/05_software/train/rc_mjlab/mjcf/meshes/rr_hip_abduction_Link.STL b/05_software/train/rc_mjlab/mjcf/meshes/rr_hip_abduction_Link.STL
new file mode 100644
index 0000000..4013ccf
Binary files /dev/null and b/05_software/train/rc_mjlab/mjcf/meshes/rr_hip_abduction_Link.STL differ
diff --git a/05_software/train/rc_mjlab/mjcf/meshes/rr_hip_pitch_Link.STL b/05_software/train/rc_mjlab/mjcf/meshes/rr_hip_pitch_Link.STL
new file mode 100644
index 0000000..5f81a5f
Binary files /dev/null and b/05_software/train/rc_mjlab/mjcf/meshes/rr_hip_pitch_Link.STL differ
diff --git a/05_software/train/rc_mjlab/mjcf/meshes/rr_knee_Link.STL b/05_software/train/rc_mjlab/mjcf/meshes/rr_knee_Link.STL
new file mode 100644
index 0000000..28dccab
Binary files /dev/null and b/05_software/train/rc_mjlab/mjcf/meshes/rr_knee_Link.STL differ
diff --git a/05_software/train/rc_mjlab/mjcf/meshes/rr_wheel_Link.STL b/05_software/train/rc_mjlab/mjcf/meshes/rr_wheel_Link.STL
new file mode 100644
index 0000000..a9d9ecb
Binary files /dev/null and b/05_software/train/rc_mjlab/mjcf/meshes/rr_wheel_Link.STL differ
diff --git a/05_software/train/rc_mjlab/mjcf/scene.xml b/05_software/train/rc_mjlab/mjcf/scene.xml
new file mode 100644
index 0000000..155ffb2
--- /dev/null
+++ b/05_software/train/rc_mjlab/mjcf/scene.xml
@@ -0,0 +1,22 @@
+
+  
+
+  
diff --git a/05_software/train/rc_mjlab/mjcf/sim2sim_temp.xml b/05_software/train/rc_mjlab/mjcf/sim2sim_temp.xml
new file mode 100644
index 0000000..80306ee
--- /dev/null
+++ b/05_software/train/rc_mjlab/mjcf/sim2sim_temp.xml
@@ -0,0 +1,327 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/05_software/train/rc_mjlab/mjcf/wheelleg.xml b/05_software/train/rc_mjlab/mjcf/wheelleg.xml
new file mode 100644
index 0000000..960a3cb
--- /dev/null
+++ b/05_software/train/rc_mjlab/mjcf/wheelleg.xml
@@ -0,0 +1,157 @@
+
+  
+
+  
+    
+  
+  
+    
+    
+    
+    
+    
+    
+    
+    
+    
+    
+    
+    
+    
+    
+    
+    
+    
+  
+
+  
+    
+      
+      
+      
+      
+      
+        
+        
+        
+        
+          
+          
+          
+          
+          
+          
+            
+            
+            
+            
+            
+            
+              
+              
+              
+              
+            
+          
+        
+      
+      
+        
+        
+        
+        
+          
+          
+          
+          
+          
+          
+            
+            
+            
+            
+            
+            
+              
+              
+              
+              
+            
+          
+        
+      
+      
+        
+        
+        
+        
+          
+          
+          
+          
+          
+          
+            
+            
+            
+            
+            
+            
+              
+              
+              
+              
+            
+          
+        
+      
+      
+        
+        
+        
+        
+          
+          
+          
+          
+          
+          
+            
+            
+            
+            
+            
+            
+              
+              
+              
+              
+            
+          
+        
+      
+      
+        
+      
+    
+  
+
+  
+    
+    
+    
+    
+    
+    
+    
+    
+    
+    
+    
+    
+    
+    
+    
+    
+  
+
diff --git a/05_software/train/rc_mjlab/mjlab/.claude/commands/commit-push-pr.md b/05_software/train/rc_mjlab/mjlab/.claude/commands/commit-push-pr.md
new file mode 100644
index 0000000..0a624d6
--- /dev/null
+++ b/05_software/train/rc_mjlab/mjlab/.claude/commands/commit-push-pr.md
@@ -0,0 +1,19 @@
+---
+allowed-tools: Bash(git checkout --branch:*), Bash(git add:*), Bash(git status:*), Bash(git push:*), Bash(git commit:*), Bash(gh pr create:*)
+description: Commit, push, and open a PR
+---
+
+## Context
+
+- Current git status: !`git status`
+- Current git diff (staged and unstaged changes): !`git diff HEAD`
+- Current branch: !`git branch --show-current`
+
+## Your task
+
+Based on the above changes:
+1. Create a new branch if on main
+2. Create a single commit with an appropriate message
+3. Push the branch to origin
+4. Create a pull request using `gh pr create`
+5. You have the capability to call multiple tools in a single response. You MUST do all of the above in a single message. Do not use any other tools or do anything else. Do not send any other text or messages besides these tool calls.
diff --git a/05_software/train/rc_mjlab/mjlab/.claude/commands/update-mjwarp.md b/05_software/train/rc_mjlab/mjlab/.claude/commands/update-mjwarp.md
new file mode 100644
index 0000000..f345864
--- /dev/null
+++ b/05_software/train/rc_mjlab/mjlab/.claude/commands/update-mjwarp.md
@@ -0,0 +1,18 @@
+---
+allowed-tools: Bash(uv lock), Bash(git checkout:*), Bash(git add:*), Bash(git status:*), Bash(git push:*), Bash(git commit:*), Bash(gh pr create:*), Edit, Read
+description: Update the mujoco-warp dependency to a given commit
+---
+
+Update the mujoco-warp dependency to commit $ARGUMENTS.
+
+Steps:
+1. Read `pyproject.toml` and find the `mujoco-warp` line under `[tool.uv.sources]`.
+2. Use Edit to replace the current `rev = "..."` value with `rev = "$ARGUMENTS"` on that line.
+3. Run `uv lock` to regenerate the lockfile.
+4. Create and switch to a new branch named `update-mjwarp/` (e.g. `update-mjwarp/e28c6038`).
+5. Stage `pyproject.toml` and `uv.lock`, then commit with message: `Update mujoco-warp to `.
+6. Push the branch and open a PR with title `Update mujoco-warp to `.
+
+Important:
+- The commit hash is required. If `$ARGUMENTS` is empty, ask the user for a commit hash.
+- Do NOT modify anything else in `pyproject.toml`.
diff --git a/05_software/train/rc_mjlab/mjlab/.claude/settings.json b/05_software/train/rc_mjlab/mjlab/.claude/settings.json
new file mode 100644
index 0000000..6c51c58
--- /dev/null
+++ b/05_software/train/rc_mjlab/mjlab/.claude/settings.json
@@ -0,0 +1,33 @@
+{
+  "permissions": {
+    "allow": [
+      "Bash(make:*)",
+      "Bash(uv run:*)",
+      "Bash(uv lock:*)",
+      "Bash(uv sync:*)",
+      "Bash(uv add:*)",
+      "Bash(git:*)",
+      "Bash(gh:*)",
+      "WebSearch",
+      "Skill(commit-push-pr)",
+      "Skill(pr-review-toolkit:review-pr)"
+    ]
+  },
+  "hooks": {
+    "PostToolUse": [
+      {
+        "matcher": "Write|Edit",
+        "hooks": [
+          {
+            "type": "command",
+            "command": "uv run ruff format"
+          }
+        ]
+      }
+    ]
+  },
+  "enabledPlugins": {
+    "code-simplifier@claude-plugins-official": true,
+    "pr-review-toolkit@claude-plugins-official": true
+  }
+}
diff --git a/05_software/train/rc_mjlab/mjlab/.dockerignore b/05_software/train/rc_mjlab/mjlab/.dockerignore
new file mode 100644
index 0000000..986f193
--- /dev/null
+++ b/05_software/train/rc_mjlab/mjlab/.dockerignore
@@ -0,0 +1,34 @@
+# Large runtime directories
+.venv/
+logs/
+wandb/
+artifacts/
+benchmark_results/
+dist/
+
+# Build/cache
+__pycache__/
+*.pyc
+.ruff_cache/
+.pytest_cache/
+.uv-cache/
+*.egg-info/
+
+# Git/CI
+.git/
+.github/
+.gitignore
+.pre-commit-config.yaml
+
+# IDE/local
+.vscode/
+.claude/
+notebooks/
+
+# Docker
+Dockerfile
+.dockerignore
+
+# Docs build artifacts
+docs/source/_build/
+docs/source/generated/
diff --git a/05_software/train/rc_mjlab/mjlab/.github/workflows/ci.yml b/05_software/train/rc_mjlab/mjlab/.github/workflows/ci.yml
new file mode 100644
index 0000000..06fa858
--- /dev/null
+++ b/05_software/train/rc_mjlab/mjlab/.github/workflows/ci.yml
@@ -0,0 +1,95 @@
+name: tests
+
+on:
+  push:
+    branches: [main]
+    paths-ignore:
+      - '**.md'
+      - '**.rst'
+      - 'docs/**'
+      - 'Makefile'
+      - 'LICENSE'
+      - 'scripts/benchmarks/**'
+  pull_request:
+    branches: [main]
+    paths-ignore:
+      - '**.md'
+      - '**.rst'
+      - 'docs/**'
+      - 'Makefile'
+      - 'LICENSE'
+      - 'scripts/benchmarks/**'
+
+env:
+  UV_FROZEN: "1"
+
+jobs:
+  lint-format:
+    runs-on: ubuntu-latest
+    steps:
+      - name: Checkout repository
+        uses: actions/checkout@v4
+      - name: Setup uv
+        uses: astral-sh/setup-uv@v6
+        with:
+          enable-cache: true
+          version: "0.9.27"
+      - name: Run lint
+        run: uvx ruff@0.14.14 check --diff
+      - name: Run format
+        run: uvx ruff@0.14.14 format --diff
+
+  tests:
+    runs-on: ubuntu-latest
+    strategy:
+      matrix:
+        python-version: ["3.10", "3.11", "3.12", "3.13"]
+    steps:
+      - uses: actions/checkout@v4
+      - name: Setup uv
+        uses: astral-sh/setup-uv@v6
+        with:
+          python-version: ${{ matrix.python-version }}
+          enable-cache: true
+          version: "0.9.27"
+      - name: Restore Warp kernel cache
+        uses: actions/cache@v4
+        with:
+          path: ~/.cache/warp
+          key: warp-kernels-${{ runner.os }}-${{ runner.arch }}-${{ matrix.python-version }}-${{ hashFiles('uv.lock', 'mjlab/**/*.py') }}
+          restore-keys: |
+            warp-kernels-${{ runner.os }}-${{ runner.arch }}-${{ matrix.python-version }}-
+      - name: Test with python ${{ matrix.python-version }}
+        run: uv run --extra cpu pytest
+
+  pyright:
+    runs-on: ubuntu-latest
+    strategy:
+      matrix:
+        python-version: ["3.10", "3.11", "3.12", "3.13"]
+    steps:
+      - uses: actions/checkout@v4
+      - name: Setup uv
+        uses: astral-sh/setup-uv@v6
+        with:
+          python-version: ${{ matrix.python-version }}
+          enable-cache: true
+          version: "0.9.27"
+      - name: Test with python ${{ matrix.python-version }}
+        run: uv run --extra cpu pyright
+
+  ty-check:
+    runs-on: ubuntu-latest
+    strategy:
+      matrix:
+        python-version: ["3.10", "3.11", "3.12", "3.13"]
+    steps:
+      - uses: actions/checkout@v4
+      - name: Setup uv
+        uses: astral-sh/setup-uv@v6
+        with:
+          python-version: ${{ matrix.python-version }}
+          enable-cache: true
+          version: "0.9.27"
+      - name: Type check with python ${{ matrix.python-version }}
+        run: uv run --extra cpu ty check
diff --git a/05_software/train/rc_mjlab/mjlab/.github/workflows/docker.yml b/05_software/train/rc_mjlab/mjlab/.github/workflows/docker.yml
new file mode 100644
index 0000000..b1fcf51
--- /dev/null
+++ b/05_software/train/rc_mjlab/mjlab/.github/workflows/docker.yml
@@ -0,0 +1,91 @@
+name: Docker
+
+on:
+  workflow_dispatch:
+
+  push:
+    branches:
+      - "main"
+
+  pull_request:
+    types:
+      - opened
+      - reopened
+      - synchronize
+      - ready_for_review
+
+concurrency:
+  group: docker-${{ github.ref }}
+  cancel-in-progress: true
+
+defaults:
+  run:
+    shell: bash
+
+env:
+  FORCE_COLOR: 1
+  REGISTRY: ghcr.io
+  IMAGE_NAME: mujocolab/mjlab
+
+permissions:
+  id-token: write
+  packages: write
+
+jobs:
+  check_paths:
+    runs-on: ubuntu-22.04
+    outputs:
+      build: ${{ steps.filter.outputs.any }}
+    steps:
+      - uses: actions/checkout@v6
+      - id: filter
+        uses: dorny/paths-filter@v3
+        with:
+          list-files: shell
+          filters: |
+            any:
+              - ".github/workflows/docker.yml"
+              - "Dockerfile"
+
+  build:
+    needs: check_paths
+    if: ${{ needs.check_paths.outputs.build == 'true' }}
+    runs-on: ubuntu-22.04
+
+    steps:
+      - name: Checkout repo
+        uses: actions/checkout@v6
+
+      - name: Setup Docker buildx
+        uses: docker/setup-buildx-action@v3
+
+      - name: Log into registry
+        uses: docker/login-action@v3
+        with:
+          registry: ${{ env.REGISTRY }}
+          username: ${{ github.actor }}
+          password: ${{ secrets.GITHUB_TOKEN }}
+
+      - name: Extract Docker metadata
+        id: meta
+        uses: docker/metadata-action@v5
+        with:
+          images: ${{ env.REGISTRY }}/${{ env.IMAGE_NAME }}
+          tags: |
+            type=ref,event=branch
+            type=raw,value=latest,enable={{is_default_branch}}
+
+      - name: Build and push Docker image
+        uses: docker/build-push-action@v6
+        with:
+          context: .
+          push: ${{ github.ref == 'refs/heads/main' }}
+          tags: ${{ steps.meta.outputs.tags }}
+          labels: ${{ steps.meta.outputs.labels }}
+          cache-from: |
+            type=gha
+            type=registry,ref=ghcr.io/mujocolab/mjlab/mjlab:buildcache
+          cache-to: |
+            type=gha,mode=max
+            type=registry,ref=ghcr.io/mujocolab/mjlab/mjlab:buildcache,mode=max
+          platforms: linux/amd64
diff --git a/05_software/train/rc_mjlab/mjlab/.github/workflows/docs.yml b/05_software/train/rc_mjlab/mjlab/.github/workflows/docs.yml
new file mode 100644
index 0000000..cd140b0
--- /dev/null
+++ b/05_software/train/rc_mjlab/mjlab/.github/workflows/docs.yml
@@ -0,0 +1,45 @@
+name: docs
+
+on:
+  push:
+    branches:
+      - main
+    tags:
+      - 'v*'
+
+permissions:
+  contents: write
+
+env:
+  UV_FROZEN: "1"
+
+jobs:
+  build:
+    runs-on: ubuntu-latest
+    steps:
+      - uses: actions/checkout@v4
+        with:
+          fetch-depth: 0
+
+      - uses: actions/setup-python@v5
+        with:
+          python-version: '3.13'
+
+      - name: Install uv
+        uses: astral-sh/setup-uv@v4
+
+      - name: Build Sphinx Documentation
+        run: uv run --group docs sphinx-multiversion docs docs/_build
+
+      - name: Add root redirect
+        run: echo '' > docs/_build/index.html
+
+      - name: Remove Sphinx build artifacts
+        run: find docs/_build -type d -name .doctrees -exec rm -rf {} +
+
+      - name: Deploy to GitHub Pages
+        uses: peaceiris/actions-gh-pages@v4
+        with:
+          github_token: ${{ secrets.GITHUB_TOKEN }}
+          publish_dir: ./docs/_build/
+          keep_files: true
diff --git a/05_software/train/rc_mjlab/mjlab/.github/workflows/release.yml b/05_software/train/rc_mjlab/mjlab/.github/workflows/release.yml
new file mode 100644
index 0000000..7b6da9b
--- /dev/null
+++ b/05_software/train/rc_mjlab/mjlab/.github/workflows/release.yml
@@ -0,0 +1,30 @@
+name: "Publish"
+
+on:
+  push:
+    tags:
+      - v*
+
+jobs:
+  run:
+    runs-on: ubuntu-latest
+    environment:
+      name: pypi
+    permissions:
+      id-token: write
+      contents: read
+    steps:
+      - name: Checkout
+        uses: actions/checkout@v4
+      - name: Install uv
+        uses: astral-sh/setup-uv@v6
+      - name: Install Python 3.13
+        run: uv python install 3.13
+      - name: Build
+        run: uv build
+      - name: Smoke test (wheel)
+        run: uv run --isolated --no-project --with dist/*.whl tests/smoke_test.py
+      - name: Smoke test (source distribution)
+        run: uv run --isolated --no-project --with dist/*.tar.gz tests/smoke_test.py
+      - name: Publish
+        run: uv publish
diff --git a/05_software/train/rc_mjlab/mjlab/.gitignore b/05_software/train/rc_mjlab/mjlab/.gitignore
new file mode 100644
index 0000000..4144e2b
--- /dev/null
+++ b/05_software/train/rc_mjlab/mjlab/.gitignore
@@ -0,0 +1,19 @@
+wandb/
+logs/
+onnx/
+videos/
+__pycache__/
+MUJOCO_LOG.TXT
+debug.py
+.vscode/
+*.ipynb_checkpoints/
+motions/
+*_rerun*
+artifacts/
+.venv/
+render_robots.py
+benchmark_results/
+
+# Documentation outputs.
+**/_build/*
+**/generated/*
diff --git a/05_software/train/rc_mjlab/mjlab/.pre-commit-config.yaml b/05_software/train/rc_mjlab/mjlab/.pre-commit-config.yaml
new file mode 100644
index 0000000..f0c2dbf
--- /dev/null
+++ b/05_software/train/rc_mjlab/mjlab/.pre-commit-config.yaml
@@ -0,0 +1,10 @@
+repos:
+- repo: https://github.com/astral-sh/ruff-pre-commit
+  # Ruff version.
+  rev: v0.14.14
+  hooks:
+    # Run the linter.
+    - id: ruff-check
+      args: [ --fix ]
+    # Run the formatter.
+    - id: ruff-format
diff --git a/05_software/train/rc_mjlab/mjlab/.python-version b/05_software/train/rc_mjlab/mjlab/.python-version
new file mode 100644
index 0000000..24ee5b1
--- /dev/null
+++ b/05_software/train/rc_mjlab/mjlab/.python-version
@@ -0,0 +1 @@
+3.13
diff --git a/05_software/train/rc_mjlab/mjlab/AGENTS.md b/05_software/train/rc_mjlab/mjlab/AGENTS.md
new file mode 100644
index 0000000..681311e
--- /dev/null
+++ b/05_software/train/rc_mjlab/mjlab/AGENTS.md
@@ -0,0 +1 @@
+CLAUDE.md
\ No newline at end of file
diff --git a/05_software/train/rc_mjlab/mjlab/CITATION.cff b/05_software/train/rc_mjlab/mjlab/CITATION.cff
new file mode 100644
index 0000000..f506d30
--- /dev/null
+++ b/05_software/train/rc_mjlab/mjlab/CITATION.cff
@@ -0,0 +1,60 @@
+# This CITATION.cff file was generated with cffinit.
+# Visit https://bit.ly/cffinit to generate yours today!
+
+cff-version: 1.2.0
+title: >-
+  mjlab: A Lightweight Framework for GPU-Accelerated Robot Learning
+message: >-
+  If you use this software, please cite it using the
+  metadata from this file.
+type: software
+authors:
+  - given-names: Kevin
+    family-names: Zakka
+    email: zakka@berkeley.edu
+  - given-names: Brent
+    family-names: Yi
+    email: brentyi@berkeley.edu
+  - given-names: Qiayuan
+    family-names: Liao
+    email: qiayuanl@berkeley.edu
+  - given-names: Louis
+    family-names: Le Lay
+    email: le.lay.louis@gmail.com
+  - given-names: Koushil
+    family-names: Sreenath
+  - given-names: Pieter
+    family-names: Abbeel
+repository-code: 'https://github.com/mujocolab/mjlab'
+keywords:
+  - mujoco
+  - mujoco-warp
+  - simulation
+  - reinforcement-learning
+  - robotics
+license: Apache-2.0
+commit: e2f33c6fb49caa26ec11f7b2de3c0c9aba71e9fd
+version: 1.3.0
+date-released: '2026-04-14'
+preferred-citation:
+  type: article
+  title: >-
+    mjlab: A Lightweight Framework for GPU-Accelerated Robot Learning
+  authors:
+    - given-names: Kevin
+      family-names: Zakka
+    - given-names: Qiayuan
+      family-names: Liao
+    - given-names: Brent
+      family-names: Yi
+    - given-names: Louis
+      family-names: Le Lay
+    - given-names: Koushil
+      family-names: Sreenath
+    - given-names: Pieter
+      family-names: Abbeel
+  year: 2026
+  url: https://arxiv.org/abs/2601.22074
+  identifiers:
+    - type: arxiv
+      value: 2601.22074
diff --git a/05_software/train/rc_mjlab/mjlab/CLAUDE.md b/05_software/train/rc_mjlab/mjlab/CLAUDE.md
new file mode 100644
index 0000000..ac4b1db
--- /dev/null
+++ b/05_software/train/rc_mjlab/mjlab/CLAUDE.md
@@ -0,0 +1,60 @@
+# Development Workflow
+
+**Always use `uv run`, not python**.
+
+```sh
+
+# 1. Make changes.
+
+# 2. Type check.
+uv run ty check  # Fast
+uv run pyright  # More thorough, but slower
+
+# 3. Run tests.
+uv run pytest tests/  # Single suite
+uv run pytest tests/.py  # Specific file
+
+# 4. Format and lint before committing.
+uv run ruff format
+uv run ruff check --fix
+```
+
+We've bundled common commands into a Makefile for convenience.
+
+```sh
+make format     # Format and lint
+make type       # Type-check
+make check      # make format && make type
+make test-fast  # Run tests excluding slow ones
+make test       # Run the full test suite
+make docs       # Build documentation
+```
+
+Always run `make check` before committing. This runs formatting, linting,
+and type checking. Do not commit code that fails type checking.
+
+Before creating a PR, ensure all checks pass with `make test`.
+
+When making user-facing changes, add an entry to `docs/source/changelog.rst`
+under the "Upcoming version (not yet released)" section using
+Added/Changed/Fixed categories. Reference issues with `:issue:\`123\``
+(renders as a link to the GitHub issue).
+
+# Commits and PRs
+
+- Put `Fixes #` at the end of the commit message body, not in
+  the title.
+- PR body should be plain, concise prose. No section headers, checklists,
+  or structured templates. Describe the problem, what the change does, and
+  any non-obvious tradeoffs. A good PR description reads like a short
+  paragraph to a colleague, not a form.
+- PR and commit messages are rendered on GitHub, so don't hard-wrap them
+  at 88 columns. Let each sentence flow on one line.
+
+Some style guidelines to follow:
+- Line length limit is 88 columns. This applies to code, comments, and docstrings.
+- Avoid local imports unless they are strictly necessary (e.g. circular imports).
+- Tests should follow these principles:
+  - Use functions and fixtures; do not use test classes.
+  - Favor targeted, efficient tests over exhaustive edge-case coverage.
+  - Prefer running individual tests rather than the full test suite to improve iteration speed.
diff --git a/05_software/train/rc_mjlab/mjlab/CONTRIBUTING.md b/05_software/train/rc_mjlab/mjlab/CONTRIBUTING.md
new file mode 100644
index 0000000..4fec317
--- /dev/null
+++ b/05_software/train/rc_mjlab/mjlab/CONTRIBUTING.md
@@ -0,0 +1,25 @@
+# Contributing
+
+Bug fixes and documentation improvements are always welcome. For new features, please open an issue first so we can discuss whether it fits and work out the design, as we're intentional about keeping the scope focused.
+
+## Workflow
+
+1. Fork the repository and create a feature branch.
+2. Make your changes.
+3. Ensure formatting, type checking, and tests pass: `make test-all`.
+4. Submit a pull request.
+
+Type checking (`make type`) is required, PRs that don't pass will be blocked. You can optionally install pre-commit hooks (`pre-commit install`) to catch issues early.
+
+## Changelog
+
+Add entries to the "Upcoming version" section in `docs/source/changelog.rst` under the appropriate category (Added / Changed / Fixed), following [Keep a Changelog](https://keepachangelog.com/) conventions.
+
+## Getting Help
+
+- **Issues**: https://github.com/mujocolab/mjlab/issues
+- **Discussions**: https://github.com/mujocolab/mjlab/discussions
+
+## License
+
+By contributing, you agree your contributions will be licensed under Apache 2.0.
\ No newline at end of file
diff --git a/05_software/train/rc_mjlab/mjlab/Dockerfile b/05_software/train/rc_mjlab/mjlab/Dockerfile
new file mode 100644
index 0000000..1db4e56
--- /dev/null
+++ b/05_software/train/rc_mjlab/mjlab/Dockerfile
@@ -0,0 +1,36 @@
+# Refer to uv-docker-example:
+# https://github.com/astral-sh/uv-docker-example/blob/main/standalone.Dockerfile
+# Note that we use uv to launch, so we omit the second half of the example (non-UV final image)
+
+FROM nvidia/cuda:12.8.0-runtime-ubuntu24.04
+COPY --from=ghcr.io/astral-sh/uv:latest /uv /uvx /bin/
+
+ENV DEBIAN_FRONTEND=noninteractive
+RUN apt-get update && apt-get install -y \
+    git \
+    curl \
+    libegl-dev \
+    && rm -rf /var/lib/apt/lists/*
+
+ENV UV_COMPILE_BYTECODE=1
+ENV UV_LINK_MODE=copy
+ENV UV_PYTHON_PREFERENCE=only-managed
+
+RUN uv python install 3.13
+
+WORKDIR /app
+
+RUN --mount=type=cache,target=/root/.cache/uv \
+    --mount=type=bind,source=uv.lock,target=uv.lock \
+    --mount=type=bind,source=pyproject.toml,target=pyproject.toml \
+    uv sync --locked --no-install-project --no-editable --no-dev
+
+ADD . /app
+
+RUN --mount=type=cache,target=/root/.cache/uv \
+    uv sync --locked --no-editable --no-dev
+
+ENV MUJOCO_GL=egl
+EXPOSE 8080
+
+CMD ["uv", "run", "python", "tests/smoke_test.py"]
diff --git a/05_software/train/rc_mjlab/mjlab/LICENSE b/05_software/train/rc_mjlab/mjlab/LICENSE
new file mode 100644
index 0000000..7eb574f
--- /dev/null
+++ b/05_software/train/rc_mjlab/mjlab/LICENSE
@@ -0,0 +1,202 @@
+
+                                 Apache License
+                           Version 2.0, January 2004
+                        http://www.apache.org/licenses/
+
+   TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION
+
+   1. Definitions.
+
+      "License" shall mean the terms and conditions for use, reproduction,
+      and distribution as defined by Sections 1 through 9 of this document.
+
+      "Licensor" shall mean the copyright owner or entity authorized by
+      the copyright owner that is granting the License.
+
+      "Legal Entity" shall mean the union of the acting entity and all
+      other entities that control, are controlled by, or are under common
+      control with that entity. For the purposes of this definition,
+      "control" means (i) the power, direct or indirect, to cause the
+      direction or management of such entity, whether by contract or
+      otherwise, or (ii) ownership of fifty percent (50%) or more of the
+      outstanding shares, or (iii) beneficial ownership of such entity.
+
+      "You" (or "Your") shall mean an individual or Legal Entity
+      exercising permissions granted by this License.
+
+      "Source" form shall mean the preferred form for making modifications,
+      including but not limited to software source code, documentation
+      source, and configuration files.
+
+      "Object" form shall mean any form resulting from mechanical
+      transformation or translation of a Source form, including but
+      not limited to compiled object code, generated documentation,
+      and conversions to other media types.
+
+      "Work" shall mean the work of authorship, whether in Source or
+      Object form, made available under the License, as indicated by a
+      copyright notice that is included in or attached to the work
+      (an example is provided in the Appendix below).
+
+      "Derivative Works" shall mean any work, whether in Source or Object
+      form, that is based on (or derived from) the Work and for which the
+      editorial revisions, annotations, elaborations, or other modifications
+      represent, as a whole, an original work of authorship. For the purposes
+      of this License, Derivative Works shall not include works that remain
+      separable from, or merely link (or bind by name) to the interfaces of,
+      the Work and Derivative Works thereof.
+
+      "Contribution" shall mean any work of authorship, including
+      the original version of the Work and any modifications or additions
+      to that Work or Derivative Works thereof, that is intentionally
+      submitted to Licensor for inclusion in the Work by the copyright owner
+      or by an individual or Legal Entity authorized to submit on behalf of
+      the copyright owner. For the purposes of this definition, "submitted"
+      means any form of electronic, verbal, or written communication sent
+      to the Licensor or its representatives, including but not limited to
+      communication on electronic mailing lists, source code control systems,
+      and issue tracking systems that are managed by, or on behalf of, the
+      Licensor for the purpose of discussing and improving the Work, but
+      excluding communication that is conspicuously marked or otherwise
+      designated in writing by the copyright owner as "Not a Contribution."
+
+      "Contributor" shall mean Licensor and any individual or Legal Entity
+      on behalf of whom a Contribution has been received by Licensor and
+      subsequently incorporated within the Work.
+
+   2. Grant of Copyright License. Subject to the terms and conditions of
+      this License, each Contributor hereby grants to You a perpetual,
+      worldwide, non-exclusive, no-charge, royalty-free, irrevocable
+      copyright license to reproduce, prepare Derivative Works of,
+      publicly display, publicly perform, sublicense, and distribute the
+      Work and such Derivative Works in Source or Object form.
+
+   3. Grant of Patent License. Subject to the terms and conditions of
+      this License, each Contributor hereby grants to You a perpetual,
+      worldwide, non-exclusive, no-charge, royalty-free, irrevocable
+      (except as stated in this section) patent license to make, have made,
+      use, offer to sell, sell, import, and otherwise transfer the Work,
+      where such license applies only to those patent claims licensable
+      by such Contributor that are necessarily infringed by their
+      Contribution(s) alone or by combination of their Contribution(s)
+      with the Work to which such Contribution(s) was submitted. If You
+      institute patent litigation against any entity (including a
+      cross-claim or counterclaim in a lawsuit) alleging that the Work
+      or a Contribution incorporated within the Work constitutes direct
+      or contributory patent infringement, then any patent licenses
+      granted to You under this License for that Work shall terminate
+      as of the date such litigation is filed.
+
+   4. Redistribution. You may reproduce and distribute copies of the
+      Work or Derivative Works thereof in any medium, with or without
+      modifications, and in Source or Object form, provided that You
+      meet the following conditions:
+
+      (a) You must give any other recipients of the Work or
+          Derivative Works a copy of this License; and
+
+      (b) You must cause any modified files to carry prominent notices
+          stating that You changed the files; and
+
+      (c) You must retain, in the Source form of any Derivative Works
+          that You distribute, all copyright, patent, trademark, and
+          attribution notices from the Source form of the Work,
+          excluding those notices that do not pertain to any part of
+          the Derivative Works; and
+
+      (d) If the Work includes a "NOTICE" text file as part of its
+          distribution, then any Derivative Works that You distribute must
+          include a readable copy of the attribution notices contained
+          within such NOTICE file, excluding those notices that do not
+          pertain to any part of the Derivative Works, in at least one
+          of the following places: within a NOTICE text file distributed
+          as part of the Derivative Works; within the Source form or
+          documentation, if provided along with the Derivative Works; or,
+          within a display generated by the Derivative Works, if and
+          wherever such third-party notices normally appear. The contents
+          of the NOTICE file are for informational purposes only and
+          do not modify the License. You may add Your own attribution
+          notices within Derivative Works that You distribute, alongside
+          or as an addendum to the NOTICE text from the Work, provided
+          that such additional attribution notices cannot be construed
+          as modifying the License.
+
+      You may add Your own copyright statement to Your modifications and
+      may provide additional or different license terms and conditions
+      for use, reproduction, or distribution of Your modifications, or
+      for any such Derivative Works as a whole, provided Your use,
+      reproduction, and distribution of the Work otherwise complies with
+      the conditions stated in this License.
+
+   5. Submission of Contributions. Unless You explicitly state otherwise,
+      any Contribution intentionally submitted for inclusion in the Work
+      by You to the Licensor shall be under the terms and conditions of
+      this License, without any additional terms or conditions.
+      Notwithstanding the above, nothing herein shall supersede or modify
+      the terms of any separate license agreement you may have executed
+      with Licensor regarding such Contributions.
+
+   6. Trademarks. This License does not grant permission to use the trade
+      names, trademarks, service marks, or product names of the Licensor,
+      except as required for reasonable and customary use in describing the
+      origin of the Work and reproducing the content of the NOTICE file.
+
+   7. Disclaimer of Warranty. Unless required by applicable law or
+      agreed to in writing, Licensor provides the Work (and each
+      Contributor provides its Contributions) on an "AS IS" BASIS,
+      WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
+      implied, including, without limitation, any warranties or conditions
+      of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A
+      PARTICULAR PURPOSE. You are solely responsible for determining the
+      appropriateness of using or redistributing the Work and assume any
+      risks associated with Your exercise of permissions under this License.
+
+   8. Limitation of Liability. In no event and under no legal theory,
+      whether in tort (including negligence), contract, or otherwise,
+      unless required by applicable law (such as deliberate and grossly
+      negligent acts) or agreed to in writing, shall any Contributor be
+      liable to You for damages, including any direct, indirect, special,
+      incidental, or consequential damages of any character arising as a
+      result of this License or out of the use or inability to use the
+      Work (including but not limited to damages for loss of goodwill,
+      work stoppage, computer failure or malfunction, or any and all
+      other commercial damages or losses), even if such Contributor
+      has been advised of the possibility of such damages.
+
+   9. Accepting Warranty or Additional Liability. While redistributing
+      the Work or Derivative Works thereof, You may choose to offer,
+      and charge a fee for, acceptance of support, warranty, indemnity,
+      or other liability obligations and/or rights consistent with this
+      License. However, in accepting such obligations, You may act only
+      on Your own behalf and on Your sole responsibility, not on behalf
+      of any other Contributor, and only if You agree to indemnify,
+      defend, and hold each Contributor harmless for any liability
+      incurred by, or claims asserted against, such Contributor by reason
+      of your accepting any such warranty or additional liability.
+
+   END OF TERMS AND CONDITIONS
+
+   APPENDIX: How to apply the Apache License to your work.
+
+      To apply the Apache License to your work, attach the following
+      boilerplate notice, with the fields enclosed by brackets "[]"
+      replaced with your own identifying information. (Don't include
+      the brackets!)  The text should be enclosed in the appropriate
+      comment syntax for the file format. We also recommend that a
+      file or class name and description of purpose be included on the
+      same "printed page" as the copyright notice for easier
+      identification within third-party archives.
+
+   Copyright 2025, The mjlab Developers
+
+   Licensed under the Apache License, Version 2.0 (the "License");
+   you may not use this file except in compliance with the License.
+   You may obtain a copy of the License at
+
+       http://www.apache.org/licenses/LICENSE-2.0
+
+   Unless required by applicable law or agreed to in writing, software
+   distributed under the License is distributed on an "AS IS" BASIS,
+   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+   See the License for the specific language governing permissions and
+   limitations under the License.
diff --git a/05_software/train/rc_mjlab/mjlab/Makefile b/05_software/train/rc_mjlab/mjlab/Makefile
new file mode 100644
index 0000000..7640c58
--- /dev/null
+++ b/05_software/train/rc_mjlab/mjlab/Makefile
@@ -0,0 +1,66 @@
+.PHONY: sync
+sync:
+	uv sync --all-extras --all-packages --group dev
+
+.PHONY: format
+format:
+	uv run ruff format
+	uv run ruff check --fix
+
+.PHONY: type
+type:
+	uv run ty check
+	uv run pyright
+
+.PHONY: check
+check: format type
+
+.PHONY: test
+test:
+	uv run pytest
+
+.PHONY: test-fast
+test-fast:
+	uv run pytest -m "not slow"
+
+.PHONY: test-cpu
+test-cpu:
+	FORCE_CPU=1 uv run pytest
+
+.PHONY: test-cpu-fast
+test-cpu-fast:
+	FORCE_CPU=1 uv run pytest -m "not slow"
+
+.PHONY: test-all
+test-all: check test
+
+.PHONY: build
+build:
+	uv build
+	uv run --isolated --no-project --with dist/*.whl tests/smoke_test.py
+	uv run --isolated --no-project --with dist/*.tar.gz tests/smoke_test.py
+	@echo "Build and import test successful"
+
+.PHONY: docs
+docs:
+	uv run --group docs sphinx-build -j auto docs docs/_build
+
+.PHONY: docs-multiversion
+docs-multiversion:
+	uv run --group docs sphinx-multiversion docs docs/_build
+
+.PHONY: docs-watch
+docs-watch:
+	uv run --group docs sphinx-autobuild -j auto docs docs/_build
+
+.PHONY: publish-test
+publish-test: build
+	uv publish --publish-url https://test.pypi.org/legacy/
+
+.PHONY: publish
+publish: build
+	uv publish
+
+.PHONY: docker-build
+docker-build:
+	docker build -t mjlab:latest .
diff --git a/05_software/train/rc_mjlab/mjlab/README.md b/05_software/train/rc_mjlab/mjlab/README.md
new file mode 100644
index 0000000..ea0ae1d
--- /dev/null
+++ b/05_software/train/rc_mjlab/mjlab/README.md
@@ -0,0 +1,140 @@
+![Project banner](https://raw.githubusercontent.com/mujocolab/mjlab/main/docs/source/_static/mjlab-banner.jpg)
+
+# mjlab
+
+[![GitHub Actions](https://img.shields.io/github/actions/workflow/status/mujocolab/mjlab/ci.yml?branch=main)](https://github.com/mujocolab/mjlab/actions/workflows/ci.yml?query=branch%3Amain)
+[![Documentation](https://github.com/mujocolab/mjlab/actions/workflows/docs.yml/badge.svg)](https://mujocolab.github.io/mjlab/)
+[![License](https://img.shields.io/github/license/mujocolab/mjlab)](https://github.com/mujocolab/mjlab/blob/main/LICENSE)
+[![Nightly Benchmarks](https://img.shields.io/badge/Nightly-Benchmarks-blue)](https://mujocolab.github.io/mjlab/nightly/)
+[![PyPI](https://img.shields.io/pypi/v/mjlab)](https://pypi.org/project/mjlab/)
+[![PyPI downloads](https://img.shields.io/pypi/dm/mjlab?color=blue)](https://pypistats.org/packages/mjlab)
+
+mjlab combines [Isaac Lab](https://github.com/isaac-sim/IsaacLab)'s manager-based API with [MuJoCo Warp](https://github.com/google-deepmind/mujoco_warp), a GPU-accelerated version of [MuJoCo](https://github.com/google-deepmind/mujoco).
+The framework provides composable building blocks for environment design,
+with minimal dependencies and direct access to native MuJoCo data structures.
+
+## Getting Started
+
+mjlab requires an NVIDIA GPU for training. macOS is supported for evaluation only.
+
+**Try it now:**
+
+Run the demo (no installation needed):
+
+```bash
+uvx --from mjlab --refresh demo
+```
+
+Or try in [Google Colab](https://colab.research.google.com/github/mujocolab/mjlab/blob/main/notebooks/demo.ipynb) (no local setup required).
+
+**Install from source:**
+
+```bash
+git clone https://github.com/mujocolab/mjlab.git && cd mjlab
+uv run demo
+```
+
+For alternative installation methods (PyPI, Docker), see the [Installation Guide](https://mujocolab.github.io/mjlab/main/source/installation.html).
+
+## Training Examples
+
+### 1. Velocity Tracking
+
+Train a Unitree G1 humanoid to follow velocity commands on flat terrain:
+
+```bash
+uv run train Mjlab-Velocity-Flat-Unitree-G1 --env.scene.num-envs 4096
+```
+
+**Multi-GPU Training:** Scale to multiple GPUs using `--gpu-ids`:
+
+```bash
+uv run train Mjlab-Velocity-Flat-Unitree-G1 \
+  --gpu-ids "[0, 1]" \
+  --env.scene.num-envs 4096
+```
+
+See the [Distributed Training guide](https://mujocolab.github.io/mjlab/main/source/training/distributed_training.html) for details.
+
+Evaluate a policy while training (fetches latest checkpoint from Weights & Biases):
+
+```bash
+uv run play Mjlab-Velocity-Flat-Unitree-G1 --wandb-run-path your-org/mjlab/run-id
+```
+
+### 2. Motion Imitation
+
+Train a humanoid to mimic reference motions. See the [motion imitation guide](https://mujocolab.github.io/mjlab/main/source/training/motion_imitation.html) for preprocessing setup.
+
+```bash
+uv run train Mjlab-Tracking-Flat-Unitree-G1 --registry-name your-org/motions/motion-name --env.scene.num-envs 4096
+uv run play Mjlab-Tracking-Flat-Unitree-G1 --wandb-run-path your-org/mjlab/run-id
+```
+
+### 3. Sanity-check with Dummy Agents
+
+Use built-in agents to sanity check your MDP before training:
+
+```bash
+uv run play Mjlab-Your-Task-Id --agent zero  # Sends zero actions
+uv run play Mjlab-Your-Task-Id --agent random  # Sends uniform random actions
+```
+
+When running motion-tracking tasks, add `--registry-name your-org/motions/motion-name` to the command.
+
+
+## Documentation
+
+Full documentation is available at **[mujocolab.github.io/mjlab](https://mujocolab.github.io/mjlab/)**.
+
+## Development
+
+```bash
+make test          # Run all tests
+make test-fast     # Skip slow tests
+make format        # Format and lint
+make docs          # Build docs locally
+```
+
+For development setup: `uvx pre-commit install`
+
+## Citation
+
+mjlab is used in published research and open-source robotics projects. See the [Research](https://mujocolab.github.io/mjlab/main/source/research.html) page for publications and projects, or share your own in [Show and Tell](https://github.com/mujocolab/mjlab/discussions/categories/show-and-tell).
+
+If you use mjlab in your research, please consider citing:
+
+```bibtex
+@misc{zakka2026mjlablightweightframeworkgpuaccelerated,
+  title={mjlab: A Lightweight Framework for GPU-Accelerated Robot Learning},
+  author={Kevin Zakka and Qiayuan Liao and Brent Yi and Louis Le Lay and Koushil Sreenath and Pieter Abbeel},
+  year={2026},
+  eprint={2601.22074},
+  archivePrefix={arXiv},
+  primaryClass={cs.RO},
+  url={https://arxiv.org/abs/2601.22074},
+}
+```
+
+## License
+
+mjlab is licensed under the [Apache License, Version 2.0](LICENSE).
+
+### Third-Party Code
+
+Some portions of mjlab are forked from external projects:
+
+- **`src/mjlab/utils/lab_api/`** — Utilities forked from [NVIDIA Isaac
+  Lab](https://github.com/isaac-sim/IsaacLab) (BSD-3-Clause license, see file
+  headers)
+
+Forked components retain their original licenses. See file headers for details.
+
+## Acknowledgments
+
+mjlab wouldn't exist without the excellent work of the Isaac Lab team, whose API
+design and abstractions mjlab builds upon.
+
+Thanks to the MuJoCo Warp team — especially Erik Frey and Taylor Howell — for
+answering our questions, giving helpful feedback, and implementing features
+based on our requests countless times.
diff --git a/05_software/train/rc_mjlab/mjlab/RELEASING.md b/05_software/train/rc_mjlab/mjlab/RELEASING.md
new file mode 100644
index 0000000..84a9b93
--- /dev/null
+++ b/05_software/train/rc_mjlab/mjlab/RELEASING.md
@@ -0,0 +1,76 @@
+# Releasing
+
+## Pre-release checklist
+
+1. Bump `version` in `pyproject.toml`.
+2. Update `version` and `date-released` in `CITATION.cff`.
+3. Update the "Upcoming version (not yet released)" heading in `docs/source/changelog.rst` to the new version number and date.
+4. Commit the version bump, then create an annotated tag:
+
+```sh
+git tag -a vX.Y.Z -m "Release vX.Y.Z"
+git push origin vX.Y.Z
+```
+
+## Build and verify
+
+Clean previous build artifacts, then build:
+
+```sh
+rm -rf dist/
+make build
+```
+
+This runs `uv build` to produce a wheel and sdist in `dist/`, then smoke-tests
+both artifacts in isolated environments.
+
+## Test on TestPyPI (optional but recommended)
+
+Upload to TestPyPI first to catch packaging issues before the real release:
+
+```sh
+UV_PUBLISH_TOKEN= make publish-test
+```
+
+Then verify the upload works end-to-end. Use `--index-strategy unsafe-best-match`
+because TestPyPI won't have all dependencies and uv needs to fall back to real
+PyPI for them:
+
+```sh
+uvx --extra-index-url https://test.pypi.org/simple/ \
+    --index-strategy unsafe-best-match \
+    --from mjlab \
+    demo
+```
+
+Note: TestPyPI requires a separate account and token from real PyPI.
+Generate one at https://test.pypi.org/manage/account/token/.
+
+## Publish to PyPI
+
+```sh
+UV_PUBLISH_TOKEN= make publish
+```
+
+Generate a token at https://pypi.org/manage/account/token/.
+
+## Post-release
+
+Verify the release installs and runs correctly. Use `--refresh` to bypass
+the `uvx` cache (which may still hold the TestPyPI version):
+
+```sh
+uvx --refresh --from mjlab demo
+```
+
+## Releasing from a past tag
+
+If the tag has already been created and HEAD has moved ahead, check out the
+tag before building:
+
+```sh
+git checkout vX.Y.Z
+make build
+make publish
+git checkout main
+```
diff --git a/05_software/train/rc_mjlab/mjlab/docs/_templates/versioning.html b/05_software/train/rc_mjlab/mjlab/docs/_templates/versioning.html
new file mode 100644
index 0000000..8c7af90
--- /dev/null
+++ b/05_software/train/rc_mjlab/mjlab/docs/_templates/versioning.html
@@ -0,0 +1,13 @@
+{% if versions %}
+

+  
+  
+

+{% endif %}
diff --git a/05_software/train/rc_mjlab/mjlab/docs/conf.py b/05_software/train/rc_mjlab/mjlab/docs/conf.py
new file mode 100644
index 0000000..897323c
--- /dev/null
+++ b/05_software/train/rc_mjlab/mjlab/docs/conf.py
@@ -0,0 +1,200 @@
+import os
+import sys
+
+import sphinx_book_theme
+
+sys.path.insert(0, os.path.abspath("../src"))
+sys.path.insert(0, os.path.abspath("../src/mjlab"))
+
+
+project = "mjlab"
+copyright = "2025, The mjlab Developers"
+author = "The mjlab Developers"
+
+extensions = [
+  "sphinx.ext.autodoc",
+  "sphinx.ext.autosummary",
+  "autodocsumm",
+  "myst_parser",
+  "sphinx.ext.napoleon",
+  "sphinxemoji.sphinxemoji",
+  "sphinx.ext.intersphinx",
+  "sphinx.ext.mathjax",
+  "sphinx.ext.todo",
+  "sphinx.ext.viewcode",
+  "sphinxcontrib.bibtex",
+  "sphinxcontrib.icon",
+  "sphinx_copybutton",
+  "sphinx_design",
+  "sphinx_tabs.tabs",
+  "sphinx_multiversion",
+  "sphinx.ext.extlinks",
+]
+
+extlinks = {
+  "issue": (
+    "https://github.com/mujocolab/mjlab/issues/%s",
+    "#%s",
+  ),
+}
+
+mathjax3_config = {
+  "tex": {
+    "inlineMath": [["\\(", "\\)"]],
+    "displayMath": [["\\[", "\\]"]],
+  },
+}
+
+panels_add_bootstrap_css = False
+panels_add_fontawesome_css = True
+
+source_suffix = {
+  ".rst": "restructuredtext",
+  ".md": "markdown",
+}
+
+nitpick_ignore = [
+  ("py:obj", "slice(None)"),
+]
+
+nitpick_ignore_regex = [
+  (r"py:.*", r"pxr.*"),
+  (r"py:.*", r"trimesh.*"),
+]
+
+# emoji style
+sphinxemoji_style = "twemoji"
+autodoc_typehints = "signature"
+autoclass_content = "class"
+autodoc_class_signature = "separated"
+autodoc_member_order = "bysource"
+autodoc_inherit_docstrings = True
+bibtex_bibfiles = ["source/_static/refs.bib"]
+autosummary_generate = True
+autosummary_generate_overwrite = False
+autodoc_default_options = {
+  "member-order": "bysource",
+}
+intersphinx_mapping = {
+  "python": ("https://docs.python.org/3", None),
+}
+
+exclude_patterns = [
+  "_build",
+  "_redirect",
+  "_templates",
+  "Thumbs.db",
+  ".DS_Store",
+  "README.md",
+  "licenses/*",
+]
+
+autodoc_mock_imports = [
+  "matplotlib",
+  "scipy",
+  "carb",
+  "warp",
+  "pxr",
+  "h5py",
+  "hid",
+  "prettytable",
+  "tqdm",
+  "tensordict",
+  "trimesh",
+  "toml",
+  "mjviser",
+  "mujoco_warp",
+  "gymnasium",
+  "rsl_rl",
+  "viser",
+  "wandb",
+  "torchvision",
+]
+
+suppress_warnings = [
+  "ref.python",
+  "docutils",
+]
+
+language = "en"
+
+html_title = "mjlab Documentation"
+html_theme_path = [sphinx_book_theme.get_html_theme_path()]
+html_theme = "sphinx_book_theme"
+html_favicon = "source/_static/favicon.ico"
+html_show_copyright = True
+html_show_sphinx = False
+html_last_updated_fmt = ""
+
+html_static_path = ["source/_static"]
+html_css_files = ["css/custom.css"]
+
+html_theme_options = {
+  "path_to_docs": "docs/",
+  "collapse_navigation": True,
+  "repository_url": "https://github.com/mujocolab/mjlab",
+  "use_repository_button": True,
+  "use_issues_button": True,
+  "use_edit_page_button": True,
+  "show_toc_level": 2,
+  "use_sidenotes": True,
+  "logo": {
+    "text": "mjlab Documentation",
+  },
+  "icon_links": [
+    {
+      "name": "Benchmarks",
+      "url": "https://mujocolab.github.io/mjlab/nightly/",
+      "icon": "fa-solid fa-chart-line",
+      "type": "fontawesome",
+    },
+  ],
+  "icon_links_label": "Quick Links",
+}
+
+templates_path = [
+  "_templates",
+]
+
+smv_remote_whitelist = r"^.*$"
+smv_branch_whitelist = os.getenv("SMV_BRANCH_WHITELIST", r"^(main|devel)$")
+smv_tag_whitelist = os.getenv("SMV_TAG_WHITELIST", r"^v[1-9]\d*\.\d+\.\d+$")
+
+html_sidebars = {
+  "**": [
+    "navbar-logo.html",
+    "search-field.html",
+    "versioning.html",
+    "sbt-sidebar-nav.html",
+  ]
+}
+
+
+def skip_member(app, what, name, obj, skip, options):
+  exclusions = ["from_dict", "to_dict", "replace", "copy", "validate", "__post_init__"]
+  if name in exclusions:
+    return True
+  return None
+
+
+def process_signature(app, what, name, obj, options, signature, return_annotation):
+  """Suppress the ugly __init__ signature for dataclass Cfg classes."""
+  if what == "class" and "exclude-members" in options:
+    if "__init__" in options["exclude-members"]:
+      return ("", None)
+  return None
+
+
+def process_docstring(app, what, name, obj, options, lines):
+  """Strip auto-generated dataclass docstrings (e.g. 'ClassName(*, ...)')."""
+  import dataclasses
+
+  if what == "class" and dataclasses.is_dataclass(obj):
+    if lines and lines[0].startswith(f"{obj.__name__}("):
+      lines.clear()
+
+
+def setup(app):
+  app.connect("autodoc-skip-member", skip_member)
+  app.connect("autodoc-process-signature", process_signature)
+  app.connect("autodoc-process-docstring", process_docstring)
diff --git a/05_software/train/rc_mjlab/mjlab/docs/index.rst b/05_software/train/rc_mjlab/mjlab/docs/index.rst
new file mode 100644
index 0000000..e6123f5
--- /dev/null
+++ b/05_software/train/rc_mjlab/mjlab/docs/index.rst
@@ -0,0 +1,128 @@
+Welcome to mjlab!
+=================
+
+.. figure:: source/_static/mjlab-banner.jpg
+   :width: 100%
+   :alt: mjlab
+
+mjlab is a lightweight, open-source framework for robot learning that
+combines GPU-accelerated simulation with composable environments and minimal
+setup friction. It adopts the manager-based API introduced by
+`Isaac Lab `_, where users compose
+modular building blocks for observations, rewards, and events, and pairs it
+with `MuJoCo Warp `_ for
+GPU-accelerated physics. The result is a framework installable with a single
+command, requiring minimal dependencies, and providing direct access to
+native `MuJoCo `_ data
+structures.
+
+**Key features:**
+
+- **Composable environments:** users define observations, rewards,
+  terminations, and other MDP terms as modular building blocks
+- **Minimal dependencies:** single-command install via ``uv``, low startup
+  latency
+- **Direct MuJoCo data structures:** native ``MjModel``/``MjData`` access
+  with no translation layers
+- **PyTorch-native:** observations, rewards, and actions are PyTorch
+  tensors backed by zero-copy GPU memory sharing
+
+For more on the design decisions behind mjlab, see :doc:`source/motivation`.
+
+**Try it now** (no installation needed):
+
+.. code-block:: bash
+
+   uvx --from mjlab --refresh demo
+
+Table of Contents
+-----------------
+
+.. toctree::
+   :maxdepth: 1
+   :caption: User Guide
+
+   source/installation
+   source/tutorials
+   source/contributing
+
+.. toctree::
+   :maxdepth: 1
+   :caption: Concepts
+
+   source/architecture_overview
+   source/entity/index
+   source/actuators
+   source/sensors/index
+   source/scene
+   source/terrain
+
+.. toctree::
+   :maxdepth: 1
+   :caption: The Manager Layer
+
+   source/environment_config
+   source/observations
+   source/actions
+   source/rewards
+   source/terminations
+   source/commands
+   source/events
+   source/randomization
+   source/curriculum
+   source/metrics
+   source/recorders
+
+.. toctree::
+   :maxdepth: 1
+   :caption: Training & Debugging
+
+   source/training/rsl_rl
+   source/viewers
+   source/training/distributed_training
+   source/training/cloud
+   source/debugging/nan_guard
+   source/debugging/export_scene
+
+.. toctree::
+   :maxdepth: 2
+   :caption: API Reference
+
+   source/api/index
+
+.. toctree::
+   :maxdepth: 1
+   :caption: Further Reading
+
+   source/motivation
+   source/migration_isaac_lab
+   source/faq
+   source/research
+   source/changelog
+
+License & citation
+------------------
+
+mjlab is licensed under the Apache License, Version 2.0.
+Please refer to the `LICENSE file `_ for details.
+
+If you use mjlab in your research, we would appreciate a citation:
+
+.. code-block:: bibtex
+
+    @article{Zakka_mjlab_A_Lightweight_2026,
+        author = {Zakka, Kevin and Liao, Qiayuan and Yi, Brent and Le Lay, Louis and Sreenath, Koushil and Abbeel, Pieter},
+        title = {{mjlab: A Lightweight Framework for GPU-Accelerated Robot Learning}},
+        url = {https://arxiv.org/abs/2601.22074},
+        year = {2026}
+    }
+
+Acknowledgments
+---------------
+
+mjlab would not exist without the excellent work of the Isaac Lab team, whose API design
+and abstractions mjlab builds upon.
+
+Thanks also to the MuJoCo Warp team — especially Erik Frey and Taylor Howell — for
+answering our questions, giving helpful feedback, and implementing features based
+on our requests countless times.
diff --git a/05_software/train/rc_mjlab/mjlab/docs/source/_static/architecture_diagram.png b/05_software/train/rc_mjlab/mjlab/docs/source/_static/architecture_diagram.png
new file mode 100644
index 0000000..b6e8ac8
Binary files /dev/null and b/05_software/train/rc_mjlab/mjlab/docs/source/_static/architecture_diagram.png differ
diff --git a/05_software/train/rc_mjlab/mjlab/docs/source/_static/changelog/native_reward.png b/05_software/train/rc_mjlab/mjlab/docs/source/_static/changelog/native_reward.png
new file mode 100644
index 0000000..0d0c572
Binary files /dev/null and b/05_software/train/rc_mjlab/mjlab/docs/source/_static/changelog/native_reward.png differ
diff --git a/05_software/train/rc_mjlab/mjlab/docs/source/_static/changelog/terrain_visualizer.jpg b/05_software/train/rc_mjlab/mjlab/docs/source/_static/changelog/terrain_visualizer.jpg
new file mode 100644
index 0000000..03a29f1
Binary files /dev/null and b/05_software/train/rc_mjlab/mjlab/docs/source/_static/changelog/terrain_visualizer.jpg differ
diff --git a/05_software/train/rc_mjlab/mjlab/docs/source/_static/content/cartpole-env.jpg b/05_software/train/rc_mjlab/mjlab/docs/source/_static/content/cartpole-env.jpg
new file mode 100644
index 0000000..d65b9fb
Binary files /dev/null and b/05_software/train/rc_mjlab/mjlab/docs/source/_static/content/cartpole-env.jpg differ
diff --git a/05_software/train/rc_mjlab/mjlab/docs/source/_static/content/cartpole_trained.gif b/05_software/train/rc_mjlab/mjlab/docs/source/_static/content/cartpole_trained.gif
new file mode 100644
index 0000000..0586ac5
Binary files /dev/null and b/05_software/train/rc_mjlab/mjlab/docs/source/_static/content/cartpole_trained.gif differ
diff --git a/05_software/train/rc_mjlab/mjlab/docs/source/_static/content/g1.png b/05_software/train/rc_mjlab/mjlab/docs/source/_static/content/g1.png
new file mode 100644
index 0000000..52daa42
Binary files /dev/null and b/05_software/train/rc_mjlab/mjlab/docs/source/_static/content/g1.png differ
diff --git a/05_software/train/rc_mjlab/mjlab/docs/source/_static/content/go1.png b/05_software/train/rc_mjlab/mjlab/docs/source/_static/content/go1.png
new file mode 100644
index 0000000..b1f5eb1
Binary files /dev/null and b/05_software/train/rc_mjlab/mjlab/docs/source/_static/content/go1.png differ
diff --git a/05_software/train/rc_mjlab/mjlab/docs/source/_static/content/mjlab-banner.jpg b/05_software/train/rc_mjlab/mjlab/docs/source/_static/content/mjlab-banner.jpg
new file mode 100644
index 0000000..eed1fc7
Binary files /dev/null and b/05_software/train/rc_mjlab/mjlab/docs/source/_static/content/mjlab-banner.jpg differ
diff --git a/05_software/train/rc_mjlab/mjlab/docs/source/_static/content/nan_debug.gif b/05_software/train/rc_mjlab/mjlab/docs/source/_static/content/nan_debug.gif
new file mode 100644
index 0000000..22814cd
Binary files /dev/null and b/05_software/train/rc_mjlab/mjlab/docs/source/_static/content/nan_debug.gif differ
diff --git a/05_software/train/rc_mjlab/mjlab/docs/source/_static/content/rough_terrain.png b/05_software/train/rc_mjlab/mjlab/docs/source/_static/content/rough_terrain.png
new file mode 100644
index 0000000..8af9eb6
Binary files /dev/null and b/05_software/train/rc_mjlab/mjlab/docs/source/_static/content/rough_terrain.png differ
diff --git a/05_software/train/rc_mjlab/mjlab/docs/source/_static/content/yam.png b/05_software/train/rc_mjlab/mjlab/docs/source/_static/content/yam.png
new file mode 100644
index 0000000..4cc444d
Binary files /dev/null and b/05_software/train/rc_mjlab/mjlab/docs/source/_static/content/yam.png differ
diff --git a/05_software/train/rc_mjlab/mjlab/docs/source/_static/css/custom.css b/05_software/train/rc_mjlab/mjlab/docs/source/_static/css/custom.css
new file mode 100644
index 0000000..2a1adc8
--- /dev/null
+++ b/05_software/train/rc_mjlab/mjlab/docs/source/_static/css/custom.css
@@ -0,0 +1,172 @@
+/*
+ * PyData Sphinx Theme — Option A (Indigo/Teal)
+ * Aesthetic: modern lab — indigo primary, teal accent, neutral grays
+ */
+
+/* LIGHT THEME */
+html[data-theme="light"] {
+    /* Brand */
+    --pst-color-primary: #4F46E5;
+    /* Indigo-600 */
+    --pst-color-secondary: #14B8A6;
+    /* Teal-500 */
+    --pst-color-secondary-highlight: #2DD4BF;
+    /* Teal-400 */
+
+    /* Links / code links */
+    --pst-color-inline-code-links: #0D9488;
+    /* Teal-600 */
+    --pst-color-link: var(--pst-color-primary);
+    --pst-color-link-hover: #4338CA;
+    /* Indigo-700 */
+
+    /* Semantic */
+    --pst-color-info: var(--pst-color-secondary);
+    --pst-color-info-highlight: var(--pst-color-secondary);
+    --pst-color-info-bg: #D1FAE5;
+    /* Teal-50 */
+    --pst-color-attention: #F59E0B;
+    /* Amber-500 */
+    --pst-color-target: #EEF2FF;
+    /* Indigo-50 */
+
+    /* Text */
+    --pst-color-text-base: #1F2937;
+    /* Slate-800 */
+    --pst-color-text-muted: #6B7280;
+    /* Slate-500 */
+
+    /* Surfaces */
+    --pst-color-background: #FFFFFF;
+    --pst-color-on-background: #FFFFFF;
+    --pst-color-surface: #F3F4F6;
+    /* Gray-100 */
+    --pst-color-on-surface: #E5E7EB;
+    /* Gray-200 */
+    --pst-color-shadow: #D1D5DB;
+    --pst-color-border: #E5E7EB;
+
+    /* Inline code */
+    --pst-color-inline-code: #0D9488;
+    /* Teal-600 */
+
+    /* Tables / hovers */
+    --pst-color-table-row-hover-bg: #EEF2FF;
+    /* Indigo-50 */
+
+    /* Accent (sparingly) */
+    --pst-color-accent: #10B981;
+    /* Emerald-500 */
+}
+
+/* DARK THEME */
+html[data-theme="dark"] {
+    /* Brand */
+    --pst-color-primary: #A5B4FC;
+    /* Indigo-300/200 mix for readability */
+    --pst-color-secondary: #5EEAD4;
+    /* Teal-300 */
+    --pst-color-secondary-highlight: #2DD4BF;
+
+    /* Links / code links */
+    --pst-color-inline-code-links: #93C5FD;
+    /* Indigo-300 */
+    --pst-color-link: var(--pst-color-primary);
+    --pst-color-link-hover: #818CF8;
+    /* Indigo-400 */
+
+    /* Semantic */
+    --pst-color-info: var(--pst-color-secondary);
+    --pst-color-info-highlight: var(--pst-color-secondary);
+    --pst-color-info-bg: #042F2E;
+    /* Deep teal */
+    --pst-color-attention: #F59E0B;
+    --pst-color-target: #1B1C2A;
+    /* Indigo-tinted surface */
+
+    /* Text */
+    --pst-color-text-base: #E5E7EB;
+    /* Gray-200 */
+    --pst-color-text-muted: #9CA3AF;
+    /* Gray-400 */
+
+    /* Surfaces */
+    --pst-color-background: #0B0C10;
+    /* Deep graphite */
+    --pst-color-on-background: #12131A;
+    --pst-color-surface: #111827;
+    /* Slate-900 */
+    --pst-color-on-surface: #1F2937;
+    /* Slate-800 */
+    --pst-color-shadow: #0F172A;
+    --pst-color-border: #2A2D3A;
+
+    /* Inline code */
+    --pst-color-inline-code: #5EEAD4;
+    /* Teal-300 */
+
+    /* Tables / hovers */
+    --pst-color-table-row-hover-bg: #1B1C2A;
+
+    /* Accent */
+    --pst-color-accent: #34D399;
+    /* Emerald-400 */
+}
+
+/* General tweaks */
+a {
+    text-decoration: none !important;
+}
+
+.bd-header-announcement a,
+.bd-header-version-warning a {
+    color: #5EEAD4;
+}
+
+.form-control {
+    border-radius: 0 !important;
+    border: none !important;
+    outline: none !important;
+}
+
+.navbar-brand,
+.navbar-icon-links {
+    padding-top: 0rem !important;
+    padding-bottom: 0rem !important;
+}
+
+/* Version switcher */
+.sidebar-version-switcher {
+    display: flex;
+    align-items: center;
+    gap: 0.5rem;
+    padding: 0.4rem 1rem;
+    margin-bottom: 0.5rem;
+}
+
+.sidebar-version-label {
+    font-size: 0.8rem;
+    font-weight: 600;
+    color: var(--pst-color-text-muted);
+    white-space: nowrap;
+}
+
+.sidebar-version-select {
+    flex: 1;
+    font-size: 0.8rem;
+    padding: 0.25rem 0.5rem;
+    border: 1px solid var(--pst-color-border);
+    border-radius: 4px;
+    background: var(--pst-color-background);
+    color: var(--pst-color-text-base);
+    cursor: pointer;
+}
+
+.sidebar-version-select:hover {
+    border-color: var(--pst-color-primary);
+}
+
+/* Sidebar section spacing */
+.bd-sidebar .navbar-icon-links {
+    padding: 0 1rem 0.25rem !important;
+}
\ No newline at end of file
diff --git a/05_software/train/rc_mjlab/mjlab/docs/source/_static/dr_combined_rand.gif b/05_software/train/rc_mjlab/mjlab/docs/source/_static/dr_combined_rand.gif
new file mode 100644
index 0000000..6b1a99f
Binary files /dev/null and b/05_software/train/rc_mjlab/mjlab/docs/source/_static/dr_combined_rand.gif differ
diff --git a/05_software/train/rc_mjlab/mjlab/docs/source/_static/dr_pseudo_inertia.gif b/05_software/train/rc_mjlab/mjlab/docs/source/_static/dr_pseudo_inertia.gif
new file mode 100644
index 0000000..fa6f5be
Binary files /dev/null and b/05_software/train/rc_mjlab/mjlab/docs/source/_static/dr_pseudo_inertia.gif differ
diff --git a/05_software/train/rc_mjlab/mjlab/docs/source/_static/favicon.ico b/05_software/train/rc_mjlab/mjlab/docs/source/_static/favicon.ico
new file mode 100644
index 0000000..2f9ce5c
Binary files /dev/null and b/05_software/train/rc_mjlab/mjlab/docs/source/_static/favicon.ico differ
diff --git a/05_software/train/rc_mjlab/mjlab/docs/source/_static/ghost_visualization.png b/05_software/train/rc_mjlab/mjlab/docs/source/_static/ghost_visualization.png
new file mode 100644
index 0000000..370877b
Binary files /dev/null and b/05_software/train/rc_mjlab/mjlab/docs/source/_static/ghost_visualization.png differ
diff --git a/05_software/train/rc_mjlab/mjlab/docs/source/_static/mjlab-banner.jpg b/05_software/train/rc_mjlab/mjlab/docs/source/_static/mjlab-banner.jpg
new file mode 100644
index 0000000..eed1fc7
Binary files /dev/null and b/05_software/train/rc_mjlab/mjlab/docs/source/_static/mjlab-banner.jpg differ
diff --git a/05_software/train/rc_mjlab/mjlab/docs/source/_static/native_viewer.png b/05_software/train/rc_mjlab/mjlab/docs/source/_static/native_viewer.png
new file mode 100644
index 0000000..b99d592
Binary files /dev/null and b/05_software/train/rc_mjlab/mjlab/docs/source/_static/native_viewer.png differ
diff --git a/05_software/train/rc_mjlab/mjlab/docs/source/_static/pattern_grid.jpg b/05_software/train/rc_mjlab/mjlab/docs/source/_static/pattern_grid.jpg
new file mode 100644
index 0000000..a259ad1
Binary files /dev/null and b/05_software/train/rc_mjlab/mjlab/docs/source/_static/pattern_grid.jpg differ
diff --git a/05_software/train/rc_mjlab/mjlab/docs/source/_static/pattern_grid.mp4 b/05_software/train/rc_mjlab/mjlab/docs/source/_static/pattern_grid.mp4
new file mode 100644
index 0000000..65d4ae8
Binary files /dev/null and b/05_software/train/rc_mjlab/mjlab/docs/source/_static/pattern_grid.mp4 differ
diff --git a/05_software/train/rc_mjlab/mjlab/docs/source/_static/pattern_pinhole.mp4 b/05_software/train/rc_mjlab/mjlab/docs/source/_static/pattern_pinhole.mp4
new file mode 100644
index 0000000..10c12f0
Binary files /dev/null and b/05_software/train/rc_mjlab/mjlab/docs/source/_static/pattern_pinhole.mp4 differ
diff --git a/05_software/train/rc_mjlab/mjlab/docs/source/_static/ray_alignment_comparison.mp4 b/05_software/train/rc_mjlab/mjlab/docs/source/_static/ray_alignment_comparison.mp4
new file mode 100644
index 0000000..97c30b1
Binary files /dev/null and b/05_software/train/rc_mjlab/mjlab/docs/source/_static/ray_alignment_comparison.mp4 differ
diff --git a/05_software/train/rc_mjlab/mjlab/docs/source/_static/raycast_demo.mp4 b/05_software/train/rc_mjlab/mjlab/docs/source/_static/raycast_demo.mp4
new file mode 100644
index 0000000..c0ba6a2
Binary files /dev/null and b/05_software/train/rc_mjlab/mjlab/docs/source/_static/raycast_demo.mp4 differ
diff --git a/05_software/train/rc_mjlab/mjlab/docs/source/_static/refs.bib b/05_software/train/rc_mjlab/mjlab/docs/source/_static/refs.bib
new file mode 100644
index 0000000..e69de29
diff --git a/05_software/train/rc_mjlab/mjlab/docs/source/_static/terrains/box_flat.png b/05_software/train/rc_mjlab/mjlab/docs/source/_static/terrains/box_flat.png
new file mode 100644
index 0000000..c5ef0f1
Binary files /dev/null and b/05_software/train/rc_mjlab/mjlab/docs/source/_static/terrains/box_flat.png differ
diff --git a/05_software/train/rc_mjlab/mjlab/docs/source/_static/terrains/box_inverted_pyramid_stairs.png b/05_software/train/rc_mjlab/mjlab/docs/source/_static/terrains/box_inverted_pyramid_stairs.png
new file mode 100644
index 0000000..c0f4495
Binary files /dev/null and b/05_software/train/rc_mjlab/mjlab/docs/source/_static/terrains/box_inverted_pyramid_stairs.png differ
diff --git a/05_software/train/rc_mjlab/mjlab/docs/source/_static/terrains/box_narrow_beams.png b/05_software/train/rc_mjlab/mjlab/docs/source/_static/terrains/box_narrow_beams.png
new file mode 100644
index 0000000..f88f87f
Binary files /dev/null and b/05_software/train/rc_mjlab/mjlab/docs/source/_static/terrains/box_narrow_beams.png differ
diff --git a/05_software/train/rc_mjlab/mjlab/docs/source/_static/terrains/box_nested_rings.png b/05_software/train/rc_mjlab/mjlab/docs/source/_static/terrains/box_nested_rings.png
new file mode 100644
index 0000000..0c38834
Binary files /dev/null and b/05_software/train/rc_mjlab/mjlab/docs/source/_static/terrains/box_nested_rings.png differ
diff --git a/05_software/train/rc_mjlab/mjlab/docs/source/_static/terrains/box_open_stairs.png b/05_software/train/rc_mjlab/mjlab/docs/source/_static/terrains/box_open_stairs.png
new file mode 100644
index 0000000..9eff84d
Binary files /dev/null and b/05_software/train/rc_mjlab/mjlab/docs/source/_static/terrains/box_open_stairs.png differ
diff --git a/05_software/train/rc_mjlab/mjlab/docs/source/_static/terrains/box_pyramid_stairs.png b/05_software/train/rc_mjlab/mjlab/docs/source/_static/terrains/box_pyramid_stairs.png
new file mode 100644
index 0000000..c4d3eff
Binary files /dev/null and b/05_software/train/rc_mjlab/mjlab/docs/source/_static/terrains/box_pyramid_stairs.png differ
diff --git a/05_software/train/rc_mjlab/mjlab/docs/source/_static/terrains/box_random_grid.png b/05_software/train/rc_mjlab/mjlab/docs/source/_static/terrains/box_random_grid.png
new file mode 100644
index 0000000..681176f
Binary files /dev/null and b/05_software/train/rc_mjlab/mjlab/docs/source/_static/terrains/box_random_grid.png differ
diff --git a/05_software/train/rc_mjlab/mjlab/docs/source/_static/terrains/box_random_spread.png b/05_software/train/rc_mjlab/mjlab/docs/source/_static/terrains/box_random_spread.png
new file mode 100644
index 0000000..746edb5
Binary files /dev/null and b/05_software/train/rc_mjlab/mjlab/docs/source/_static/terrains/box_random_spread.png differ
diff --git a/05_software/train/rc_mjlab/mjlab/docs/source/_static/terrains/box_random_stairs.png b/05_software/train/rc_mjlab/mjlab/docs/source/_static/terrains/box_random_stairs.png
new file mode 100644
index 0000000..0b47f77
Binary files /dev/null and b/05_software/train/rc_mjlab/mjlab/docs/source/_static/terrains/box_random_stairs.png differ
diff --git a/05_software/train/rc_mjlab/mjlab/docs/source/_static/terrains/box_stepping_stones.png b/05_software/train/rc_mjlab/mjlab/docs/source/_static/terrains/box_stepping_stones.png
new file mode 100644
index 0000000..cf127a0
Binary files /dev/null and b/05_software/train/rc_mjlab/mjlab/docs/source/_static/terrains/box_stepping_stones.png differ
diff --git a/05_software/train/rc_mjlab/mjlab/docs/source/_static/terrains/box_tilted_grid.png b/05_software/train/rc_mjlab/mjlab/docs/source/_static/terrains/box_tilted_grid.png
new file mode 100644
index 0000000..385a576
Binary files /dev/null and b/05_software/train/rc_mjlab/mjlab/docs/source/_static/terrains/box_tilted_grid.png differ
diff --git a/05_software/train/rc_mjlab/mjlab/docs/source/_static/terrains/flat_patch_group.png b/05_software/train/rc_mjlab/mjlab/docs/source/_static/terrains/flat_patch_group.png
new file mode 100644
index 0000000..60a38d3
Binary files /dev/null and b/05_software/train/rc_mjlab/mjlab/docs/source/_static/terrains/flat_patch_group.png differ
diff --git a/05_software/train/rc_mjlab/mjlab/docs/source/_static/terrains/hf_discrete_obstacles.png b/05_software/train/rc_mjlab/mjlab/docs/source/_static/terrains/hf_discrete_obstacles.png
new file mode 100644
index 0000000..6d63726
Binary files /dev/null and b/05_software/train/rc_mjlab/mjlab/docs/source/_static/terrains/hf_discrete_obstacles.png differ
diff --git a/05_software/train/rc_mjlab/mjlab/docs/source/_static/terrains/hf_perlin_noise.png b/05_software/train/rc_mjlab/mjlab/docs/source/_static/terrains/hf_perlin_noise.png
new file mode 100644
index 0000000..15183f7
Binary files /dev/null and b/05_software/train/rc_mjlab/mjlab/docs/source/_static/terrains/hf_perlin_noise.png differ
diff --git a/05_software/train/rc_mjlab/mjlab/docs/source/_static/terrains/hf_pyramid_slope.png b/05_software/train/rc_mjlab/mjlab/docs/source/_static/terrains/hf_pyramid_slope.png
new file mode 100644
index 0000000..8866224
Binary files /dev/null and b/05_software/train/rc_mjlab/mjlab/docs/source/_static/terrains/hf_pyramid_slope.png differ
diff --git a/05_software/train/rc_mjlab/mjlab/docs/source/_static/terrains/hf_random_uniform.png b/05_software/train/rc_mjlab/mjlab/docs/source/_static/terrains/hf_random_uniform.png
new file mode 100644
index 0000000..38dabbe
Binary files /dev/null and b/05_software/train/rc_mjlab/mjlab/docs/source/_static/terrains/hf_random_uniform.png differ
diff --git a/05_software/train/rc_mjlab/mjlab/docs/source/_static/terrains/hf_wave.png b/05_software/train/rc_mjlab/mjlab/docs/source/_static/terrains/hf_wave.png
new file mode 100644
index 0000000..d936b23
Binary files /dev/null and b/05_software/train/rc_mjlab/mjlab/docs/source/_static/terrains/hf_wave.png differ
diff --git a/05_software/train/rc_mjlab/mjlab/docs/source/_static/tutorials/cartpole_swingup.mp4 b/05_software/train/rc_mjlab/mjlab/docs/source/_static/tutorials/cartpole_swingup.mp4
new file mode 100644
index 0000000..e5a43b5
Binary files /dev/null and b/05_software/train/rc_mjlab/mjlab/docs/source/_static/tutorials/cartpole_swingup.mp4 differ
diff --git a/05_software/train/rc_mjlab/mjlab/docs/source/_static/tutorials/cartpole_training_curve.png b/05_software/train/rc_mjlab/mjlab/docs/source/_static/tutorials/cartpole_training_curve.png
new file mode 100644
index 0000000..0b1b3dd
Binary files /dev/null and b/05_software/train/rc_mjlab/mjlab/docs/source/_static/tutorials/cartpole_training_curve.png differ
diff --git a/05_software/train/rc_mjlab/mjlab/docs/source/_static/viser_camera_pane.png b/05_software/train/rc_mjlab/mjlab/docs/source/_static/viser_camera_pane.png
new file mode 100644
index 0000000..dd7f3a3
Binary files /dev/null and b/05_software/train/rc_mjlab/mjlab/docs/source/_static/viser_camera_pane.png differ
diff --git a/05_software/train/rc_mjlab/mjlab/docs/source/_static/viser_viewer.png b/05_software/train/rc_mjlab/mjlab/docs/source/_static/viser_viewer.png
new file mode 100644
index 0000000..4fed515
Binary files /dev/null and b/05_software/train/rc_mjlab/mjlab/docs/source/_static/viser_viewer.png differ
diff --git a/05_software/train/rc_mjlab/mjlab/docs/source/actions.rst b/05_software/train/rc_mjlab/mjlab/docs/source/actions.rst
new file mode 100644
index 0000000..3e8b6ec
--- /dev/null
+++ b/05_software/train/rc_mjlab/mjlab/docs/source/actions.rst
@@ -0,0 +1,173 @@
+.. _actions:
+
+Actions
+=======
+
+Actions define how the policy controls the simulation. The action
+manager receives the policy's output tensor each step, splits it across
+registered action terms, and routes each slice to the appropriate
+entity's actuators. Each term maps a contiguous segment of the policy
+output to a control mode (position, velocity, effort) on a set of
+joints, tendons, or sites.
+
+.. code-block:: python
+
+    from mjlab.envs.mdp.actions import JointPositionActionCfg
+
+    actions = {
+        "joint_pos": JointPositionActionCfg(
+            entity_name="robot",
+            actuator_names=(".*",),   # regex matching actuator names
+            scale=0.5,
+            use_default_offset=True,  # action 0 = default pose
+        ),
+    }
+
+
+Common parameters
+-----------------
+
+All action types share a base set of parameters inherited from
+``BaseActionCfg``.
+
+``entity_name`` identifies the scene entity to control. ``actuator_names``
+is a tuple of regex patterns matched against actuator (or tendon/site)
+names to select the controlled targets.
+
+``scale`` multiplies the raw policy output before any offset is applied.
+It accepts a scalar or a dict mapping actuator name patterns to
+per-target values. This keeps policy outputs in a normalized range while
+mapping to physically meaningful units. ``offset`` is added after
+scaling; joint action types also provide ``use_default_offset``, which
+automatically loads the entity's default joint positions or velocities
+as the offset so that a raw output of zero produces the default pose.
+
+``clip`` optionally clamps the processed action (after scale and offset)
+before it reaches the actuator. It accepts a dict mapping actuator name
+patterns to ``(min, max)`` tuples, resolved the same way as ``scale``
+and ``offset``.
+
+.. code-block:: python
+
+    JointPositionActionCfg(
+        entity_name="robot",
+        actuator_names=(".*",),
+        scale=0.5,
+        clip={".*_hip_.*": (-1.0, 1.0), ".*_knee_.*": (-0.5, 2.0)},
+    )
+
+Actions are written to actuator targets on every decimation substep
+(physics step), not just once per policy step. This is in contrast to
+observation delay, which operates in units of policy steps.
+
+
+Action types
+------------
+
+.. list-table::
+   :header-rows: 1
+   :widths: 28 72
+
+   * - Type
+     - Description
+   * - ``JointPositionAction``
+     - Sets joint position targets. With ``use_default_offset=True``
+       (the default), a policy output of zero commands the default pose.
+       Encoder bias from ``dr.encoder_bias`` is subtracted automatically
+       so that randomized offsets propagate correctly to the control
+       command.
+   * - ``RelativeJointPositionAction``
+     - Sets joint position targets relative to the current joint positions.
+       The target is ``current_pos + action * scale``, so a policy output of
+       zero holds the robot in place regardless of its current configuration.
+   * - ``JointVelocityAction``
+     - Sets joint velocity targets. ``use_default_offset=True`` uses the
+       default joint velocities (typically zero).
+   * - ``JointEffortAction``
+     - Sets joint effort (torque) targets directly. No default offset.
+   * - ``TendonLengthAction``
+     - Sets tendon length targets. Targets are resolved by matching
+       ``actuator_names`` against tendon names.
+   * - ``TendonVelocityAction``
+     - Sets tendon velocity targets.
+   * - ``TendonEffortAction``
+     - Sets tendon effort targets.
+   * - ``SiteEffortAction``
+     - Applies forces and torques at named sites. Useful for
+       quadrotors and drones where thrust is applied at rotor sites
+       rather than through joint actuators.
+
+
+Task-space actions
+------------------
+
+``DifferentialIKAction`` converts Cartesian position and orientation
+commands into joint-space position targets via damped least-squares
+inverse kinematics. One IK step is executed per decimation substep, so
+the end-effector tracks the target continuously across substeps rather
+than only at policy frequency.
+
+The action dimension is selected automatically based on configuration:
+
+- ``orientation_weight == 0``: **3D** (position only)
+- ``orientation_weight > 0, use_relative_mode=True``: **6D** (delta
+  position + delta axis-angle)
+- ``orientation_weight > 0, use_relative_mode=False``: **7D** (absolute
+  position + quaternion)
+
+All objectives (position, orientation, joint limits, posture) are
+stacked into a single DLS system. Setting a weight to zero disables
+that objective with no overhead in the solve.
+
+The ``compute_dq()`` method returns joint displacements without writing
+to actuator targets, enabling multi-iteration IK in standalone scripts
+outside of RL training.
+
+
+Action dimensions and history
+------------------------------
+
+The total action dimension presented to the policy is the sum of each
+registered term's ``action_dim``. For joint, tendon, and site actions
+this equals the number of matched targets. For ``DifferentialIKAction``
+it is 3, 6, or 7 depending on the active objectives.
+
+The action manager tracks the three most recent action vectors:
+``action``, ``prev_action``, and ``prev_prev_action``. Observation terms
+such as ``last_action`` and reward terms such as ``action_rate_l2`` and
+``action_acc_l2`` read from these buffers. Action history is zeroed on
+environment reset so that episode boundaries do not leak information.
+
+
+Multiple action terms
+---------------------
+
+An environment can register any number of terms. The action manager
+concatenates their dimensions in registration order, splits the
+policy's output tensor at the corresponding boundaries, and routes
+each slice independently.
+
+.. code-block:: python
+
+    from mjlab.envs.mdp.actions import (
+        JointPositionActionCfg,
+        JointVelocityActionCfg,
+    )
+
+    actions = {
+        "arm_joints": JointPositionActionCfg(
+            entity_name="robot",
+            actuator_names=(".*_arm_.*",),
+            scale=0.5,
+        ),
+        "wheel_joints": JointVelocityActionCfg(
+            entity_name="robot",
+            actuator_names=(".*_wheel_.*",),
+            scale=10.0,
+        ),
+    }
+
+The policy outputs a tensor whose width equals the total number of
+matched targets across all terms. Terms can also target different
+entities, for example one term for a robot and another for an object
+being manipulated.
diff --git a/05_software/train/rc_mjlab/mjlab/docs/source/actuators.rst b/05_software/train/rc_mjlab/mjlab/docs/source/actuators.rst
new file mode 100644
index 0000000..bb4ff6e
--- /dev/null
+++ b/05_software/train/rc_mjlab/mjlab/docs/source/actuators.rst
@@ -0,0 +1,450 @@
+.. _actuators:
+
+Actuators
+=========
+
+Actuators convert high-level commands (position, velocity, effort) into
+low-level efforts that drive joints. They are configured through the
+``articulation`` field of :ref:`EntityCfg `. mjlab provides
+**built-in** actuators that leverage the physics engine's implicit
+integration for best stability, and **explicit** actuators for custom
+control laws and actuator dynamics.
+
+
+Quick start
+-----------
+
+Basic PD control with ``BuiltinPositionActuator``, the most common
+starting point.
+
+.. code-block:: python
+
+    from mjlab.actuator import BuiltinPositionActuatorCfg
+    from mjlab.entity import EntityCfg, EntityArticulationInfoCfg
+
+    robot_cfg = EntityCfg(
+        spec_fn=lambda: load_robot_spec(),
+        articulation=EntityArticulationInfoCfg(
+            actuators=(
+                BuiltinPositionActuatorCfg(
+                    target_names_expr=(".*_hip_.*", ".*_knee_.*"),
+                    stiffness=80.0,
+                    damping=10.0,
+                    effort_limit=100.0,
+                ),
+            ),
+        ),
+    )
+
+Add delay fields directly on any actuator config to model communication
+latency.
+
+.. code-block:: python
+
+    from mjlab.actuator import BuiltinPositionActuatorCfg
+
+    BuiltinPositionActuatorCfg(
+        target_names_expr=(".*",),
+        stiffness=80.0,
+        damping=10.0,
+        delay_min_lag=2,  # Minimum 2 physics steps
+        delay_max_lag=5,  # Maximum 5 physics steps
+    )
+
+
+Built-in vs explicit actuators
+------------------------------
+
+The key design decision when configuring actuators is whether to use
+**built-in** or **explicit** types. The difference comes down to how
+MuJoCo's integrator handles velocity-dependent forces.
+
+**Built-in actuators** (``BuiltinPositionActuator``,
+``BuiltinVelocityActuator``, ``BuiltinMotorActuator``,
+``BuiltinMuscleActuator``) create native MuJoCo actuator elements in the
+MjSpec. The physics engine computes the control law and integrates
+velocity-dependent damping forces implicitly. This provides the best
+numerical stability, particularly with high gains or large timesteps.
+
+**Explicit actuators** (``IdealPdActuator``, ``DcMotorActuator``,
+``LearnedMlpActuator``) compute torques in user code and forward them
+through a ```` actuator acting as a passthrough. Because the
+integrator cannot account for the velocity derivatives of these
+externally computed forces, they are less numerically robust than built-in
+types. Use explicit actuators when you need custom control laws or actuator
+dynamics that cannot be expressed with built-in types (e.g.,
+velocity-dependent torque limits, learned actuator networks).
+
+The two approaches match closely in the linear, unconstrained regime at
+small timesteps. At larger timesteps or higher gains, built-in actuators
+are more forgiving.
+
+**Integrator choice.** mjlab places damping inside the actuator rather than
+in joints. The ``euler`` integrator treats joint damping implicitly but
+actuator damping explicitly, limiting stability. The ``implicitfast``
+integrator treats all known velocity-dependent forces implicitly, handling
+both proportional and damping terms of the actuator without additional cost.
+
+.. note::
+
+     mjlab defaults to ``implicitfast``, as it is MuJoCo's recommended
+     integrator and provides superior stability for actuator-side damping.
+
+
+Actuator types
+--------------
+
+All actuator configs share a few common fields inherited from
+``ActuatorCfg``:
+
+- ``target_names_expr``: Tuple of regex patterns matched against joint
+  names (or tendon/site names when using a different
+  ``transmission_type``).
+- ``armature``: Reflected rotor inertia added to the target joint.
+- ``frictionloss``: Static friction (stiction) modeled as a constraint
+  on the target joint. See MuJoCo's
+  `frictionloss `_.
+
+Built-in actuators
+^^^^^^^^^^^^^^^^^^
+
+Built-in actuators use MuJoCo's native actuator types via the MjSpec API.
+
+**BuiltinPositionActuator**: Creates ```` actuators for PD
+control.
+
+**BuiltinVelocityActuator**: Creates ```` actuators for velocity
+control.
+
+**BuiltinMotorActuator**: Creates ```` actuators for direct torque
+control.
+
+**BuiltinMuscleActuator**: Creates ```` actuators for
+biologically-inspired muscle dynamics with force-length-velocity
+characteristics.
+
+.. code-block:: python
+
+    from mjlab.actuator import BuiltinPositionActuatorCfg, BuiltinVelocityActuatorCfg
+
+    # Mobile manipulator: PD for arm joints, velocity control for wheels.
+    actuators = (
+        BuiltinPositionActuatorCfg(
+            target_names_expr=(".*_shoulder_.*", ".*_elbow_.*", ".*_wrist_.*"),
+            stiffness=100.0,
+            damping=10.0,
+            effort_limit=150.0,
+        ),
+        BuiltinVelocityActuatorCfg(
+            target_names_expr=(".*_wheel_.*",),
+            damping=20.0,
+            effort_limit=50.0,
+        ),
+    )
+
+
+Explicit actuators
+^^^^^^^^^^^^^^^^^^
+
+Explicit actuators compute efforts and forward them to an underlying
+```` actuator acting as a passthrough. See
+`Built-in vs explicit actuators`_ above for stability implications.
+
+**IdealPdActuator**: Implements an ideal PD controller. Computes torques
+as ``tau = Kp * pos_error + Kd * vel_error``.
+
+**DcMotorActuator**: Extends ``IdealPdActuator`` with velocity-dependent
+torque saturation to model DC motor torque-speed curves (back-EMF
+effects). Implements a linear torque-speed curve: maximum torque at zero
+velocity, zero torque at maximum velocity.
+
+**LearnedMlpActuator**: Neural network-based actuator that uses a
+trained MLP to predict torque outputs from joint state history. Useful
+when analytical models cannot capture complex actuator dynamics like
+delays, nonlinearities, and friction effects. Inherits DC motor
+velocity-based torque limits.
+
+.. code-block:: python
+
+    from mjlab.actuator import IdealPdActuatorCfg, DcMotorActuatorCfg
+
+    # Ideal PD for hips, DC motor model with torque-speed curve for knees.
+    actuators = (
+        IdealPdActuatorCfg(
+            target_names_expr=(".*_hip_.*",),
+            stiffness=80.0,
+            damping=10.0,
+            effort_limit=100.0,
+        ),
+        DcMotorActuatorCfg(
+            target_names_expr=(".*_knee_.*",),
+            stiffness=80.0,
+            damping=10.0,
+            effort_limit=25.0,       # Continuous torque limit
+            saturation_effort=50.0,  # Peak torque at stall
+            velocity_limit=30.0,     # No-load speed (rad/s)
+        ),
+    )
+
+
+XML actuators
+^^^^^^^^^^^^^
+
+XML actuators wrap actuators already defined in your robot's XML file. The
+config finds existing actuators by matching their ``target`` joint name
+against the ``target_names_expr`` patterns. Each joint must have exactly one
+matching actuator.
+
+**XmlActuator**: Wraps any actuator already defined in the XML. The
+actuator type (position, velocity, motor, muscle) is auto detected from
+the XML element, or you can set ``command_field`` explicitly.
+
+.. code-block:: python
+
+    from mjlab.actuator import XmlActuatorCfg
+
+    # Robot XML already has:
+    # 
+    #   
+    # 
+
+    # Wrap existing XML actuators.
+    actuators = (
+        XmlActuatorCfg(target_names_expr=("hip_joint",)),
+    )
+
+Actuator delays
+^^^^^^^^^^^^^^^
+
+Any actuator config supports inline delay fields for modeling command
+latency. On a real robot, the onboard PD loop runs at KHz with direct
+encoder access, but the position target from the policy arrives late due
+to inference time and communication bus cycles. Actuator
+delay models this: the command target is delayed, but the control law
+still sees fresh joint state.
+
+This is distinct from observation delay, which models sensor pipeline
+latency (stale state going into the policy). Together they cover both
+legs of the round trip: sensor to policy to motor.
+
+.. code-block:: python
+
+    from mjlab.actuator import IdealPdActuatorCfg
+
+    # Add 2-5 step delay to position commands.
+    actuators = (
+        IdealPdActuatorCfg(
+            target_names_expr=(".*",),
+            stiffness=80.0,
+            damping=10.0,
+            delay_min_lag=2,
+            delay_max_lag=5,
+            delay_hold_prob=0.3,         # 30% chance to keep current lag
+            delay_update_period=10,      # Resample lag every 10 steps
+        ),
+    )
+
+Each step, a lag is sampled uniformly from ``[delay_min_lag,
+delay_max_lag]``. Delays are quantized to physics timesteps. For
+example, with 500Hz physics (2ms/step), ``delay_min_lag=2`` represents
+a 4ms minimum delay.
+
+
+Authoring actuator configs
+--------------------------
+
+Since actuator parameters are uniform within each config, use separate
+actuator configs for joints that need different parameters:
+
+.. code-block:: python
+
+    from mjlab.actuator import BuiltinPositionActuatorCfg
+
+    # G1 humanoid with different gains per joint group.
+    G1_ACTUATORS = (
+        BuiltinPositionActuatorCfg(
+            target_names_expr=(".*_hip_.*", "waist_yaw_joint"),
+            stiffness=180.0,
+            damping=18.0,
+            effort_limit=88.0,
+            armature=0.0015,
+        ),
+        BuiltinPositionActuatorCfg(
+            target_names_expr=("left_hip_pitch_joint", "right_hip_pitch_joint"),
+            stiffness=200.0,
+            damping=20.0,
+            effort_limit=88.0,
+            armature=0.0015,
+        ),
+        BuiltinPositionActuatorCfg(
+            target_names_expr=(".*_knee_joint",),
+            stiffness=150.0,
+            damping=15.0,
+            effort_limit=139.0,
+            armature=0.0025,
+        ),
+        BuiltinPositionActuatorCfg(
+            target_names_expr=(".*_ankle_.*",),
+            stiffness=40.0,
+            damping=5.0,
+            effort_limit=25.0,
+            armature=0.0008,
+        ),
+    )
+
+This design choice reflects a deliberate simplification in mjlab: each
+``ActuatorCfg`` represents a single actuator type (e.g., a specific
+motor/gearbox model) applied uniformly across all joints it drives.
+Hardware parameters such as ``armature`` (reflected rotor inertia) and
+``gear`` describe properties of the actuator hardware, even though they
+are implemented in MuJoCo as joint or actuator fields. In other frameworks
+(like Isaac Lab), these fields may accept ``float | dict[str, float]`` to
+support per-joint variation. mjlab instead encourages one config per
+actuator type or per joint group, keeping the hardware model physically
+consistent and explicit. The main trade-off is verbosity in special cases,
+such as parallel linkages, where per-joint overrides could have been
+convenient, but the benefit is clearer semantics and simpler maintenance.
+
+See :ref:`actions` for how action terms route policy outputs to actuators
+(including DifferentialIK for task-space control), and
+:ref:`domain_randomization` for randomizing gains and effort limits.
+
+
+Computing hardware parameters
+------------------------------
+
+This section is relevant when configuring actuators from real motor
+datasheets. If you are using manually tuned gains, you can skip ahead.
+
+mjlab provides utilities in ``mjlab.utils.actuator`` to compute actuator
+parameters from physical motor specifications. This is particularly
+useful for computing reflected inertia (``armature``) and deriving
+appropriate control gains from hardware datasheets.
+
+**Example: Unitree G1 motor configuration**
+
+.. code-block:: python
+
+    from math import pi
+
+    from mjlab.utils.actuator import (
+        reflected_inertia_from_two_stage_planetary,
+        ElectricActuator
+    )
+
+    # Motor specs from manufacturer datasheet.
+    ROTOR_INERTIAS_7520_14 = (
+        0.489e-4,  # Motor rotor inertia (kg*m**2)
+        0.098e-4,  # Planet carrier inertia
+        0.533e-4,  # Output stage inertia
+    )
+    GEARS_7520_14 = (
+        1,            # First stage (motor to planet)
+        4.5,          # Second stage (planet to carrier)
+        1 + (48/22),  # Third stage (carrier to output)
+    )
+
+    # Compute reflected inertia at joint output.
+    # J_reflected = J_motor*(N1*N2)**2 + J_carrier*N2**2 + J_output.
+    ARMATURE_7520_14 = reflected_inertia_from_two_stage_planetary(
+        ROTOR_INERTIAS_7520_14, GEARS_7520_14
+    )
+
+    # Create motor spec container.
+    ACTUATOR_7520_14 = ElectricActuator(
+        reflected_inertia=ARMATURE_7520_14,
+        velocity_limit=32.0,   # rad/s at joint
+        effort_limit=88.0,     # N*m continuous torque
+    )
+
+    # Derive PD gains from natural frequency and damping ratio.
+    NATURAL_FREQ = 10 * 2*pi  # 10 Hz bandwidth.
+    DAMPING_RATIO = 2.0       # Overdamped, see note below.
+    STIFFNESS = ARMATURE_7520_14 * NATURAL_FREQ**2
+    DAMPING = 2 * DAMPING_RATIO * ARMATURE_7520_14 * NATURAL_FREQ
+
+    # Use in actuator config.
+    from mjlab.actuator import BuiltinPositionActuatorCfg
+
+    actuator = BuiltinPositionActuatorCfg(
+        target_names_expr=(".*_hip_pitch_joint",),
+        stiffness=STIFFNESS,
+        damping=DAMPING,
+        effort_limit=ACTUATOR_7520_14.effort_limit,
+        armature=ACTUATOR_7520_14.reflected_inertia,
+    )
+
+.. note::
+
+     The example uses ``DAMPING_RATIO = 2.0``
+     (overdamped) rather than the critically damped value of 1.0. This is
+     because the reflected inertia calculation only accounts for the motor's
+     rotor inertia, not the apparent inertia of the links being moved. In
+     practice, the total effective inertia at the joint is higher than just
+     the reflected motor inertia, so using an overdamped ratio provides
+     better stability margins when the true system inertia is
+     underestimated.
+
+**Parallel linkage approximation:**
+
+For joints driven by parallel linkages (like the G1's ankles with dual
+motors), the effective armature in the nominal configuration can be
+approximated as the sum of the individual motor armatures:
+
+.. code-block:: python
+
+    # Two 5020 motors driving ankle through parallel linkage.
+    G1_ACTUATOR_ANKLE = BuiltinPositionActuatorCfg(
+        target_names_expr=(".*_ankle_pitch_joint", ".*_ankle_roll_joint"),
+        stiffness=STIFFNESS_5020 * 2,
+        damping=DAMPING_5020 * 2,
+        effort_limit=ACTUATOR_5020.effort_limit * 2,
+        armature=ACTUATOR_5020.reflected_inertia * 2,
+    )
+
+
+Extending: custom actuators
+----------------------------
+
+All actuators implement a unified ``compute()`` interface that receives an
+``ActuatorCmd`` (containing position, velocity, and effort targets) and
+returns control signals for the low-level MuJoCo actuators driving each
+joint.
+
+**Core interface:**
+
+.. code-block:: python
+
+    def compute(self, cmd: ActuatorCmd) -> torch.Tensor:
+        """Convert high-level commands to control signals.
+
+        Args:
+            cmd: Command containing position_target, velocity_target,
+                effort_target (each is a [num_envs, num_targets] tensor
+                or None)
+
+        Returns:
+            Control signals for this actuator
+            ([num_envs, num_targets] tensor)
+        """
+
+**Lifecycle hooks:**
+
+- ``edit_spec``: Modify MjSpec before compilation (add actuators, set
+  gains)
+- ``initialize``: Post-compilation setup (resolve indices, allocate
+  buffers)
+- ``reset``: Per-environment reset logic
+- ``update``: Pre-step updates
+- ``compute``: Convert commands to control signals
+
+**Properties:**
+
+- ``target_ids``: Tensor of local target indices controlled by this
+  actuator
+- ``target_names``: List of target names controlled by this actuator
+- ``ctrl_ids``: Tensor of global control input indices for this actuator
+
+``IdealPdActuator`` is the recommended base class for custom explicit
+actuators. ``DcMotorActuator`` and ``LearnedMlpActuator`` are both
+built on top of it and serve as examples of the extension pattern.
diff --git a/05_software/train/rc_mjlab/mjlab/docs/source/api/actuator.rst b/05_software/train/rc_mjlab/mjlab/docs/source/api/actuator.rst
new file mode 100644
index 0000000..80b1456
--- /dev/null
+++ b/05_software/train/rc_mjlab/mjlab/docs/source/api/actuator.rst
@@ -0,0 +1,141 @@
+mjlab.actuator
+==============
+
+.. automodule:: mjlab.actuator
+
+.. rubric:: Classes
+
+.. hlist::
+  :columns: 3
+
+  - :class:`Actuator`
+  - :class:`ActuatorCfg`
+  - :class:`ActuatorCmd`
+  - :class:`BuiltinActuatorGroup`
+  - :class:`BuiltinMotorActuator`
+  - :class:`BuiltinMotorActuatorCfg`
+  - :class:`BuiltinPositionActuator`
+  - :class:`BuiltinPositionActuatorCfg`
+  - :class:`BuiltinVelocityActuator`
+  - :class:`BuiltinVelocityActuatorCfg`
+  - :class:`BuiltinMuscleActuator`
+  - :class:`BuiltinMuscleActuatorCfg`
+  - :class:`XmlActuator`
+  - :class:`XmlActuatorCfg`
+  - :class:`IdealPdActuator`
+  - :class:`IdealPdActuatorCfg`
+  - :class:`DcMotorActuator`
+  - :class:`DcMotorActuatorCfg`
+  - :class:`LearnedMlpActuator`
+  - :class:`LearnedMlpActuatorCfg`
+
+Base
+----
+
+.. autoclass:: Actuator
+  :members:
+  :show-inheritance:
+
+.. autoclass:: ActuatorCfg
+  :members:
+  :exclude-members: __init__
+  :undoc-members:
+
+
+.. autoclass:: ActuatorCmd
+  :members:
+  :exclude-members: __init__
+  :undoc-members:
+
+Builtin Actuators
+-----------------
+
+.. autoclass:: BuiltinActuatorGroup
+  :members:
+  :show-inheritance:
+
+.. autoclass:: BuiltinMotorActuator
+  :members:
+  :show-inheritance:
+
+.. autoclass:: BuiltinMotorActuatorCfg
+  :members:
+  :exclude-members: __init__
+  :undoc-members:
+
+
+.. autoclass:: BuiltinPositionActuator
+  :members:
+  :show-inheritance:
+
+.. autoclass:: BuiltinPositionActuatorCfg
+  :members:
+  :exclude-members: __init__
+  :undoc-members:
+
+
+.. autoclass:: BuiltinVelocityActuator
+  :members:
+  :show-inheritance:
+
+.. autoclass:: BuiltinVelocityActuatorCfg
+  :members:
+  :exclude-members: __init__
+  :undoc-members:
+
+
+.. autoclass:: BuiltinMuscleActuator
+  :members:
+  :show-inheritance:
+
+.. autoclass:: BuiltinMuscleActuatorCfg
+  :members:
+  :exclude-members: __init__
+  :undoc-members:
+
+XML Actuators
+-------------
+
+.. autoclass:: XmlActuator
+  :members:
+  :show-inheritance:
+
+.. autoclass:: XmlActuatorCfg
+  :members:
+  :exclude-members: __init__
+  :undoc-members:
+
+Ideal PD Actuator
+-----------------
+
+.. autoclass:: IdealPdActuator
+  :members:
+  :show-inheritance:
+
+.. autoclass:: IdealPdActuatorCfg
+  :members:
+  :exclude-members: __init__
+  :undoc-members:
+
+DC Motor Actuator
+-----------------
+
+.. autoclass:: DcMotorActuator
+  :members:
+  :show-inheritance:
+
+.. autoclass:: DcMotorActuatorCfg
+  :members:
+  :exclude-members: __init__
+  :undoc-members:
+
+Learned MLP Actuator
+--------------------
+
+.. autoclass:: LearnedMlpActuator
+  :members:
+  :show-inheritance:
+
+.. autoclass:: LearnedMlpActuatorCfg
+  :members:
+  :exclude-members: __init__
diff --git a/05_software/train/rc_mjlab/mjlab/docs/source/api/entity.rst b/05_software/train/rc_mjlab/mjlab/docs/source/api/entity.rst
new file mode 100644
index 0000000..d6e6e5d
--- /dev/null
+++ b/05_software/train/rc_mjlab/mjlab/docs/source/api/entity.rst
@@ -0,0 +1,45 @@
+mjlab.entity
+============
+
+.. automodule:: mjlab.entity
+
+.. rubric:: Classes
+
+.. hlist::
+  :columns: 3
+
+  - :class:`Entity`
+  - :class:`EntityCfg`
+  - :class:`EntityArticulationInfoCfg`
+  - :class:`EntityIndexing`
+  - :class:`EntityData`
+
+Entity
+------
+
+.. autoclass:: Entity
+  :members:
+  :show-inheritance:
+
+.. autoclass:: EntityCfg
+  :members:
+  :exclude-members: __init__
+  :undoc-members:
+
+
+.. autoclass:: EntityArticulationInfoCfg
+  :members:
+  :exclude-members: __init__
+  :undoc-members:
+
+EntityIndexing
+--------------
+
+.. autoclass:: EntityIndexing
+  :members:
+
+EntityData
+----------
+
+.. autoclass:: EntityData
+  :members:
diff --git a/05_software/train/rc_mjlab/mjlab/docs/source/api/envs.rst b/05_software/train/rc_mjlab/mjlab/docs/source/api/envs.rst
new file mode 100644
index 0000000..4dc4257
--- /dev/null
+++ b/05_software/train/rc_mjlab/mjlab/docs/source/api/envs.rst
@@ -0,0 +1,36 @@
+mjlab.envs
+==========
+
+.. automodule:: mjlab.envs
+
+.. rubric:: Classes
+
+.. hlist::
+  :columns: 3
+
+  - :class:`ManagerBasedRlEnv`
+  - :class:`ManagerBasedRlEnvCfg`
+  - :data:`VecEnvObs`
+  - :data:`VecEnvStepReturn`
+
+ManagerBasedRlEnv
+-----------------
+
+.. autoclass:: ManagerBasedRlEnv
+  :members:
+  :show-inheritance:
+
+.. autoclass:: ManagerBasedRlEnvCfg
+  :members:
+  :exclude-members: __init__
+  :undoc-members:
+
+VecEnvObs
+---------
+
+.. autodata:: VecEnvObs
+
+VecEnvStepReturn
+----------------
+
+.. autodata:: VecEnvStepReturn
diff --git a/05_software/train/rc_mjlab/mjlab/docs/source/api/index.rst b/05_software/train/rc_mjlab/mjlab/docs/source/api/index.rst
new file mode 100644
index 0000000..9db9efb
--- /dev/null
+++ b/05_software/train/rc_mjlab/mjlab/docs/source/api/index.rst
@@ -0,0 +1,19 @@
+API Reference
+=============
+
+This section provides detailed API documentation for all public modules in mjlab.
+
+.. toctree::
+   :maxdepth: 1
+
+   envs
+   scene
+   sim
+   entity
+   actuator
+   sensor
+   managers
+   terrains
+   rl
+   viewer
+   tasks
diff --git a/05_software/train/rc_mjlab/mjlab/docs/source/api/managers.rst b/05_software/train/rc_mjlab/mjlab/docs/source/api/managers.rst
new file mode 100644
index 0000000..8852919
--- /dev/null
+++ b/05_software/train/rc_mjlab/mjlab/docs/source/api/managers.rst
@@ -0,0 +1,208 @@
+mjlab.managers
+==============
+
+.. automodule:: mjlab.managers
+
+.. rubric:: Classes
+
+.. hlist::
+  :columns: 3
+
+  - :class:`ManagerBase`
+  - :class:`ManagerTermBase`
+  - :class:`ManagerTermBaseCfg`
+  - :class:`SceneEntityCfg`
+  - :class:`ActionManager`
+  - :class:`ActionTerm`
+  - :class:`ActionTermCfg`
+  - :class:`ObservationManager`
+  - :class:`ObservationGroupCfg`
+  - :class:`ObservationTermCfg`
+  - :class:`RewardManager`
+  - :class:`RewardTermCfg`
+  - :class:`TerminationManager`
+  - :class:`TerminationTermCfg`
+  - :class:`CommandManager`
+  - :class:`NullCommandManager`
+  - :class:`CommandTerm`
+  - :class:`CommandTermCfg`
+  - :class:`CurriculumManager`
+  - :class:`NullCurriculumManager`
+  - :class:`CurriculumTermCfg`
+  - :class:`EventManager`
+  - :class:`EventMode`
+  - :class:`EventTermCfg`
+  - :class:`MetricsManager`
+  - :class:`NullMetricsManager`
+  - :class:`MetricsTermCfg`
+  - :class:`RecorderManager`
+  - :class:`NullRecorderManager`
+  - :class:`RecorderTerm`
+  - :class:`RecorderTermCfg`
+
+Base
+----
+
+.. autoclass:: ManagerBase
+  :members:
+  :show-inheritance:
+
+.. autoclass:: ManagerTermBase
+  :members:
+  :show-inheritance:
+
+.. autoclass:: ManagerTermBaseCfg
+  :members:
+  :exclude-members: __init__
+  :undoc-members:
+
+
+.. autoclass:: SceneEntityCfg
+  :members:
+  :exclude-members: __init__
+  :undoc-members:
+
+Action Manager
+--------------
+
+.. autoclass:: ActionManager
+  :members:
+  :show-inheritance:
+
+.. autoclass:: ActionTerm
+  :members:
+  :show-inheritance:
+
+.. autoclass:: ActionTermCfg
+  :members:
+  :exclude-members: __init__
+  :undoc-members:
+
+Observation Manager
+-------------------
+
+.. autoclass:: ObservationManager
+  :members:
+  :show-inheritance:
+
+.. autoclass:: ObservationGroupCfg
+  :members:
+  :exclude-members: __init__
+  :undoc-members:
+
+
+.. autoclass:: ObservationTermCfg
+  :members:
+  :exclude-members: __init__
+  :undoc-members:
+
+Reward Manager
+--------------
+
+.. autoclass:: RewardManager
+  :members:
+  :show-inheritance:
+
+.. autoclass:: RewardTermCfg
+  :members:
+  :exclude-members: __init__
+  :undoc-members:
+
+Termination Manager
+-------------------
+
+.. autoclass:: TerminationManager
+  :members:
+  :show-inheritance:
+
+.. autoclass:: TerminationTermCfg
+  :members:
+  :exclude-members: __init__
+  :undoc-members:
+
+Command Manager
+---------------
+
+.. autoclass:: CommandManager
+  :members:
+  :show-inheritance:
+
+.. autoclass:: NullCommandManager
+  :members:
+  :show-inheritance:
+
+.. autoclass:: CommandTerm
+  :members:
+  :show-inheritance:
+
+.. autoclass:: CommandTermCfg
+  :members:
+  :exclude-members: __init__
+  :undoc-members:
+
+Curriculum Manager
+------------------
+
+.. autoclass:: CurriculumManager
+  :members:
+  :show-inheritance:
+
+.. autoclass:: NullCurriculumManager
+  :members:
+  :show-inheritance:
+
+.. autoclass:: CurriculumTermCfg
+  :members:
+  :exclude-members: __init__
+  :undoc-members:
+
+Event Manager
+-------------
+
+.. autoclass:: EventManager
+  :members:
+  :show-inheritance:
+
+.. autoclass:: EventMode
+  :members:
+  :undoc-members:
+
+.. autoclass:: EventTermCfg
+  :members:
+  :exclude-members: __init__
+  :undoc-members:
+
+Metrics Manager
+---------------
+
+.. autoclass:: MetricsManager
+  :members:
+  :show-inheritance:
+
+.. autoclass:: NullMetricsManager
+  :members:
+  :show-inheritance:
+
+.. autoclass:: MetricsTermCfg
+  :members:
+  :exclude-members: __init__
+
+Recorder Manager
+----------------
+
+.. autoclass:: RecorderManager
+  :members:
+  :show-inheritance:
+
+.. autoclass:: NullRecorderManager
+  :members:
+  :show-inheritance:
+
+.. autoclass:: RecorderTerm
+  :members:
+  :show-inheritance:
+
+.. autoclass:: RecorderTermCfg
+  :members:
+  :exclude-members: __init__
+  :undoc-members:
diff --git a/05_software/train/rc_mjlab/mjlab/docs/source/api/rl.rst b/05_software/train/rc_mjlab/mjlab/docs/source/api/rl.rst
new file mode 100644
index 0000000..2f173a3
--- /dev/null
+++ b/05_software/train/rc_mjlab/mjlab/docs/source/api/rl.rst
@@ -0,0 +1,52 @@
+mjlab.rl
+========
+
+.. automodule:: mjlab.rl
+
+.. rubric:: Classes
+
+.. hlist::
+  :columns: 3
+
+  - :class:`MjlabOnPolicyRunner`
+  - :class:`RslRlVecEnvWrapper`
+  - :class:`RslRlOnPolicyRunnerCfg`
+  - :class:`RslRlPpoAlgorithmCfg`
+  - :class:`RslRlModelCfg`
+  - :class:`RslRlBaseRunnerCfg`
+
+Runner
+------
+
+.. autoclass:: MjlabOnPolicyRunner
+  :members:
+  :show-inheritance:
+
+.. autoclass:: RslRlVecEnvWrapper
+  :members:
+  :show-inheritance:
+
+Configuration
+-------------
+
+.. autoclass:: RslRlOnPolicyRunnerCfg
+  :members:
+  :exclude-members: __init__
+  :undoc-members:
+
+
+.. autoclass:: RslRlPpoAlgorithmCfg
+  :members:
+  :exclude-members: __init__
+  :undoc-members:
+
+
+.. autoclass:: RslRlModelCfg
+  :members:
+  :exclude-members: __init__
+  :undoc-members:
+
+
+.. autoclass:: RslRlBaseRunnerCfg
+  :members:
+  :exclude-members: __init__
diff --git a/05_software/train/rc_mjlab/mjlab/docs/source/api/scene.rst b/05_software/train/rc_mjlab/mjlab/docs/source/api/scene.rst
new file mode 100644
index 0000000..5d5f983
--- /dev/null
+++ b/05_software/train/rc_mjlab/mjlab/docs/source/api/scene.rst
@@ -0,0 +1,23 @@
+mjlab.scene
+===========
+
+.. automodule:: mjlab.scene
+
+.. rubric:: Classes
+
+.. hlist::
+  :columns: 3
+
+  - :class:`Scene`
+  - :class:`SceneCfg`
+
+Scene
+-----
+
+.. autoclass:: Scene
+  :members:
+
+.. autoclass:: SceneCfg
+  :members:
+  :exclude-members: __init__
+  :undoc-members:
diff --git a/05_software/train/rc_mjlab/mjlab/docs/source/api/sensor.rst b/05_software/train/rc_mjlab/mjlab/docs/source/api/sensor.rst
new file mode 100644
index 0000000..70edd71
--- /dev/null
+++ b/05_software/train/rc_mjlab/mjlab/docs/source/api/sensor.rst
@@ -0,0 +1,126 @@
+mjlab.sensor
+============
+
+.. automodule:: mjlab.sensor
+
+.. rubric:: Classes
+
+.. hlist::
+  :columns: 3
+
+  - :class:`Sensor`
+  - :class:`SensorCfg`
+  - :class:`SensorContext`
+  - :class:`BuiltinSensor`
+  - :class:`BuiltinSensorCfg`
+  - :class:`ObjRef`
+  - :class:`ContactSensor`
+  - :class:`ContactSensorCfg`
+  - :class:`ContactData`
+  - :class:`ContactMatch`
+  - :class:`RayCastSensor`
+  - :class:`RayCastSensorCfg`
+  - :class:`RayCastData`
+  - :class:`GridPatternCfg`
+  - :class:`PinholeCameraPatternCfg`
+  - :class:`CameraSensor`
+  - :class:`CameraSensorCfg`
+  - :class:`CameraSensorData`
+
+Base
+----
+
+.. autoclass:: Sensor
+  :members:
+  :show-inheritance:
+
+.. autoclass:: SensorCfg
+  :members:
+  :exclude-members: __init__
+  :undoc-members:
+
+
+.. autoclass:: SensorContext
+  :members:
+
+Builtin Sensor
+--------------
+
+.. autoclass:: BuiltinSensor
+  :members:
+  :show-inheritance:
+
+.. autoclass:: BuiltinSensorCfg
+  :members:
+  :exclude-members: __init__
+  :undoc-members:
+
+
+.. autoclass:: ObjRef
+  :members:
+  :exclude-members: __init__
+  :undoc-members:
+
+Contact Sensor
+--------------
+
+.. autoclass:: ContactSensor
+  :members:
+  :show-inheritance:
+
+.. autoclass:: ContactSensorCfg
+  :members:
+  :exclude-members: __init__
+  :undoc-members:
+
+
+.. autoclass:: ContactData
+  :members:
+
+.. autoclass:: ContactMatch
+  :members:
+  :exclude-members: __init__
+  :undoc-members:
+
+Ray Cast Sensor
+---------------
+
+.. autoclass:: RayCastSensor
+  :members:
+  :show-inheritance:
+
+.. autoclass:: RayCastSensorCfg
+  :members:
+  :exclude-members: __init__
+  :undoc-members:
+
+
+.. autoclass:: RayCastData
+  :members:
+
+.. autoclass:: GridPatternCfg
+  :members:
+  :exclude-members: __init__
+  :undoc-members:
+
+
+.. autoclass:: PinholeCameraPatternCfg
+  :members:
+  :exclude-members: __init__
+  :undoc-members:
+
+Camera Sensor
+-------------
+
+.. autoclass:: CameraSensor
+  :members:
+  :show-inheritance:
+
+.. autoclass:: CameraSensorCfg
+  :members:
+  :exclude-members: __init__
+  :undoc-members:
+
+
+.. autoclass:: CameraSensorData
+  :members:
diff --git a/05_software/train/rc_mjlab/mjlab/docs/source/api/sim.rst b/05_software/train/rc_mjlab/mjlab/docs/source/api/sim.rst
new file mode 100644
index 0000000..f3faa6b
--- /dev/null
+++ b/05_software/train/rc_mjlab/mjlab/docs/source/api/sim.rst
@@ -0,0 +1,44 @@
+mjlab.sim
+=========
+
+.. automodule:: mjlab.sim
+
+.. rubric:: Classes
+
+.. hlist::
+  :columns: 3
+
+  - :class:`Simulation`
+  - :class:`SimulationCfg`
+  - :class:`MujocoCfg`
+  - :class:`TorchArray`
+  - :class:`WarpBridge`
+
+Simulation
+----------
+
+.. autoclass:: Simulation
+  :members:
+
+.. autoclass:: SimulationCfg
+  :members:
+  :exclude-members: __init__
+  :undoc-members:
+
+
+.. autoclass:: MujocoCfg
+  :members:
+  :exclude-members: __init__
+  :undoc-members:
+
+TorchArray
+----------
+
+.. autoclass:: TorchArray
+  :members:
+
+WarpBridge
+----------
+
+.. autoclass:: WarpBridge
+  :members:
diff --git a/05_software/train/rc_mjlab/mjlab/docs/source/api/tasks.rst b/05_software/train/rc_mjlab/mjlab/docs/source/api/tasks.rst
new file mode 100644
index 0000000..4b9dd8b
--- /dev/null
+++ b/05_software/train/rc_mjlab/mjlab/docs/source/api/tasks.rst
@@ -0,0 +1,25 @@
+mjlab.tasks
+===========
+
+.. automodule:: mjlab.tasks.registry
+
+.. rubric:: Functions
+
+.. hlist::
+  :columns: 3
+
+  - :func:`register_mjlab_task`
+  - :func:`list_tasks`
+  - :func:`load_env_cfg`
+  - :func:`load_rl_cfg`
+  - :func:`load_runner_cls`
+
+.. autofunction:: register_mjlab_task
+
+.. autofunction:: list_tasks
+
+.. autofunction:: load_env_cfg
+
+.. autofunction:: load_rl_cfg
+
+.. autofunction:: load_runner_cls
diff --git a/05_software/train/rc_mjlab/mjlab/docs/source/api/terrains.rst b/05_software/train/rc_mjlab/mjlab/docs/source/api/terrains.rst
new file mode 100644
index 0000000..976e394
--- /dev/null
+++ b/05_software/train/rc_mjlab/mjlab/docs/source/api/terrains.rst
@@ -0,0 +1,167 @@
+mjlab.terrains
+==============
+
+.. automodule:: mjlab.terrains
+
+.. rubric:: Classes
+
+.. hlist::
+  :columns: 3
+
+  - :class:`TerrainEntity`
+  - :class:`TerrainEntityCfg`
+  - :class:`TerrainGenerator`
+  - :class:`TerrainGeneratorCfg`
+  - :class:`SubTerrainCfg`
+  - :class:`FlatPatchSamplingCfg`
+  - :class:`HfDiscreteObstaclesTerrainCfg`
+  - :class:`HfPerlinNoiseTerrainCfg`
+  - :class:`HfPyramidSlopedTerrainCfg`
+  - :class:`HfRandomUniformTerrainCfg`
+  - :class:`HfWaveTerrainCfg`
+  - :class:`BoxFlatTerrainCfg`
+  - :class:`BoxInvertedPyramidStairsTerrainCfg`
+  - :class:`BoxNarrowBeamsTerrainCfg`
+  - :class:`BoxNestedRingsTerrainCfg`
+  - :class:`BoxOpenStairsTerrainCfg`
+  - :class:`BoxPyramidStairsTerrainCfg`
+  - :class:`BoxRandomGridTerrainCfg`
+  - :class:`BoxRandomSpreadTerrainCfg`
+  - :class:`BoxRandomStairsTerrainCfg`
+  - :class:`BoxSteppingStonesTerrainCfg`
+  - :class:`BoxTiltedGridTerrainCfg`
+
+Core
+----
+
+.. autoclass:: TerrainEntity
+  :members:
+  :show-inheritance:
+
+.. autoclass:: TerrainEntityCfg
+  :members:
+  :exclude-members: __init__
+  :undoc-members:
+
+
+.. autoclass:: TerrainGenerator
+  :members:
+
+.. autoclass:: TerrainGeneratorCfg
+  :members:
+  :exclude-members: __init__
+  :undoc-members:
+
+
+.. autoclass:: SubTerrainCfg
+  :members:
+  :exclude-members: __init__
+  :undoc-members:
+
+
+.. autoclass:: FlatPatchSamplingCfg
+  :members:
+  :exclude-members: __init__
+  :undoc-members:
+
+Heightfield Terrains
+--------------------
+
+.. autoclass:: HfDiscreteObstaclesTerrainCfg
+  :members:
+  :exclude-members: __init__
+  :undoc-members:
+  :show-inheritance:
+
+.. autoclass:: HfPerlinNoiseTerrainCfg
+  :members:
+  :exclude-members: __init__
+  :undoc-members:
+  :show-inheritance:
+
+.. autoclass:: HfRandomUniformTerrainCfg
+  :members:
+  :exclude-members: __init__
+  :undoc-members:
+  :show-inheritance:
+
+.. autoclass:: HfPyramidSlopedTerrainCfg
+  :members:
+  :exclude-members: __init__
+  :undoc-members:
+  :show-inheritance:
+
+.. autoclass:: HfWaveTerrainCfg
+  :members:
+  :exclude-members: __init__
+  :undoc-members:
+  :show-inheritance:
+
+Primitive (Box) Terrains
+------------------------
+
+.. autoclass:: BoxFlatTerrainCfg
+  :members:
+  :exclude-members: __init__
+  :undoc-members:
+  :show-inheritance:
+
+.. autoclass:: BoxInvertedPyramidStairsTerrainCfg
+  :members:
+  :exclude-members: __init__
+  :undoc-members:
+  :show-inheritance:
+
+.. autoclass:: BoxNarrowBeamsTerrainCfg
+  :members:
+  :exclude-members: __init__
+  :undoc-members:
+  :show-inheritance:
+
+.. autoclass:: BoxNestedRingsTerrainCfg
+  :members:
+  :exclude-members: __init__
+  :undoc-members:
+  :show-inheritance:
+
+.. autoclass:: BoxOpenStairsTerrainCfg
+  :members:
+  :exclude-members: __init__
+  :undoc-members:
+  :show-inheritance:
+
+.. autoclass:: BoxPyramidStairsTerrainCfg
+  :members:
+  :exclude-members: __init__
+  :undoc-members:
+  :show-inheritance:
+
+.. autoclass:: BoxRandomGridTerrainCfg
+  :members:
+  :exclude-members: __init__
+  :undoc-members:
+  :show-inheritance:
+
+.. autoclass:: BoxRandomSpreadTerrainCfg
+  :members:
+  :exclude-members: __init__
+  :undoc-members:
+  :show-inheritance:
+
+.. autoclass:: BoxRandomStairsTerrainCfg
+  :members:
+  :exclude-members: __init__
+  :undoc-members:
+  :show-inheritance:
+
+.. autoclass:: BoxSteppingStonesTerrainCfg
+  :members:
+  :exclude-members: __init__
+  :undoc-members:
+  :show-inheritance:
+
+.. autoclass:: BoxTiltedGridTerrainCfg
+  :members:
+  :exclude-members: __init__
+  :undoc-members:
+  :show-inheritance:
diff --git a/05_software/train/rc_mjlab/mjlab/docs/source/api/viewer.rst b/05_software/train/rc_mjlab/mjlab/docs/source/api/viewer.rst
new file mode 100644
index 0000000..21c6570
--- /dev/null
+++ b/05_software/train/rc_mjlab/mjlab/docs/source/api/viewer.rst
@@ -0,0 +1,72 @@
+mjlab.viewer
+============
+
+.. automodule:: mjlab.viewer
+
+.. rubric:: Classes
+
+.. hlist::
+  :columns: 3
+
+  - :class:`ViewerConfig`
+  - :class:`BaseViewer`
+  - :class:`NativeMujocoViewer`
+  - :class:`ViserPlayViewer`
+  - :class:`OffscreenRenderer`
+
+.. rubric:: Protocols
+
+.. hlist::
+  :columns: 3
+
+  - :class:`EnvProtocol`
+  - :class:`PolicyProtocol`
+  - :class:`VerbosityLevel`
+
+ViewerConfig
+------------
+
+.. autoclass:: ViewerConfig
+  :members:
+  :exclude-members: __init__
+  :undoc-members:
+
+BaseViewer
+----------
+
+.. autoclass:: BaseViewer
+  :members:
+  :show-inheritance:
+
+NativeMujocoViewer
+------------------
+
+.. autoclass:: NativeMujocoViewer
+  :members:
+  :show-inheritance:
+
+ViserPlayViewer
+---------------
+
+.. autoclass:: ViserPlayViewer
+  :members:
+  :show-inheritance:
+
+OffscreenRenderer
+-----------------
+
+.. autoclass:: OffscreenRenderer
+  :members:
+  :show-inheritance:
+
+Protocols
+---------
+
+.. autoclass:: EnvProtocol
+  :members:
+
+.. autoclass:: PolicyProtocol
+  :members:
+
+.. autoclass:: VerbosityLevel
+  :members:
diff --git a/05_software/train/rc_mjlab/mjlab/docs/source/architecture_overview.rst b/05_software/train/rc_mjlab/mjlab/docs/source/architecture_overview.rst
new file mode 100644
index 0000000..43a8d1b
--- /dev/null
+++ b/05_software/train/rc_mjlab/mjlab/docs/source/architecture_overview.rst
@@ -0,0 +1,191 @@
+.. _architecture_overview:
+
+Architecture Overview
+=====================
+
+mjlab is organized into two layers: a **simulation layer** that models
+the robot and world, and a **manager layer** that defines the
+reinforcement learning problem on top of it. Understanding this separation
+is the fastest way to build a mental map of the system.
+
+.. figure:: _static/architecture_diagram.png
+   :width: 60%
+   :align: center
+   :alt: mjlab architecture diagram
+
+   Entities are composed into an MjSpec, compiled, and transferred to
+   MuJoCo Warp for GPU simulation. The ManagerBasedRlEnv orchestrates the
+   MDP; RSL-RL handles training.
+
+
+The simulation layer
+--------------------
+
+**Scene pipeline.**
+mjlab constructs scenes by composing entity descriptions into a single
+`MjSpec `_.
+Each entity starts from an
+`MJCF `_ file
+loaded via ``MjSpec.from_file()``. Users who define everything in XML can
+use this directly. For more control, Python dataclasses can extend or
+override properties on the loaded spec: actuators, collision rules,
+materials, sensors, and initial state. This hybrid approach lets users
+start from existing MuJoCo models and layer on task-specific configuration
+without modifying the original XML. The composed specification is compiled
+into an ``MjModel`` on the CPU, then transferred to the GPU via
+`MuJoCo Warp `_,
+which is built on `NVIDIA Warp `_.
+
+**MuJoCo Warp.**
+MuJoCo Warp is a GPU-accelerated backend for MuJoCo. It preserves
+MuJoCo's ``MjModel``/``MjData`` paradigm but adds a leading *world*
+dimension: a single ``MjData`` object holds the state of N independent
+simulation instances in parallel, enabling thousands of environments to
+be stepped simultaneously. Model parameters are shared across all worlds
+by default, and individual fields can be expanded to vary per-world when
+domain randomization requires it. mjlab captures the simulation step as a
+`CUDA graph `_: the kernel
+execution sequence is recorded once and replayed on subsequent calls,
+eliminating CPU-side dispatch overhead.
+
+.. note::
+
+   CUDA graph capture is a one-time cost at environment startup. Per-episode
+   resets and domain randomization events run as regular Python between graph
+   replays and do not break the capture.
+
+**Components.**
+The simulation layer provides four core components, each with its own
+documentation page:
+
+- :ref:`entity`: a robot, a manipulated object, or a static object such
+  as :ref:`terrain `, defined by an MJCF description plus
+  optional Python configuration for actuators, collision rules, and
+  initial state.
+- :ref:`actuators`: how entities are controlled. Users can wrap actuators
+  already defined in MJCF or create new ones from Python configuration.
+- :ref:`sensors`: how the world is observed. Includes MuJoCo-native
+  sensors as well as custom sensors like RGB-D cameras and raycasters.
+- :ref:`scene`: scene composition and environment placement.
+
+
+The manager layer
+-----------------
+
+On top of the simulation layer, mjlab adopts the manager-based environment
+design introduced by Isaac Lab. Users define their environment by composing
+small, self-contained *terms* (reward functions, observation computations,
+domain randomization events) and register them with the appropriate manager.
+Each manager handles the lifecycle of its terms: calling them at the right
+point in the simulation loop, aggregating their outputs, and exposing
+diagnostics.
+
+Terms can be plain functions for stateless computations, or classes that
+inherit from ``ManagerTermBase`` when they need to cache expensive setup
+(such as resolving regex patterns to joint indices at initialization) or
+maintain per-episode state through a ``reset()`` hook.
+
+Environments are configured through ``ManagerBasedRlEnvCfg``, a plain
+dataclass that holds term configuration dictionaries for each manager.
+
+.. code-block:: python
+
+    from mjlab.envs import ManagerBasedRlEnvCfg
+
+    cfg = ManagerBasedRlEnvCfg(
+        decimation=4,           # 4 physics steps per policy step
+        episode_length_s=20.0,
+        scene=...,              # SceneCfg: terrain, entities, sensors
+        sim=...,                # SimulationCfg: timestep, solver, integrator
+        observations={...},     # ObservationManager terms
+        actions={...},          # ActionManager terms
+        rewards={...},          # RewardManager terms
+        terminations={...},     # TerminationManager terms
+        events={...},           # EventManager terms (resets, DR)
+        commands={...},         # CommandManager terms (velocity targets, etc.)
+        curriculum={...},       # CurriculumManager terms
+        metrics={...},          # MetricsManager terms
+    )
+
+.. rubric:: The eight managers
+
+- **ObservationManager**: assembles observation groups with configurable
+  processing (clipping, noise, delay, history). Supports asymmetric
+  actor-critic. See :ref:`observations`.
+- **ActionManager**: routes the policy's output tensor to entity actuators,
+  handling scaling and offset. See :ref:`actions`.
+- **RewardManager**: computes a weighted sum of reward terms, scaled by step
+  duration for frequency invariance. See :ref:`rewards`.
+- **TerminationManager**: evaluates stop conditions, distinguishing terminal
+  resets from timeouts. See :ref:`terminations`.
+- **EventManager**: fires terms at lifecycle points (startup, reset,
+  interval). Domain randomization is implemented through event terms.
+  See :ref:`events` and :ref:`domain_randomization`.
+- **CommandManager**: generates and resamples goal signals (velocity
+  targets, pose targets). See :ref:`commands`.
+- **CurriculumManager**: adjusts training conditions based on policy
+  performance. See :ref:`curriculum`.
+- **MetricsManager**: logs custom per-step values as episode averages.
+  See :ref:`metrics`.
+
+For the full configuration reference covering all managers, see
+:ref:`environment_config`.
+
+
+The environment lifecycle
+-------------------------
+
+Each environment instance passes through four phases.
+
+1. **Build.** ``Scene`` composes entity MJCF files via ``MjSpec`` and
+   compiles ``MjModel`` on the CPU. ``Simulation`` uploads the model to the
+   GPU via MuJoCo Warp, allocating a single ``MjData`` with N parallel
+   worlds. CUDA graphs for ``step``, ``forward``, ``reset``, and ``sense``
+   are captured.
+
+2. **Initialize.** Managers are constructed from the term configuration
+   dictionaries. Regex patterns are matched to joint, body, and geom
+   indices. Observation history and delay buffers are allocated. Model
+   fields required by domain randomization terms are expanded from shared
+   to per-world storage, and CUDA graphs are rebuilt to reflect the new
+   layout. Startup events are fired once.
+
+3. **Reset.** Called at the start of training and whenever an environment
+   terminates or times out. The ``EventManager`` fires ``reset`` terms,
+   which return the scene to an initial state with optional randomization.
+   Command targets are resampled. Observation history buffers are cleared.
+
+4. **Step.** The policy action is processed by the ``ActionManager``. The
+   physics simulation advances ``decimation`` times, with actuator commands
+   applied and entity state updated each sub-step. After the decimation
+   loop, the ``TerminationManager`` checks stop conditions, the
+   ``RewardManager`` computes the reward signal, and any terminated
+   environments are reset. A single ``forward()`` call refreshes derived
+   quantities for all environments. The ``CommandManager`` advances or
+   resamples goals. Interval events fire if scheduled. Sensors update. The
+   ``ObservationManager`` assembles the observation for the next policy
+   query.
+
+The step sequence in order:
+
+.. code-block:: text
+
+    action_manager.process_action(action)
+    for _ in range(decimation):
+        action_manager.apply_action()
+        sim.step()
+        scene.update()
+    termination_manager.compute()
+    reward_manager.compute()
+    metrics_manager.compute()
+    [reset terminated envs]
+    sim.forward()
+    command_manager.compute()
+    event_manager.apply(mode="interval")
+    sim.sense()
+    observation_manager.compute()
+
+With this mental model in place, the Concepts pages cover each simulation
+layer component in detail, and The Manager Layer pages walk through each
+manager's configuration and built-in terms. If you are coming from Isaac
+Lab, :ref:`migration_isaac_lab` describes the key API differences.
diff --git a/05_software/train/rc_mjlab/mjlab/docs/source/changelog.rst b/05_software/train/rc_mjlab/mjlab/docs/source/changelog.rst
new file mode 100644
index 0000000..f769a1d
--- /dev/null
+++ b/05_software/train/rc_mjlab/mjlab/docs/source/changelog.rst
@@ -0,0 +1,603 @@
+=========
+Changelog
+=========
+
+Upcoming version (not yet released)
+-----------------------------------
+
+Added
+^^^^^
+
+- Added ``--log-root`` CLI option to ``train``, ``play``, and ``evaluate``
+  scripts for choosing where training logs are stored. Defaults to
+  ``logs/rsl_rl`` (unchanged behavior). Useful for directing outputs to a
+  scratch disk or shared mount.
+- ``RewardManager``, ``TerminationManager``, and ``MetricsManager`` now
+  validate that every term function returns a tensor of shape
+  ``(num_envs,)`` when evaluated, raising a clear ``ValueError``
+  naming the offending term instead of silently broadcasting or crashing
+  with an opaque error later during training.
+- Added ``ContactSensor.primary_names`` property to expose the resolved
+  primary names in the order they appear along the per-contact axis of the
+  output tensors. This makes it possible to map a contact-data column back
+  to the primary it belongs to (:issue:`914`).
+- Added per-world mesh variant support via ``VariantEntityCfg`` and
+  ``VariantCfg``. Each world in a batched simulation can now use a
+  different mesh asset for the same logical entity (e.g. world 0 holds a
+  cube, world 1 a sphere), with weights controlling the proportion of
+  worlds assigned to each variant. Mesh-derived constants (collision
+  bounds, body inertials, subtree mass, inverse weights) are compiled
+  per-variant and stored as per-world arrays in the Warp model, so domain
+  randomization, the native viewer, the offscreen renderer, and the Viser
+  viewer all pick up the variant assignment automatically. Variants must
+  share the same kinematic structure (same bodies, joints, joint types);
+  only mesh geoms may differ. Assignment is fixed at simulation init.
+  See :ref:`per_world_mesh` for usage. With help from @XiangruiJiang.
+
+Changed
+^^^^^^^
+
+- Bumped ``mujoco`` to 3.8 and ``mujoco-warp`` to 3.8.0. The ``multiccd``
+  enable flag was removed in mujoco 3.8 (it became default-on), so configs
+  that listed ``"multiccd"`` in ``MujocoCfg.enableflags`` need to drop it.
+- Camera segmentation now matches ``mujoco_warp``'s typed segmentation
+  output. ``CameraSensorData.segmentation`` stores ``(object_id,
+  object_type)`` pairs in shape ``[B, H, W, 2]`` instead of the previous
+  legacy geom-id-only layout. Contribution by @tkelestemur.
+- Sped up ``RayCaster`` post-processing by removing boolean-mask indexing
+  operations and replacing them with ``masked_fill_`` plus a clamped-distance
+  formulation of ``hit_pos_w`` that places misses at the world origin. This
+  removes all CUDA syncs from the ray post-process, letting the CPU thread
+  proceed while GPU-based sensing runs. Contribution by @bd-pdomanico.
+- Bumped ``rsl-rl-lib`` from 5.0.1 to 5.2.0. This brings ``torch.compile`` support for
+  PPO and Distillation, and optional std clamping and constant std in
+  ``GaussianDistribution``. No code changes required on the mjlab side.
+- ``TerrainEntityCfg`` debug visualization sites (environment origins,
+  terrain origins, flat patches) are now off by default. Set
+  ``debug_vis=True`` to re-enable them. The sites inflated ``nsite`` and
+  caused a measurable slowdown in the per-step ``site_local_to_global``
+  kernel (:issue:`942`).
+- Task package load failures during ``mjlab`` import now print the full
+  traceback (and the entry point's module path) to ``stderr`` instead of
+  just the exception message, making it easier to pinpoint the source of
+  import errors when running commands like ``list-envs`` (:issue:`910`).
+  Contribution by @saikishor.
+- Clarified ``ContactSensor`` shape conventions: per-contact fields
+  (``found``, ``force``, ``torque``, ``dist``, ``pos``, ``normal``,
+  ``tangent``) have shape ``[B, P * num_slots, ...]`` while per-primary
+  air-time fields (``current_air_time``, ``last_air_time``,
+  ``current_contact_time``, ``last_contact_time``) have shape ``[B, P]``,
+  where ``P`` is the number of resolved primaries (:issue:`914`).
+
+Fixed
+^^^^^
+
+- Fixed ``ManagerBasedRlEnv`` initializing Warp on all visible CUDA devices
+  even when constructed with ``device="cpu"``. ``seed_rng`` now accepts a
+  ``device`` argument and skips ``wp.rand_init`` on CPU devices, so a
+  CPU-only env no longer claims a CUDA context on machines with a visible
+  GPU (:issue:`949`).
+- Fixed ``ContactSensor.compute_first_contact`` and ``compute_first_air``
+  occasionally missing events when a contact began or ended right at the
+  last physics substep of a control step. ``current_contact_time`` /
+  ``current_air_time`` accumulate in float32 and can drift a few ULPs past
+  ``dt``, but the default ``abs_tol`` of ``1e-8`` sat at the noise floor
+  and rejected the comparison. Raised the default to ``1e-6``, which stays
+  well below typical control ``dt`` while comfortably covering float32
+  accumulation noise (:issue:`933`). Contribution by @paLeziart.
+- Fixed ``out_of_terrain_bounds`` using stale terrain dimensions. It read
+  ``TerrainGeneratorCfg.num_cols`` directly, which is ignored in curriculum
+  mode (the generator uses ``len(sub_terrains)`` columns instead), and it
+  did not account for ``border_width``. The termination now reads the
+  effective grid shape from ``terrain.terrain_origins`` and includes the
+  border in the footprint, so robots no longer reset while still on valid
+  terrain (or fail to reset after running off it) (:issue:`923`).
+- ``ObservationManager`` now skips observation groups that end up with
+  zero active terms (e.g. all terms set to ``None``) with a log message,
+  instead of crashing later in ``torch.stack``/``torch.cat``. This lets
+  a shared runner config define groups that become empty under certain
+  runtime flags (e.g. model-specific terms all disabled for one variant).
+  The whole group can still be set to ``None`` to disable it explicitly.
+- Fixed a runtime broadcast error in ``ContactSensor`` when combining
+  ``num_slots > 1`` with ``track_air_time=True`` and more than one primary.
+  Air-time tracking now reduces ``found`` across slots so that a primary is
+  considered in contact when any of its slots reports a match (:issue:`914`).
+- Updated the ``create_new_task.ipynb`` Colab tutorial to import
+  ``XmlActuatorCfg`` instead of the removed ``XmlVelocityActuatorCfg``.
+  Added a regression test (``tests/test_notebooks.py``) that parses each
+  notebook cell and verifies that every ``from mjlab... import X``
+  reference resolves, so future renames in the mjlab public API can't
+  silently rot the tutorials (:issue:`913`).
+- Fixed ``ObservationManager`` silently sharing a single ``NoiseModelCfg``
+  instance across observation groups that declared terms with the same
+  name. ``_group_obs_class_instances`` was keyed by term name alone, so
+  the last group processed in ``_prepare_terms`` overwrote earlier
+  groups' instances. Symptoms included the wrong noise config being
+  applied, shared per-episode state for ``NoiseModelWithAdditiveBias``
+  (e.g. bias drawn from the wrong ``bias_noise_cfg``), and missed
+  ``reset()`` calls for overwritten instances. Instances are now keyed
+  by ``(group_name, term_name)`` so each group owns its own noise model.
+- Fixed ``CurriculumManager.get_active_iterable_terms`` raising
+  ``TypeError`` when a term's state was a dict. The dict branch indexed
+  the output list by term name instead of appending to the local ``data``
+  list. No in-tree caller currently invokes this method, so the bug was
+  latent.
+
+Version 1.3.0 (April 14, 2026)
+------------------------------
+
+Added
+^^^^^
+
+- Added ``ManagerBasedRlEnvCfg.auto_reset`` flag. When ``True`` (default),
+  ``step()`` continues to reset done environments in place and returns the
+  post-reset observation. When ``False``, ``step()`` skips the reset block
+  and returns the terminal observation directly; the caller must call
+  ``reset(env_ids=...)`` for done environments before the next ``step()``
+  or a ``RuntimeError`` is raised. Enables access to the true terminal
+  state for algorithms that need it. Note that mjlab's bundled ``train.py``
+  uses rsl_rl's ``OnPolicyRunner``, which does not drive manual resets, so
+  ``auto_reset=False`` is intended for custom training loops (:issue:`900`).
+- Added ``ActuatorCfg.viscous_damping`` for passive velocity proportional
+  damping (``f = -b·v``), distinct from the PD derivative gain ``damping``
+  used by position and velocity actuators. Maps to ```` for
+  JOINT transmission and ```` for TENDON transmission.
+  Defaults to ``None`` (preserves the XML value).
+- Added :class:`~mjlab.managers.RecorderManager` for logging observations,
+  actions, or arbitrary environment data during rollouts. Implement a
+  :class:`~mjlab.managers.RecorderTerm` subclass and register it in the
+  ``recorders`` dict on ``ManagerBasedRlEnvCfg``. The manager provides
+  ``record_pre_reset``, ``record_post_reset``, and ``record_post_step``
+  lifecycle hooks with no opinion on how data is stored.
+- Added :func:`~mjlab.envs.mdp.curriculums.termination_curriculum` for
+  scheduling changes to termination term parameters during training,
+  matching the existing ``reward_curriculum`` pattern. Both now share a
+  single internal engine with init-time validation of stage ordering,
+  field existence, and param keys.
+- Added ``reduce`` field to ``MetricsTermCfg``. Setting ``reduce="last"``
+  reports the value from the final step of the episode rather than the
+  episode mean, which is useful for binary success metrics.
+- Added :class:`~mjlab.envs.mdp.actions.RelativeJointPositionAction` for
+  joint position control relative to the current configuration. The target is
+  ``current_pos + action * scale``, so a zero action holds the current
+  configuration rather than commanding the default pose.
+- Added :func:`~mjlab.envs.mdp.dr.pair_friction` for randomizing geom-pair
+  friction overrides (``pair_friction`` in ``mjModel``), with an
+  ``isotropic=True`` option that mirrors the symmetric tangent and roll
+  axes so single-axis randomization does not leave the paired axis stale.
+- Added ``STAIRS_TERRAINS_CFG`` terrain preset for progressive stair
+  curriculum training and ``@terrain_preset`` decorator for composing
+  terrain configurations from reusable presets.
+- Added cartpole balance and swingup tasks (``Mjlab-Cartpole-Balance`` and
+  ``Mjlab-Cartpole-Swingup``) with a :ref:`tutorial `
+  that walks through building an environment from scratch.
+- Added :ref:`motion imitation ` documentation with
+  preprocessing instructions. The README now links here instead of the
+  BeyondMimic repository, which produced incompatible NPZ files when used
+  with mjlab (:issue:`777`).
+- Added ``margin``, ``gap``, and ``solmix`` fields to ``CollisionCfg``
+  for per geom contact parameter configuration (:issue:`766`).
+- NaN guard now captures mocap body poses (``mocap_pos``, ``mocap_quat``)
+  when the model has mocap bodies, enabling full state reconstruction in
+  the dump viewer for fixed-base entities.
+- Implemented ``ActionTermCfg.clip`` for clamping processed actions after
+  scale and offset (:issue:`771`).
+- Added ``qfrc_actuator`` and ``qfrc_external`` generalized force accessors
+  to ``EntityData``. ``qfrc_actuator`` gives actuator forces in joint space
+  (projected through the transmission). ``qfrc_external`` recovers the
+  generalized force from body external wrenches (``xfrc_applied``)
+  (:issue:`776`).
+- Added ``RewardBarPanel`` to the Viser viewer, showing horizontal bars for
+  each reward term with a running mean over ~1 second (:issue:`800`).
+- Added ``per_substep`` flag to ``MetricsTermCfg`` for evaluating metrics
+  once per physics substep inside the decimation loop. The per substep
+  values are averaged within each environment step, so episode averages
+  remain comparable to regular per step metrics.
+- Added ``project-instinct/InstinctMJ`` to the research page's list of
+  projects built on mjlab.
+- Added a Checkpoints tab to the Viser play viewer for hot-swapping
+  checkpoints without restarting. Works with local directories and W&B
+  runs (:issue:`751`). Contribution by @omarrayyann.
+- Added ``"segmentation"`` camera data type for per-pixel geom ID output
+  alongside RGB and depth, and a multi-cube goal-conditioned lifting task
+  (``Mjlab-Multi-Cube-Seg-Yam``) that uses it (:issue:`862`).
+  Contribution by @pthangeda.
+
+Changed
+^^^^^^^
+
+- Renamed the ``list_envs`` console script to ``list-envs`` for consistency
+  with the other hyphenated entry points (``viz-nan``, ``export-scene``).
+  Invoke via ``uv run list-envs``.
+- ``ActuatorCfg.armature`` and ``ActuatorCfg.frictionloss`` now default to
+  ``None`` instead of ``0.0``. ``None`` preserves the value defined in the
+  XML. Previously, builtin actuators would silently overwrite XML joint and
+  tendon properties with zero when these fields were not explicitly set.
+  To restore the old behavior, pass ``armature=0.0`` or ``frictionloss=0.0``
+  explicitly.
+- Actuator delay is now configured inline on any ``ActuatorCfg`` subclass
+  (e.g. ``BuiltinPositionActuatorCfg(..., delay_min_lag=2, delay_max_lag=5)``)
+  instead of wrapping with ``DelayedActuatorCfg``. ``DelayedActuator``,
+  ``DelayedActuatorCfg``, and ``DelayedBuiltinActuatorGroup`` are removed.
+- Removed ``delay_target`` from ``ActuatorCfg``. Delay now always applies to
+  the actuator's ``command_field`` automatically. Multi-target delay
+  (``delay_target=("position", "velocity")``) is no longer supported.
+- ``XmlPositionActuatorCfg``, ``XmlVelocityActuatorCfg``, ``XmlMotorActuatorCfg``,
+  and ``XmlMuscleActuatorCfg`` are replaced by a single ``XmlActuatorCfg`` that auto
+  detects the actuator type from XML. Pass ``command_field=...`` to override detection.
+- Replaced the viser viewer internals with the ``mjviser`` package. Scene
+  creation, mesh conversion, and overlay rendering (contacts, forces,
+  inertia, tendons, joints, frames) are now provided by mjviser. The viewer
+  exposes a new Visualization tab for overlay controls and a Groups tab for
+  geom/site visibility. Debug visualization and warp tensor conversion remain
+  in mjlab's ``MjlabViserScene`` subclass (:issue:`839`).
+- In curriculum terrain mode, each terrain type now gets exactly one column
+  (``num_cols`` is set to ``len(sub_terrains)``). The ``proportion`` field
+  now controls robot spawning distribution across columns rather than column
+  count. Random mode is unchanged (:issue:`811`).
+- ``BoxSteppingStonesTerrainCfg`` stone size now decreases with difficulty,
+  interpolating from the large end of ``stone_size_range`` at difficulty 0
+  to the small end at difficulty 1 (:issue:`785`).
+- Removed deprecated ``TerrainImporter`` and ``TerrainImporterCfg`` aliases.
+  Use ``TerrainEntity`` and ``TerrainEntityCfg`` instead (:issue:`667`).
+- ``Entity.clear_state()`` is deprecated. Use ``Entity.reset()`` instead.
+  ``clear_state`` only zeroed actuator targets without resetting actuator
+  internal state (e.g. delay buffers), which could cause stale commands
+  after teleporting the robot to a new pose.
+- Removed ``EntityData.generalized_force``. The property was bugged (indexed
+  free joint DOFs instead of articulated DOFs) and the name was ambiguous.
+  Use ``qfrc_actuator`` or ``qfrc_external`` instead (:issue:`776`).
+- ``get_wandb_checkpoint_path`` now filters checkpoints server-side via the
+  ``pattern`` parameter, avoiding unnecessary pagination and tolerance to
+  corrupted metadata (:issue:`898`).
+
+Fixed
+^^^^^
+
+- ``train`` and ``play`` now print a top-level usage message when invoked
+  with ``-h`` / ``--help`` and no task argument, pointing users at
+  ``list-envs`` and `` --help`` (:issue:`905`).
+- Fixed ghost geom filtering in the Viser viewer. Ghost geoms were selected
+  by collision flags, so collision-disabled robot geoms appeared as ghosts.
+  The viewer now uses visual alpha to determine which geoms to render.
+- Scene now warns when an attached entity or terrain spec has non-default
+  ``