"""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)