caowangrenbo
7ba2bba450
- MotionState 增加 error_code，新增 invicta_msgs/error_code.h 共用位定义；SafeController 写入、remote 展开成协议 ErrorList（component 暂为 0） - InvictaInterface 增加 IMU 断流/恢复判定（imu_timeout_s / imu_recover_s），covariance=-1 视为不可用；RL 丢失 IMU 走正常趴下 - invicta_hardware 不再发布硬编码单位四元数 IMU（含错误的 covariance[0]=0） - 更新 README 与 docs/TODO.md：记录已完成项、反馈帧 CANID 1~4 已确认、[joint_limits] 重复维护入待办
invicta_hardware
关节数据源与关节指令执行端。已接 SocketCAN + 智身关节模组 CANFD 协议（协议见 docs/zhishen_joint_module_canfd.md）。
它负责什么
以 publish_rate_hz（默认 200 Hz）为一个周期，循环做三件事：
ReadHardware()— 非阻塞收各路总线上的 12 字节反馈帧，更新关节位置/速度/力矩、温度、状态码、电压PublishState()— 发sensor_msgs/JointStateWriteHardware()— 使能后每周期发 MIT 运行帧（p / v / kp / kd / tau）
上游指令断流时（command_timeout_s，默认 100 ms）自动转阻尼帧（kp=0、速度/力矩 0、只留 timeout_*_kd），
避免 deploy 挂掉/卡住后本节点一直重发最后一帧。指令恢复后自动退出阻尼。
另按 diagnostics_rate_hz（默认 10 Hz）发 invicta_msgs/JointDiagnostics（温度/状态/电压）。
反馈不需要主动查询：模组对每条指令都回一帧状态（手册 §3.2），所以"发了指令就有反馈"，读回和写入天然同频。
| 话题 | 方向 | 消息 |
|---|---|---|
/joint_states |
发布 | sensor_msgs/JointState |
/joint_diagnostics |
发布 | invicta_msgs/JointDiagnostics（默认 10 Hz） |
/joint_commands |
订阅 | invicta_msgs/JointCommandArray |
控制字 → CAN 帧
control_word |
实际动作 |
|---|---|
kDisable(1) |
每关节块 0xFF×9 + 0xFD，线圈断开，之后不再发运行帧 |
kEnable(2) |
0xFF×9 + 0xFC，按 enable_frames/enable_interval_s 连续刷几帧 |
kMotorControl(3) |
发运行块；未使能时拒发（手册 §9.1 要求先使能再运行） |
kErrorReset(4) |
本协议没有独立清错指令（手册 §6：故障后靠重新使能/重启），按"重发使能"处理 |
kGetStatus(5) |
不需要发帧：状态随每条指令自动返回 |
kSetZero(6) |
0xFF×9 + 0xEF：把当前机械位置写成模组零位。只作用于指令 name 选中的关节，且只发摆臂（轮毂不支持，手册 §4.4）。之后该位姿反馈角≈0，请把对应 offset_deg 置 0 |
关节选择（部分接入 / 单腿调试）
JointCommandArray 的 name 列表就是本次要控制的关节子集：
name为空 → 全部已配置关节（deploy 一直填全部，行为不变）。name是子集 → 只驱动/置零这些关节；不在子集里的关节会被写入一个零力矩「自由」块（kp=kd=0、tau=0），既不驱动也不保持，也不参与置零。- ⚠ 这里不能简单地把未选中的块留成全 0：按协议
(0,0,0,0,0)会解码成P_MIN/V_MIN/T_MIN，其中tau=T_MIN（摆臂 −100 N·m）是危险的前馈力矩。所以显式写零力矩块。
- ⚠ 这里不能简单地把未选中的块留成全 0：按协议
- 数组长度（
position/velocity/effort/kp/kd）仍必须是全部关节数，只有name决定选择范围。 kEnable/kDisable/kErrorReset仍作用于全部已配置关节（总线级动作）；只有运行帧与kSetZero按name选择。- 控制字只在变化时落地：要对另一个子集重新置零，先把控制字切到别的值（如
kGetStatus）再切回kSetZero。
这样台架上只接一条腿也能单独控制、单独置零，不必把电机全接上。
同一路总线上最多 4 个关节（CANID 1~4），合成一帧下发：Byte0-1 是 MSGID 0x0100，每个 CANID 占
10 字节，块起点 2 + 10×(CANID−1)，长度按手册裁剪表（本机 4 个 ID → 48 字节）。
约定（最重要的部分）
- 关节角
[rad]、角速度[rad/s]、力矩[Nm]，按joint_names的固定顺序。 - 边界和限幅是两件事：
[motor.*]的 p/v/t 是归一化边界——只决定 CAN 帧里0~65535映射到多少物理量（编码/解码必须用同一套，手册 §5/§9.6），不是运行限幅；[joint_limits]才是唯一实际生效的限幅。下发顺序是「先按限幅夹紧 → 再归一化编码」。加载时会校验限幅不超过边界，超了直接报错，不靠编码饱和兜着。 - kp/kd 不是策略算的，来自 deploy 的
[policy].kp/kd与[leg.gains]（用户配置）。越界时本节点会夹到[0, kp_max]/[0, kd_max]并打警告——那种日志说明配置写错了，不要靠饱和兜着。 - 前馈力矩按
[joint_limits].torque_limit（默认取厂家峰值扭矩）夹紧，超过也告警。 - 本节点是 raw↔model 的唯一边界：话题上收发的都是模型帧（模型坐标、rad）。
[[legs]]的dir/offset_deg由本节点施加（q_model = q_motor·dir + offset，反向q_motor = (q_model − offset)·dir；速度/力矩只乘dir，不含 offset）。invicta_deploy侧不再做任何标定，只处理模型量。这样[joint_limits]（模型帧限幅）与被限幅的指令同帧，不会错位；任何看/joint_states的消费者拿到的也是有物理意义的角度。 - IMU 不由本节点发布。之前这里发的是硬编码的单位四元数 + 零角速度/零加速度（假数据），还把
orientation_covariance[0]标成0（有效），会让下游把「永远水平静止」当真值——已删除。姿态/角速度由独立的 IMU 节点发布到imu/data（话题名见 deploy/remote 的[topics].imu），契约见 invicta_deploy/README.md。 - 温度取自反馈帧 Byte8：
T = raw − 40（0255 ↔ −40215 ℃）；电压 Byte10 直接是伏。 - 状态码 Byte9：bit0 使能，bit1~6 故障（过压/过流/过温/过速/编码器/欠压）。故障位出现时本节点会打 ERROR 日志，并原样放进
/joint_diagnostics。
总线前置条件
位率由系统配置，本节点只负责 socket 收发：
sudo ip link set can0 up type can bitrate 1000000 dbitrate 5000000 fd on
sudo ip link set can1 up type can bitrate 1000000 dbitrate 5000000 fd on
[motor.can].bitrate/data_bitrate 只用于出错时的提示，不能透过 SocketCAN 设置。
无硬件调试开关
| 位置 | 默认 | 作用 |
|---|---|---|
[hardware].require_can |
true |
CAN 打不开就退出，避免"以为在控车其实没连上" |
roslaunch ... require_can:=false |
— | 无 CAN 也启动：照常发零位状态、缓存指令，但不下发电机指令 |
src/invicta_deploy/scripts/smoke_test.sh 用的就是 require_can:=false，因此容器/podman 里没有 CAN 也能跑通。
参数（本节点自己的 config/invicta_hardware.toml）
| TOML 段 | 内容 |
|---|---|
[robot].joint_names |
关节名与顺序（8 个：fl/fr/hl/hr × swing/wheel） |
[joint_limits] |
人为限幅（模型帧）：位置 [rad]、速度 [rad/s]、前馈力矩 [N·m]（下发前夹紧） |
[[legs]] ×4 |
每腿 swing/wheel 的 dir、offset_deg（硬件标定，本节点施加）、bus、canid |
[motor.joint] / [motor.wheel] |
归一化边界（0~65535 ↔ 物理量），编码/解码必须一致（P65MAX-S / W190-S） |
[motor.can] |
总线名、仲裁 ID、1M/5M、上电使能帧数与间隔 |
[motor.gains] |
bench_* 仅作参考（实际 kp/kd 由指令给）；timeout_*_kd 是看门狗阻尼 |
[topics] [hardware] |
本节点话题、200 Hz 状态频率、10 Hz 诊断频率、require_can、command_timeout_s |
运行
roslaunch invicta_hardware invicta_hardware.launch # 真机
roslaunch invicta_hardware invicta_hardware.launch require_can:=false # 无 CAN 调试
自查：rostopic echo /joint_diagnostics 看温度/状态；candump can0 看反馈帧的仲裁 ID（当前按
"反馈使用关节自身 CANID" 实现，若模组回帧 ID 不同需改 ReadHardware 的匹配）。