refactor(config) + feat(model): 每节点一份 TOML 取代 ROS yaml；8 关节两自由度腿与单连杆 IK
配置（节点自治，跨包不共享）： - 新增各节点自己的 config/<节点>.toml（deploy / remote / hardware），只放本节点参数；删除三份 *.yaml - 各节点自带配置结构与加载器（deploy_config.h / remote_config.h / hardware_config.h）， 删除 state_machine/parameters/ 与 ControlParameters - TOML 解析库 toml++ 改为系统安装：find_package(tomlplusplus REQUIRED)，以 header-only 方式编译， 库源码不进仓库、目标机不需部署运行时 .so - launch 只传 config_file；改 TOML 不需重编，改配置头自动触发该节点重编 机器人模型（按本机构型改）： - 16 关节三关节腿 → 8 关节（每腿 摆臂 + 轮） - IK 改为单连杆反解 θ = splay_sign·acos(h/L)；趴下 h=0 → 摆臂水平（±90°） - 步态倒向：趴下时前腿向前、后腿向后（splay_sign 前 +1 / 后 −1），与 M20 相反 - 起立/趴下用样条 + 速度前馈；轮 kp 恒 0；限位改为逐关节查表 - 精简死参数：机身长宽、髋偏置、轮半径、力矩限制、通用策略参数，以及 auto_calibrate 与 ±360° 回绕标定 - 硬件侧参数入库：电机量程（P65MAXS/W190S）、CAN 通道与仲裁 ID、使能帧数/间隔、每腿 bus/canid、台架增益 保留（未改动 HEAD 已有功能）：多步态模型预加载与运行中切换、站立后自动进 RL 与就绪自检、 遥测四路上报、阻尼态单调时钟计时、策略输出 NaN 保护 验证（ROS Noetic 容器，干净构建）： - 四个包 catkin_make rc=0 - /joint_states 200 Hz（8 关节名与配置一致）、/motion_state 20 Hz - IK 目标角：站立 ±16.3°、收腿 ±66.4°、趴下 ±90.0°；趴下指令实测峰值 1.5708 rad - 协议 MotionParam=1/4 触发 idle→standup→liedown；自动进 RL 自检在零反馈下正确拦住 待实测：摆臂长度/站趴高度、各关节 dir/offset_deg；invicta_hardware 的 CAN 读写仍为 TODO
This commit is contained in:
@@ -13,9 +13,12 @@
|
||||
```
|
||||
invicta_ws/src/
|
||||
├── invicta_msgs/ 消息与共享值域（只用 3 条自定义消息，其余复用 sensor_msgs）
|
||||
├── invicta_remote/ 遥控/设备协议节点：UDP + JSON ↔ ROS，厂家错误码、状态上报（待实现）
|
||||
├── 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 稳定运行、最怕重启。两者生命周期和变更节奏不同。
|
||||
@@ -55,22 +58,44 @@ invicta_hardware ──/joint_states (200 Hz)──→ invicta_deploy ──/joi
|
||||
|
||||
上游厂商侧口径：Lite3 的数据面上限 200 Hz 且必须整除 1000（内部 1 kHz）。M20 实机的真实上报频率请在接上硬件后用 `rostopic hz` 实测。
|
||||
|
||||
## 关键约定：硬件约定 vs policy 约定
|
||||
## 配置：每个节点一份自己的 TOML
|
||||
|
||||
关节量有两套约定，**必须只施加一次**：
|
||||
原来的 `config/*.yaml`（ROS 参数表）换成 TOML，**粒度不变：每个节点一份，节点自己决定读什么**。
|
||||
没有共享配置包，节点之间不通过配置文件耦合：
|
||||
|
||||
- 硬件约定：电机方向、编码器零点、单位（CAN 原始值）。
|
||||
- policy 约定：与 `policy.onnx` 训练一致（rad / rad/s / Nm）。
|
||||
| 文件 | 归属 | 内容 |
|
||||
|---|---|---|
|
||||
| `invicta_deploy/config/invicta_deploy.toml` | RL 节点 | 关节表与标定、腿的运动学（IK）、时序与各状态增益、站立后自动进 RL、策略与步态、本节点话题/队列/控制周期 |
|
||||
| `invicta_remote/config/invicta_remote.toml` | 协议节点 | UDP 端口、心跳超时、未实现项语义、本节点话题、四路状态上报频率 |
|
||||
| `invicta_hardware/config/invicta_hardware.toml` | 硬件节点 | 关节表与硬件标定、每条腿挂的总线/CANID、**电机量程与 CAN 参数**、本节点话题与发布频率 |
|
||||
|
||||
默认由 `invicta_hardware` 承担换算，话题上流动的就是 policy 约定；因此 `invicta_deploy` 的 `calibration/joint_dir` 全 1、`joint_offset_deg` 全 0、`auto_calibrate=false`。若反过来让 RL 节点承担，则填 M20 的表并打开 `auto_calibrate`：
|
||||
launch 只传一个路径：`<param name="config_file" value="$(find <本包>)/config/<本包>.toml"/>`；
|
||||
不传则默认取本包 `config/` 下的同名文件。TOML 解析用 toml++，以 **header-only 方式**使用（`-DTOML_HEADER_ONLY=1`），CMake 里只有一句 `find_package(tomlplusplus REQUIRED)`——库源码不进仓库，也不引入运行时动态库依赖。
|
||||
|
||||
| | 值 |
|
||||
|---|---|
|
||||
| `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]` |
|
||||
改动节奏：**改 TOML 不用重编**；改各节点的配置头（`deploy_config.h` / `remote_config.h` / `hardware_config.h`）会自动触发该节点重编。
|
||||
|
||||
两条 M20 遗留语义保持原样：轮关节（下标 `%4==3`）指令 `kp` 强制 0（`wheel_kp_zero`）；关节顺序固定为 `fl/fr/hl/hr × hipx/hipy/knee/wheel`，与 `policy.onnx` 绑定。
|
||||
两个 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`）。
|
||||
|
||||
硬件约定（电机方向、机械零点）在 `[[legs]]` 里逐关节给：
|
||||
|
||||
```toml
|
||||
[[legs]]
|
||||
name = "fl"
|
||||
splay_sign = 1.0 # 模型姿态：趴下时向前倒
|
||||
swing = { dir = 1.0, offset_deg = 0.0 } # 硬件：电机正方向 / 机械零点
|
||||
wheel = { dir = 1.0, offset_deg = 0.0 }
|
||||
```
|
||||
|
||||
指令换算：`q_cmd = dir · θ_model + offset`，反馈反向（`invicta_interface` 一处完成）。
|
||||
第一次上机先按默认值跑，用 idle 打印的关节角与实测对照，再填 `dir`/`offset_deg`。
|
||||
## 步态 = RL 模型
|
||||
|
||||
厂家协议里的 `GaitParam` 不是简单参数，**每个步态对应一个 RL 模型**：
|
||||
@@ -95,6 +120,27 @@ source devel/setup.bash
|
||||
|
||||
依赖：`roscpp`、`roslib`、`sensor_msgs`、`std_msgs` + 自建 `invicta_msgs`。`eigen`、`onnxruntime 1.22`（x86/aarch64）、`policy.onnx` 已 vendor，无需联网。
|
||||
|
||||
### 一次性依赖：toml++（系统安装，源码不入仓库）
|
||||
|
||||
三个节点都用 `find_package(tomlplusplus REQUIRED)` 找它，**以 header-only 方式编译**（实现编进节点里），
|
||||
因此目标机不需要额外部署 `libtomlplusplus.so`。
|
||||
|
||||
```bash
|
||||
# 方式一：Ubuntu 22.04+ 有官方包
|
||||
sudo apt install libtomlplusplus-dev
|
||||
|
||||
# 方式二：目标机是 Ubuntu 20.04（Noetic 基线）或要交叉编译 aarch64 —— 源里没有此包，源码编译安装到系统
|
||||
# 源码自行获取（不放进本仓库），例如 tomlplusplus 3.4.0
|
||||
cmake -S tomlplusplus -B build -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=/usr/local
|
||||
cmake --build build -j
|
||||
sudo cmake --install build # 装头文件 + CMake 配置（tomlplusplusConfig.cmake）
|
||||
|
||||
# 方式三：离线镜像里预装 .deb（只含头文件与 CMake 配置，123 KB）
|
||||
sudo dpkg -i libtomlplusplus-dev_3.4.0+ds-0.2build2_amd64.deb
|
||||
```
|
||||
|
||||
装完 `cmake --find-package ... tomlplusplus` 能找到即可；`catkin_make` 会在 configure 阶段报缺库并停下。
|
||||
|
||||
## 运行
|
||||
|
||||
```bash
|
||||
@@ -120,29 +166,25 @@ python3 src/invicta_remote/scripts/protocol_udp_test.py --steer 0.3,0,0 --motion
|
||||
|
||||
## 已验证到什么程度
|
||||
|
||||
在 `localhost/custom-ros-noetic:v1.0`（Ubuntu 20.04 + ROS Noetic）容器里实测：
|
||||
在 `localhost/custom-ros-noetic:v1.0`（Ubuntu 20.04 + ROS Noetic）容器里实测（8 关节 + 每节点自己的 TOML）：
|
||||
|
||||
| 项目 | 结果 |
|
||||
|---|---|
|
||||
| 四个包 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）、实机上报频率、新机器人几何参数、状态上报（未实现）。
|
||||
| 四个包 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`。
|
||||
|
||||
删除或改写：ROS2→ROS1；`drdds` 消息→标准消息 + 3 条自定义；电池订阅（上游死代码）；键盘控制模式；`third_party/gamepad`（未被引用）；Mujoco 仿真与 `M20_description`；Lite3 三个包；`backward.hpp` 死代码；M20 几何硬编码→ROS 参数；原厂控制字→平台无关控制字；**UDP/JSON 协议层从 RL 进程拆到 `invicta_remote`**。
|
||||
删除或改写：**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`**。
|
||||
|
||||
## 已知缺口
|
||||
|
||||
|
||||
Reference in New Issue
Block a user