invicta（火蚁）RL sim2real 工程

从 DeepRobotics sdk_deploy 的 M20 sim2real 部分剥离出来的 ROS1（Noetic）工程，并按照厂家完整协议 proto.md 把遥控/设备协议与RL 控制拆成两个节点。

• 上游参考代码：sdk_deploy/src/M20_sdk_deploy（ROS2 / ament / drdds）
• 本工程：ROS1 / catkin / Noetic / Ubuntu 20.04
• 协议依据：proto.md（山猫 M20 basic_server V1.2.0，UDP + JSON）
• 关节模组 CANFD 协议：docs/zhishen_joint_module_canfd.md（智身模组手册 V1.2）；文档索引见 docs/README.md

代码源自 DeepRoboticsLab/sdk_deploy 的 M20 sim2real 部分，遵循 BSD-3-Clause（见 LICENSE）；已按 ROS1、厂家完整协议与本机器人形态重构。

目录与职责

invicta_ws/src/
├── invicta_msgs/     消息与共享值域（只用 3 条自定义消息，其余复用 sensor_msgs）
├── invicta_remote/   遥控/设备协议节点：UDP + JSON ↔ ROS，厂家错误码、状态上报
│                     config/invicta_remote.toml      节点参数
├── invicta_deploy/   RL 控制节点：状态机 + ONNX 推理 + 安全检查（不碰网络协议）
│                     config/invicta_deploy.toml      节点参数（含机器人模型/策略）
└── invicta_hardware/ 关节/IMU 数据源 + 关节指令执行端（骨架，硬件 I/O 待实现）
                      config/invicta_hardware.toml    节点参数（含电机/CAN）

这样拆的依据：协议面是平台级接口（控制 + 上报 + 充电/休眠/导航），会跟厂家版本演进；控制面要求 200 Hz 稳定运行、最怕重启。两者生命周期和变更节奏不同。

数据流

遥控 App ──UDP:30000 JSON──→ invicta_remote ──/motion_command──→ invicta_deploy
                               ↑                                    │
                               └────────── /motion_state ───────────┘
                                                                     │
invicta_hardware ──/joint_states (200 Hz)──→ invicta_deploy ──/joint_commands──→ invicta_hardware
                 ──/imu/data (200 Hz)──────→                (200 Hz，RL 状态下 50 Hz)

话题与消息

话题	消息	频率	说明
/joint_states	sensor_msgs/JointState（标准）	200 Hz	关节状态回馈
/imu/data	sensor_msgs/Imu（标准）	200 Hz	IMU 回报（四元数，弧度）
/joint_commands	invicta_msgs/JointCommandArray（自定义）	200 Hz / RL 下 50 Hz	关节指令（kp/kd/前馈/控制字）
/motion_command	invicta_msgs/MotionCommand（自定义）	事件 + 轴指令帧	协议节点 → RL 节点
/motion_state	invicta_msgs/MotionState（自定义）	20 Hz	RL 节点 → 协议节点（真实状态）

消息细节见 src/invicta_msgs/README.md。

频率

环节	频率	出处
控制周期	200 Hz（5 ms）	control_period_ms: 5
RL 策略	50 Hz（decimation 4）	InvictaPolicyRunner::SetDecimation(4)，agent_timestep=0.02
关节状态 / IMU 上报	200 Hz	与上游 M20 仿真发布一致；低于 100 Hz 明显退化，50 Hz 是功能下限
状态回报 /motion_state	20 Hz	协议里基础状态上报是 2 Hz，从这里取即可
协议轴指令	协议建议 ≥20 Hz	实测老版 App 静杆时更低，动杆时待确认

上游厂商侧口径：Lite3 的数据面上限 200 Hz 且必须整除 1000（内部 1 kHz）。M20 实机的真实上报频率请在接上硬件后用 rostopic hz 实测。

配置：每个节点一份自己的 TOML

