feat(hardware): 标定归硬件;按 name 选关节 + kSetZero;toml++ 改 vendored

标定边界上移到 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 验证通过。
This commit is contained in:
2026-10-23 13:44:27 +08:00
parent 594885b9e8
commit 46bbb06978
24 changed files with 54455 additions and 129 deletions
+24 -25
View File
@@ -71,7 +71,9 @@ invicta_hardware ──/joint_states (200 Hz)──→ invicta_deploy ──/joi
| `invicta_hardware/config/invicta_hardware.toml` | 硬件节点 | 关节表与硬件标定、每条腿挂的总线/CANID、**电机量程与 CAN 参数**、本节点话题与发布频率 |
launch 只传一个路径:`<param name="config_file" value="$(find <本包>)/config/<本包>.toml"/>`;
不传则默认取本包 `config/` 下的同名文件。TOML 解析用 toml++,以 **header-only 方式**使用(`-DTOML_HEADER_ONLY=1`),CMake 里只有一句 `find_package(tomlplusplus REQUIRED)`——库源码不进仓库,也不引入运行时动态库依赖。
不传则默认取本包 `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`)会自动触发该节点重编。
@@ -85,18 +87,24 @@ launch 只传一个路径:`<param name="config_file" value="$(find <本包>)/c
- 趴下:h = `[leg].lie_height`(默认 0.10 m → ±78.5°),**前腿向前倒、后腿向后倒**(由 `[[legs]].splay_sign` 决定,与 M20 相反)。
- 轮关节指令 `kp` 恒为 0(速度/力矩模式,`wheel_kp_zero`)。
硬件约定(电机方向、机械零点)在 `[[legs]]` 里逐关节给:
硬件约定(电机方向、机械零点)由 **`invicta_hardware`** 独家承担,写在它自己的
`config/invicta_hardware.toml` 的 `[[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 }
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_cmd = dir · θ_model + offset`,反馈反向(`invicta_interface` 一处完成)。
第一次上机先按默认值跑,用 idle 打印的关节角与实测对照,再填 `dir`/`offset_deg`。
指令换算 `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 模型**:
@@ -119,28 +127,19 @@ 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,无需联网。
依赖:`roscpp`、`roslib`、`sensor_msgs`、`std_msgs` + 自建 `invicta_msgs`。`eigen`、`onnxruntime 1.22`(x86/aarch64)、`policy.onnx`、`toml++` 均已 vendor,无需联网、无需系统装库。
### 一次性依赖:toml++(系统安装,源码不入仓库)
### toml++(已 vendor 进仓库,无需安装)
三个节点都用 `find_package(tomlplusplus REQUIRED)` 找它,**以 header-only 方式编译**(实现编进节点里),
因此目标机不需要额外部署 `libtomlplusplus.so`。
`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"),不需要任何宏**,实现直接编进节点:
```bash
# 方式一:Ubuntu 22.04+ 有官方包
sudo apt install libtomlplusplus-dev
- **不需要 `find_package(tomlplusplus)`、不需要 `apt install`、不需要 `-DTOML_HEADER_ONLY`**,`catkin_make` 开箱即用;
- 目标机不需要 `libtomlplusplus.so`;
- 升级:下载新的 `toml.hpp` 覆盖三个包里的副本(保持同版本)即可,见各 `third_party/tomlplusplus/README.md`。
# 方式二:目标机是 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 阶段报缺库并停下。
(早先的「系统安装 / 离线 .deb / 源码装到 /usr/local」三种方式已不再需要。)
## 运行