caowangrenbo
46bbb06978
标定边界上移到 invicta_hardware： - raw↔model 的 dir/offset 由 hardware 独家施加（话题收发都是模型帧）， deploy 的 InvictaInterface 退化为透传，invicta_deploy.toml 不再有 dir/offset_deg。 这样 [joint_limits]（模型帧）与被限幅的指令同帧，/joint_states 也是模型角。 关节选择（部分接入 / 单腿调试）： - JointCommandArray 的 name 即本次要控制的关节子集（空 = 全部已配置关节）。 - 未选中的关节写「零力矩自由」块，而不是留全 0——按协议 (0,0,0,0,0) 会解码成 P_MIN/V_MIN/T_MIN，其中 tau=T_MIN（摆臂 -100 N·m）是危险的前馈力矩。 - 新增 control_word kSetZero(6) -> 0xEF，只作用于选中的摆臂（轮毂不支持，跳过）。 调试脚本 invicta_hardware/scripts/hw_debug_test.py： - 只起 hardware 节点调试：monitor/damping/angle/wheel/offset/zero 六种模式 + --joints。 - 限幅参数硬编码；发指令时默认回显模型角。 构建：toml++ 改为 vendored single-header（三个包各 third_party/tomlplusplus/toml++/toml.hpp）， 去掉 find_package(tomlplusplus) 与 -DTOML_HEADER_ONLY（单头自带 header-only）。 已在容器（localhost/custom-ros-noetic）中全新 catkin_make + smoke_test 验证通过。
201 lines
13 KiB
Markdown
201 lines
13 KiB
Markdown
# 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`](docs/zhishen_joint_module_canfd.md)（智身模组手册 V1.2）；文档索引见 [`docs/README.md`](docs/README.md)
|
||
|
||
> 代码源自 [DeepRoboticsLab/sdk_deploy](https://github.com/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`](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]]` 里，逐关节给：
|
||
|
||
```toml
|
||
[[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）
|
||
|
||
```bash
|
||
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」三种方式已不再需要。）
|
||
|
||
## 运行
|
||
|
||
```bash
|
||
# 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。
|