# 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）

> 代码源自 [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，厂家错误码、状态上报（待实现）
├── invicta_deploy/   RL 控制节点：状态机 + ONNX 推理 + 安全检查（不碰网络协议）
└── invicta_hardware/ 关节/IMU 数据源 + 关节指令执行端（骨架，硬件 I/O 待实现）
```

这样拆的依据：协议面是平台级接口（控制 + 上报 + 充电/休眠/导航），会跟厂家版本演进；控制面要求 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` 实测。

## 关键约定：硬件约定 vs policy 约定

关节量有两套约定，**必须只施加一次**：

- 硬件约定：电机方向、编码器零点、单位（CAN 原始值）。
- policy 约定：与 `policy.onnx` 训练一致（rad / rad/s / Nm）。

默认由 `invicta_hardware` 承担换算，话题上流动的就是 policy 约定；因此 `invicta_deploy` 的 `calibration/joint_dir` 全 1、`joint_offset_deg` 全 0、`auto_calibrate=false`。若反过来让 RL 节点承担，则填 M20 的表并打开 `auto_calibrate`：

| | 值 |
|---|---|
| `joint_dir` | `[1,1,-1,1, 1,-1,1,-1, -1,1,-1,1, -1,-1,1,-1]` |
| `joint_offset_deg` | `[-25,-131,160,0, 25,-131,160,0, -25,131,-160,0, 25,131,-160,0]` |

两条 M20 遗留语义保持原样：轮关节（下标 `%4==3`）指令 `kp` 强制 0（`wheel_kp_zero`）；关节顺序固定为 `fl/fr/hl/hr × hipx/hipy/knee/wheel`，与 `policy.onnx` 绑定。

## 步态 = 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` 已 vendor，无需联网。

## 运行

```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）容器里实测：

| 项目 | 结果 |
|---|---|
| 四个包 catkin_make | 通过 |
| `ldd` 解析 `libonnxruntime.so.1` | 通过（RPATH 生效） |
| `/joint_states` | 200.001 Hz，关节名/顺序正确 |
| `/joint_commands` | 站立 200 Hz；RL 状态 50.001 Hz，策略输出正常（kp=80，kd=2/0.6） |
| `/motion_state` | 20.000 Hz，RL 时 `motion_state=17`、`gait=4097` |
| 协议：心跳 / 轴指令 / MotionParam | 回 0，状态机正确切换（空闲→站立→RL） |
| 状态上报 `1002/3,4,5,6` | 2 / 10 / 2 / 2 Hz；RL 时 `BasicStatus.MotionState=17`、切步态后 `Gait` 同步变化 |
| 协议：非 RL 下切步态 | 回 `0xE008 Not allowed operation`（57352） |
| 协议：楼梯步态 | 协议节点回 0（已接收），RL 节点拒绝并记录"模型尚未注册" |
| 协议：照明灯 / 导航模式 / 未知命令 | `0xE00A`（57354）/ `0xE00A` / `0xE003`（57347） |
| 手柄实测（真遥控器） | 心跳、摇杆、MotionParam、照明灯、使用模式报文均能收到并正确应答 |

**未验证**：真实电机与 IMU（`invicta_hardware` 的 `ReadHardware/WriteHardware` 还是 TODO）、实机上报频率、新机器人几何参数、状态上报（未实现）。

## 与上游 M20 的差异

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

删除或改写：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。