原来的 config/*.yaml（ROS 参数表）换成 TOML，粒度不变：每个节点一份，节点自己决定读什么。 没有共享配置包，节点之间不通过配置文件耦合：

文件	归属	内容
invicta_deploy/config/invicta_deploy.toml	RL 节点	关节表与标定、腿的运动学（IK）、时序与各状态增益、站立后自动进 RL、策略与步态、本节点话题/队列/控制周期
invicta_remote/config/invicta_remote.toml	协议节点	UDP 端口、心跳超时、未实现项语义、本节点话题、四路状态上报频率
invicta_hardware/config/invicta_hardware.toml	硬件节点	关节表与硬件标定、每条腿挂的总线/CANID、电机量程与 CAN 参数、本节点话题与发布频率

launch 只传一个路径：<param name="config_file" value="$(find <本包>)/config/<本包>.toml"/>； 不传则默认取本包 config/ 下的同名文件。TOML 解析用 toml++ 的 single-header（toml.hpp， vendor 在三个包各自的 third_party/tomlplusplus/toml++/ 下）。单头版本本身就是 header-only， 实现直接编进节点：不需要 find_package、不需要系统装库、也不需要任何宏，目标机无运行时 .so 依赖。

改动节奏：改 TOML 不用重编；改各节点的配置头（deploy_config.h / remote_config.h / hardware_config.h）会自动触发该节点重编。

两个 TOML 结构坑（已在文件里注释）：① 数组表之后的裸键会归到最后一个元素，所以 [joint_limits]/[leg] 必须放在 legs 段之前；② 整数键要加引号（如 "4097"），roslaunch 不接受整数键。

关键约定：硬件约定 vs 模型约定（2 自由度腿）

本构型每条腿只有摆臂 + 轮两个关节（4 腿 × 2 = 8 关节），模型约定为：

• 摆臂角 θ=0 时摆臂竖直向下，θ 增大表示摆臂向外倒开；髋轴到轮轴的高度 h = L·cos(θ)。
• 站立：h = [leg].stand_height（默认 0.48 m，L=0.50 m → ±16.3°）。
• 趴下：h = [leg].lie_height（默认 0.10 m → ±78.5°），前腿向前倒、后腿向后倒（由 [[legs]].splay_sign 决定，与 M20 相反）。
• 轮关节指令 kp 恒为 0（速度/力矩模式，wheel_kp_zero）。

硬件约定（电机方向、机械零点）由 invicta_hardware 独家承担，写在它自己的 config/invicta_hardware.toml 的 [[legs]] 里，逐关节给：

[[legs]]
name = "fl"
swing_joint = "fl_swing"
wheel_joint = "fl_wheel"
swing = { dir = 1.0, offset_deg = 0.0, bus = "can0", canid = 1 }   # 硬件：电机正方向 / 机械零点
wheel = { dir = 1.0, offset_deg = 0.0, bus = "can0", canid = 3 }

指令换算 q_motor = (q_model − offset)·dir、反馈 q_model = q_motor·dir + offset（速度/力矩只乘 dir） 都在该节点一处完成；话题上收发的都是模型帧，invicta_deploy 的 invicta_interface 只做透传。 deploy 侧的 [[legs]] 只保留模型姿态约定 splay_sign。

第一次上机先按默认值（dir 见配置、offset_deg=0）跑，用调试脚本或 idle 打印的关节角与实测对照， 再填 dir/offset_deg；也可用调试脚本的置零（--mode zero）把当前位姿写为零位后置 offset_deg=0。

步态 = RL 模型

厂家协议里的 GaitParam 不是简单参数，每个步态对应一个 RL 模型：

GaitParam	步态	当前
0x1001 4097	基础（标准，平地）	✅ 默认模型
0x3002 12290	平地（敏捷）	❌ 未提供模型，请求被拒绝（训练好后在 gait/policy_map 填路径）
0x1003 4099	楼梯（标准）	❌ 同上
0x3003 12291	楼梯（敏捷）	❌ 同上

invicta_deploy 支持多模型：启动时把 gait/policy_map 里提供了路径的步态全部预加载，运行中按当前步态切换（切换时清空该模型的 last_action）。只有真正加载成功的步态才接受切换，/motion_state.gait 只报实际生效值，不会出现"显示切了但还在用旧模型"。

构建（Ubuntu 20.04 + ROS Noetic）

source /opt/ros/noetic/setup.bash
cd invicta_ws
catkin_make -j4                        # 交叉编译：-DBUILD_PLATFORM=arm
source devel/setup.bash

依赖：roscpp、roslib、sensor_msgs、std_msgs + 自建 invicta_msgs。eigen、onnxruntime 1.22（x86/aarch64）、policy.onnx、toml++ 均已 vendor，无需联网、无需系统装库。

toml++（已 vendor 进仓库，无需安装）

toml.hpp（single-header，上游 https://github.com/marzer/tomlplusplus，v3.4.0）放在三个包各自的 third_party/tomlplusplus/toml++/toml.hpp。单头版本本身就是 header-only（"drop it in, there is no step two"），不需要任何宏，实现直接编进节点：

• 不需要 find_package(tomlplusplus)、不需要 apt install、不需要 -DTOML_HEADER_ONLY，catkin_make 开箱即用；
• 目标机不需要 libtomlplusplus.so；
• 升级：下载新的 toml.hpp 覆盖三个包里的副本（保持同版本）即可，见各 third_party/tomlplusplus/README.md。

（早先的「系统安装 / 离线 .deb / 源码装到 /usr/local」三种方式已不再需要。）

运行

# 1) 硬件桥接（当前是骨架：发零位状态、不驱动电机）
roslaunch invicta_hardware invicta_hardware.launch

# 2) 协议节点（UDP 30000）
roslaunch invicta_remote invicta_remote.launch

# 3) RL 控制
roslaunch invicta_deploy invicta_deploy.launch
#    无硬件台架（状态是零位）时必须关掉自检，否则站立后不会自动进 RL：
#    roslaunch invicta_deploy invicta_deploy.launch rl_require_pose:=false rl_require_imu:=false

# 无硬件自检（起 roscore + 三个节点，测话题/频率/协议/ONNX）
bash src/invicta_deploy/scripts/smoke_test.sh

# 无遥控器时模拟协议报文
python3 src/invicta_remote/scripts/protocol_udp_test.py --steer 0.3,0,0 --motion 1

遥控器（云卓 G20）在 App 里把目标地址设为本机 IP、端口 30000。老版 App 用 2/22 MotionParam 切状态，新版（≥ V1.5.11）用 1101/12 按键事件，两套都支持。

已验证到什么程度

在 localhost/custom-ros-noetic:v1.0（Ubuntu 20.04 + ROS Noetic）容器里实测（8 关节 + 每节点自己的 TOML）：

项目	结果
四个包 catkin_make	干净构建通过（rc=0）
三份配置加载	deploy：dof=8 legs=4 L=0.500 stand=0.480 pre=0.200 lie=0.000；remote：UDP 30000、上报 2/10/2/2 Hz；hardware：8 关节、fl: swing can0#1 / wheel can0#3、电机 P65MAXS/W190S、CAN 2 路 @1M/5M
IK 目标角	站立 ±16.3°、收腿 ±66.4°、趴下 ±90.0°（摆臂水平），前腿 +、后腿 −（与 M20 相反）✓
趴下指令实测	订阅 /joint_commands 采样，四个摆臂峰值均为 1.5708 rad = 90.0°，轮关节 0 ✓
/joint_states	200.000 Hz，8 个关节名与配置一致
/motion_state	20.000 Hz
协议	心跳回 0；MotionParam=1 → idle→standup；MotionParam=4 → standup→liedown
站立后自动进 RL 自检	生效（骨架发零位 → [RL] 暂不进入 RL：关节 fl_swing 未到站立位姿，正确地拦住）

未验证：真实电机与 IMU（invicta_hardware 的 ReadHardware/WriteHardware 仍是 TODO）、实机上报频率、新机器人几何与 dir/offset_deg 的实测标定（TOML 里的 0.50 m / 0.48 m 等是待实测替换的占位值）。

与上游 M20 的差异

保留：状态机五状态、ONNX 策略与关节顺序/动作尺度、安全检查、厂家协议的头/JSON 结构与心跳、eigen/onnxruntime/policy.onnx。

删除或改写：16 关节三关节腿 → 8 关节（摆臂 + 轮）2 自由度腿，IK 换成单连杆反解、新增 splay_sign 倒向；死参数（机身长宽/髋偏置/轮半径/力矩限制/通用策略参数）全部删除；ROS2→ROS1；drdds 消息→标准消息 + 3 条自定义；电池订阅（上游死代码）；键盘控制模式；third_party/gamepad（未被引用）；Mujoco 仿真与 M20_description；Lite3 三个包；backward.hpp 死代码；M20 几何硬编码→ROS 参数；原厂控制字→平台无关控制字；UDP/JSON 协议层从 RL 进程拆到 invicta_remote。

已知缺口

1. 设备状态数据未接（1002/5 的电池/温度/CPU/GPS）：报文结构已按协议发出，但数值是占位 0，需要 invicta_hardware 提供（建议标准消息，如 sensor_msgs/BatteryState）。运控状态里的线速度/高度/剩余里程同理。
2. 遥控断连不停机：协议层 3 s 判离线只登记/log；UserCommand.time_stamp 已写入但断连判断未接线，策略会沿用最后一帧指令。当前按"硬件急停兜底"处理。
3. 未实现功能：照明灯、休眠、自主充电、导航模式/速度指令（均回 0xE00A）。
4. 关节温度与驱动器状态字缺失：sensor_msgs/JointState 无对应字段，是相对 M20 少的保护项（详见 invicta_msgs/README.md）。
5. 中文日志：ROS_* 文案在非 UTF-8 locale 下显示为 ?（容器内实测），机器人上用 UTF-8 终端则正常。
6. charge/hes/sleep 恒为 0：需要硬件侧提供充电状态、硬急停信号与休眠管理。

版本管理

third_party/eigen（22 MB）与 third_party/onnxruntime（113 MB）是 vendor 的二进制/头文件，便于离线构建；嫌大可改为系统 Eigen + 单独分发 ORT。