[software] 添加16DOF早期训练仿真与Sim2Real闭环
This commit is contained in:
@@ -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)
|
||||
Reference in New Issue
Block a user