[software] 添加16DOF早期训练仿真与Sim2Real闭环

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

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)

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"

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