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

README.md

@@ -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)`——库源码不进仓库，也不引入运行时动态库依赖。
 
-| | 值 |
+改动节奏：**改 TOML 不用重编**；改各节点的配置头（`deploy_config.h` / `remote_config.h` / `hardware_config.h`）会自动触发该节点重编。
-|---|---|
-| `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` 绑定。
+两个 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 | 通过 |
+| 四个包 catkin_make | 干净构建通过（`rc=0`） |
-| `ldd` 解析 `libonnxruntime.so.1` | 通过（RPATH 生效） |
+| 三份配置加载 | 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 |
-| `/joint_states` | 200.001 Hz，关节名/顺序正确 |
+| IK 目标角 | 站立 ±16.3°、收腿 ±66.4°、**趴下 ±90.0°（摆臂水平）**，前腿 +、后腿 −（与 M20 相反）✓ |
-| `/joint_commands` | 站立 200 Hz；RL 状态 50.001 Hz，策略输出正常（kp=80，kd=2/0.6） |
+| 趴下指令实测 | 订阅 `/joint_commands` 采样，四个摆臂峰值均为 **1.5708 rad = 90.0°**，轮关节 0 ✓ |
-| `/motion_state` | 20.000 Hz，RL 时 `motion_state=17`、`gait=4097` |
+| `/joint_states` | 200.000 Hz，8 个关节名与配置一致 |
-| 协议：心跳 / 轴指令 / MotionParam | 回 0，状态机正确切换（空闲→站立→RL） |
+| `/motion_state` | 20.000 Hz |
-| 状态上报 `1002/3,4,5,6` | 2 / 10 / 2 / 2 Hz；RL 时 `BasicStatus.MotionState=17`、切步态后 `Gait` 同步变化 |
+| 协议 | 心跳回 0；`MotionParam=1` → idle→standup；`MotionParam=4` → standup→liedown |
-| 协议：非 RL 下切步态 | 回 `0xE008 Not allowed operation`（57352） |
+| 站立后自动进 RL 自检 | 生效（骨架发零位 → `[RL] 暂不进入 RL：关节 fl_swing 未到站立位姿`，正确地拦住） |
-| 协议：楼梯步态 | 协议节点回 0（已接收），RL 节点拒绝并记录"模型尚未注册" |
-| 协议：照明灯 / 导航模式 / 未知命令 | `0xE00A`（57354）/ `0xE00A` / `0xE003`（57347） |
-| 手柄实测（真遥控器） | 心跳、摇杆、MotionParam、照明灯、使用模式报文均能收到并正确应答 |
-
-**未验证**：真实电机与 IMU（`invicta_hardware` 的 `ReadHardware/WriteHardware` 还是 TODO）、实机上报频率、新机器人几何参数、状态上报（未实现）。
 
+**未验证**：真实电机与 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`**。
 
 ## 已知缺口
 

src/invicta_deploy/CMakeLists.txt

@@ -31,10 +31,18 @@ catkin_package(
   CATKIN_DEPENDS roscpp roslib sensor_msgs invicta_msgs
 )
 
+# TOML 解析库：系统安装（见 README「依赖」），CMake 里体现为 required
+find_package(tomlplusplus REQUIRED)
+get_target_property(TOMLPLUSPLUS_INCLUDE_DIR tomlplusplus::tomlplusplus INTERFACE_INCLUDE_DIRECTORIES)
+include_directories(${TOMLPLUSPLUS_INCLUDE_DIR})
+# 以 header-only 方式使用：实现编进本节点，避免引入运行时动态库依赖（目标机无需部署 .so）
+add_definitions(-DTOML_HEADER_ONLY=1)
+
 set(THIRD_PARTY ${CMAKE_CURRENT_SOURCE_DIR}/third_party)
 set(ORT_ROOT ${THIRD_PARTY}/onnxruntime/${BUILD_PLATFORM})
 
 include_directories(
+  include
   include/types
   include/utils
   interface
@@ -42,7 +50,6 @@ include_directories(
   interface/robot/hardware
   interface/user_command
   state_machine
-  state_machine/parameters
   run_policy
   ${THIRD_PARTY}/eigen
   ${ORT_ROOT}/include
@@ -51,9 +58,17 @@ include_directories(
 
 file(GLOB_RECURSE STATE_MACHINE_SRC "state_machine/*.cpp")
 
+# 把本节点自己的配置头列进源列表：改该头会触发重编（改 TOML 不用重编）
+set(NODE_CONFIG_HEADER "${CMAKE_CURRENT_SOURCE_DIR}/include/config/deploy_config.h")
+if(NOT EXISTS "${NODE_CONFIG_HEADER}")
+  message(WARNING "invicta_deploy: 找不到本节点配置头")
+  set(NODE_CONFIG_HEADER "")
+endif()
+
 add_executable(invicta_deploy
   main.cpp
   ${STATE_MACHINE_SRC}
+  ${NODE_CONFIG_HEADER}
 )
 
 add_library(onnxruntime SHARED IMPORTED)
@@ -88,6 +103,6 @@ install(DIRECTORY ${ORT_ROOT}/lib/
 install(DIRECTORY policy
   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
 )
-install(DIRECTORY config launch
+install(DIRECTORY launch config
   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
 )

src/invicta_deploy/README.md

@@ -7,7 +7,7 @@ RL 控制节点：ROS 回调跑在 main 线程，状态机跑在 200 Hz 控制
 ## 源码结构
 
 ```
-main.cpp                              ros::init("invicta_deploy") + 状态机启动
+main.cpp                              ros::init + 加载 invicta.toml + 启动状态机
 include/types/                        RobotBasicState / RobotAction / UserCommand / 枚举
 include/utils/                        basic_function(Eigen 工具)、safe_controller(1 kHz 安全检查)、
                                       motion_state_feedback(状态回执)
@@ -21,7 +21,6 @@ interface/user_command/
 state_machine/state_machine_base.h    控制线程框架（timerfd + epoll，默认 5 ms）
 state_machine/state_base.h            状态基类
 state_machine/quadruped_wheel/        五个状态 + 状态机装配（invicta_state_machine.hpp）
-state_machine/parameters/             控制参数（全部来自 ROS 参数，默认值 = 上游 M20）
 run_policy/                           ONNX 推理：policy_runner_base + invicta_policy_runner
 policy/policy.onnx                    策略模型（与关节顺序/动作尺度绑定）
 scripts/smoke_test.sh                 无硬件自检
@@ -102,41 +101,40 @@ gait:
 
 > 实测（把同一份 ONNX 复制成另一个文件模拟新模型）：预加载成功、来回切换 3 轮共 6 次，`gait` 正确跟随、`/joint_commands` 保持 50 Hz、无报错。当前仓库配置只登记 `0x1001`，其余步态请求会被拒绝。
 
-## 参数（私有命名空间，`config/invicta_deploy.yaml`）
+## 参数（本节点自己的 `config/invicta_deploy.toml`）
 
-| 参数 | 默认 | 说明 |
+本节点不再读 ROS 参数表，也不读别的节点的配置；launch 只传 `config_file`（默认取本包同名文件）。
-|---|---|---|
-| `control_period_ms` | 5 | 控制周期，5 ms = 200 Hz |
-| `policy_path` | 空 | 空 = `$(find invicta_deploy)/policy/policy.onnx` |
-| `topics/joint_states` `topics/imu` `topics/joint_commands` | 见 yaml | 数据面话题 |
-| `topics/motion_command` / `topics/motion_state` | `motion_command` / `motion_state` | 与协议节点的接口 |
-| `queues/motion_command` | 1 | 指令只取最新值 |
-| `queues/joint_commands` / `queues/state` | 1 / 10 | 指令队列 1（新鲜度优先），状态队列 10 |
-| `motion_state_report_hz` | 20.0 | `/motion_state` 频率 |
-| `robot_version` | `INVICTA` | 协议 `BasicStatus.Version` |
-| `gait/default` `gait/policy_map` | 4097 / 两项 | 步态与模型 |
-| `joint_names` | 16 个 | 关节顺序，与 policy 绑定 |
-| `calibration/*` | 恒等 | 见根 README「硬件约定 vs policy 约定」 |
-| `wheel_kp_zero` | true | 轮关节 `kp` 强制 0 |
-| `timing/joint_damping_duration` | 3.0 | 阻尼过渡时长（0.1~30 s） |
-| `geometry/*` `gains/*` `limits/*` `timing/stand_duration` `timing/liedown_duration` | M20 值 | 换机器人主要改这里 |
 
-启动时会打印实际读到的 `joint_dir`、`joint_offset_deg`、话题名与模型路径。
+| TOML 段 | 内容 |
+|---|---|
+| `[robot]` | 关节名与顺序（8 个）、机型标识 |
+| `[joint_limits]` | 逐关节位置/速度限位（顺序同 joint_names） |
+| `[leg]` `[leg.timing]` `[leg.gains]` | 2 自由度腿的几何（摆臂长度、站/收腿/趴下高度）、时序、各状态 PD 增益 |
+| `[[legs]]` ×4 | 每腿的摆臂/轮关节名、`splay_sign`（倒向）、`dir`/`offset_deg`（硬件标定） |
+| `[rl]` | 站立完成后自动进 RL 的开关与自检阈值 |
+| `[policy]` | 模型路径、decimation、观测缩放、关节顺序、动作尺度、默认姿态、kp/kd |
+| `[gait.policies]` | 步态 → 模型文件（空 = 该步态不可用） |
+| `[topics]` `[queues]` `[control]` | 本节点话题、队列、控制周期、状态回报频率 |
 
+启动时打印配置摘要与各腿目标角，便于第一次上机核对。改 TOML 不需重编；改 `deploy_config.h` 会自动重编。
 ## 关节顺序（与 `policy.onnx` 绑定，勿改）
 
+机器人顺序（`robot.joint_names`，即 `/joint_states`、`/joint_commands` 的下标）：
+
 ```
-0  fl_hipx  1  fl_hipy  2  fl_knee  3  fl_wheel
+0 fl_swing   1 fl_wheel
-4  fr_hipx  5  fr_hipy  6  fr_knee  7  fr_wheel
+2 fr_swing   3 fr_wheel
-8  hl_hipx  9  hl_hipy 10  hl_knee 11  hl_wheel
+4 hl_swing   5 hl_wheel
-12 hr_hipx 13  hr_hipy 14  hr_knee 15  hr_wheel
+6 hr_swing   7 hr_wheel
 ```
 
-策略内部顺序不同（12 个腿关节在前、4 个轮在后），`InvictaPolicyRunner` 用 `robot2policy_idx` / `policy2robot_idx` 映射。
+策略内部顺序由 `[policy].joint_order` 给出（默认 4 个摆臂在前、4 个轮在后）；
+`InvictaPolicyRunner` 用置换矩阵在两种顺序之间映射，轮关节位置默认不进观测（`zero_obs_wheels`）。
 
+**模型文件放在本包 `policy/` 下**，TOML 里写相对路径（相对本包目录）；每个步态可指向不同模型。
 ## 数据面行为
 
-- `/joint_states`：优先按 `name` 匹配关节（顺序可不同）；`name` 缺失时按数组下标解析并告警一次；长度不等于 16 的帧丢弃；缺某个关节名时保留上一帧值并告警。
+- `/joint_states`：优先按 `name` 匹配关节（顺序可不同）；`name` 缺失时按数组下标解析并告警一次；长度不等于关节数的帧丢弃；缺某个关节名时保留上一帧值并告警。
 - `/imu/data`：`orientation_covariance[0] == -1` 视为"不提供姿态"，丢弃该帧并告警（不伪造姿态）；四元数模长近零的帧也丢弃。rpy 由四元数换算（ZYX，与 `RpyToRm` 一致）。
 - `/joint_commands`：`control_word` 见 `invicta_msgs/control_word.h`；启动时依次发 `kDisable → kErrorReset → kEnable`，运行时恒为 `kMotorControl`。
 
@@ -144,5 +142,5 @@ gait:
 
 ```bash
 source devel/setup.bash
-bash src/invicta_deploy/scripts/smoke_test.sh     # 三节点 + 话题/频率/协议/ONNX
+bash src/invicta_deploy/scripts/smoke_test.sh     # 三节点 + 话题/频率/协议/ONNX/配置摘要
 ```

src/invicta_deploy/config/invicta_deploy.toml

@@ -0,0 +1,145 @@
+# =============================================================================
+# invicta_deploy（RL 节点）自己的配置
+# 只放本节点需要的参数；话题由本节点声明，别的节点不读这份文件
+# 关节电机/CAN 相关参数在 invicta_hardware/config/invicta_hardware.toml
+# =============================================================================
+
+[robot]
+name    = "invicta"
+version = "INVICTA"                     # 协议 BasicStatus.Version
+# 关节顺序：下标即 /joint_states、/joint_commands 里的索引
+# 每腿两关节：摆臂 swing + 轮 wheel；腿序 前左(fl) / 前右(fr) / 后左(hl) / 后右(hr)
+joint_names = [
+  "fl_swing", "fl_wheel",
+  "fr_swing", "fr_wheel",
+  "hl_swing", "hl_wheel",
+  "hr_swing", "hr_wheel",
+]
+
+# ------------------------------------------------------------------ 关节限位
+# 顺序与 joint_names 一致；轮关节的 lower/upper 填 ±100 表示不限（用 vel_limit 约束）
+[joint_limits]
+lower     = [-1.40, -100.0,  -1.40, -100.0,  -1.40, -100.0,  -1.40, -100.0]   # [rad]
+upper     = [ 1.40,  100.0,   1.40,  100.0,   1.40,  100.0,   1.40,  100.0]   # [rad]
+vel_limit = [ 22.40, 100.0,  22.40, 100.0,  22.40, 100.0,  22.40, 100.0]      # [rad/s]
+
+# ------------------------------------------------------------------ 腿的运动学（2 自由度：摆臂 + 轮）
+# 模型约定：摆臂角 θ = 0 时摆臂竖直向下、轮轴在髋轴正下方；θ 增大表示摆臂向外倒开，
+#           髋轴到轮轴的高度 h = L·cos(θ)；反解 θ(h) = splay_sign · acos(h / L)
+# 注意：TOML 里数组表之后的裸键会归到最后一个元素，所以本节必须放在 legs 段之前
+[leg]
+swing_link_len = 0.50      # L：髋轴 → 轮轴 的距离 [m]（必须实测）
+wheel_radius   = 0.06      # 轮半径 [m]，只用于打印离地高度，IK 不用
+
+stand_height   = 0.48      # 站立：髋轴 → 轮轴 高度 [m]，应 ≤ L
+pre_height     = 0.20      # 起立中间的收腿姿态高度 [m]
+lie_height     = 0.00      # 趴下：0 = 摆臂**水平**（θ = acos(0) = 90°）
+                           # 若摆臂几何有偏置（轮轴不在摆臂轴线上），这里填该偏置
+
+[leg.timing]
+stand_duration_s   = 1.5   # 起立每一阶段（收腿 / 撑起）的样条时长
+pre_hold_s         = 0.0   # 收腿姿态额外保持（0 = 不停留）
+liedown_duration_s = 2.0   # 趴下样条时长
+damping_hold_s     = 3.0   # 关节阻尼保持多久后回 idle（缓慢松下 → 着地 → 自由）
+
+[leg.gains]
+stand_kp = 200.0           # 起立：摆臂位置刚度 [Nm/rad]
+stand_kd = 4.0             # 起立：摆臂阻尼
+lie_kp   = 300.0           # 趴下：摆臂位置刚度
+lie_kd   = 4.5             # 趴下：摆臂阻尼
+damping_kd_leg   = 4.0     # 阻尼态：摆臂 kd
+damping_kd_wheel = 1.0     # 阻尼态：轮 kd（把轮刹住）
+wheel_kp_zero    = true    # 轮的 kp 强制置 0（速度/力矩模式）
+
+# ------------------------------------------------------------------ 站立后自动进 RL
+# 原厂机制：站立到位并自检通过即自动切 RL，不必等遥控器指令
+[rl]
+auto_enter         = true
+enter_delay_s      = 0.5    # 到站立位姿后再稳定多久 [s]
+require_pose       = true   # 是否检查摆臂角已到站立位姿（无硬件台架时可关）
+joint_tolerance    = 0.15   # 摆臂角容差 [rad]
+velocity_tolerance = 1.0    # 摆臂角速度容差 [rad/s]
+require_imu        = true   # 是否检查 IMU 数据有效
+
+# ------------------------------------------------------------------ 每腿标定
+# 摆臂指令角 = dir · θ_model + offset_deg，θ_model = splay_sign · acos(h/L)
+#   splay_sign：模型姿态约定。趴下/收腿时摆臂往哪边倒——本题要求「前腿向前、后腿向后」，
+#               即前腿 +1、后腿 −1（与 M20 相反）
+#   dir       ：硬件约定，电机正方向与模型正方向的关系（±1）
+#   offset_deg：机械零点偏置 [deg]，第一次上机按 idle 打印角与实测对照后填写
+[[legs]]
+name        = "fl"
+swing_joint = "fl_swing"
+wheel_joint = "fl_wheel"
+splay_sign  =  1.0
+swing = { dir =  1.0, offset_deg = 0.0 }
+wheel = { dir =  1.0, offset_deg = 0.0 }
+
+[[legs]]
+name        = "fr"
+swing_joint = "fr_swing"
+wheel_joint = "fr_wheel"
+splay_sign  =  1.0
+swing = { dir = -1.0, offset_deg = 0.0 }
+wheel = { dir = -1.0, offset_deg = 0.0 }
+
+[[legs]]
+name        = "hl"
+swing_joint = "hl_swing"
+wheel_joint = "hl_wheel"
+splay_sign  = -1.0
+swing = { dir =  1.0, offset_deg = 0.0 }
+wheel = { dir =  1.0, offset_deg = 0.0 }
+
+[[legs]]
+name        = "hr"
+swing_joint = "hr_swing"
+wheel_joint = "hr_wheel"
+splay_sign  = -1.0
+swing = { dir = -1.0, offset_deg = 0.0 }
+wheel = { dir = -1.0, offset_deg = 0.0 }
+
+# ------------------------------------------------------------------ 策略模型
+[policy]
+# 相对路径基于本包目录（模型放在 invicta_deploy/policy/ 下）
+path       = "policy/policy.onnx"
+decimation = 4                     # 每 N 个控制周期推理一次（200 Hz / 4 = 50 Hz）
+omega_scale   = 0.25               # 观测里的角速度缩放
+dof_vel_scale = 0.05               # 观测里的关节速度缩放
+zero_obs_wheels = true             # 轮关节位置不进观测（与 M20 同做法）
+
+# 策略内部关节顺序；与 joint_names 不同时在此列出（代码自动做置换）
+joint_order = [
+  "fl_swing", "fr_swing", "hl_swing", "hr_swing",
+  "fl_wheel", "fr_wheel", "hl_wheel", "hr_wheel",
+]
+# 以下四个数组的顺序 = joint_order
+action_scale      = [0.25, 0.25, 0.25, 0.25,  5.0, 5.0, 5.0, 5.0]
+default_joint_pos = [0.0, 0.0, 0.0, 0.0,  0.0, 0.0, 0.0, 0.0]   # 训练时的默认姿态 [rad]
+kp = [80.0, 80.0, 80.0, 80.0,  0.0, 0.0, 0.0, 0.0]
+kd = [ 2.0,  2.0,  2.0,  2.0,  0.6, 0.6, 0.6, 0.6]
+
+# ------------------------------------------------------------------ 步态 → RL 模型
+# 每个步态对应一个模型；空路径 = 该步态不可用（模型尚未训练）
+[gait]
+default = 4097                     # 0x1001 基础(平地)
+[gait.policies]
+"4097"  = ""                       # 0x1001 基础(平地) → policy.path
+"12290" = ""                       # 0x3002 平地(敏捷) → 暂共用
+
+# ------------------------------------------------------------------ 本节点的话题与节奏
+[topics]
+joint_states    = "joint_states"     # 订阅 sensor_msgs/JointState
+imu             = "imu/data"         # 订阅 sensor_msgs/Imu
+joint_commands  = "joint_commands"   # 发布 invicta_msgs/JointCommandArray
+motion_command  = "motion_command"   # 订阅 invicta_msgs/MotionCommand
+motion_state    = "motion_state"     # 发布 invicta_msgs/MotionState
+
+[control]
+period_ms       = 5                  # 控制周期，5 ms = 200 Hz
+state_report_hz = 20.0               # /motion_state 发布频率
+
+[queues]
+motion_command  = 1
+joint_commands  = 1
+state           = 10

src/invicta_deploy/config/invicta_deploy.yaml

@@ -1,105 +0,0 @@
-# invicta_deploy 参数（私有命名空间 ~，由 launch 加载到 /invicta_deploy/ 下）
-
-# ---- 控制周期 ----
-control_period_ms: 5           # 5 ms = 200 Hz，与策略 decimation=4（50 Hz）配套
-
-# ---- 策略 ----
-policy_path: ""                # 留空 = $(find invicta_deploy)/policy/policy.onnx
-
-# ---- 话题 ----
-topics:
-  joint_states: joint_states              # 订阅 sensor_msgs/JointState
-  imu: imu/data                           # 订阅 sensor_msgs/Imu
-  joint_commands: joint_commands           # 发布 invicta_msgs/JointCommandArray
-  motion_command: motion_command           # 订阅 invicta_msgs/MotionCommand（来自 invicta_remote）
-  motion_state: motion_state               # 发布 invicta_msgs/MotionState（给 invicta_remote）
-
-queues:
-  motion_command: 1            # 指令只关心最新值
-  joint_commands: 1            # 指令只关心最新值，队列不堆积
-  state: 10                    # 状态流按 200 Hz 设计，队列给足以吸收抖动
-
-# ---- 站立完成后自动进入 RL（原厂机制）----
-# 原厂不是等用户发 MotionParam=17，而是站立完成后自检通过就自动切 RL；用户指令也可以，
-# 但同样要满足自检。自检不通过时一直保持站立（/motion_state 报 1），每秒打印一次原因。
-rl:
-  auto_enter: true
-  enter_delay: 0.5             # 到达站立位姿后再稳定多久 [s]
-  require_pose: true           # 关节角必须到达站立位姿（无硬件台架时设为 false）
-  joint_tolerance: 0.15        # 关节角容差 [rad]
-  velocity_tolerance: 1.0      # 关节速度容差 [rad/s]
-  require_imu: true            # IMU 数据必须有效（无硬件台架时设为 false）
-
-# ---- 状态回报 ----
-motion_state_report_hz: 20.0   # /motion_state 发布频率（协议状态上报 2 Hz 时从这里取）
-robot_version: "INVICTA"       # 对应协议 BasicStatus.Version
-
-# ---- 步态 → RL 模型 ----
-# 不同步态对应不同的 RL 模型：
-#   0x1001 基础(平地) / 0x3002 平地(敏捷) → 当前共用 policy_path 里的模型
-#   0x1003 楼梯 / 0x3003 楼梯(敏捷)      → 模型尚未训练，请求会被拒绝并记录日志
-# 键是十进制，等号右边是模型文件路径（空 = 用 policy_path）
-gait:
-  default: 4097                # 0x1001 基础，协议规定起立后回到基础步态
-  # 键必须加引号（roslaunch 不接受整数键）；值 = 模型文件路径，留空表示"没有模型"。
-  # 只有这里提供了模型路径的步态才会在启动时加载、才允许切换；填了路径但加载失败的也不可用。
-  policy_map:
-    "4097": ""                 # 0x1001 基础(平地) → 复用 policy_path 的默认模型
-    # "12290": "/path/to/flat_agile.onnx"   # 0x3002 平地(敏捷)，训练好后填这里
-    # "4099":  "/path/to/stairs.onnx"       # 0x1003 楼梯
-    # "12291": "/path/to/stairs_agile.onnx" # 0x3003 楼梯(敏捷)
-
-# ---- 关节顺序（索引即指令/状态数组下标，与 policy.onnx 绑定，勿随意改）----
-joint_names:
-  [fl_hipx_joint, fl_hipy_joint, fl_knee_joint, fl_wheel_joint,
-   fr_hipx_joint, fr_hipy_joint, fr_knee_joint, fr_wheel_joint,
-   hl_hipx_joint, hl_hipy_joint, hl_knee_joint, hl_wheel_joint,
-   hr_hipx_joint, hr_hipy_joint, hr_knee_joint, hr_wheel_joint]
-
-# ---- 关节标定 ----
-# 默认恒等：约定硬件约定（方向/零点/单位）由 invicta_hardware 负责，
-# 话题上流动的就是 policy 约定下的量。
-# 如果反过来让本节点承担 M20 的原始约定，就把下面两张表填成 M20 的值并打开 auto_calibrate，
-# 参考值（上游 M20，角度制）：
-#   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]
-# 二者只能选一处施加，不要两边都做。
-calibration:
-  joint_dir:        [1.0,1.0,1.0,1.0, 1.0,1.0,1.0,1.0, 1.0,1.0,1.0,1.0, 1.0,1.0,1.0,1.0]
-  joint_offset_deg: [0.0,0.0,0.0,0.0, 0.0,0.0,0.0,0.0, 0.0,0.0,0.0,0.0, 0.0,0.0,0.0,0.0]
-  auto_calibrate: false        # 仅当上报的是原始编码器角（需要等 16 关节数据更新并做 ±360 回绕修正）时打开
-  wrap_hip_deg: 140.0          # 标定用回绕阈值，对应 hipy（下标 %4==1）
-  wrap_knee_deg: 164.0         # 标定用回绕阈值，对应 knee（下标 %4==2）
-
-# ---- 轮关节是否强制 kp=0（速度/力矩模式）----
-wheel_kp_zero: true
-
-# ---- 机器人几何（默认值来自上游 M20，换机器人时改这里）----
-geometry:
-  body_len_x: 0.619
-  body_len_y: 0.13
-  hip_len: 0.104
-  thigh_len: 0.25
-  shank_len: 0.25
-  pre_height: 0.12
-  stand_height: 0.48
-  wheel_link_len: 0.040575
-
-# ---- PD 增益（站立/摆动相）----
-gains:
-  swing_leg_kp: [200.0, 200.0, 200.0]
-  swing_leg_kd: [4.0, 4.0, 4.0]
-
-# ---- 限制 ----
-limits:
-  fl_joint_lower: [-0.4363, -2.443, -2.758]   # rad
-  fl_joint_upper: [0.6109, 2.443, 2.758]      # rad
-  joint_vel_limit: [22.4, 22.4, 22.4]         # rad/s
-  torque_limit: [76.4, 76.4, 76.4]            # Nm
-  wheel_vel_limit: 100.0                      # rad/s
-
-# ---- 时长 ----
-timing:
-  stand_duration: 1.5          # 起立曲线时长（总时长约为 2 倍）
-  liedown_duration: 2.0        # 趴下曲线时长（总时长约为 2 倍）
-  joint_damping_duration: 3.0  # 阻尼过渡时长：松腿→着地→转空闲（0.1~30 s）

src/invicta_deploy/include/config/deploy_config.h

@@ -0,0 +1,341 @@
+/**
+ * @file deploy_config.h
+ * @brief invicta_deploy 自己的配置（TOML）：config/invicta_deploy.toml
+ *
+ * 只包含本节点需要的参数（关节表与标定、腿的运动学、状态机时序与增益、策略与步态、
+ * 本节点的话题/队列/控制周期）。本节点不读别的节点的配置文件。
+ */
+#pragma once
+
+#include <toml++/toml.hpp>
+
+#include <algorithm>
+#include <cmath>
+#include <cstdint>
+#include <map>
+#include <string>
+#include <vector>
+
+namespace invicta {
+
+/// 单个关节：限位与硬件标定
+struct JointSpec {
+    std::string name;
+    bool is_wheel = false;
+    int leg = -1;
+    double lower = 0., upper = 0.;  ///< 位置限位 [rad]
+    double vel_limit = 0.;          ///< 速度限位 [rad/s]
+    double dir = 1.;                ///< 硬件方向 ±1
+    double offset_deg = 0.;         ///< 硬件零点偏置 [deg]
+};
+
+/// 一条腿：摆臂 + 轮
+struct LegSpec {
+    std::string name;
+    std::string swing_joint, wheel_joint;
+    double splay_sign = 1.;  ///< 模型姿态约定：趴下时摆臂倒向（前腿 +1 向前、后腿 −1 向后）
+    int swing_index = -1, wheel_index = -1;
+};
+
+class DeployConfig {
+public:
+    // ---- 机器人 ----
+    std::string robot_name = "invicta";
+    std::string robot_version = "INVICTA";
+    std::vector<std::string> joint_names;
+    std::vector<JointSpec> joints;  ///< 顺序同 joint_names
+    std::vector<LegSpec> legs;
+    int dof = 0;
+
+    // ---- 腿的运动学（2 自由度：摆臂 + 轮）----
+    double swing_link_len = 0.5;  ///< L：髋轴 → 轮轴 [m]
+    double wheel_radius = 0.06;
+    double stand_height = 0.48, pre_height = 0.20, lie_height = 0.0;
+
+    // ---- 时序与增益 ----
+    double stand_duration_s = 1.5, pre_hold_s = 0.0, liedown_duration_s = 2.0, damping_hold_s = 3.0;
+    double stand_kp = 200., stand_kd = 4., lie_kp = 300., lie_kd = 4.5;
+    double damping_kd_leg = 4., damping_kd_wheel = 1.;
+    bool wheel_kp_zero = true;
+
+    // ---- 站立后自动进 RL ----
+    bool rl_auto_enter = true;
+    double rl_enter_delay = 0.5;
+    bool rl_require_pose = true;
+    double rl_joint_tolerance = 0.15, rl_velocity_tolerance = 1.0;
+    bool rl_require_imu = true;
+
+    // ---- 策略 ----
+    std::string policy_path;  ///< 相对本包目录、或绝对路径
+    int policy_decimation = 4;
+    double omega_scale = 0.25, dof_vel_scale = 0.05;
+    bool zero_obs_wheels = true;
+    std::vector<std::string> policy_joint_order;
+    std::vector<double> action_scale, default_joint_pos, policy_kp, policy_kd;
+    int action_dim = 0, obs_dim = 0;
+
+    // ---- 步态 → 模型 ----
+    uint32_t default_gait = 0x1001;
+    std::map<uint32_t, std::string> gait_policies;
+
+    // ---- 本节点话题与节奏 ----
+    std::string topic_joint_states = "joint_states", topic_imu = "imu/data";
+    std::string topic_joint_commands = "joint_commands";
+    std::string topic_motion_command = "motion_command", topic_motion_state = "motion_state";
+    int control_period_ms = 5;
+    double state_report_hz = 20.0;
+    int queue_motion_command = 1, queue_joint_commands = 1, queue_state = 10;
+
+    bool Load(const std::string& path, std::string* error = nullptr);
+
+    // ---- 查询与运动学 ----
+    int IndexOf(const std::string& name) const {
+        auto it = std::find(joint_names.begin(), joint_names.end(), name);
+        return it == joint_names.end() ? -1 : int(it - joint_names.begin());
+    }
+    const JointSpec& Joint(int i) const { return joints[size_t(i)]; }
+    bool IsWheel(int i) const { return joints[size_t(i)].is_wheel; }
+    std::vector<int> WheelIndices() const {
+        std::vector<int> v;
+        for (int i = 0; i < dof; ++i)
+            if (joints[size_t(i)].is_wheel) v.push_back(i);
+        return v;
+    }
+    /// 模型约定的摆臂角：θ = splay_sign · acos(h / L)
+    double SwingAngleForHeight(double h, int leg) const {
+        const double L = swing_link_len;
+        double c = (L > 1e-9) ? h / L : 1.0;
+        c = std::max(-1.0, std::min(1.0, c));
+        return legs[size_t(leg)].splay_sign * std::acos(c);
+    }
+    double HeightForSwingAngle(double theta) const { return swing_link_len * std::cos(theta); }
+
+    std::string Summary() const;
+};
+
+inline DeployConfig& Config() {
+    static DeployConfig cfg;
+    return cfg;
+}
+
+// ------------------------------------------------------------------ TOML 读取小工具
+namespace detail {
+
+inline const toml::table* Section(const toml::table& t, const char* key) { return t[key].as_table(); }
+
+template <typename T>
+inline void Get(const toml::table* sect, const char* key, T& out) {
+    if (!sect) return;
+    if (auto v = (*sect)[key].value<T>()) out = *v;
+}
+inline void GetStr(const toml::table* sect, const char* key, std::string& out) {
+    if (!sect) return;
+    if (auto v = (*sect)[key].value<std::string>()) out = *v;
+}
+inline void GetBool(const toml::table* sect, const char* key, bool& out) {
+    if (!sect) return;
+    if (auto v = (*sect)[key].value<bool>()) out = *v;
+}
+inline void GetDoubleArr(const toml::table* sect, const char* key, std::vector<double>& out) {
+    if (!sect) return;
+    if (const auto* arr = (*sect)[key].as_array()) {
+        out.clear();
+        for (const auto& e : *arr)
+            if (auto v = e.value<double>()) out.push_back(*v);
+    }
+}
+inline void GetStrArr(const toml::table* sect, const char* key, std::vector<std::string>& out) {
+    if (!sect) return;
+    if (const auto* arr = (*sect)[key].as_array()) {
+        out.clear();
+        for (const auto& e : *arr)
+            if (auto v = e.value<std::string>()) out.push_back(*v);
+    }
+}
+
+}  // namespace detail
+
+inline bool DeployConfig::Load(const std::string& path, std::string* error) {
+    const auto fail = [&](const std::string& msg) {
+        if (error) *error = msg;
+        return false;
+    };
+
+    toml::table tbl;
+    try {
+        tbl = toml::parse_file(path);
+    } catch (const toml::parse_error& e) {
+        return fail(std::string("TOML 解析失败: ") + std::string(e.description()) + " @ " + path);
+    }
+
+    const toml::table* robot = detail::Section(tbl, "robot");
+    if (!robot) return fail("缺少 [robot] 段");
+    detail::GetStr(robot, "name", robot_name);
+    detail::GetStr(robot, "version", robot_version);
+    detail::GetStrArr(robot, "joint_names", joint_names);
+    if (joint_names.empty()) return fail("[robot].joint_names 为空");
+    dof = int(joint_names.size());
+    if (dof % 2 != 0) return fail("joint_names 数量必须是偶数（每腿 摆臂+轮）");
+
+    std::vector<double> lower, upper, vel_limit;
+    const toml::table* limits = detail::Section(tbl, "joint_limits");
+    detail::GetDoubleArr(limits, "lower", lower);
+    detail::GetDoubleArr(limits, "upper", upper);
+    detail::GetDoubleArr(limits, "vel_limit", vel_limit);
+    if (int(lower.size()) != dof || int(upper.size()) != dof || int(vel_limit.size()) != dof) {
+        return fail("[joint_limits] 的 lower/upper/vel_limit 长度必须等于关节数量");
+    }
+    joints.assign(size_t(dof), JointSpec{});
+    for (int i = 0; i < dof; ++i) {
+        joints[size_t(i)].name = joint_names[size_t(i)];
+        joints[size_t(i)].lower = lower[size_t(i)];
+        joints[size_t(i)].upper = upper[size_t(i)];
+        joints[size_t(i)].vel_limit = vel_limit[size_t(i)];
+    }
+
+    legs.clear();
+    if (const auto* leg_arr = tbl["legs"].as_array()) {
+        for (const auto& elem : *leg_arr) {
+            const toml::table* lt = elem.as_table();
+            if (!lt) continue;
+            LegSpec leg;
+            detail::GetStr(lt, "name", leg.name);
+            detail::GetStr(lt, "swing_joint", leg.swing_joint);
+            detail::GetStr(lt, "wheel_joint", leg.wheel_joint);
+            detail::Get(lt, "splay_sign", leg.splay_sign);
+            leg.swing_index = IndexOf(leg.swing_joint);
+            leg.wheel_index = IndexOf(leg.wheel_joint);
+            if (leg.swing_index < 0 || leg.wheel_index < 0) {
+                return fail("[[legs]] " + leg.name + " 的 swing_joint/wheel_joint 不在 joint_names 里");
+            }
+            const toml::table* sw = detail::Section(*lt, "swing");
+            const toml::table* wh = detail::Section(*lt, "wheel");
+            detail::Get(sw, "dir", joints[size_t(leg.swing_index)].dir);
+            detail::Get(sw, "offset_deg", joints[size_t(leg.swing_index)].offset_deg);
+            detail::Get(wh, "dir", joints[size_t(leg.wheel_index)].dir);
+            detail::Get(wh, "offset_deg", joints[size_t(leg.wheel_index)].offset_deg);
+            joints[size_t(leg.swing_index)].leg = int(legs.size());
+            joints[size_t(leg.wheel_index)].leg = int(legs.size());
+            joints[size_t(leg.wheel_index)].is_wheel = true;
+            legs.push_back(leg);
+        }
+    }
+    if (legs.empty()) return fail("缺少 [[legs]] 段");
+    if (int(legs.size()) * 2 != dof) return fail("腿数与关节数不匹配（每腿 2 个关节）");
+
+    const toml::table* gc = detail::Section(tbl, "leg");
+    detail::Get(gc, "swing_link_len", swing_link_len);
+    detail::Get(gc, "wheel_radius", wheel_radius);
+    detail::Get(gc, "stand_height", stand_height);
+    detail::Get(gc, "pre_height", pre_height);
+    detail::Get(gc, "lie_height", lie_height);
+    const toml::table* timing = gc ? detail::Section(*gc, "timing") : nullptr;
+    detail::Get(timing, "stand_duration_s", stand_duration_s);
+    detail::Get(timing, "pre_hold_s", pre_hold_s);
+    detail::Get(timing, "liedown_duration_s", liedown_duration_s);
+    detail::Get(timing, "damping_hold_s", damping_hold_s);
+    const toml::table* gains = gc ? detail::Section(*gc, "gains") : nullptr;
+    detail::Get(gains, "stand_kp", stand_kp);
+    detail::Get(gains, "stand_kd", stand_kd);
+    detail::Get(gains, "lie_kp", lie_kp);
+    detail::Get(gains, "lie_kd", lie_kd);
+    detail::Get(gains, "damping_kd_leg", damping_kd_leg);
+    detail::Get(gains, "damping_kd_wheel", damping_kd_wheel);
+    detail::GetBool(gains, "wheel_kp_zero", wheel_kp_zero);
+
+    if (swing_link_len <= 1e-6) return fail("[leg].swing_link_len 必须 > 0");
+    for (const auto& pair : {std::make_pair("stand_height", stand_height),
+                             std::make_pair("pre_height", pre_height),
+                             std::make_pair("lie_height", lie_height)}) {
+        if (pair.second > swing_link_len) {
+            return fail(std::string("[leg].") + pair.first + " 大于 swing_link_len，物理上不可达");
+        }
+    }
+
+    const toml::table* rl = detail::Section(tbl, "rl");
+    detail::GetBool(rl, "auto_enter", rl_auto_enter);
+    detail::Get(rl, "enter_delay_s", rl_enter_delay);
+    detail::GetBool(rl, "require_pose", rl_require_pose);
+    detail::Get(rl, "joint_tolerance", rl_joint_tolerance);
+    detail::Get(rl, "velocity_tolerance", rl_velocity_tolerance);
+    detail::GetBool(rl, "require_imu", rl_require_imu);
+
+    if (const toml::table* pol = detail::Section(tbl, "policy")) {
+        detail::GetStr(pol, "path", policy_path);
+        detail::Get(pol, "decimation", policy_decimation);
+        detail::Get(pol, "omega_scale", omega_scale);
+        detail::Get(pol, "dof_vel_scale", dof_vel_scale);
+        detail::GetBool(pol, "zero_obs_wheels", zero_obs_wheels);
+        detail::GetStrArr(pol, "joint_order", policy_joint_order);
+        detail::GetDoubleArr(pol, "action_scale", action_scale);
+        detail::GetDoubleArr(pol, "default_joint_pos", default_joint_pos);
+        detail::GetDoubleArr(pol, "kp", policy_kp);
+        detail::GetDoubleArr(pol, "kd", policy_kd);
+        if (policy_joint_order.empty()) policy_joint_order = joint_names;
+        if (int(policy_joint_order.size()) != dof) return fail("[policy].joint_order 长度必须等于关节数量");
+        for (const auto& n : policy_joint_order) {
+            if (IndexOf(n) < 0) return fail("[policy].joint_order 里的 " + n + " 不在 joint_names 里");
+        }
+        action_dim = dof;
+        for (const auto& arr : {&action_scale, &default_joint_pos, &policy_kp, &policy_kd}) {
+            if (int(arr->size()) != action_dim) {
+                return fail("[policy] 的 action_scale/default_joint_pos/kp/kd 长度必须等于关节数量");
+            }
+        }
+        obs_dim = 9 + 3 * action_dim;  // 角速度(3) + 投影重力(3) + 速度指令(3) + 关节角 + 关节速度 + 上一步动作
+    }
+
+    const toml::table* gait = detail::Section(tbl, "gait");
+    detail::Get(gait, "default", default_gait);
+    if (gait) {
+        if (const toml::table* pol = detail::Section(*gait, "policies")) {
+            for (const auto& [k, v] : *pol) {
+                try {
+                    gait_policies[uint32_t(std::stoul(std::string(k.str())))] = v.value_or(std::string{});
+                } catch (const std::exception&) {
+                    return fail("[gait.policies] 的键必须能转成整数步态值");
+                }
+            }
+        }
+    }
+
+    const toml::table* topics = detail::Section(tbl, "topics");
+    detail::GetStr(topics, "joint_states", topic_joint_states);
+    detail::GetStr(topics, "imu", topic_imu);
+    detail::GetStr(topics, "joint_commands", topic_joint_commands);
+    detail::GetStr(topics, "motion_command", topic_motion_command);
+    detail::GetStr(topics, "motion_state", topic_motion_state);
+
+    const toml::table* control = detail::Section(tbl, "control");
+    detail::Get(control, "period_ms", control_period_ms);
+    detail::Get(control, "state_report_hz", state_report_hz);
+
+    const toml::table* queues = detail::Section(tbl, "queues");
+    detail::Get(queues, "motion_command", queue_motion_command);
+    detail::Get(queues, "joint_commands", queue_joint_commands);
+    detail::Get(queues, "state", queue_state);
+
+    if (error) error->clear();
+    return true;
+}
+
+inline std::string DeployConfig::Summary() const {
+    char buf[512];
+    std::string s;
+    snprintf(buf, sizeof(buf), "[config] %s dof=%d legs=%zu L=%.3f m  stand=%.3f pre=%.3f lie=%.3f\n",
+             robot_name.c_str(), dof, legs.size(), swing_link_len, stand_height, pre_height, lie_height);
+    s += buf;
+    for (size_t i = 0; i < legs.size(); ++i) {
+        const double sw = SwingAngleForHeight(stand_height, int(i));
+        const double li = SwingAngleForHeight(lie_height, int(i));
+        snprintf(buf, sizeof(buf), "[config]   %s splay=%+.0f 站立摆臂=%+.1f° 趴下=%+.1f°\n",
+                 legs[i].name.c_str(), legs[i].splay_sign, sw * 180. / M_PI, li * 180. / M_PI);
+        s += buf;
+    }
+    snprintf(buf, sizeof(buf), "[config] policy=%s obs=%d action=%d decimation=%d gait(default)=0x%04X\n",
+             policy_path.c_str(), obs_dim, action_dim, policy_decimation, default_gait);
+    s += buf;
+    return s;
+}
+
+}  // namespace invicta

src/invicta_deploy/interface/robot/hardware/invicta_interface.hpp

@@ -1,13 +1,15 @@
 /**
  * @file invicta_interface.hpp
- * @brief invicta 的 ROS1 数据面：订阅关节状态与 IMU，发布关节指令
+ * @brief ROS1 数据面：订阅关节状态与 IMU，发布关节指令。关节表、话题、标定全部来自 invicta.toml
  *
- * 由上游 M20 的 DdsInterface + M20Interface 合并而来（原来分两层是为了和 Lite3 共用，
+ * 硬件约定（方向 dir、零点 offset、单位）在本层一次性归一：
- * 这里只有一台机器人，合成一个类更好读）。差异：
+ *   反馈：q_model = q_robot · dir + offset
- *   - ros2/drdds  →  ROS1 + sensor_msgs/JointState + sensor_msgs/Imu
+ *   指令：q_robot = (q_des − offset) · dir
- *   - 控制字从原厂驱动器私有值改为 invicta_msgs/control_word.h 的平台无关集合
+ * 上层的状态机与策略只看到「模型约定」下的量（rad / rad/s / Nm）。
- *   - 删除电池订阅（上游是死代码）
+ *
- *   - 关节方向/零点标定改为 ROS 参数，默认「恒等」，即约定硬件桥接节点已给出 policy 约定下的数据
+ * 相比之前：删掉了 auto_calibrate / ±360° 回绕标定（那是 M20 三关节腿的绝对编码器问题，
+ * 本构型摆臂行程只有 ±80°，无回绕；标定统一由 invicta.toml 的 dir/offset 表达），
+ * 轮关节判定也从 i%4==3 改为按配置的 is_wheel。
  */
 #pragma once
 
@@ -21,6 +23,7 @@
 
 #include "basic_function.hpp"
 #include "common_types.h"
+#include "config/deploy_config.h"
 #include "invicta_msgs/JointCommandArray.h"
 #include "invicta_msgs/control_word.h"
 #include "robot_interface.h"
@@ -28,68 +31,40 @@
 namespace interface {
 
 class InvictaInterface : public RobotInterface {
-protected:
-    ros::NodeHandle nh_;             ///< 话题用：相对节点命名空间（默认 /joint_states 等）
-    ros::NodeHandle pnh_{"~"};       ///< 参数用：私有命名空间 ~topics/... ~calibration/...
-
-    // ---- 话题与队列 ----
-    std::string joints_topic_ = "joint_states";
-    std::string imu_topic_    = "imu/data";
-    std::string cmd_topic_    = "joint_commands";
-    int cmd_queue_            = 1;   ///< 指令流只关心最新值，队列不堆积
-    int state_queue_          = 10;  ///< 状态流按 200 Hz 设计，队列给足以吸收抖动
-
-    // ---- 关节约定 ----
-    std::vector<std::string> joint_names_;
-    std::vector<float> joint_dir_;      ///< +1 / -1
-    std::vector<float> pos_offset_deg_; ///< 关节零点偏置 [deg]
-
-    bool auto_calibrate_ = false;
-    float wrap_hip_deg_  = 140.f;  ///< 标定时 hipy(k%4==1) 的回绕阈值
-    float wrap_knee_deg_ = 164.f;  ///< 标定时 knee(k%4==2) 的回绕阈值
-    bool wheel_kp_zero_ = true;    ///< 轮关节 (k%4==3) 强制 kp=0，速度/力矩模式
-
-    // ---- 状态缓存 ----
-    double ri_ts_ = 0., imu_ts_ = 0.;
-    Vec3f omega_body_, rpy_, acc_;
-    VecXf joint_pos_, joint_vel_, joint_tau_;
-    std::unordered_map<std::string, int> name2slot_;
-    uint16_t joint_data_frame_ = 0;
-    std::vector<uint16_t> joint_data_id_;
-    std::vector<bool> joint_data_updated_;  ///< 标定用：该关节是否已看到数据变化
-    std::vector<uint16_t> driver_status_;
-    bool warned_name_mismatch_ = false;
-    bool warned_imu_no_orientation_ = false;
-
-    ros::Publisher cmd_pub_;
-    ros::Subscriber joint_state_sub_, imu_sub_;
-
 public:
-    InvictaInterface(const std::string& robot_name, int dof_num) : RobotInterface(robot_name, dof_num) {
+    InvictaInterface(const std::string& robot_name, int dof_num)
+        : RobotInterface(robot_name, dof_num), cfg_(invicta::Config()) {
+        if (dof_num_ != cfg_.dof) {
+            ROS_WARN("[InvictaInterface] 传入 dof=%d 与配置的 %d 不一致，按配置执行", dof_num_, cfg_.dof);
+        }
         joint_pos_ = VecXf::Zero(dof_num_);
         joint_vel_ = VecXf::Zero(dof_num_);
         joint_tau_ = VecXf::Zero(dof_num_);
-        joint_data_id_.resize(dof_num_, 0);
+        joint_data_id_.resize(size_t(dof_num_), 0);
-        driver_status_.resize(dof_num_, 1);
+        driver_status_.assign(size_t(dof_num_), 1);
 
-        LoadParams();
+        for (int i = 0; i < dof_num_; ++i) name2slot_[cfg_.joint_names[size_t(i)]] = i;
 
-        cmd_pub_ = nh_.advertise<invicta_msgs::JointCommandArray>(cmd_topic_, cmd_queue_);
+        cmd_pub_ = nh_.advertise<invicta_msgs::JointCommandArray>(cfg_.topic_joint_commands,
-        joint_state_sub_ = nh_.subscribe(joints_topic_, state_queue_, &InvictaInterface::JointStateHandler, this);
+                                                                  cfg_.queue_joint_commands);
-        imu_sub_ = nh_.subscribe(imu_topic_, state_queue_, &InvictaInterface::ImuHandler, this);
+        joint_state_sub_ = nh_.subscribe(cfg_.topic_joint_states, cfg_.queue_state,
+                                        &InvictaInterface::JointStateHandler, this);
+        imu_sub_ = nh_.subscribe(cfg_.topic_imu, cfg_.queue_state, &InvictaInterface::ImuHandler, this);
     }
 
     virtual ~InvictaInterface() {}
 
     // ------------------------------------------------------------------ 接口实现
     virtual void Start() {
-        // 启动控制序列：先关使能，清错误，再使能。此后恒发 kMotorControl。
         SendStartupSequence();
-        if (auto_calibrate_) {
+        std::cout << "[InvictaInterface] topics: " << cfg_.topic_joint_states << " / " << cfg_.topic_imu << " -> "
-            ResetPositionOffset();
+                  << cfg_.topic_joint_commands << std::endl;
-        } else {
+        std::cout << "[InvictaInterface] 关节标定（invicta.toml）: ";
-            std::cout << "[InvictaInterface] auto_calibrate=false，按参数给定的 dir/offset 直接使用\n";
+        for (int i = 0; i < dof_num_; ++i) {
+            std::cout << cfg_.joint_names[size_t(i)] << "(dir=" << cfg_.Joint(i).dir << ",off="
+                      << cfg_.Joint(i).offset_deg << "°) ";
         }
+        std::cout << std::endl;
     }
 
     virtual void Stop() {}
@@ -110,22 +85,20 @@ public:
         joint_cmd_ = input;
         invicta_msgs::JointCommandArray msg;
         msg.header.stamp = ros::Time::now();
-        msg.name = joint_names_;
+        msg.name = cfg_.joint_names;
-        msg.position.resize(dof_num_);
+        msg.position.resize(size_t(dof_num_));
-        msg.velocity.resize(dof_num_);
+        msg.velocity.resize(size_t(dof_num_));
-        msg.effort.resize(dof_num_);
+        msg.effort.resize(size_t(dof_num_));
-        msg.kp.resize(dof_num_);
+        msg.kp.resize(size_t(dof_num_));
-        msg.kd.resize(dof_num_);
+        msg.kd.resize(size_t(dof_num_));
         for (int i = 0; i < dof_num_; ++i) {
-            msg.position[i] = (input(i, 1) - Deg2Rad(pos_offset_deg_[i])) * joint_dir_[i];
+            const auto& j = cfg_.Joint(i);
-            msg.velocity[i] = input(i, 3) * joint_dir_[i];
+            msg.position[i] = (input(i, 1) - Deg2Rad(float(j.offset_deg))) * j.dir;
-            msg.effort[i] = input(i, 4) * joint_dir_[i];
+            msg.velocity[i] = input(i, 3) * j.dir;
-            msg.kp[i] = input(i, 0);
+            msg.effort[i] = input(i, 4) * j.dir;
+            msg.kp[i] = (cfg_.wheel_kp_zero && j.is_wheel) ? 0.f : input(i, 0);
             msg.kd[i] = input(i, 2);
         }
-        if (wheel_kp_zero_) {
-            for (int i = 3; i < dof_num_; i += 4) msg.kp[i] = 0.;
-        }
         msg.control_word = invicta_msgs::kMotorControl;
         cmd_pub_.publish(msg);
     }
@@ -133,161 +106,61 @@ public:
     virtual void RefreshRobotData() {}
 
 protected:
-    // ------------------------------------------------------------------ 参数
+    // ------------------------------------------------------------------ 启动序列
-    void LoadParams() {
-        pnh_.param<std::string>("topics/joint_states", joints_topic_, "joint_states");
-        pnh_.param<std::string>("topics/imu", imu_topic_, "imu/data");
-        pnh_.param<std::string>("topics/joint_commands", cmd_topic_, "joint_commands");
-        pnh_.param<int>("queues/joint_commands", cmd_queue_, 1);
-        pnh_.param<int>("queues/state", state_queue_, 10);
-
-        joint_names_ = DefaultJointNames();
-        pnh_.getParam("joint_names", joint_names_);
-        if (static_cast<int>(joint_names_.size()) != dof_num_) {
-            ROS_ERROR("[InvictaInterface] 参数 joint_names 长度 %zu != dof_num %d，使用默认顺序",
-                      joint_names_.size(), dof_num_);
-            joint_names_ = DefaultJointNames();
-        }
-        name2slot_.clear();
-        for (int i = 0; i < dof_num_; ++i) {
-            name2slot_[joint_names_[i]] = i;
-        }
-
-        joint_dir_.assign(dof_num_, 1.f);
-        pos_offset_deg_.assign(dof_num_, 0.f);
-        pnh_.getParam("calibration/joint_dir", joint_dir_);
-        pnh_.getParam("calibration/joint_offset_deg", pos_offset_deg_);
-        if (static_cast<int>(joint_dir_.size()) != dof_num_) joint_dir_.assign(dof_num_, 1.f);
-        if (static_cast<int>(pos_offset_deg_.size()) != dof_num_) pos_offset_deg_.assign(dof_num_, 0.f);
-
-        pnh_.param<bool>("calibration/auto_calibrate", auto_calibrate_, false);
-        pnh_.param("calibration/wrap_hip_deg", wrap_hip_deg_, 140.f);
-        pnh_.param("calibration/wrap_knee_deg", wrap_knee_deg_, 164.f);
-        pnh_.param<bool>("wheel_kp_zero", wheel_kp_zero_, true);
-
-        ROS_INFO_STREAM("[InvictaInterface] topics: " << joints_topic_ << " / " << imu_topic_ << " / " << cmd_topic_
-                                                       << " | auto_calibrate=" << auto_calibrate_
-                                                       << " wheel_kp_zero=" << wheel_kp_zero_);
-        std::cout << "[InvictaInterface] joint_dir:        ";
-        for (float d : joint_dir_) std::cout << d << " ";
-        std::cout << "\n[InvictaInterface] joint_offset_deg: ";
-        for (float o : pos_offset_deg_) std::cout << o << " ";
-        std::cout << std::endl;
-    }
-
-    static std::vector<std::string> DefaultJointNames() {
-        // 与 policy.onnx 的动作/观测顺序一致，见 invicta_deploy/README.md
-        return {"fl_hipx_joint", "fl_hipy_joint", "fl_knee_joint", "fl_wheel_joint",
-                "fr_hipx_joint", "fr_hipy_joint", "fr_knee_joint", "fr_wheel_joint",
-                "hl_hipx_joint", "hl_hipy_joint", "hl_knee_joint", "hl_wheel_joint",
-                "hr_hipx_joint", "hr_hipy_joint", "hr_knee_joint", "hr_wheel_joint"};
-    }
-
-    // ------------------------------------------------------------------ 启动/标定
     void PublishRawCommand(uint16_t control_word) {
         invicta_msgs::JointCommandArray msg;
         msg.header.stamp = ros::Time::now();
-        msg.name = joint_names_;
+        msg.name = cfg_.joint_names;
-        msg.position.assign(dof_num_, 0.);
+        msg.position.assign(size_t(dof_num_), 0.);
-        msg.velocity.assign(dof_num_, 0.);
+        msg.velocity.assign(size_t(dof_num_), 0.);
-        msg.effort.assign(dof_num_, 0.);
+        msg.effort.assign(size_t(dof_num_), 0.);
-        msg.kp.assign(dof_num_, 0.);
+        msg.kp.assign(size_t(dof_num_), 0.);
-        msg.kd.assign(dof_num_, 0.);
+        msg.kd.assign(size_t(dof_num_), 0.);
         msg.control_word = control_word;
         cmd_pub_.publish(msg);
     }
 
+    /// 使能时序：disable → error reset → enable；此后运行期恒为 motor control
     void SendStartupSequence() {
         const uint16_t seq[] = {invicta_msgs::kDisable, invicta_msgs::kErrorReset, invicta_msgs::kEnable};
         for (uint16_t word : seq) {
             PublishRawCommand(word);
             ros::Duration(0.01).sleep();
         }
-        std::cout << "[InvictaInterface] 启动控制序列完成 (disable -> error_reset -> enable)\n";
+        std::cout << "[InvictaInterface] 启动控制序列完成 (disable -> error_reset -> enable)" << std::endl;
-    }
-
-    bool IsDataUpdatedFinished() {
-        bool res = true;
-        for (int i = 0; i < dof_num_; ++i) res = res && joint_data_updated_[i];
-        return res;
-    }
-
-    /**
-     * @brief 上电标定：等待 16 个关节数据都发生更新，并按阈值修正 ±360° 回绕
-     *
-     * 只有在硬件桥接节点上报的是「原始编码器角 + M20 约定」时才需要打开
-     * （calibration/auto_calibrate=true）。若桥接节点已经给出 policy 约定的角度，
-     * 保持 false。
-     */
-    void ResetPositionOffset() {
-        joint_data_updated_.assign(dof_num_, false);
-        SetJointCommand(MatXf::Zero(dof_num_, 5));
-        ros::Duration(0.1).sleep();
-
-        VecXf last = GetJointPosition();
-        int cnt = 0;
-        while (!IsDataUpdatedFinished()) {
-            ++cnt;
-            ros::spinOnce();
-            VecXf cur = GetJointPosition();
-            for (int i = 0; i < dof_num_; ++i) {
-                if (!joint_data_updated_[i] && cur(i) != last(i) && !std::isnan(cur(i))) {
-                    joint_data_updated_[i] = true;
-                    std::cout << "joint " << i << " data updated at " << cnt << " cnt!" << std::endl;
-                }
-            }
-            last = cur;
-            ros::Duration(0.001).sleep();
-            if (cnt > 10000) {
-                ROS_ERROR("[InvictaInterface] 关节数据未全部更新，标定失败，继续用参数值");
-                return;
-            }
-        }
-        VecXf cur = GetJointPosition();
-        for (int k = 0; k < dof_num_; ++k) {
-            float th = (k % 4 == 1) ? wrap_hip_deg_ : wrap_knee_deg_;
-            if (k % 4 == 3) continue;  // 轮关节不做回绕
-            if (cur(k) < -th / 180. * M_PI) {
-                pos_offset_deg_[k] += 360.;
-                std::cout << "joint " << k << " offset -> " << pos_offset_deg_[k] << "\n";
-            } else if (cur(k) > th / 180. * M_PI) {
-                pos_offset_deg_[k] -= 360.;
-                std::cout << "joint " << k << " offset -> " << pos_offset_deg_[k] << "\n";
-            }
-        }
     }
 
     // ------------------------------------------------------------------ 回调
     void JointStateHandler(const sensor_msgs::JointState::ConstPtr& msg) {
-        if (static_cast<int>(msg->position.size()) != dof_num_ ||
+        const size_t n = size_t(dof_num_);
-            static_cast<int>(msg->velocity.size()) != dof_num_ ||
+        if (msg->position.size() != n || msg->velocity.size() != n || msg->effort.size() != n) {
-            static_cast<int>(msg->effort.size()) != dof_num_) {
             ROS_WARN_THROTTLE(5.0, "[InvictaInterface] %s 长度应为 %d，收到 pos=%zu vel=%zu eff=%zu，丢帧",
-                              joints_topic_.c_str(), dof_num_, msg->position.size(), msg->velocity.size(),
+                              cfg_.topic_joint_states.c_str(), dof_num_, msg->position.size(), msg->velocity.size(),
                               msg->effort.size());
             return;
         }
 
-        bool by_name = static_cast<int>(msg->name.size()) == dof_num_;
+        const bool by_name = msg->name.size() == n;
         int unknown = 0;
-        for (int j = 0; j < dof_num_; ++j) {
+        for (size_t k = 0; k < n; ++k) {
-            int i = by_name ? LookupSlot(msg->name[j]) : j;
+            const int i = by_name ? LookupSlot(msg->name[k]) : int(k);
             if (i < 0) {
                 ++unknown;
                 continue;
             }
-            joint_pos_(i) = float(msg->position[j]) * joint_dir_[i] + Deg2Rad(pos_offset_deg_[i]);
+            const auto& j = cfg_.Joint(i);
-            joint_vel_(i) = float(msg->velocity[j]) * joint_dir_[i];
+            joint_pos_(i) = float(msg->position[k]) * j.dir + Deg2Rad(float(j.offset_deg));
-            joint_tau_(i) = float(msg->effort[j]) * joint_dir_[i];
+            joint_vel_(i) = float(msg->velocity[k]) * j.dir;
+            joint_tau_(i) = float(msg->effort[k]) * j.dir;
         }
         if (!by_name && !warned_name_mismatch_) {
             warned_name_mismatch_ = true;
             ROS_WARN("[InvictaInterface] %s 未按约定给出 %d 个 name，按数组下标解析",
-                     joints_topic_.c_str(), dof_num_);
+                     cfg_.topic_joint_states.c_str(), dof_num_);
         }
         if (unknown > 0) {
             ROS_WARN_THROTTLE(5.0, "[InvictaInterface] %s 有 %d 个关节名不认识，已忽略",
-                              joints_topic_.c_str(), unknown);
+                              cfg_.topic_joint_states.c_str(), unknown);
         }
 
         for (int i = 0; i < dof_num_; ++i) joint_data_id_[i] = joint_data_frame_;
@@ -301,29 +174,44 @@ protected:
     }
 
     void ImuHandler(const sensor_msgs::Imu::ConstPtr& msg) {
-        // orientation_covariance[0] == -1 表示「不提供姿态」，此时不能拿它当有效姿态用
+        // orientation_covariance[0] == -1 表示「不提供姿态」
         if (msg->orientation_covariance[0] == -1.0) {
             if (!warned_imu_no_orientation_) {
                 warned_imu_no_orientation_ = true;
                 ROS_WARN("[InvictaInterface] %s 没有提供 orientation（covariance=-1），姿态观测无效",
-                         imu_topic_.c_str());
+                         cfg_.topic_imu.c_str());
             }
             return;
         }
-
         Eigen::Quaternionf q(float(msg->orientation.w), float(msg->orientation.x), float(msg->orientation.y),
                              float(msg->orientation.z));
         if (q.norm() < 1e-6f) {
-            ROS_WARN_THROTTLE(5.0, "[InvictaInterface] %s 四元数模长近零，忽略该帧", imu_topic_.c_str());
+            ROS_WARN_THROTTLE(5.0, "[InvictaInterface] %s 四元数模长近零，忽略该帧", cfg_.topic_imu.c_str());
             return;
         }
         q.normalize();
-        // eulerAngles(0,1,2) 即 ZYX 顺序，与 basic_function.hpp 的 RpyToRm 一致
+        rpy_ = q.toRotationMatrix().eulerAngles(0, 1, 2).cast<float>();  // ZYX，与 RpyToRm 一致
-        rpy_ = q.toRotationMatrix().eulerAngles(0, 1, 2).cast<float>();
         omega_body_ << msg->angular_velocity.x, msg->angular_velocity.y, msg->angular_velocity.z;
         acc_ << msg->linear_acceleration.x, msg->linear_acceleration.y, msg->linear_acceleration.z;
         imu_ts_ = msg->header.stamp.toSec();
     }
+
+protected:
+    const invicta::DeployConfig& cfg_;
+    ros::NodeHandle nh_;
+    std::unordered_map<std::string, int> name2slot_;
+
+    double ri_ts_ = 0., imu_ts_ = 0.;
+    Vec3f omega_body_, rpy_, acc_;
+    VecXf joint_pos_, joint_vel_, joint_tau_;
+    uint16_t joint_data_frame_ = 0;
+    std::vector<uint16_t> joint_data_id_;
+    std::vector<uint16_t> driver_status_;
+    bool warned_name_mismatch_ = false;
+    bool warned_imu_no_orientation_ = false;
+
+    ros::Publisher cmd_pub_;
+    ros::Subscriber joint_state_sub_, imu_sub_;
 };
 
 };  // namespace interface

src/invicta_deploy/interface/user_command/ros_command_interface.hpp

@@ -21,6 +21,7 @@
 
 #include "basic_function.hpp"
 #include "custom_types.h"
+#include "config/deploy_config.h"
 #include "invicta_msgs/MotionCommand.h"
 #include "invicta_msgs/motion_defs.h"
 #include "user_command_interface.h"
@@ -36,9 +37,9 @@ public:
      * @param default_gait 默认步态（协议 GaitParam）
      */
     RosCommandInterface(std::shared_ptr<std::set<int>> loaded_gaits, int default_gait)
-        : loaded_gaits_(std::move(loaded_gaits)), default_gait_(default_gait) {
+        : loaded_gaits_(std::move(loaded_gaits)), default_gait_(default_gait), cfg_(invicta::Config()) {
-        pnh_.param<std::string>("topics/motion_command", topic_, "motion_command");
+        topic_ = cfg_.topic_motion_command;
-        pnh_.param<int>("queues/motion_command", cmd_queue_, 1);
+        cmd_queue_ = cfg_.queue_motion_command;
         sub_ = nh_.subscribe(topic_, cmd_queue_, &RosCommandInterface::CommandCallback, this);
     }
 
@@ -183,6 +184,7 @@ private:
     std::string topic_ = "motion_command";
     int cmd_queue_ = 1;
 
+    const invicta::DeployConfig& cfg_;
     std::shared_ptr<std::set<int>> loaded_gaits_;  ///< 实际加载成功的步态（由 RLControlState 填充）
     int default_gait_ = int(kGaitBasic);
 

src/invicta_deploy/launch/invicta_deploy.launch

@@ -1,14 +1,6 @@
 <launch>
-  <!-- RL 部署节点：状态机 + 策略推理。参数在节点的私有命名空间下。
+  <!-- RL 部署节点：状态机 + 策略推理。参数来自本节点自己的 config/invicta_deploy.toml -->
-       无硬件台架自测时（invicta_hardware 只发零位状态）关掉两个自检，
-       否则站立完成后永远不会自动进入 RL：
-         roslaunch invicta_deploy invicta_deploy.launch rl_require_pose:=false rl_require_imu:=false
-  -->
-  <arg name="rl_require_pose" default="true"/>
-  <arg name="rl_require_imu" default="true"/>
   <node pkg="invicta_deploy" type="invicta_deploy" name="invicta_deploy" output="screen">
-    <rosparam command="load" file="$(find invicta_deploy)/config/invicta_deploy.yaml"/>
+    <param name="config_file" value="$(find invicta_deploy)/config/invicta_deploy.toml"/>
-    <param name="rl/require_pose" value="$(arg rl_require_pose)"/>
-    <param name="rl/require_imu" value="$(arg rl_require_imu)"/>
   </node>
 </launch>

src/invicta_deploy/main.cpp

@@ -1,12 +1,17 @@
 /**
  * @file main.cpp
- * @brief invicta RL deploy 入口：ROS 回调跑在 main 线程，控制线程跑 200 Hz 状态机
+ * @brief invicta RL deploy 入口：先加载 TOML 配置，再跑状态机
+ *
+ * 配置路径：ROS 参数 ~config_file（launch 传入），默认本包 config/invicta_deploy.toml。
  */
+#include <ros/package.h>
 #include <ros/ros.h>
 
 #include <iostream>
 #include <memory>
+#include <string>
 
+#include "config/deploy_config.h"
 #include "quadruped_wheel/invicta_state_machine.hpp"
 
 using namespace types;
@@ -15,8 +20,21 @@ MotionStateFeedback StateBase::msfb_ = MotionStateFeedback();
 
 int main(int argc, char** argv) {
     ros::init(argc, argv, "invicta_deploy");
-    std::cout << "Invicta State Machine Start Running" << std::endl;
+    ros::NodeHandle pnh("~");
 
+    std::string config_path;
+    pnh.param<std::string>("config_file", config_path, "");
+    if (config_path.empty()) config_path = ros::package::getPath("invicta_deploy") + "/config/invicta_deploy.toml";
+
+    std::string err;
+    auto& cfg = invicta::Config();
+    if (!cfg.Load(config_path, &err)) {
+        ROS_FATAL("[invicta_deploy] 配置加载失败：%s（%s）", err.c_str(), config_path.c_str());
+        return 1;
+    }
+    std::cout << cfg.Summary() << std::flush;
+
+    std::cout << "Invicta State Machine Start Running" << std::endl;
     std::shared_ptr<StateMachineBase> fsm = std::make_shared<invicta::InvictaStateMachine>();
     fsm->Start();
     fsm->Run();

src/invicta_deploy/run_policy/invicta_policy_runner.hpp

@@ -1,88 +1,57 @@
 /**
  * @file invicta_policy_runner.hpp
- * @brief ONNX 策略推理（由上游 M20PolicyRunner 移植）
+ * @brief ONNX 策略推理。关节数量、顺序、动作尺度、默认姿态、增益全部来自 invicta.toml
  *
- * 策略与关节顺序、动作尺度、默认关节角绑定，改这些就等于换策略，请一起改。
+ * 观测（M20 同款配方，维度 = 9 + 3N）：
- * 观测 57 维 = 角速度(3) + 投影重力(3) + 速度指令(3) + 关节角(16) + 关节角速度(16) + 上一步动作(16)
+ *   [角速度(3)·omega_scale, 投影重力(3), 速度指令(3), 关节角(N), 关节角速度(N)·dof_vel_scale, 上一步动作(N)]
+ * N = 关节数量（本构型 8）。轮关节位置是否进观测由 zero_obs_wheels 控制。
+ *
+ * 动作 → 指令：非轮关节给位置目标（action·scale + default），轮关节给速度目标。
+ * 配置里的 action_scale / default_joint_pos / kp / kd 按「策略顺序」（policy.joint_order）给出，
+ * 这里自动置换到机器人顺序。
  */
 #pragma once
-#define PI 3.14159265358979323846
 
 #include <onnxruntime_c_api.h>
 #include <onnxruntime_cxx_api.h>
 
-#include <array>
+#include <algorithm>
 #include <cmath>
-#include <ctime>
 #include <string>
 #include <unordered_map>
-#include <utility>
 #include <vector>
 
+#include "config/deploy_config.h"
 #include "policy_runner_base.hpp"
 
 class InvictaPolicyRunner : public PolicyRunnerBase {
-private:
-    VecXf kp_, kd_;
-    VecXf dof_default_eigen_policy, dof_default_eigen_robot;
-    Vec3f gravity_direction = Vec3f(0., 0., -1.);
-
-    const int motor_num = 16;
-    const int observation_dim = 57;
-    const int action_dim = 16;
-    float agent_timestep = 0.02;  ///< 策略决策周期 0.02 s = 50 Hz
-
-    VecXf joint_pos_rl = VecXf(action_dim);  ///< policy 关节顺序
-    VecXf joint_vel_rl = VecXf(action_dim);
-
-    const std::string policy_path_;
-
-    float omega_scale_ = 0.25;
-    float dof_vel_scale_ = 0.05;
-    VecXf imu_w_eigen, base_acc_eigen, motor_p_eigen, motor_v_eigen, current_action_eigen, last_action_eigen,
-        current_observation_, tmp_action_eigen;
-
-    RobotAction robot_action;
-
-    /// 机器人（话题）关节顺序 = /joint_states 的 name 顺序
-    std::vector<std::string> robot_order = {"fl_hipx_joint", "fl_hipy_joint", "fl_knee_joint", "fl_wheel_joint",
-                                            "fr_hipx_joint", "fr_hipy_joint", "fr_knee_joint", "fr_wheel_joint",
-                                            "hl_hipx_joint", "hl_hipy_joint", "hl_knee_joint", "hl_wheel_joint",
-                                            "hr_hipx_joint", "hr_hipy_joint", "hr_knee_joint", "hr_wheel_joint"};
-
-    /// 策略内部关节顺序（12 个腿关节在前，4 个轮在后），与训练导出绑定
-    std::vector<std::string> policy_order = {
-        "fl_hipx_joint", "fl_hipy_joint", "fl_knee_joint", "fr_hipx_joint", "fr_hipy_joint", "fr_knee_joint",
-        "hl_hipx_joint", "hl_hipy_joint", "hl_knee_joint", "hr_hipx_joint", "hr_hipy_joint", "hr_knee_joint",
-        "fl_wheel_joint", "fr_wheel_joint", "hl_wheel_joint", "hr_wheel_joint",
-    };
-
-    std::vector<float> action_scale_robot = {0.125, 0.25, 0.25, 5, 0.125, 0.25, 0.25, 5,
-                                            0.125, 0.25, 0.25, 5, 0.125, 0.25, 0.25, 5};
-
-    Ort::SessionOptions session_options_;
-    Ort::Session session_{nullptr};
-    Ort::Env env_;
-    std::vector<int> robot2policy_idx, policy2robot_idx;
-
-    const char* input_names_[1] = {"obs"};  // 必须与模型导出时一致
-    const char* output_names_[1] = {"actions"};
-    Ort::MemoryInfo memory_info{nullptr};
-    const std::array<int64_t, 2> input_observationShape = {1, observation_dim};
-
 public:
     InvictaPolicyRunner(const std::string& policy_name, const std::string& policy_path)
         : PolicyRunnerBase(policy_name),
-          policy_path_(policy_path),
+          cfg_(invicta::Config()),
+          policy_path_(policy_path.empty() ? cfg_.policy_path : policy_path),
           env_(ORT_LOGGING_LEVEL_WARNING, "InvictaPolicyRunner"),
           session_options_{},
           session_{nullptr},
           memory_info(Ort::MemoryInfo::CreateCpu(OrtArenaAllocator, OrtMemTypeDefault)) {
-        dof_default_eigen_policy.setZero(action_dim);
+        action_dim_ = cfg_.action_dim;
-        dof_default_eigen_robot.setZero(action_dim);
+        obs_dim_ = cfg_.obs_dim;
-        dof_default_eigen_policy << 0.0, -0.3, 0.6, 0.0, -0.3, 0.6, 0.0, 0.3, -0.6, 0.0, 0.3, -0.6, 0.0, 0.0, 0.0, 0.0;
+        if (action_dim_ <= 0 || obs_dim_ <= 0) {
-        dof_default_eigen_robot << 0.0, -0.3, 0.6, 0.0, 0.0, -0.3, 0.6, 0.0, 0.0, 0.3, -0.6, 0.0, 0.0, 0.3, -0.6, 0.0;
+            throw std::runtime_error("策略维度为 0：配置未加载或 [policy] 段不完整");
-        SetDecimation(4);
+        }
+
+        robot2policy_idx_ = Permutation(cfg_.joint_names, cfg_.policy_joint_order);
+        policy2robot_idx_ = Permutation(cfg_.policy_joint_order, cfg_.joint_names);
+
+        ToRobotOrder(cfg_.action_scale, action_scale_robot_);
+        ToRobotOrder(cfg_.default_joint_pos, default_robot_);
+        ToRobotOrder(cfg_.policy_kp, kp_robot_);
+        ToRobotOrder(cfg_.policy_kd, kd_robot_);
+        default_policy_.resize(action_dim_);
+        for (int i = 0; i < action_dim_; ++i) default_policy_(i) = float(cfg_.default_joint_pos[size_t(i)]);
+        wheel_robot_idx_ = cfg_.WheelIndices();
+
+        SetDecimation(cfg_.policy_decimation);
         session_options_.SetIntraOpNumThreads(4);
         session_options_.SetGraphOptimizationLevel(GraphOptimizationLevel::ORT_ENABLE_EXTENDED);
 
@@ -90,99 +59,125 @@ public:
             std::cerr << "Model file not found: " << policy_path_ << std::endl;
             throw std::runtime_error("Model file missing");
         }
-
         session_ = Ort::Session(env_, policy_path_.c_str(), session_options_);
-        kp_ = Vec4f(80, 80, 80, 0.).replicate(4, 1);
-        kd_ = Vec4f(2, 2, 2, 0.6).replicate(4, 1);
 
-        robot2policy_idx = generate_permutation(robot_order, policy_order);
+        current_observation_.setZero(obs_dim_);
-        policy2robot_idx = generate_permutation(policy_order, robot_order);
+        last_action_eigen_.setZero(action_dim_);
+        tmp_action_eigen_.setZero(action_dim_);
+        current_action_eigen_.setZero(action_dim_);
+        joint_pos_rl_.setZero(action_dim_);
+        joint_vel_rl_.setZero(action_dim_);
 
-        robot_action.kp = kp_;
+        robot_action_.kp = kp_robot_;
-        robot_action.kd = kd_;
+        robot_action_.kd = kd_robot_;
-        robot_action.tau_ff = VecXf::Zero(motor_num);
+        robot_action_.tau_ff = VecXf::Zero(action_dim_);
-        robot_action.goal_joint_pos = VecXf::Zero(motor_num);
+        robot_action_.goal_joint_pos = VecXf::Zero(action_dim_);
-        robot_action.goal_joint_vel = VecXf::Zero(motor_num);
+        robot_action_.goal_joint_vel = VecXf::Zero(action_dim_);
 
-        current_observation_.setZero(observation_dim);
+        input_observation_shape_ = {1, obs_dim_};
-        last_action_eigen.setZero(action_dim);
-        tmp_action_eigen.setZero(action_dim);
-        current_action_eigen.setZero(action_dim);
     }
 
     ~InvictaPolicyRunner() override = default;
 
-    static std::vector<int> generate_permutation(const std::vector<std::string>& from,
+    void DisplayPolicyInfo() {
-                                                 const std::vector<std::string>& to, int default_index = 0) {
+        std::cout << "[policy] " << policy_path_ << " obs=" << obs_dim_ << " action=" << action_dim_
-        std::unordered_map<std::string, int> idx_map;
+                  << " decimation=" << decimation_ << std::endl;
-        for (int i = 0; i < static_cast<int>(from.size()); ++i) idx_map[from[i]] = i;
-
-        std::vector<int> perm;
-        for (const auto& name : to) {
-            auto it = idx_map.find(name);
-            perm.push_back(it != idx_map.end() ? it->second : default_index);
     }
-        return perm;
-    }
-
-    void DisplayPolicyInfo() {}
 
     void OnEnter() {
         run_cnt_ = 0;
         cmd_vel_input_.setZero();
-        last_action_eigen.setZero(action_dim);
+        last_action_eigen_.setZero(action_dim_);
-        tmp_action_eigen.setZero(action_dim);
+        tmp_action_eigen_.setZero(action_dim_);
-        motor_p_eigen.setZero(12);
-        motor_v_eigen.setZero(motor_num);
-    }
-
-    VecXf Onnx_infer(VecXf current_observation) {
-        Ort::Value input_tensor =
-            Ort::Value::CreateTensor<float>(memory_info, current_observation.data(),
-                                            current_observation.size(), input_observationShape.data(),
-                                            input_observationShape.size());
-
-        std::vector<Ort::Value> inputs;
-        inputs.emplace_back(std::move(input_tensor));
-
-        auto outputs =
-            session_.Run(Ort::RunOptions{nullptr}, input_names_, inputs.data(), 1, output_names_, 1);
-
-        float* action_data = outputs[0].GetTensorMutableData<float>();
-        Eigen::Map<Eigen::VectorXf> action_map(action_data, action_dim);
-        return VecXf(action_map);
     }
 
     RobotAction getRobotAction(const RobotBasicState& ro, const UserCommand& uc) {
-        Vec3f base_omega = ro.base_omega * omega_scale_;
+        const Vec3f base_omega = ro.base_omega * float(cfg_.omega_scale);
-        Vec3f projected_gravity_vec = ro.base_rot_mat.inverse() * gravity_direction;
+        const Vec3f projected_gravity = ro.base_rot_mat.inverse() * gravity_direction_;
-        Vec3f cmd = Vec3f(uc.forward_vel_scale, uc.side_vel_scale, uc.turnning_vel_scale);
+        const Vec3f cmd(uc.forward_vel_scale, uc.side_vel_scale, uc.turnning_vel_scale);
 
-        for (int i = 0; i < action_dim; ++i) {
+        for (int i = 0; i < action_dim_; ++i) {
-            joint_pos_rl(i) = ro.joint_pos(robot2policy_idx[i]);
+            const int r = robot2policy_idx_[size_t(i)];  // 策略第 i 位对应机器人第 r 个关节
-            joint_vel_rl(i) = ro.joint_vel(robot2policy_idx[i]) * dof_vel_scale_;
+            joint_pos_rl_(i) = ro.joint_pos(r);
+            joint_vel_rl_(i) = ro.joint_vel(r) * float(cfg_.dof_vel_scale);
         }
-        joint_pos_rl.segment(12, 4).setZero();  // 轮关节角不进观测
+        if (cfg_.zero_obs_wheels) {
-
+            for (int w : wheel_robot_idx_) joint_pos_rl_(robot2policy_idx_[size_t(w)]) = 0.f;
-        joint_pos_rl -= dof_default_eigen_policy;
-        current_observation_ << base_omega, projected_gravity_vec, cmd, joint_pos_rl, joint_vel_rl,
-            last_action_eigen;
-
-        current_action_eigen = Onnx_infer(current_observation_);
-        last_action_eigen = current_action_eigen;
-
-        for (int i = 0; i < action_dim; ++i) {
-            tmp_action_eigen(i) = current_action_eigen(policy2robot_idx[i]);
-            tmp_action_eigen(i) *= action_scale_robot[i];
         }
-        tmp_action_eigen += dof_default_eigen_robot;
+        joint_pos_rl_ -= default_policy_;
 
-        for (int i = 0; i < 4; ++i) {
+        current_observation_ << base_omega, projected_gravity, cmd, joint_pos_rl_, joint_vel_rl_, last_action_eigen_;
-            robot_action.goal_joint_pos.segment(i * 4, 3) = tmp_action_eigen.segment(i * 4, 3);
+        current_action_eigen_ = OnnxInfer(current_observation_);
-            robot_action.goal_joint_vel(i * 4 + 3) = tmp_action_eigen(i * 4 + 3);
+        last_action_eigen_ = current_action_eigen_;
+
+        for (int i = 0; i < action_dim_; ++i) {
+            tmp_action_eigen_(i) = current_action_eigen_(policy2robot_idx_[size_t(i)]) * action_scale_robot_(i);
+        }
+        tmp_action_eigen_ += default_robot_;
+
+        for (int i = 0; i < action_dim_; ++i) {
+            if (cfg_.IsWheel(i)) {
+                robot_action_.goal_joint_pos(i) = 0.f;
+                robot_action_.goal_joint_vel(i) = tmp_action_eigen_(i);
+            } else {
+                robot_action_.goal_joint_pos(i) = tmp_action_eigen_(i);
+                robot_action_.goal_joint_vel(i) = 0.f;
+            }
         }
 
         ++run_cnt_;
-        return robot_action;
+        return robot_action_;
     }
+
+private:
+    static std::vector<int> Permutation(const std::vector<std::string>& from, const std::vector<std::string>& to) {
+        std::unordered_map<std::string, int> idx;
+        for (size_t i = 0; i < from.size(); ++i) idx[from[i]] = int(i);
+        std::vector<int> perm;
+        for (const auto& n : to) {
+            auto it = idx.find(n);
+            perm.push_back(it == idx.end() ? 0 : it->second);
+        }
+        return perm;
+    }
+
+    /// 把「策略顺序」的数组搬到「机器人顺序」
+    void ToRobotOrder(const std::vector<double>& policy_order_values, VecXf& out) {
+        out.resize(action_dim_);
+        for (int i = 0; i < action_dim_; ++i) {
+            out(i) = float(policy_order_values[size_t(robot2policy_idx_[size_t(i)])]);
+        }
+    }
+
+    VecXf OnnxInfer(VecXf observation) {
+        Ort::Value input_tensor =
+            Ort::Value::CreateTensor<float>(memory_info, observation.data(), observation.size(),
+                                            input_observation_shape_.data(), input_observation_shape_.size());
+        std::vector<Ort::Value> inputs;
+        inputs.emplace_back(std::move(input_tensor));
+        auto outputs = session_.Run(Ort::RunOptions{nullptr}, input_names_, inputs.data(), 1, output_names_, 1);
+        float* data = outputs[0].GetTensorMutableData<float>();
+        Eigen::Map<Eigen::VectorXf> mapped(data, action_dim_);
+        return VecXf(mapped);
+    }
+
+    const invicta::DeployConfig& cfg_;
+    const std::string policy_path_;
+    int action_dim_ = 0, obs_dim_ = 0;
+    const Vec3f gravity_direction_ = Vec3f(0., 0., -1.);
+
+    std::vector<int> robot2policy_idx_, policy2robot_idx_, wheel_robot_idx_;
+    VecXf action_scale_robot_, default_robot_, kp_robot_, kd_robot_, default_policy_;
+
+    VecXf joint_pos_rl_, joint_vel_rl_;
+    VecXf current_observation_, last_action_eigen_, tmp_action_eigen_, current_action_eigen_;
+    RobotAction robot_action_;
+
+    Ort::SessionOptions session_options_;
+    Ort::Session session_;
+    Ort::Env env_;
+    const char* input_names_[1] = {"obs"};  // 必须与模型导出时一致
+    const char* output_names_[1] = {"actions"};
+    Ort::MemoryInfo memory_info;
+    std::vector<int64_t> input_observation_shape_;
 };

src/invicta_deploy/scripts/smoke_test.sh

@@ -45,9 +45,8 @@ sleep 8
 echo "--- /joint_commands 频率（idle 状态期望 ~200 Hz）---"
 timeout 5 rostopic hz /joint_commands 2>&1 | tail -2
 
-echo "--- 参数是否从 yaml 读到 ---"
+echo "--- 配置是否读到（TOML）---"
-rosparam get /invicta_deploy/geometry/stand_height
+grep -aE "^\[config\]" /tmp/dep.log | head -3
-rosparam get /invicta_deploy/calibration/joint_dir
 
 echo "--- 节点连接关系 ---"
 rosnode info /invicta_deploy 2>&1 | sed -n '1,14p'

src/invicta_deploy/state_machine/parameters/control_parameters.h

@@ -1,61 +0,0 @@
-/**
- * @file control_parameters.h
- * @brief 机器人几何/控制参数。全部来自 ROS 参数，默认值是上游 M20 的取值
- *
- * 换成 invicta 自己的机器人时，改 config/invicta_deploy.yaml 即可，不用改代码。
- */
-#pragma once
-
-#include <ros/ros.h>
-
-#include <string>
-
-#include "common_types.h"
-#include "custom_types.h"
-
-using namespace types;
-
-class ControlParameters {
-public:
-    explicit ControlParameters(const ros::NodeHandle& nh) { Load(nh); }
-    ~ControlParameters() {}
-
-    /// 机身尺寸 [m]
-    float body_len_x_, body_len_y_;
-    float hip_len_, thigh_len_, shank_len_;
-
-    /// 站立高度配置 [m]
-    float pre_height_, stand_height_;
-
-    /// 单腿摆动相 PD 增益
-    Vec3f swing_leg_kp_, swing_leg_kd_;
-
-    /// 腿关节位置限制 [rad]（左前腿；右侧镜像）
-    Vec3f fl_joint_lower_, fl_joint_upper_;
-
-    /// 关节速度限制 [rad/s]、力矩限制 [Nm]
-    Vec3f joint_vel_limit_;
-    Vec3f torque_limit_;
-
-    /// 站立/卧倒时长 [s]
-    float stand_duration_ = 1.5;
-    float liedown_duration_ = 2.0;
-
-    /// 关节阻尼时长 [s]：缓慢松下 → 机器人着地 → 转入空闲（自由）
-    float joint_damping_duration_ = 3.0;
-
-    // ---- 站立完成后自动进入 RL（原厂机制：模型推理健康 + 姿态检查通过即切换，不等指令）----
-    bool rl_auto_enter_ = true;
-    float rl_enter_delay_ = 0.5f;        ///< 到达站立位姿后再稳定多久 [s]
-    bool rl_require_pose_ = true;        ///< 是否检查关节角已到站立位姿（无硬件台架要关掉）
-    float rl_joint_tolerance_ = 0.15f;   ///< 关节角容差 [rad]
-    float rl_velocity_tolerance_ = 1.0f; ///< 关节速度容差 [rad/s]
-    bool rl_require_imu_ = true;         ///< 是否检查 IMU 数据有效
-
-    /// 轮关节速度限制与轮半径相关长度 [m]
-    float wheel_vel_limit_ = 100;
-    float wheel_link_len_ = 0.040575;
-
-private:
-    void Load(const ros::NodeHandle& nh);
-};

src/invicta_deploy/state_machine/parameters/invicta_control_parameters.cpp

@@ -1,58 +0,0 @@
-/**
- * @file invicta_control_parameters.cpp
- * @brief 从 ROS 参数读取控制参数，默认值为上游 M20
- */
-#include "parameters/control_parameters.h"
-
-namespace {
-
-Vec3f GetVec3(const ros::NodeHandle& nh, const std::string& key, const Vec3f& fallback) {
-    std::vector<double> v;
-    if (!nh.getParam(key, v) || v.size() != 3) {
-        return fallback;
-    }
-    return Vec3f(static_cast<float>(v[0]), static_cast<float>(v[1]), static_cast<float>(v[2]));
-}
-
-}  // namespace
-
-void ControlParameters::Load(const ros::NodeHandle& nh) {
-    nh.param<float>("geometry/body_len_x", body_len_x_, 0.3095f * 2.f);
-    nh.param<float>("geometry/body_len_y", body_len_y_, 0.065f * 2.f);
-    nh.param<float>("geometry/hip_len", hip_len_, 0.104f);
-    nh.param<float>("geometry/thigh_len", thigh_len_, 0.25f);
-    nh.param<float>("geometry/shank_len", shank_len_, 0.25f);
-
-    nh.param<float>("geometry/pre_height", pre_height_, 0.12f);
-    nh.param<float>("geometry/stand_height", stand_height_, 0.48f);
-
-    swing_leg_kp_ = GetVec3(nh, "gains/swing_leg_kp", Vec3f(200., 200., 200.));
-    swing_leg_kd_ = GetVec3(nh, "gains/swing_leg_kd", Vec3f(4., 4., 4.));
-
-    fl_joint_lower_ = GetVec3(nh, "limits/fl_joint_lower", Vec3f(-0.4363, -2.443, -2.758));
-    fl_joint_upper_ = GetVec3(nh, "limits/fl_joint_upper", Vec3f(0.6109, 2.443, 2.758));
-    joint_vel_limit_ = GetVec3(nh, "limits/joint_vel_limit", Vec3f(22.4, 22.4, 22.4));
-    torque_limit_ = GetVec3(nh, "limits/torque_limit", Vec3f(76.4, 76.4, 76.4));
-
-    nh.param<float>("timing/stand_duration", stand_duration_, 1.5f);
-    nh.param<float>("timing/liedown_duration", liedown_duration_, 2.0f);
-    nh.param<float>("timing/joint_damping_duration", joint_damping_duration_, 3.0f);
-    if (joint_damping_duration_ < 0.1f || joint_damping_duration_ > 30.f) {
-        ROS_WARN("[ControlParameters] timing/joint_damping_duration=%.2f 超出 [0.1, 30] s，回退到 3.0 s",
-                 joint_damping_duration_);
-        joint_damping_duration_ = 3.0f;
-    }
-    ROS_INFO("[ControlParameters] joint_damping_duration = %.2f s", joint_damping_duration_);
-
-    nh.param<bool>("rl/auto_enter", rl_auto_enter_, true);
-    nh.param<float>("rl/enter_delay", rl_enter_delay_, 0.5f);
-    nh.param<bool>("rl/require_pose", rl_require_pose_, true);
-    nh.param<float>("rl/joint_tolerance", rl_joint_tolerance_, 0.15f);
-    nh.param<float>("rl/velocity_tolerance", rl_velocity_tolerance_, 1.0f);
-    nh.param<bool>("rl/require_imu", rl_require_imu_, true);
-    ROS_INFO("[ControlParameters] 站立后自动进 RL: %s（delay %.2f s, require_pose=%d, require_imu=%d）",
-             rl_auto_enter_ ? "开" : "关", rl_enter_delay_, int(rl_require_pose_), int(rl_require_imu_));
-
-    nh.param<float>("limits/wheel_vel_limit", wheel_vel_limit_, 100.f);
-    nh.param<float>("geometry/wheel_link_len", wheel_link_len_, 0.040575f);
-}

src/invicta_deploy/state_machine/quadruped_wheel/idle_state.hpp

@@ -1,6 +1,9 @@
 /**
  * @file idle_state.hpp
- * @brief 待机：确认关节/IMU 数据正常，等手柄指令
+ * @brief 待机：按 invicta.toml 的逐关节限位检查本体数据，等指令
+ *
+ * 相比 M20：不再有「每腿 4 关节 / 下标 %4」的假设，限位改成逐关节查表；
+ * 左右/前后的差异已经在配置的标定（dir/offset）里归一。
  */
 #pragma once
 
@@ -25,25 +28,18 @@ private:
         omg_ = ri_ptr_->GetImuOmega();
     }
 
+    /// 0 = 正常，非 0 为异常码（会打印在 warning 里）
     int JointDataNormalCheck() {
-        VecXf joint_pos_lower(16), joint_pos_upper(16);
+        const auto& cfg = invicta::Config();
-        Vec4f fl_lower, fl_upper;
+        for (int i = 0; i < int(joint_pos_.size()); ++i) {
-        fl_lower << cp_ptr_->fl_joint_lower_, -INFINITY;
+            const auto& j = cfg.Joint(i);
-        fl_upper << cp_ptr_->fl_joint_upper_, INFINITY;
-        Vec4f fr_lower(-fl_upper(0), fl_lower(1), fl_lower(2), -INFINITY);
-        Vec4f fr_upper(-fl_lower(0), fl_upper(1), fl_upper(2), INFINITY);
-        joint_pos_lower << fl_lower, fr_lower, fl_lower, fr_lower;
-        joint_pos_upper << fl_upper, fr_upper, fl_upper, fr_upper;
-        for (int i = 0; i < 16; ++i) {
             if (std::isnan(joint_pos_(i))) return 1;
-            if (joint_pos_(i) > joint_pos_upper(i) + 0.2) return 2;
+            if (!j.is_wheel) {
-            if (joint_pos_(i) < joint_pos_lower(i) - 0.2) return 3;
+                if (joint_pos_(i) > j.upper + 0.2) return 2;
-            if (std::isnan(joint_vel_(i))) return 4;
+                if (joint_pos_(i) < j.lower - 0.2) return 3;
-            if (i % 4 == 3) {
-                if (joint_vel_(i) > cp_ptr_->wheel_vel_limit_ + 0.1) return 5;
-            } else {
-                if (joint_vel_(i) > cp_ptr_->joint_vel_limit_(i % 4) + 0.1) return 6;
             }
+            if (std::isnan(joint_vel_(i))) return 4;
+            if (std::fabs(joint_vel_(i)) > j.vel_limit + 0.1) return 6;
         }
         return 0;
     }
@@ -57,39 +53,25 @@ private:
         return 0;
     }
 
-    void PrintEigenVectorf(const VecXf& vec, int width = 6) {
-        for (int i = 0; i < vec.size(); ++i) {
-            std::cout << std::setw(width) << std::setprecision(3) << std::right << vec(i) << " ";
-        }
-        std::cout << "\n";
-    }
-
     void DisplayProprioceptiveInfo() {
-        std::cout << "Joint Data: \n";
+        const auto& cfg = invicta::Config();
-        std::cout << "FL pos: ";
+        std::cout << "Joint Data (rad, rad/s): \n";
-        PrintEigenVectorf(joint_pos_.segment(0, 4));
+        for (size_t l = 0; l < cfg.legs.size(); ++l) {
-        std::cout << "FR pos: ";
+            const int si = cfg.legs[l].swing_index, wi = cfg.legs[l].wheel_index;
-        PrintEigenVectorf(joint_pos_.segment(4, 4));
+            std::cout << "  " << cfg.legs[l].name << "  swing " << std::setw(7) << std::setprecision(3)
-        std::cout << "HL pos: ";
+                      << joint_pos_(si) << " (vel " << std::setw(6) << joint_vel_(si) << ")   wheel vel " << std::setw(7)
-        PrintEigenVectorf(joint_pos_.segment(8, 4));
+                      << joint_vel_(wi) << std::endl;
-        std::cout << "HR pos: ";
+        }
-        PrintEigenVectorf(joint_pos_.segment(12, 4));
+        std::cout << "  tau: " << joint_tau_.transpose() << std::endl;
-        std::cout << "vel: ";
+        std::cout << "  Imu rpy " << rpy_.transpose() << " | acc " << acc_.transpose() << " | omg " << omg_.transpose()
-        PrintEigenVectorf(joint_vel_);
+                  << std::endl;
-        std::cout << "tau: ";
-        PrintEigenVectorf(joint_tau_);
-        std::cout << "Imu Data: \n";
-        std::cout << "rpy: " << rpy_.transpose() << std::endl;
-        std::cout << "acc: " << acc_.transpose() << std::endl;
-        std::cout << "omg: " << omg_.transpose() << std::endl;
     }
 
     void DisplayAxisValue() {
         auto cmd = uc_ptr_->GetUserCommand();
-        std::cout << "User Command Input: \n";
+        std::cout << "User Command: axis " << cmd->forward_vel_scale << " " << cmd->side_vel_scale << " "
-        std::cout << "axis value:  " << cmd->forward_vel_scale << " " << cmd->side_vel_scale << " "
+                  << cmd->turnning_vel_scale << " | target_mode " << int(cmd->target_mode) << " | gait 0x" << std::hex
-                  << cmd->turnning_vel_scale << std::endl;
+                  << cmd->gait << std::dec << std::endl;
-        std::cout << "target mode: " << int(cmd->target_mode) << std::endl;
     }
 
 public:
@@ -98,35 +80,30 @@ public:
 
     ~IdleState() {}
 
-    virtual void OnEnter() {
+    virtual void OnEnter() { StateBase::msfb_.UpdateCurrentState(RobotMotionState::WaitingForStand); }
-        StateBase::msfb_.UpdateCurrentState(RobotMotionState::WaitingForStand);
-    }
 
     virtual void OnExit() { first_enter_flag_ = false; }
 
     virtual void Run() {
         GetProprioceptiveData();
         joint_normal_flag_ = JointDataNormalCheck();
-        if (((ri_ptr_->GetInterfaceTimeStamp() - last_print_time) > 1)) {
+        if ((ri_ptr_->GetInterfaceTimeStamp() - last_print_time) > 1) {
             DisplayProprioceptiveInfo();
             DisplayAxisValue();
             last_print_time = ri_ptr_->GetInterfaceTimeStamp();
         }
-        MatXf cmd = MatXf::Zero(16, 5);
+        // 全零指令 = 不给力矩
-        ri_ptr_->SetJointCommand(cmd);
+        ri_ptr_->SetJointCommand(MatXf::Zero(int(ri_ptr_->dof_num_), 5));
     }
 
     virtual bool LoseControlJudge() { return false; }
 
     virtual StateName GetNextStateName() {
-        if ((joint_normal_flag_ != 0)) {
+        if (joint_normal_flag_ != 0) {
-            std::cout << "========== warning ========>> "
+            std::cout << "========== warning ========>> joint status: " << joint_normal_flag_ << std::endl;
-                      << "joint status: " << joint_normal_flag_ << std::endl;
-            return StateName::kIdle;
-        }
-        if (uc_ptr_->GetUserCommand()->safe_control_mode != 0) {
             return StateName::kIdle;
         }
+        if (uc_ptr_->GetUserCommand()->safe_control_mode != 0) return StateName::kIdle;
         if (uc_ptr_->GetUserCommand()->target_mode == uint8_t(RobotMotionState::StandingUp))
             return StateName::kStandUp;
         return StateName::kIdle;

src/invicta_deploy/state_machine/quadruped_wheel/invicta_state_machine.hpp

@@ -1,8 +1,9 @@
 /**
  * @file invicta_state_machine.hpp
- * @brief 状态机装配：运动指令来源（ROS 话题）+ 数据面 + 五个状态 + 安全检查 + 状态回报
+ * @brief 状态机装配：指令来源（ROS 话题）+ 数据面 + 五个状态 + 安全检查 + 状态回报
  *
- * 遥控协议（UDP/JSON）已拆到 invicta_remote 节点，本节点只消费 /motion_command：
+ * 所有参数来自 invicta::Config()（invicta.toml）；模型文件留在本包 policy/ 下，
+ * 配置里的相对路径在这里解析为「包目录 + 相对路径」。
  *   invicta_remote --/motion_command--> invicta_deploy --/motion_state--> invicta_remote
  */
 #pragma once
@@ -17,6 +18,7 @@
 #include <string>
 
 #include "hardware/invicta_interface.hpp"
+#include "config/deploy_config.h"
 #include "motion_state_reporter.hpp"
 #include "quadruped_wheel/idle_state.hpp"
 #include "quadruped_wheel/joint_damping_state.hpp"
@@ -29,9 +31,6 @@
 
 namespace invicta {
 
-/// 关节数量，与 policy.onnx 绑定（M20 是 4 腿 × 4 关节）
-constexpr int kDofNum = 16;
-
 class InvictaStateMachine : public StateMachineBase {
 private:
     std::shared_ptr<StateBase> idle_controller_;
@@ -42,46 +41,37 @@ private:
 
     std::shared_ptr<interface::MotionStateReporter> state_reporter_;
 
+    /// 模型路径：绝对路径原样使用；相对路径基于 invicta_deploy 包目录（模型跟着 RL 包走）
+    static std::string ResolveModelPath(const std::string& path) {
+        if (path.empty() || path.front() == '/') return path;
+        return ros::package::getPath("invicta_deploy") + "/" + path;
+    }
+
 public:
     InvictaStateMachine() : StateMachineBase() {}
     ~InvictaStateMachine() {}
 
     void Start() {
-        ros::NodeHandle pnh("~");  // 参数都在 ~ 下，见 config/invicta_deploy.yaml
+        const auto& cfg = invicta::Config();
-
-        pnh.param<int>("control_period_ms", control_period_ms_, 5);  // 默认 200 Hz
-
-        std::string policy_path;
-        pnh.param<std::string>("policy_path", policy_path, "");
-        if (policy_path.empty()) {
-            policy_path = ros::package::getPath("invicta_deploy") + "/policy/policy.onnx";
-        }
-
-        std::string state_topic = "motion_state";
-        double state_hz = 20.0;
-        std::string version = "INVICTA";
-        pnh.param<std::string>("topics/motion_state", state_topic, "motion_state");
-        pnh.param<double>("motion_state_report_hz", state_hz, 20.0);
-        pnh.param<std::string>("robot_version", version, "INVICTA");
 
+        const std::string policy_path = ResolveModelPath(cfg.policy_path);
         std::map<std::string, std::string> gait_policy_map;
-        pnh.getParam("gait/policy_map", gait_policy_map);
+        for (const auto& kv : cfg.gait_policies) {
-        int default_gait = invicta_msgs::kGaitBasic;
+            gait_policy_map[std::to_string(kv.first)] = ResolveModelPath(kv.second);
-        pnh.param<int>("gait/default", default_gait, int(invicta_msgs::kGaitBasic));
+        }
+        const int default_gait = int(cfg.default_gait);
         auto loaded_gaits = std::make_shared<std::set<int>>();
 
         // ---- 指令来源：订阅 /motion_command ----
         uc_ptr_ = std::make_shared<interface::RosCommandInterface>(loaded_gaits, default_gait);
         uc_ptr_->SetMotionStateFeedback(&StateBase::msfb_);
 
-        // ---- 机器人数据面 + 控制参数 ----
+        // ---- 机器人数据面 ----
-        ri_ptr_ = std::make_shared<interface::InvictaInterface>("invicta", kDofNum);
+        ri_ptr_ = std::make_shared<interface::InvictaInterface>(cfg.robot_name, cfg.dof);
-        cp_ptr_ = std::make_shared<ControlParameters>(pnh);
 
         std::shared_ptr<ControllerData> data_ptr = std::make_shared<ControllerData>();
         data_ptr->ri_ptr = ri_ptr_;
         data_ptr->uc_ptr = uc_ptr_;
-        data_ptr->cp_ptr = cp_ptr_;
 
         sc_ptr_ = std::make_shared<SafeController>();
         sc_ptr_->SetRobotDataSource(ri_ptr_);
@@ -98,18 +88,19 @@ public:
         current_state_name_ = kIdle;
         next_state_name_ = kIdle;
 
-        std::cout << "control period: " << control_period_ms_ << " ms, policy: " << policy_path << std::endl;
+        std::cout << "control period: " << cfg.control_period_ms << " ms, 默认模型: " << policy_path << std::endl;
 
-        // ---- 状态回报（供 invicta_remote 判前提 + 将来做状态上报）----
+        // ---- 状态回报（供 invicta_remote 判前提 + 状态上报）----
         state_reporter_ = std::make_shared<interface::MotionStateReporter>(
-            &StateBase::msfb_, uc_ptr_->GetUserCommand(), state_topic, state_hz, version);
+            &StateBase::msfb_, uc_ptr_->GetUserCommand(), cfg.topic_motion_state, cfg.state_report_hz,
+            cfg.robot_version);
 
         ri_ptr_->Start();
         uc_ptr_->Start();
         sc_ptr_->Start();
         state_reporter_->Start();
         current_controller_->OnEnter();
-        std::cout << "state report: " << state_topic << " @ " << state_hz << " Hz" << std::endl;
+        std::cout << "state report: " << cfg.topic_motion_state << " @ " << cfg.state_report_hz << " Hz" << std::endl;
     }
 
     std::shared_ptr<StateBase> GetStateControllerPtr(StateName state_name) {

src/invicta_deploy/state_machine/quadruped_wheel/joint_damping_state.hpp

@@ -2,13 +2,10 @@
  * @file joint_damping_state.hpp
  * @brief 关节阻尼（过渡态）：缓慢松下 → 机器人着地 → 转入空闲（自由）
  *
- * 设计意图：这不是一个「保持」状态，而是松腿过程。只给 kd、不给位置增益，关节在重力下缓慢下沉；
+ * 只给 kd、不给位置增益，关节在重力下缓慢下沉；到时后转 Idle（Idle 输出零增益 = 完全自由）。
- * 到时后转 Idle（Idle 输出零增益 = 完全自由）。
+ * 时长与增益来自 invicta.toml（[leg.timing].damping_hold_s、[leg.gains].damping_kd_*）。
  *
- * 相对上游 M20 的两处改动：
+ * 计时用单调时钟：若关节数据断流，用数据时间戳计时会永远停在阻尼（真机上等于松不了腿）。
- *   1. 3 s 写死 → 改为参数 timing/joint_damping_duration（0.1~30 s）
- *   2. 计时源从「关节数据时间戳」改为单调时钟：原先若关节数据断流，时间戳不前进，
- *      状态会永远停在阻尼（真机上等于松不了腿也进不了后续状态）
  */
 #pragma once
 
@@ -26,24 +23,24 @@ private:
 public:
     JointDampingState(const std::string& state_name, std::shared_ptr<ControllerData> data_ptr)
         : StateBase(state_name, data_ptr) {
-        Vec4f one_leg_kd;
+        const auto& cfg = invicta::Config();
-        one_leg_kd << cp_ptr_->swing_leg_kd_, 1.;
+        joint_cmd_ = MatXf::Zero(int(ri_ptr_->dof_num_), 5);
-        VecXf kd_ = one_leg_kd.replicate(4, 1);
+        for (size_t l = 0; l < cfg.legs.size(); ++l) {
-        joint_cmd_ = MatXf::Zero(16, 5);
+            joint_cmd_(cfg.legs[l].swing_index, 2) = float(cfg.damping_kd_leg);
-        joint_cmd_.col(2) = kd_;
+            joint_cmd_(cfg.legs[l].wheel_index, 2) = float(cfg.damping_kd_wheel);
+        }
     }
     ~JointDampingState() {}
 
     virtual void OnEnter() {
         start_time_ = std::chrono::steady_clock::now();
-        std::cout << "[DAMPING] 进入关节阻尼，持续 " << cp_ptr_->joint_damping_duration_ << " s 后转入空闲"
+        std::cout << "[DAMPING] 进入关节阻尼，持续 " << invicta::Config().damping_hold_s << " s 后转入空闲"
                   << std::endl;
         msfb_.UpdateCurrentState(RobotMotionState::JointDamping);
     }
 
     virtual void OnExit() {
-        const double elapsed = Elapsed();
+        std::cout << "[DAMPING] 结束（实际 " << Elapsed() << " s），转入空闲" << std::endl;
-        std::cout << "[DAMPING] 结束（实际 " << elapsed << " s），转入空闲" << std::endl;
     }
 
     virtual void Run() { ri_ptr_->SetJointCommand(joint_cmd_); }
@@ -51,9 +48,7 @@ public:
     virtual bool LoseControlJudge() { return false; }
 
     virtual StateName GetNextStateName() {
-        if (Elapsed() < double(cp_ptr_->joint_damping_duration_)) {
+        if (Elapsed() < invicta::Config().damping_hold_s) return StateName::kJointDamping;
-            return StateName::kJointDamping;
-        }
         return StateName::kIdle;
     }
 

src/invicta_deploy/state_machine/quadruped_wheel/liedown_state.hpp

@@ -1,150 +1,112 @@
 /**
  * @file liedown_state.hpp
- * @brief liedown state
+ * @brief 趴下：摆臂角样条到趴下目标 → 阻尼保持 → 完全放松
- * @author DeepRobotics
- * @version 1.0
- * @date 2026-02-12
- * 
- * @copyright Copyright (c) 2025  DeepRobotics
  *
+ * 目标高度、倒向、增益都来自 invicta.toml：
+ *   [leg].lie_height、[[legs]].splay_sign（前腿向前、后腿向后，与 M20 相反）、
+ *   [leg.gains].lie_kp/lie_kd/damping_kd_*、[leg.timing].liedown_duration_s/damping_hold_s
  */
 #pragma once
 
 #include "state_base.h"
 
 namespace invicta {
-class LieDownState : public StateBase{
+
+class LieDownState : public StateBase {
 private:
-    VecXf init_joint_pos_, init_joint_vel_, current_joint_pos_, current_joint_vel_;
+    VecXf init_swing_, target_swing_;
-    double time_stamp_record_, run_time_;
+    VecXf current_joint_pos_, current_joint_vel_;
-    VecXf goal_joint_pos_, kp_, kd_;
+    double time_stamp_record_ = 0., run_time_ = 0.;
     MatXf joint_cmd_;
-    float liedown_duration_ = 2.;
 
-    const float init_hipx_pos_ = Deg2Rad(0.);
+    void GetRobotJointValue() {
-    const float set_wheel_kd_ = 1.;
-
-    void GetRobotJointValue(){
         current_joint_pos_ = ri_ptr_->GetJointPosition();
         current_joint_vel_ = ri_ptr_->GetJointVelocity();
         run_time_ = ri_ptr_->GetInterfaceTimeStamp();
     }
 
-    void RecordJointData(){
-        init_joint_pos_ = current_joint_pos_;
-        init_joint_vel_ = current_joint_vel_;
-        time_stamp_record_ = run_time_;
-    }
-    
-    float GetHipYPosByHeight(float h){
-        float l1 = cp_ptr_->thigh_len_;
-        float l2 = cp_ptr_->shank_len_;
-        float default_pos = (cp_ptr_->fl_joint_lower_(1)+cp_ptr_->fl_joint_upper_(1)) / 2.;
-        if(fabs(h) >= l1 + l2) {
-            std::cerr << "error height input" << std::endl;
-            return 0;
-        }
-        float theta = -acos((l1*l1+h*h-l2*l2)/(2.*h*l1));
-        LimitNumber(theta, cp_ptr_->fl_joint_lower_(1), cp_ptr_->fl_joint_upper_(1));
-        return theta;
-    }
-
-    float GetKneePosByHeight(float h){
-        float l1 = cp_ptr_->thigh_len_;
-        float l2 = cp_ptr_->shank_len_;
-        float default_pos = (cp_ptr_->fl_joint_lower_(2)+cp_ptr_->fl_joint_upper_(2)) / 2.;
-        if(fabs(h) >= l1 + l2) {
-            std::cerr << "error height input" << std::endl;
-            return 0;
-        }
-        float theta = M_PI-acos((l1*l1+l2*l2-h*h)/(2*l1*l2));
-        LimitNumber(theta, cp_ptr_->fl_joint_lower_(2), cp_ptr_->fl_joint_upper_(2));
-        return theta;
-    }
-
 public:
     LieDownState(const std::string& state_name, std::shared_ptr<ControllerData> data_ptr)
         : StateBase(state_name, data_ptr) {
-            init_joint_pos_ = VecXf::Zero(16); 
+        const auto& cfg = invicta::Config();
-            init_joint_vel_ = VecXf::Zero(16);
+        const size_t n = cfg.legs.size();
-            current_joint_pos_ = VecXf::Zero(16);
-            current_joint_vel_ = VecXf::Zero(16);
 
-            goal_joint_pos_ = Vec4f(init_hipx_pos_, GetHipYPosByHeight(0.03), GetKneePosByHeight(0.03), 0).replicate(4, 1);
+        target_swing_.resize(int(n));
-            goal_joint_pos_(4) = -init_hipx_pos_; goal_joint_pos_(12) = -init_hipx_pos_;
+        for (size_t l = 0; l < n; ++l) {
-
+            target_swing_(int(l)) = float(cfg.SwingAngleForHeight(cfg.lie_height, int(l)));
-            goal_joint_pos_(9) = -goal_joint_pos_(9);
-            goal_joint_pos_(10) = -goal_joint_pos_(10);
-            goal_joint_pos_(13) = -goal_joint_pos_(13);
-            goal_joint_pos_(14) = -goal_joint_pos_(14);
-
-            Vec4f one_leg_kp, one_leg_kd;
-
-            one_leg_kp << 300, 300, 300, 0;
-            one_leg_kd << 4.5, 4.5, 4.5, 3; //2; //0;
-            kp_ = one_leg_kp.replicate(4, 1);
-            kd_ = one_leg_kd.replicate(4, 1);
-            liedown_duration_ = cp_ptr_->liedown_duration_;
         }
-    ~LieDownState(){}
+
+        joint_cmd_ = MatXf::Zero(int(ri_ptr_->dof_num_), 5);
+        for (size_t l = 0; l < n; ++l) {
+            joint_cmd_(cfg.legs[l].swing_index, 0) = float(cfg.lie_kp);
+            joint_cmd_(cfg.legs[l].swing_index, 2) = float(cfg.lie_kd);
+            joint_cmd_(cfg.legs[l].wheel_index, 0) = 0.f;
+            joint_cmd_(cfg.legs[l].wheel_index, 2) = float(cfg.damping_kd_wheel);
+
+            char buf[160];
+            snprintf(buf, sizeof(buf), "[GAIT] 趴下: %s 目标高度 %.3f m → 摆臂 %+.1f°\n", cfg.legs[l].name.c_str(),
+                     cfg.lie_height, target_swing_(int(l)) * 180. / M_PI);
+            std::cout << buf;
+        }
+    }
+    ~LieDownState() {}
 
     virtual void OnEnter() {
-        joint_cmd_ = MatXf::Zero(16, 5);
+        const auto& cfg = invicta::Config();
-        joint_cmd_.col(0) = kp_;
-        joint_cmd_.col(2) = kd_;
         GetRobotJointValue();
-        RecordJointData();  
+        init_swing_.resize(int(cfg.legs.size()));
+        for (size_t l = 0; l < cfg.legs.size(); ++l) init_swing_(int(l)) = current_joint_pos_(cfg.legs[l].swing_index);
+        time_stamp_record_ = run_time_;
         StateBase::msfb_.UpdateCurrentState(RobotMotionState::LieDown);
-    };
-
-    virtual void OnExit() {
     }
 
+    virtual void OnExit() {}
+
     virtual void Run() {
+        const auto& cfg = invicta::Config();
         GetRobotJointValue();
-        VecXf planning_joint_pos(current_joint_pos_.rows());
+        const double elapsed = run_time_ - time_stamp_record_;
-        VecXf planning_joint_vel(current_joint_pos_.rows());
+
-        if(run_time_ - time_stamp_record_ <= liedown_duration_){
+        if (elapsed <= cfg.liedown_duration_s) {
-            for(int i=0;i<current_joint_pos_.rows();++i){
+            // ① 样条到趴下姿态（摆臂带速度前馈；轮不给位置，只靠 kd）
-                planning_joint_pos(i) = GetCubicSplinePos(init_joint_pos_(i), init_joint_vel_(i), goal_joint_pos_(i), 0, 
+            for (size_t l = 0; l < cfg.legs.size(); ++l) {
-                                                run_time_ - time_stamp_record_, liedown_duration_);
+                const int si = cfg.legs[l].swing_index;
-                planning_joint_vel(i) = GetCubicSplineVel(init_joint_pos_(i), init_joint_vel_(i), goal_joint_pos_(i), 0, 
+                joint_cmd_(si, 1) = GetCubicSplinePos(init_swing_(int(l)), current_joint_vel_(si),
-                                                run_time_ - time_stamp_record_, liedown_duration_);
+                                                      target_swing_(int(l)), 0.f, float(elapsed),
-                if(i%4==3){
+                                                      float(cfg.liedown_duration_s));
-                    planning_joint_vel(i) = 0.0;
+                joint_cmd_(si, 3) = GetCubicSplineVel(init_swing_(int(l)), current_joint_vel_(si),
+                                                      target_swing_(int(l)), 0.f, float(elapsed),
+                                                      float(cfg.liedown_duration_s));
             }
+            ri_ptr_->SetJointCommand(joint_cmd_);
+            return;
         }
 
-            joint_cmd_.col(1) = planning_joint_pos;
+        // ② 阻尼保持（摆臂只给 kd），③ 之后全零（完全放松），状态机随后回 idle
-            joint_cmd_.col(3) = planning_joint_vel;
+        MatXf cmd = MatXf::Zero(int(ri_ptr_->dof_num_), 5);
-            joint_cmd_.col(2) = kd_;
+        if (elapsed <= cfg.liedown_duration_s + cfg.damping_hold_s) {
-            ri_ptr_->SetJointCommand(joint_cmd_);
+            for (size_t l = 0; l < cfg.legs.size(); ++l) {
-        } else if (run_time_ - time_stamp_record_ <=  2.0 * liedown_duration_){
+                cmd(cfg.legs[l].swing_index, 2) = float(cfg.damping_kd_leg);
-            joint_cmd_ = MatXf::Zero(16, 5);
+                cmd(cfg.legs[l].wheel_index, 2) = float(cfg.damping_kd_wheel);
-            joint_cmd_.col(2) = kd_;
+            }
+        }
+        ri_ptr_->SetJointCommand(cmd);
+    }
 
-            ri_ptr_->SetJointCommand(joint_cmd_);
-        } else {
-            joint_cmd_ = MatXf::Zero(16, 5);
-            ri_ptr_->SetJointCommand(joint_cmd_);
-        }
-    }
     virtual bool LoseControlJudge() {
         if (uc_ptr_->GetUserCommand()->target_mode == uint8_t(RobotMotionState::JointDamping)) return true;
         return false;
     }
-    virtual StateName GetNextStateName() {
-        if(uc_ptr_->GetUserCommand()->safe_control_mode!=0){
-            return StateName::kJointDamping;
-        } 
 
-        if(run_time_ - time_stamp_record_ <= 2.*liedown_duration_){
+    virtual StateName GetNextStateName() {
-            return StateName::kLieDown;
+        const auto& cfg = invicta::Config();
-        }else{
+        if (uc_ptr_->GetUserCommand()->safe_control_mode != 0) return StateName::kJointDamping;
-            if(uc_ptr_->GetUserCommand()->target_mode == uint8_t(RobotMotionState::StandingUp)){
+
+        const double elapsed = run_time_ - time_stamp_record_;
+        if (elapsed <= cfg.liedown_duration_s + cfg.damping_hold_s) return StateName::kLieDown;
+
+        if (uc_ptr_->GetUserCommand()->target_mode == uint8_t(RobotMotionState::StandingUp))
             return StateName::kStandUp;
-            }
-        }
         return StateName::kLieDown;
     }
 };

src/invicta_deploy/state_machine/quadruped_wheel/standup_state.hpp

@@ -1,190 +1,162 @@
 /**
  * @file standup_state.hpp
- * @brief from sit state to stand state
+ * @brief 起立：摆臂角三段样条（当前 → 收腿姿态 → 站立姿态），并保留站立后自动进 RL 的自检
- * @author DeepRobotics
- * @version 1.0
- * @date 2025-11-07
  *
- * @copyright Copyright (c) 2025  DeepRobotics
+ * 运动学：每条腿只有 摆臂 + 轮 两个关节，髋轴→轮轴高度 h = L·cos(θ)，
+ *         θ = splay_sign · acos(h/L)（invicta.toml 的 [leg] 与 [[legs]]）。
+ * 摆臂倒向由 splay_sign 决定：本题要求趴下/收腿时「前腿向前、后腿向后」（与 M20 相反）。
  *
+ * 与上游 M20 的差别：不再有「每腿 4 关节、i%4==3 是轮、外摆钉在 0、后腿髋膝取反」这些假设，
+ * 左右/前后的镜像已经在配置的标定（dir/offset）里归一。
  */
 #pragma once
 
 #include "state_base.h"
 
 namespace invicta {
-class StandUpState : public StateBase{
+
+class StandUpState : public StateBase {
 private:
-    VecXf init_joint_pos_, init_joint_vel_, current_joint_pos_, current_joint_vel_;
+    VecXf init_swing_, tuck_swing_, stand_swing_;  ///< 每条腿的摆臂角（模型约定）
-    double time_stamp_record_, run_time_;
+    VecXf current_joint_pos_, current_joint_vel_;
-    VecXf goal_joint_pos_, kp_, kd_;
+    double time_stamp_record_ = 0., run_time_ = 0.;
     MatXf joint_cmd_;
-    double stand_duration_ = 2.;
+    double tuck_duration_ = 1.5;   ///< 第一阶段：收腿
+    double raise_duration_ = 1.5;  ///< 第二阶段：撑起
     VecXf stand_pose_;             ///< 站立终点位姿（用于自检）
-    double last_ready_log_ = -1e9;  ///< 自检未通过日志节流
+    double last_ready_log_ = -1e9;
     bool rl_enter_logged_ = false;
 
-    const float init_hipx_pos_ = Deg2Rad(0.);
+    void GetRobotJointValue() {
-    const float set_wheel_kd_ = 1.;
-
-    void GetRobotJointValue(){
         current_joint_pos_ = ri_ptr_->GetJointPosition();
         current_joint_vel_ = ri_ptr_->GetJointVelocity();
         run_time_ = ri_ptr_->GetInterfaceTimeStamp();
     }
 
-    void RecordJointData(){
+    /// 模型约定的摆臂角（含 splay_sign）
-        init_joint_pos_ = current_joint_pos_;
+    float SwingAngleForHeight(float h, size_t leg) const {
-        init_joint_vel_ = current_joint_vel_;
+        return float(invicta::Config().SwingAngleForHeight(h, int(leg)));
-        time_stamp_record_ = run_time_;
-    }
-
-    float GetHipYPosByHeight(float h){
-        float l1 = cp_ptr_->thigh_len_;
-        float l2 = cp_ptr_->shank_len_;
-        float default_pos = (cp_ptr_->fl_joint_lower_(1)+cp_ptr_->fl_joint_upper_(1)) / 2.;
-        if(fabs(h) >= l1 + l2) {
-            std::cerr << "error height input" << std::endl;
-            return 0;
-        }
-        float theta = -acos((l1*l1+h*h-l2*l2)/(2.*h*l1));
-        theta = LimitNumber(theta, cp_ptr_->fl_joint_lower_(1), cp_ptr_->fl_joint_upper_(1));
-        return theta;
-    }
-
-    float GetKneePosByHeight(float h){
-        float l1 = cp_ptr_->thigh_len_;
-        float l2 = cp_ptr_->shank_len_;
-        float default_pos = (cp_ptr_->fl_joint_lower_(2)+cp_ptr_->fl_joint_upper_(2)) / 2.;
-        if(fabs(h) >= l1 + l2) {
-            std::cerr << "error height input" << std::endl;
-            return 0;
-        }
-        float theta = M_PI-acos((l1*l1+l2*l2-h*h)/(2*l1*l2));
-        theta = LimitNumber(theta, cp_ptr_->fl_joint_lower_(2), cp_ptr_->fl_joint_upper_(2));
-        return theta;
     }
 
 public:
     StandUpState(const std::string& state_name, std::shared_ptr<ControllerData> data_ptr)
         : StateBase(state_name, data_ptr) {
-            goal_joint_pos_ = Vec4f(init_hipx_pos_, GetHipYPosByHeight(cp_ptr_->pre_height_), GetKneePosByHeight(cp_ptr_->pre_height_), 0).replicate(4, 1);
+        const auto& cfg = invicta::Config();
-            goal_joint_pos_(4) = -init_hipx_pos_; goal_joint_pos_(12) = -init_hipx_pos_;
+        const size_t n = cfg.legs.size();
-            // 后髋膝取反
-            goal_joint_pos_(9) = -goal_joint_pos_(9);
-            goal_joint_pos_(10) = -goal_joint_pos_(10);
-            goal_joint_pos_(13) = -goal_joint_pos_(13);
-            goal_joint_pos_(14) = -goal_joint_pos_(14);
 
-            Vec4f one_leg_kp, one_leg_kd;
+        tuck_swing_.resize(int(n));
-            one_leg_kp << cp_ptr_->swing_leg_kp_, 0;
+        stand_swing_.resize(int(n));
-            one_leg_kd << cp_ptr_->swing_leg_kd_, 0;
+        for (size_t l = 0; l < n; ++l) {
-            kp_ = one_leg_kp.replicate(4, 1);
+            tuck_swing_(int(l)) = SwingAngleForHeight(float(cfg.pre_height), l);
-            kd_ = one_leg_kd.replicate(4, 1);
+            stand_swing_(int(l)) = SwingAngleForHeight(float(cfg.stand_height), l);
-            joint_cmd_ = MatXf::Zero(16, 5);
-            joint_cmd_.col(0) = kp_;
-            joint_cmd_.col(2) = kd_;
-            stand_duration_ = cp_ptr_->stand_duration_;
-
-            // 站立终点位姿：与 Run() 里 else 分支的目标一致
-            stand_pose_ = Vec4f(init_hipx_pos_, GetHipYPosByHeight(cp_ptr_->stand_height_),
-                                GetKneePosByHeight(cp_ptr_->stand_height_), 0).replicate(4, 1);
-            stand_pose_(4) = -init_hipx_pos_;
-            stand_pose_(12) = -init_hipx_pos_;
-            stand_pose_(9) = -stand_pose_(9);
-            stand_pose_(10) = -stand_pose_(10);
-            stand_pose_(13) = -stand_pose_(13);
-            stand_pose_(14) = -stand_pose_(14);
         }
-    ~StandUpState(){}
+
+        joint_cmd_ = MatXf::Zero(int(ri_ptr_->dof_num_), 5);
+        for (size_t l = 0; l < n; ++l) {
+            joint_cmd_(cfg.legs[l].swing_index, 0) = float(cfg.stand_kp);
+            joint_cmd_(cfg.legs[l].swing_index, 2) = float(cfg.stand_kd);
+            joint_cmd_(cfg.legs[l].wheel_index, 0) = 0.f;  // 轮 kp 恒 0
+
+            char buf[160];
+            snprintf(buf, sizeof(buf), "[GAIT] 起立: %s 收腿(%.3f m) %+.1f° → 站立(%.3f m) %+.1f°\n",
+                     cfg.legs[l].name.c_str(), cfg.pre_height, tuck_swing_(int(l)) * 180. / M_PI, cfg.stand_height,
+                     stand_swing_(int(l)) * 180. / M_PI);
+            std::cout << buf;
+        }
+
+        tuck_duration_ = cfg.stand_duration_s;
+        raise_duration_ = cfg.stand_duration_s;
+
+        // 站立终点位姿：只有摆臂有位置目标，轮为 0（kp=0 不生效）
+        stand_pose_ = VecXf::Zero(int(ri_ptr_->dof_num_));
+        for (size_t l = 0; l < n; ++l) stand_pose_(cfg.legs[l].swing_index) = stand_swing_(int(l));
+    }
+    ~StandUpState() {}
 
     virtual void OnEnter() {
         GetRobotJointValue();
-        RecordJointData();
+        const auto& cfg = invicta::Config();
+        init_swing_.resize(int(cfg.legs.size()));
+        for (size_t l = 0; l < cfg.legs.size(); ++l) init_swing_(int(l)) = current_joint_pos_(cfg.legs[l].swing_index);
+        time_stamp_record_ = run_time_;
         StateBase::msfb_.UpdateCurrentState(RobotMotionState::StandingUp);
-    };
-    virtual void OnExit() {
     }
+    virtual void OnExit() {}
+
     virtual void Run() {
         GetRobotJointValue();
-        VecXf planning_joint_pos(current_joint_pos_.rows());
+        const auto& cfg = invicta::Config();
-        VecXf planning_joint_vel(current_joint_pos_.rows());
+        const double elapsed = run_time_ - time_stamp_record_;
-        if(run_time_ - time_stamp_record_ <= stand_duration_){
+
-            for(int i=0;i<current_joint_pos_.rows();++i){
+        VecXf pos = VecXf::Zero(int(ri_ptr_->dof_num_));
-                planning_joint_pos(i) = GetCubicSplinePos(init_joint_pos_(i), init_joint_vel_(i), goal_joint_pos_(i), 0, 
+        VecXf vel = VecXf::Zero(int(ri_ptr_->dof_num_));
-                                                run_time_ - time_stamp_record_, stand_duration_);
+        VecXf kd = joint_cmd_.col(2);
-                planning_joint_vel(i) = GetCubicSplineVel(init_joint_pos_(i), init_joint_vel_(i), goal_joint_pos_(i), 0, 
+
-                                                run_time_ - time_stamp_record_, stand_duration_);
+        if (elapsed <= tuck_duration_) {
-                if(i%4==3){
+            // 第一阶段：把腿收到「收腿姿态」，轮自由滚动（kd=0）
-                    kd_(i) = 0;
+            for (size_t l = 0; l < cfg.legs.size(); ++l) {
+                const int si = cfg.legs[l].swing_index;
+                pos(si) = GetCubicSplinePos(init_swing_(int(l)), current_joint_vel_(si), tuck_swing_(int(l)), 0.f,
+                                            float(elapsed), float(tuck_duration_));
+                vel(si) = GetCubicSplineVel(init_swing_(int(l)), current_joint_vel_(si), tuck_swing_(int(l)), 0.f,
+                                            float(elapsed), float(tuck_duration_));
+                kd(cfg.legs[l].wheel_index) = 0.f;
             }
+        } else if (elapsed <= tuck_duration_ + raise_duration_) {
+            // 第二阶段：把机身撑到站立高度（按高度规划反解摆臂角，速度用数值微分做前馈）
+            const double t = elapsed - tuck_duration_;
+            const double dt = 0.001;
+            for (size_t l = 0; l < cfg.legs.size(); ++l) {
+                const int si = cfg.legs[l].swing_index;
+                const float h = GetCubicSplinePos(float(cfg.pre_height), 0.f, float(cfg.stand_height), 0.f,
+                                                  float(t), float(raise_duration_));
+                const float h_next = GetCubicSplinePos(float(cfg.pre_height), 0.f, float(cfg.stand_height), 0.f,
+                                                       float(t) + dt, float(raise_duration_));
+                const float th = SwingAngleForHeight(h, l);
+                const float th_next = SwingAngleForHeight(h_next, l);
+                pos(si) = th;
+                vel(si) = (th_next - th) / float(dt);
+                kd(cfg.legs[l].wheel_index) = float(cfg.damping_kd_wheel);  // 落地后刹住轮子
+            }
+        } else {
+            // 保持站立姿态
+            pos = stand_pose_;
+            for (size_t l = 0; l < cfg.legs.size(); ++l) kd(cfg.legs[l].wheel_index) = float(cfg.damping_kd_wheel);
         }
 
-        }else{
+        joint_cmd_.col(1) = pos;
-            double new_time = run_time_ - time_stamp_record_ - stand_duration_;
+        joint_cmd_.col(3) = vel;
-            float dt = 0.001;
+        joint_cmd_.col(2) = kd;
-            float plan_height = GetCubicSplinePos(cp_ptr_->pre_height_, 0, cp_ptr_->stand_height_, 0, 
-                                                new_time, stand_duration_);
-            float plan_height_next = GetCubicSplinePos(cp_ptr_->pre_height_, 0, cp_ptr_->stand_height_, 0, 
-                                                new_time+dt, stand_duration_);
-            float hipy_pos = GetHipYPosByHeight(plan_height);
-            float hipy_vel = (GetHipYPosByHeight(plan_height_next) - hipy_pos) / dt;
-            float knee_pos = GetKneePosByHeight(plan_height);
-            float knee_vel = (GetKneePosByHeight(plan_height_next) - knee_pos) / dt;
-            planning_joint_pos = Vec4f(init_hipx_pos_, hipy_pos, knee_pos, 0).replicate(4, 1);
-            planning_joint_pos(4) = -init_hipx_pos_; planning_joint_pos(12) = -init_hipx_pos_;
-
-            planning_joint_vel = Vec4f(0, hipy_vel, knee_vel, 0).replicate(4, 1);
-
-            // 后髋膝取反
-            planning_joint_pos(9) = -planning_joint_pos(9);
-            planning_joint_pos(10) = -planning_joint_pos(10);
-            planning_joint_pos(13) = -planning_joint_pos(13);
-            planning_joint_pos(14) = -planning_joint_pos(14);
-            planning_joint_vel(9) = -planning_joint_vel(9);
-            planning_joint_vel(10) = -planning_joint_vel(10);
-            planning_joint_vel(13) = -planning_joint_vel(13);
-            planning_joint_vel(14) = -planning_joint_vel(14);
-
-            for(int i=3;i<16;i+=4){
-                kd_(i) = set_wheel_kd_;
-            }
-        }
-
-        joint_cmd_.col(1) = planning_joint_pos;
-        joint_cmd_.col(3) = planning_joint_vel;
-        joint_cmd_.col(2) = kd_;
         ri_ptr_->SetJointCommand(joint_cmd_);
     }
+
     /// 进入 RL 前的自检（原厂机制：站立完成后检查通过即自动进 RL，不等指令）
     bool ReadyForRl() {
+        const auto& cfg = invicta::Config();
         if (uc_ptr_->GetUserCommand()->safe_control_mode != 0) {
             LogNotReady("安全标志非 0");
             return false;
         }
-        for (int i = 0; i < 16; ++i) {
+        for (int i = 0; i < int(current_joint_pos_.size()); ++i) {
             if (std::isnan(current_joint_pos_(i)) || std::isnan(current_joint_vel_(i))) {
                 LogNotReady("关节数据含 NaN");
                 return false;
             }
         }
-        if (cp_ptr_->rl_require_pose_) {
+        if (cfg.rl_require_pose) {
-            for (int i = 0; i < 16; ++i) {
+            for (size_t l = 0; l < cfg.legs.size(); ++l) {
-                if (i % 4 == 3) continue;  // 轮关节不要求位姿
+                const int si = cfg.legs[l].swing_index;
-                if (std::fabs(current_joint_pos_(i) - stand_pose_(i)) > cp_ptr_->rl_joint_tolerance_) {
+                if (std::fabs(current_joint_pos_(si) - stand_pose_(si)) > cfg.rl_joint_tolerance) {
-                    LogNotReady("关节 " + std::to_string(i) + " 未到站立位姿");
+                    LogNotReady("关节 " + cfg.joint_names[size_t(si)] + " 未到站立位姿");
                     return false;
                 }
-            }
+                if (std::fabs(current_joint_vel_(si)) > cfg.rl_velocity_tolerance) {
-            for (int i = 0; i < 16; ++i) {
+                    LogNotReady("关节 " + cfg.joint_names[size_t(si)] + " 速度过大");
-                if (i % 4 == 3) continue;
-                if (std::fabs(current_joint_vel_(i)) > cp_ptr_->rl_velocity_tolerance_) {
-                    LogNotReady("关节速度过大");
                     return false;
                 }
             }
         }
-        if (cp_ptr_->rl_require_imu_) {
+        if (cfg.rl_require_imu) {
             const Vec3f rpy = ri_ptr_->GetImuRpy();
             const Vec3f acc = ri_ptr_->GetImuAcc();
             for (int i = 0; i < 3; ++i) {
@@ -208,22 +180,24 @@ public:
     }
 
     virtual bool LoseControlJudge() {
-        if(uc_ptr_->GetUserCommand()->target_mode == uint8_t(RobotMotionState::JointDamping)) return true;
+        if (uc_ptr_->GetUserCommand()->target_mode == uint8_t(RobotMotionState::JointDamping)) return true;
         return false;
     }
     virtual StateName GetNextStateName() {
-        if(uc_ptr_->GetUserCommand()->safe_control_mode!=0) return StateName::kJointDamping;
+        const auto& cfg = invicta::Config();
-        if(run_time_ - time_stamp_record_ <= 2.*stand_duration_){
+        if (uc_ptr_->GetUserCommand()->safe_control_mode != 0) return StateName::kJointDamping;
-            return StateName::kStandUp;
+
-        }else{
+        const double elapsed = run_time_ - time_stamp_record_;
-            if(uc_ptr_->GetUserCommand()->target_mode == uint8_t(RobotMotionState::LieDown)){
+        const double ready_time = tuck_duration_ + raise_duration_;
+        if (elapsed <= ready_time) return StateName::kStandUp;
+
+        if (uc_ptr_->GetUserCommand()->target_mode == uint8_t(RobotMotionState::LieDown))
             return StateName::kLieDown;
-            }
+
-            const bool user_request = (uc_ptr_->GetUserCommand()->target_mode ==
+        const bool user_request =
-                                       uint8_t(RobotMotionState::RLControlMode));
+            (uc_ptr_->GetUserCommand()->target_mode == uint8_t(RobotMotionState::RLControlMode));
-            if (cp_ptr_->rl_auto_enter_ || user_request) {
+        if (cfg.rl_auto_enter || user_request) {
-                if (run_time_ - time_stamp_record_ >= 2. * stand_duration_ + cp_ptr_->rl_enter_delay_ &&
+            if (elapsed >= ready_time + cfg.rl_enter_delay && ReadyForRl()) {
-                    ReadyForRl()) {
                 if (!rl_enter_logged_) {
                     rl_enter_logged_ = true;
                     std::cout << "[RL] 站立完成且自检通过，进入 RL 控制" << std::endl;
@@ -231,7 +205,6 @@ public:
                 return StateName::kRLControl;
             }
         }
-        }
         return StateName::kStandUp;
     }
 };

src/invicta_deploy/state_machine/state_base.h

@@ -8,7 +8,7 @@
 #include "common_types.h"
 #include "custom_types.h"
 #include "motion_state_feedback.hpp"
-#include "parameters/control_parameters.h"
+#include "config/deploy_config.h"
 #include "robot_interface.h"
 #include "user_command_interface.h"
 
@@ -18,7 +18,6 @@ using namespace interface;
 struct ControllerData {
     std::shared_ptr<RobotInterface> ri_ptr;
     std::shared_ptr<UserCommandInterface> uc_ptr;
-    std::shared_ptr<ControlParameters> cp_ptr;
 };
 
 class StateBase {
@@ -27,7 +26,6 @@ public:
         : state_name_(state_name), data_ptr_(data_ptr) {
         ri_ptr_ = data_ptr_->ri_ptr;
         uc_ptr_ = data_ptr_->uc_ptr;
-        cp_ptr_ = data_ptr_->cp_ptr;
         std::memset(&msfb_, 0, sizeof(msfb_));
     }
 
@@ -53,7 +51,6 @@ public:
     std::shared_ptr<ControllerData> data_ptr_;
     std::shared_ptr<RobotInterface> ri_ptr_;
     std::shared_ptr<UserCommandInterface> uc_ptr_;
-    std::shared_ptr<ControlParameters> cp_ptr_;
 
     static MotionStateFeedback msfb_;
 };

src/invicta_deploy/state_machine/state_machine_base.h

@@ -100,7 +100,7 @@ public:
     }
 
     virtual void RunThread() {
-        set_timer.time_init(control_period_ms_);
+        set_timer.time_init(invicta::Config().control_period_ms);
         startTime = set_timer.get_start_time();
 
         while (ros::ok()) {
@@ -140,7 +140,6 @@ public:
 
     std::shared_ptr<UserCommandInterface> uc_ptr_;
     std::shared_ptr<RobotInterface> ri_ptr_;
-    std::shared_ptr<ControlParameters> cp_ptr_;
     std::shared_ptr<SafeController> sc_ptr_;
 
     std::shared_ptr<StateBase> current_controller_;

src/invicta_hardware/CMakeLists.txt

@@ -13,20 +13,36 @@ find_package(catkin REQUIRED COMPONENTS
   sensor_msgs
   geometry_msgs
   invicta_msgs
+  roslib
 )
 
 catkin_package(
   INCLUDE_DIRS include
-  CATKIN_DEPENDS roscpp sensor_msgs geometry_msgs invicta_msgs
+  CATKIN_DEPENDS roscpp roslib sensor_msgs geometry_msgs invicta_msgs
 )
 
+# TOML 解析库：系统安装（见 README「依赖」），CMake 里体现为 required
+find_package(tomlplusplus REQUIRED)
+get_target_property(TOMLPLUSPLUS_INCLUDE_DIR tomlplusplus::tomlplusplus INTERFACE_INCLUDE_DIRECTORIES)
+include_directories(${TOMLPLUSPLUS_INCLUDE_DIR})
+# 以 header-only 方式使用：实现编进本节点，避免引入运行时动态库依赖（目标机无需部署 .so）
+add_definitions(-DTOML_HEADER_ONLY=1)
+
 include_directories(
   include
   ${catkin_INCLUDE_DIRS}
 )
 
+# 把本节点自己的配置头列进源列表：改该头会触发重编（改 TOML 不用重编）
+set(NODE_CONFIG_HEADER "${CMAKE_CURRENT_SOURCE_DIR}/include/invicta_hardware/hardware_config.h")
+if(NOT EXISTS "${NODE_CONFIG_HEADER}")
+  message(WARNING "invicta_hardware_node: 找不到本节点配置头")
+  set(NODE_CONFIG_HEADER "")
+endif()
+
 add_executable(invicta_hardware_node
   src/invicta_hardware_node.cpp
+  ${NODE_CONFIG_HEADER}
 )
 
 target_link_libraries(invicta_hardware_node ${catkin_LIBRARIES})

src/invicta_hardware/README.md

@@ -34,18 +34,21 @@
 - [ ] `ReadHardware()`：SocketCAN（或你的总线）读回 16 关节的 position/velocity/effort + IMU，换算到 policy 约定。读失败返回 `false`，保留上一帧值，**不要发 NaN**。
 - [ ] `WriteHardware()`：按 `control_word` 落实使能/失能/清错，把目标角/角速度/前馈力矩/`kp`/`kd` 组装成帧下发；`tau = kp*(q_des-q) + kd*(dq_des-dq) + tau_ff` 由驱动器算还是本节点算，取决于关节模组协议，选一种并写清楚。
 - [ ] 上电顺序与安全：使能前的自检、通信超时（多久没指令就卸力）、过温/过流保护。注意 deploy 在 `idle` 状态也会以 200 Hz 发零增益指令，所以「按控制字判断是否使能」比「按有无指令判断」更稳。
-- [ ] 关节顺序必须与 `invicta_deploy` 的 `joint_names` 完全一致（默认 16 个 `fl/fr/hl/hr × hipx/hipy/knee/wheel`）。
+- [ ] 关节顺序与 `invicta_deploy` 共用同一份配置里的 `joint_names`，不会不一致。
 
-## 参数（私有命名空间，`config/invicta_hardware.yaml`）
+## 参数（本节点自己的 `config/invicta_hardware.toml`）
 
-| 参数 | 默认 | 说明 |
+| TOML 段 | 内容 |
-|---|---|---|
+|---|---|
-| `joint_states_topic` / `imu_topic` / `joint_commands_topic` | `joint_states` / `imu/data` / `joint_commands` | 话题名 |
+| `[robot].joint_names` | 关节名与顺序（8 个：`fl/fr/hl/hr × swing/wheel`） |
-| `imu_frame_id` | `base_link` | IMU 坐标系 |
+| `[joint_limits]` | 逐关节限位（下发前自检用） |
-| `publish_rate_hz` | 200.0 | 状态发布频率（需 ≥ deploy 的 200 Hz 控制周期） |
+| `[[legs]]` ×4 | 每腿 `swing`/`wheel` 的 `dir`、`offset_deg`、`bus`、`canid` |
-| `command_queue` | 1 | 指令只取最新一帧 |
+| `[motor.joint]` / `[motor.wheel]` | 电机量程：`p/v/t/kp/kd` 上下限（P65MAXS / W190S） |
-| `joint_names` | 16 个 | 关节顺序 |
+| `[motor.can]` | 总线名、仲裁 ID、1M/5M 比特率、上电使能帧数与间隔 |
+| `[motor.gains]` | 台架脚本当前增益（进 RL 前以 `invicta_deploy` 的 `[leg.gains]`/策略为准） |
+| `[topics]` `[hardware]` | 本节点话题、发布频率、IMU frame |
 
+启动时打印关节表、每条腿的总线/CANID 与电机量程，便于接线核对。
 ## 运行
 
 ```bash

src/invicta_hardware/config/invicta_hardware.toml

@@ -0,0 +1,104 @@
+# =============================================================================
+# invicta_hardware（关节/IMU 数据源 + 指令执行端）自己的配置
+# 只放本节点需要的参数：关节表与硬件标定、话题、发布频率、电机与 CAN
+# 运动学/策略/步态等属于 invicta_deploy，见 invicta_deploy/config/invicta_deploy.toml
+# =============================================================================
+
+[robot]
+# 关节顺序：必须与 /joint_states、/joint_commands 里的 name 对应
+joint_names = [
+  "fl_swing", "fl_wheel",
+  "fr_swing", "fr_wheel",
+  "hl_swing", "hl_wheel",
+  "hr_swing", "hr_wheel",
+]
+
+# ------------------------------------------------------------------ 关节限位
+# 顺序与 joint_names 一致；轮关节的 lower/upper 填 ±100 表示不限（用 vel_limit 约束）
+# 本节点实现 I/O 时用它做下发前的自检
+[joint_limits]
+lower     = [-1.40, -100.0,  -1.40, -100.0,  -1.40, -100.0,  -1.40, -100.0]   # [rad]
+upper     = [ 1.40,  100.0,   1.40,  100.0,   1.40,  100.0,   1.40,  100.0]   # [rad]
+vel_limit = [ 22.40, 100.0,  22.40, 100.0,  22.40, 100.0,  22.40, 100.0]      # [rad/s]
+
+# ------------------------------------------------------------------ 每腿的硬件标定与总线
+# 指令换算：q_motor = (q_model − offset_deg) · dir；反馈反向。
+#   dir       ：电机正方向与模型正方向的关系（±1，左右镜像、前后镜像都在这里）
+#   offset_deg：机械零点偏置 [deg]（模型零位 = 摆臂竖直向下时读到的电机角）
+#   bus/canid ：该关节挂在哪条 CANFD 总线、CANID 是多少
+# 注意：TOML 里数组表之后的裸键会归到最后一个元素，本节必须放在 motor 段之前
+[[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 }
+
+[[legs]]
+name        = "fr"
+swing_joint = "fr_swing"
+wheel_joint = "fr_wheel"
+swing = { dir = -1.0, offset_deg = 0.0, bus = "can1", canid = 1 }
+wheel = { dir = -1.0, offset_deg = 0.0, bus = "can1", canid = 3 }
+
+[[legs]]
+name        = "hl"
+swing_joint = "hl_swing"
+wheel_joint = "hl_wheel"
+swing = { dir =  1.0, offset_deg = 0.0, bus = "can0", canid = 2 }
+wheel = { dir =  1.0, offset_deg = 0.0, bus = "can0", canid = 4 }
+
+[[legs]]
+name        = "hr"
+swing_joint = "hr_swing"
+wheel_joint = "hr_wheel"
+swing = { dir = -1.0, offset_deg = 0.0, bus = "can1", canid = 2 }
+wheel = { dir = -1.0, offset_deg = 0.0, bus = "can1", canid = 4 }
+
+# ------------------------------------------------------------------ 电机与总线
+# 数值取自当前 CANFD 台架脚本（遥控 → can0/can1 双路），实现 ReadHardware/WriteHardware 时按此驱动
+[motor.joint]                # 摆臂关节电机
+model  = "P65MAXS"
+p_max  = 12.5                # [rad]
+p_min  = -12.5
+v_max  = 65.0                # [rad/s]
+v_min  = -65.0
+t_max  = 100.0               # [Nm]
+t_min  = -100.0
+kp_max = 500.0
+kd_max = 5.0
+
+[motor.wheel]                # 轮毂电机
+model  = "W190S"
+p_max  = 12.5
+p_min  = -12.5
+v_max  = 10.0                # [rad/s]
+v_min  = -10.0
+t_max  = 50.0
+t_min  = -50.0
+kp_max = 500.0
+kd_max = 5.0
+
+[motor.can]
+channels          = ["can0", "can1"]
+bitrate           = 1000000
+data_bitrate      = 5000000
+arbitration_id    = 0        # 控制报文仲裁 ID（台架脚本用 0x000）
+enable_frames     = 5        # 上电使能帧数
+enable_interval_s = 0.02
+
+[motor.gains]                # 台架脚本当前增益；接 RL 时以 [leg.gains]/策略里的 kp/kd 为准
+bench_joint_kp = 150.0
+bench_joint_kd = 2.0
+bench_wheel_kd = 0.5
+
+# ------------------------------------------------------------------ 本节点的话题与节奏
+[topics]
+joint_states   = "joint_states"      # 发布 sensor_msgs/JointState
+imu            = "imu/data"          # 发布 sensor_msgs/Imu
+joint_commands = "joint_commands"    # 订阅 invicta_msgs/JointCommandArray
+
+[hardware]
+publish_rate_hz = 200.0              # 关节/IMU 状态发布频率（RL 侧控制环 200 Hz）
+imu_frame_id    = "base_link"
+command_queue   = 1                  # 指令只取最新一帧

src/invicta_hardware/config/invicta_hardware.yaml

@@ -1,17 +0,0 @@
-# invicta_hardware 参数（私有命名空间 ~）
-
-# ---- 话题与频率 ----
-joint_states_topic: joint_states       # sensor_msgs/JointState
-imu_topic: imu/data                    # sensor_msgs/Imu
-joint_commands_topic: joint_commands    # invicta_msgs/JointCommandArray
-imu_frame_id: base_link
-
-publish_rate_hz: 200.0                 # 状态发布频率，需 >= RL 侧 200 Hz 控制周期
-command_queue: 1                       # 指令只取最新一帧
-
-# ---- 关节顺序（必须与 invicta_deploy 的 joint_names 完全一致）----
-joint_names:
-  [fl_hipx_joint, fl_hipy_joint, fl_knee_joint, fl_wheel_joint,
-   fr_hipx_joint, fr_hipy_joint, fr_knee_joint, fr_wheel_joint,
-   hl_hipx_joint, hl_hipy_joint, hl_knee_joint, hl_wheel_joint,
-   hr_hipx_joint, hr_hipy_joint, hr_knee_joint, hr_wheel_joint]

src/invicta_hardware/include/invicta_hardware/hardware_bridge.h

@@ -56,7 +56,7 @@ private:
     /**
      * @brief 从电机读回状态
      *
-     * TODO(硬件)：接 SocketCAN 读回 16 个关节的
+     * TODO(硬件)：接 SocketCAN 读回各关节（关节表见 invicta.toml）的
      *   position[rad] / velocity[rad/s] / effort[Nm]
      * 并做 raw → policy 约定的换算（方向、零点、单位）。
      * 同时读回 IMU：姿态四元数（弧度）、角速度[rad/s]（机体系）、线加速度[m/s²]（机体系）。

src/invicta_hardware/include/invicta_hardware/hardware_config.h

@@ -0,0 +1,258 @@
+/**
+ * @file hardware_config.h
+ * @brief invicta_hardware 自己的配置（TOML）：config/invicta_hardware.toml
+ *
+ * 只包含本节点需要的参数：关节表与硬件标定、每条腿挂的总线/CANID、电机与 CAN 参数、
+ * 本节点发布/订阅的话题与频率。
+ */
+#pragma once
+
+#include <toml++/toml.hpp>
+
+#include <algorithm>
+#include <string>
+#include <vector>
+
+namespace invicta_hardware {
+
+/// 单个关节：限位与硬件标定
+struct JointSpec {
+    std::string name;
+    bool is_wheel = false;
+    int leg = -1;
+    double lower = 0., upper = 0.;  ///< 位置限位 [rad]（下发前自检用）
+    double vel_limit = 0.;          ///< 速度限位 [rad/s]
+    double dir = 1.;                ///< 硬件方向 ±1
+    double offset_deg = 0.;         ///< 硬件零点偏置 [deg]
+};
+
+/// 一条腿在总线上的位置
+struct LegSpec {
+    std::string name;
+    std::string swing_joint, wheel_joint;
+    std::string swing_bus, wheel_bus;
+    int swing_canid = 0, wheel_canid = 0;
+    int swing_index = -1, wheel_index = -1;
+};
+
+/// 电机量程（MIT 类协议的 p/v/t/kp/kd 上限）
+struct MotorSpec {
+    std::string model;
+    double p_max = 12.5, p_min = -12.5;
+    double v_max = 0., v_min = 0.;
+    double t_max = 0., t_min = 0.;
+    double kp_max = 0., kd_max = 0.;
+};
+
+struct CanSpec {
+    std::vector<std::string> channels;
+    double bitrate = 1000000, data_bitrate = 5000000;
+    int arbitration_id = 0;
+    int enable_frames = 5;
+    double enable_interval_s = 0.02;
+};
+
+class HardwareConfig {
+public:
+    // ---- 关节表与标定 ----
+    std::vector<std::string> joint_names;
+    std::vector<JointSpec> joints;  ///< 顺序同 joint_names
+    std::vector<LegSpec> legs;
+    int dof = 0;
+
+    // ---- 电机与总线 ----
+    MotorSpec motor_joint, motor_wheel;
+    CanSpec can;
+    double bench_joint_kp = 150., bench_joint_kd = 2., bench_wheel_kd = 0.5;
+
+    // ---- 本节点话题与频率 ----
+    std::string topic_joint_states = "joint_states", topic_imu = "imu/data";
+    std::string topic_joint_commands = "joint_commands";
+    double publish_rate_hz = 200.0;
+    std::string imu_frame_id = "base_link";
+    int command_queue = 1;
+
+    bool Load(const std::string& path, std::string* error = nullptr);
+
+    int IndexOf(const std::string& name) const {
+        auto it = std::find(joint_names.begin(), joint_names.end(), name);
+        return it == joint_names.end() ? -1 : int(it - joint_names.begin());
+    }
+    const JointSpec& Joint(int i) const { return joints[size_t(i)]; }
+    bool IsWheel(int i) const { return joints[size_t(i)].is_wheel; }
+
+    std::string Summary() const;
+};
+
+inline HardwareConfig& Config() {
+    static HardwareConfig cfg;
+    return cfg;
+}
+
+namespace detail {
+
+inline const toml::table* Section(const toml::table& t, const char* key) { return t[key].as_table(); }
+
+template <typename T>
+inline void Get(const toml::table* sect, const char* key, T& out) {
+    if (!sect) return;
+    if (auto v = (*sect)[key].value<T>()) out = *v;
+}
+inline void GetStr(const toml::table* sect, const char* key, std::string& out) {
+    if (!sect) return;
+    if (auto v = (*sect)[key].value<std::string>()) out = *v;
+}
+inline void GetDoubleArr(const toml::table* sect, const char* key, std::vector<double>& out) {
+    if (!sect) return;
+    if (const auto* arr = (*sect)[key].as_array()) {
+        out.clear();
+        for (const auto& e : *arr)
+            if (auto v = e.value<double>()) out.push_back(*v);
+    }
+}
+inline void GetStrArr(const toml::table* sect, const char* key, std::vector<std::string>& out) {
+    if (!sect) return;
+    if (const auto* arr = (*sect)[key].as_array()) {
+        out.clear();
+        for (const auto& e : *arr)
+            if (auto v = e.value<std::string>()) out.push_back(*v);
+    }
+}
+
+}  // namespace detail
+
+inline bool HardwareConfig::Load(const std::string& path, std::string* error) {
+    const auto fail = [&](const std::string& msg) {
+        if (error) *error = msg;
+        return false;
+    };
+
+    toml::table tbl;
+    try {
+        tbl = toml::parse_file(path);
+    } catch (const toml::parse_error& e) {
+        return fail(std::string("TOML 解析失败: ") + std::string(e.description()) + " @ " + path);
+    }
+
+    const toml::table* robot = detail::Section(tbl, "robot");
+    if (!robot) return fail("缺少 [robot] 段");
+    detail::GetStrArr(robot, "joint_names", joint_names);
+    if (joint_names.empty()) return fail("[robot].joint_names 为空");
+    dof = int(joint_names.size());
+
+    std::vector<double> lower, upper, vel_limit;
+    const toml::table* limits = detail::Section(tbl, "joint_limits");
+    detail::GetDoubleArr(limits, "lower", lower);
+    detail::GetDoubleArr(limits, "upper", upper);
+    detail::GetDoubleArr(limits, "vel_limit", vel_limit);
+    if (int(lower.size()) != dof || int(upper.size()) != dof || int(vel_limit.size()) != dof) {
+        return fail("[joint_limits] 的 lower/upper/vel_limit 长度必须等于关节数量");
+    }
+    joints.assign(size_t(dof), JointSpec{});
+    for (int i = 0; i < dof; ++i) {
+        joints[size_t(i)].name = joint_names[size_t(i)];
+        joints[size_t(i)].lower = lower[size_t(i)];
+        joints[size_t(i)].upper = upper[size_t(i)];
+        joints[size_t(i)].vel_limit = vel_limit[size_t(i)];
+    }
+
+    legs.clear();
+    if (const auto* leg_arr = tbl["legs"].as_array()) {
+        for (const auto& elem : *leg_arr) {
+            const toml::table* lt = elem.as_table();
+            if (!lt) continue;
+            LegSpec leg;
+            detail::GetStr(lt, "name", leg.name);
+            detail::GetStr(lt, "swing_joint", leg.swing_joint);
+            detail::GetStr(lt, "wheel_joint", leg.wheel_joint);
+            leg.swing_index = IndexOf(leg.swing_joint);
+            leg.wheel_index = IndexOf(leg.wheel_joint);
+            if (leg.swing_index < 0 || leg.wheel_index < 0) {
+                return fail("[[legs]] " + leg.name + " 的 swing_joint/wheel_joint 不在 joint_names 里");
+            }
+            const toml::table* sw = detail::Section(*lt, "swing");
+            const toml::table* wh = detail::Section(*lt, "wheel");
+            detail::Get(sw, "dir", joints[size_t(leg.swing_index)].dir);
+            detail::Get(sw, "offset_deg", joints[size_t(leg.swing_index)].offset_deg);
+            detail::GetStr(sw, "bus", leg.swing_bus);
+            detail::Get(sw, "canid", leg.swing_canid);
+            detail::Get(wh, "dir", joints[size_t(leg.wheel_index)].dir);
+            detail::Get(wh, "offset_deg", joints[size_t(leg.wheel_index)].offset_deg);
+            detail::GetStr(wh, "bus", leg.wheel_bus);
+            detail::Get(wh, "canid", leg.wheel_canid);
+            joints[size_t(leg.swing_index)].leg = int(legs.size());
+            joints[size_t(leg.wheel_index)].leg = int(legs.size());
+            joints[size_t(leg.wheel_index)].is_wheel = true;
+            legs.push_back(leg);
+        }
+    }
+    if (legs.empty()) return fail("缺少 [[legs]] 段");
+
+    if (const toml::table* mj = detail::Section(tbl, "motor")) {
+        const auto load_motor = [](const toml::table* m, MotorSpec& out) {
+            detail::GetStr(m, "model", out.model);
+            detail::Get(m, "p_max", out.p_max);
+            detail::Get(m, "p_min", out.p_min);
+            detail::Get(m, "v_max", out.v_max);
+            detail::Get(m, "v_min", out.v_min);
+            detail::Get(m, "t_max", out.t_max);
+            detail::Get(m, "t_min", out.t_min);
+            detail::Get(m, "kp_max", out.kp_max);
+            detail::Get(m, "kd_max", out.kd_max);
+        };
+        load_motor(detail::Section(*mj, "joint"), motor_joint);
+        load_motor(detail::Section(*mj, "wheel"), motor_wheel);
+
+        if (const toml::table* c = detail::Section(*mj, "can")) {
+            detail::GetStrArr(c, "channels", can.channels);
+            detail::Get(c, "bitrate", can.bitrate);
+            detail::Get(c, "data_bitrate", can.data_bitrate);
+            detail::Get(c, "arbitration_id", can.arbitration_id);
+            detail::Get(c, "enable_frames", can.enable_frames);
+            detail::Get(c, "enable_interval_s", can.enable_interval_s);
+        }
+        const toml::table* mg = detail::Section(*mj, "gains");
+        detail::Get(mg, "bench_joint_kp", bench_joint_kp);
+        detail::Get(mg, "bench_joint_kd", bench_joint_kd);
+        detail::Get(mg, "bench_wheel_kd", bench_wheel_kd);
+    }
+
+    const toml::table* topics = detail::Section(tbl, "topics");
+    detail::GetStr(topics, "joint_states", topic_joint_states);
+    detail::GetStr(topics, "imu", topic_imu);
+    detail::GetStr(topics, "joint_commands", topic_joint_commands);
+
+    const toml::table* hw = detail::Section(tbl, "hardware");
+    detail::Get(hw, "publish_rate_hz", publish_rate_hz);
+    detail::GetStr(hw, "imu_frame_id", imu_frame_id);
+    detail::Get(hw, "command_queue", command_queue);
+    if (publish_rate_hz <= 0.) return fail("[hardware].publish_rate_hz 必须 > 0");
+
+    if (error) error->clear();
+    return true;
+}
+
+inline std::string HardwareConfig::Summary() const {
+    char buf[512];
+    std::string s;
+    snprintf(buf, sizeof(buf), "[config] %d 关节，%zu 条腿，状态发布 %.0f Hz，IMU frame=%s\n", dof, legs.size(),
+             publish_rate_hz, imu_frame_id.c_str());
+    s += buf;
+    for (const auto& leg : legs) {
+        snprintf(buf, sizeof(buf), "[config]   %s: swing %s(%s#%d) wheel %s(%s#%d)\n", leg.name.c_str(),
+                 leg.swing_joint.c_str(), leg.swing_bus.c_str(), leg.swing_canid, leg.wheel_joint.c_str(),
+                 leg.wheel_bus.c_str(), leg.wheel_canid);
+        s += buf;
+    }
+    snprintf(buf, sizeof(buf), "[config] 电机: 关节 %s(p±%.1f v±%.1f t±%.0f) 轮 %s(p±%.1f v±%.1f t±%.0f)\n",
+             motor_joint.model.c_str(), motor_joint.p_max, motor_joint.v_max, motor_joint.t_max,
+             motor_wheel.model.c_str(), motor_wheel.p_max, motor_wheel.v_max, motor_wheel.t_max);
+    s += buf;
+    snprintf(buf, sizeof(buf), "[config] CAN: %zu 路 %s.. @%.1fM/%.1fM, 使能 %d 帧 x %.3f s\n",
+             can.channels.size(), can.channels.empty() ? "-" : can.channels.front().c_str(), can.bitrate / 1e6,
+             can.data_bitrate / 1e6, can.enable_frames, can.enable_interval_s);
+    s += buf;
+    return s;
+}
+
+}  // namespace invicta_hardware

src/invicta_hardware/launch/invicta_hardware.launch

@@ -1,6 +1,6 @@
 <launch>
-  <!-- 关节/IMU 数据源 + 关节指令执行端（骨架）。参数在节点的私有命名空间下 -->
+  <!-- 关节/IMU 数据源 + 关节指令执行端（骨架）。参数来自本节点自己的 config/invicta_hardware.toml -->
   <node pkg="invicta_hardware" type="invicta_hardware_node" name="invicta_hardware" output="screen">
-    <rosparam command="load" file="$(find invicta_hardware)/config/invicta_hardware.yaml"/>
+    <param name="config_file" value="$(find invicta_hardware)/config/invicta_hardware.toml"/>
   </node>
 </launch>

src/invicta_hardware/package.xml

@@ -12,6 +12,7 @@
   <buildtool_depend>catkin</buildtool_depend>
 
   <depend>roscpp</depend>
+  <depend>roslib</depend>
   <depend>sensor_msgs</depend>
   <depend>geometry_msgs</depend>
   <depend>invicta_msgs</depend>

src/invicta_hardware/src/invicta_hardware_node.cpp

@@ -2,10 +2,12 @@
  * @file invicta_hardware_node.cpp
  * @brief 关节/IMU 数据源 + 关节指令执行端（骨架入口）
  */
+#include <ros/package.h>
 #include <ros/ros.h>
 
 #include <thread>
 
+#include "invicta_hardware/hardware_config.h"
 #include "invicta_hardware/hardware_bridge.h"
 #include "invicta_msgs/control_word.h"
 
@@ -28,18 +30,16 @@ HardwareBridge::HardwareBridge(const ros::NodeHandle& pnh) : nh_() {
 }
 
 void HardwareBridge::LoadParams(const ros::NodeHandle& pnh) {
-    pnh.param<std::string>("joint_states_topic", joint_state_topic_, "joint_states");
+    // 参数来自本节点自己的 config/invicta_hardware.toml
-    pnh.param<std::string>("imu_topic", imu_topic_, "imu/data");
+    const auto& cfg = invicta_hardware::Config();
-    pnh.param<std::string>("joint_commands_topic", command_topic_, "joint_commands");
+    joint_state_topic_ = cfg.topic_joint_states;
-    pnh.param<std::string>("imu_frame_id", imu_frame_id_, "base_link");
+    imu_topic_ = cfg.topic_imu;
-    pnh.param<double>("publish_rate_hz", publish_rate_hz_, 200.0);
+    command_topic_ = cfg.topic_joint_commands;
-    pnh.param<int>("command_queue", command_queue_, 1);
+    imu_frame_id_ = cfg.imu_frame_id;
+    publish_rate_hz_ = cfg.publish_rate_hz;
+    command_queue_ = cfg.command_queue;
 
-    joint_names_ = {"fl_hipx_joint", "fl_hipy_joint", "fl_knee_joint", "fl_wheel_joint",
+    joint_names_ = cfg.joint_names;  // 顺序必须与 invicta_deploy 一致（同一份配置，天然一致）
-                    "fr_hipx_joint", "fr_hipy_joint", "fr_knee_joint", "fr_wheel_joint",
-                    "hl_hipx_joint", "hl_hipy_joint", "hl_knee_joint", "hl_wheel_joint",
-                    "hr_hipx_joint", "hr_hipy_joint", "hr_knee_joint", "hr_wheel_joint"};
-    pnh.getParam("joint_names", joint_names_);
 
     if (publish_rate_hz_ <= 0.) {
         ROS_ERROR("[invicta_hardware] publish_rate_hz 必须 > 0，回退到 200 Hz");
@@ -167,6 +167,16 @@ int main(int argc, char** argv) {
     ros::NodeHandle nh;
     ros::NodeHandle pnh("~");
 
+    std::string config_path;
+    pnh.param<std::string>("config_file", config_path, "");
+    if (config_path.empty()) config_path = ros::package::getPath("invicta_hardware") + "/config/invicta_hardware.toml";
+    std::string err;
+    if (!invicta_hardware::Config().Load(config_path, &err)) {
+        ROS_FATAL("[invicta_hardware] 配置加载失败：%s（%s）", err.c_str(), config_path.c_str());
+        return 1;
+    }
+    std::cout << invicta_hardware::Config().Summary() << std::flush;
+
     invicta_hardware::HardwareBridge bridge(pnh);
     std::thread loop_thread(&invicta_hardware::HardwareBridge::Run, &bridge);
 

src/invicta_remote/CMakeLists.txt

@@ -12,6 +12,7 @@ find_package(catkin REQUIRED COMPONENTS
   roscpp
   sensor_msgs
   invicta_msgs
+  roslib
 )
 
 catkin_package(
@@ -19,13 +20,28 @@ catkin_package(
   CATKIN_DEPENDS roscpp sensor_msgs invicta_msgs
 )
 
+# TOML 解析库：系统安装（见 README「依赖」），CMake 里体现为 required
+find_package(tomlplusplus REQUIRED)
+get_target_property(TOMLPLUSPLUS_INCLUDE_DIR tomlplusplus::tomlplusplus INTERFACE_INCLUDE_DIRECTORIES)
+include_directories(${TOMLPLUSPLUS_INCLUDE_DIR})
+# 以 header-only 方式使用：实现编进本节点，避免引入运行时动态库依赖（目标机无需部署 .so）
+add_definitions(-DTOML_HEADER_ONLY=1)
+
 include_directories(
   include
   ${catkin_INCLUDE_DIRS}
 )
 
+# 把本节点自己的配置头列进源列表：改该头会触发重编（改 TOML 不用重编）
+set(NODE_CONFIG_HEADER "${CMAKE_CURRENT_SOURCE_DIR}/include/invicta_remote/remote_config.h")
+if(NOT EXISTS "${NODE_CONFIG_HEADER}")
+  message(WARNING "invicta_remote_node: 找不到本节点配置头")
+  set(NODE_CONFIG_HEADER "")
+endif()
+
 add_executable(invicta_remote_node
   src/invicta_remote_node.cpp
+  ${NODE_CONFIG_HEADER}
 )
 
 target_link_libraries(invicta_remote_node

src/invicta_remote/README.md

@@ -45,19 +45,15 @@
 
 错误码采用厂家定义（`0xE001`～`0xE00B`），见 `include/invicta_remote/protocol_defs.h`。
 
-## 参数（`config/invicta_remote.yaml`）
+## 参数（本节点自己的 `config/invicta_remote.toml`）
 
-| 参数 | 默认 | 说明 |
+| TOML 段 | 内容 |
-|---|---|---|
+|---|---|
-| `udp_port` | 30000 | 遥控 App 里配置的端口 |
+| `[protocol]` | `udp_port`、`heartbeat_timeout_s`、`unsupported_as_success` |
-| `topics/motion_command` | `motion_command` | 发布 `invicta_msgs/MotionCommand` |
+| `[topics]` | `motion_command` / `motion_state` / `joint_states` / `imu` 话题名 |
-| `topics/motion_state` | `motion_state` | 订阅 `invicta_msgs/MotionState` |
+| `[telemetry]` | 四路上报的开关与频率（`enabled`、`require_subscription`、`basic_hz`/`motion_hz`/`device_hz`/`error_hz`） |
-| `heartbeat_timeout_s` | 3.0 | 协议 §7 的离线判定 |
-| `unsupported_as_success` | false | 未实现功能是否回 0 |
-| `telemetry/enabled` | true | 是否推送状态上报 |
-| `telemetry/require_subscription` | false | true = 严格按协议"先查询再推送" |
-| `telemetry/{basic,motion,device,error}_hz` | 2/10/2/2 | 四路上报频率 |
 
+launch 只传 `config_file`（默认 `$(find invicta_remote)/config/invicta_remote.toml`）。
 ## 运行与测试
 
 ```bash

src/invicta_remote/config/invicta_remote.toml

@@ -0,0 +1,28 @@
+# =============================================================================
+# invicta_remote（遥控/设备协议节点）自己的配置
+# 只放本节点需要的参数：UDP 端口、协议开关、话题、状态上报频率
+# 机器人模型/策略在 invicta_deploy，电机/CAN 在 invicta_hardware
+# =============================================================================
+
+[protocol]
+udp_port               = 30000   # 遥控 App 里配置的目标端口，需一致
+heartbeat_timeout_s    = 3.0     # 协议 §7：>3 s 无任何 UDP 包即判离线
+# 未实现的功能（照明灯/休眠/充电/导航/速度指令）默认回 0xE00A "Unsupported function"；
+# 若某个 App 把非 0 当致命错误，可改成 true 让它回 0（只影响这些未实现项）
+unsupported_as_success = false
+
+# ------------------------------------------------------------------ 本节点的话题
+[topics]
+motion_command  = "motion_command"   # 发布 invicta_msgs/MotionCommand（invicta_deploy 订阅）
+motion_state    = "motion_state"     # 订阅 invicta_msgs/MotionState（判前提 + 状态上报）
+joint_states    = "joint_states"     # 订阅（1002/4 运控状态上报用）
+imu             = "imu/data"         # 订阅（状态上报用）
+
+# ------------------------------------------------------------------ 状态上报（协议 1002/*）
+[telemetry]
+enabled              = true
+require_subscription = false   # 老版 App 不发 1002 查询，默认对所有在线客户端推送
+basic_hz             = 2.0     # 1002/6 基础状态
+motion_hz            = 10.0    # 1002/4 运控状态
+device_hz            = 2.0     # 1002/5 设备状态
+error_hz             = 2.0     # 1002/3 异常状态

src/invicta_remote/config/invicta_remote.yaml

@@ -1,24 +0,0 @@
-# invicta_remote 参数（私有命名空间 ~）
-
-udp_port: 30000                  # 遥控 App 里配置的目标端口，需一致
-
-topics:
-  motion_command: motion_command   # 发布：翻译后的运动指令（invicta_deploy 订阅）
-  motion_state: motion_state       # 订阅：invicta_deploy 上报的真实状态
-
-heartbeat_timeout_s: 3.0         # 协议 §7：>3 s 无任何 UDP 包即判离线
-
-# ---- 状态上报（proto.md §3）----
-# 实测当前 App 不发 1002 查询，而是直接依赖服务端推送，所以默认主动推送；
-# 需要严格按协议"先查询再推送"时把 require_subscription 设为 true。
-telemetry:
-  enabled: true
-  require_subscription: false
-  basic_hz: 2.0                  # 1002/6 基础状态（MotionState/Gait/Charge/HES/模式/Sleep/Version）
-  motion_hz: 10.0                # 1002/4 运控状态（姿态/OmegaZ + 16 关节角/轮速）
-  device_hz: 2.0                 # 1002/5 设备状态（电池/温度/CPU/GPS 目前为占位 0）
-  error_hz: 2.0                  # 1002/3 异常（当前恒为空列表）
-
-# 未实现的功能（照明灯/休眠/充电/导航/速度指令/状态上报）默认回 0xE00A "Unsupported function"。
-# 若某个 App 会把非 0 当致命错误，可改成 true 让它回 0（仅影响这些未实现项，不影响错误校验）。
-unsupported_as_success: false

src/invicta_remote/include/invicta_remote/remote_config.h

@@ -0,0 +1,114 @@
+/**
+ * @file remote_config.h
+ * @brief invicta_remote 自己的配置（TOML）：config/invicta_remote.toml
+ *
+ * 只包含本节点需要的参数：UDP 端口与协议开关、本节点话题、状态上报频率。
+ */
+#pragma once
+
+#include <toml++/toml.hpp>
+
+#include <string>
+
+namespace invicta_remote {
+
+class RemoteConfig {
+public:
+    // ---- 协议 ----
+    int udp_port = 30000;
+    double heartbeat_timeout_s = 3.0;
+    bool unsupported_as_success = false;
+
+    // ---- 本节点话题 ----
+    std::string topic_motion_command = "motion_command";
+    std::string topic_motion_state = "motion_state";
+    std::string topic_joint_states = "joint_states";
+    std::string topic_imu = "imu/data";
+
+    // ---- 状态上报（1002/*）----
+    bool telemetry_enabled = true;
+    bool telemetry_require_subscription = false;
+    double telemetry_basic_hz = 2.0, telemetry_motion_hz = 10.0;
+    double telemetry_device_hz = 2.0, telemetry_error_hz = 2.0;
+
+    bool Load(const std::string& path, std::string* error = nullptr);
+
+    std::string Summary() const;
+};
+
+inline RemoteConfig& Config() {
+    static RemoteConfig cfg;
+    return cfg;
+}
+
+namespace detail {
+
+inline const toml::table* Section(const toml::table& t, const char* key) { return t[key].as_table(); }
+
+template <typename T>
+inline void Get(const toml::table* sect, const char* key, T& out) {
+    if (!sect) return;
+    if (auto v = (*sect)[key].value<T>()) out = *v;
+}
+inline void GetStr(const toml::table* sect, const char* key, std::string& out) {
+    if (!sect) return;
+    if (auto v = (*sect)[key].value<std::string>()) out = *v;
+}
+inline void GetBool(const toml::table* sect, const char* key, bool& out) {
+    if (!sect) return;
+    if (auto v = (*sect)[key].value<bool>()) out = *v;
+}
+
+}  // namespace detail
+
+inline bool RemoteConfig::Load(const std::string& path, std::string* error) {
+    const auto fail = [&](const std::string& msg) {
+        if (error) *error = msg;
+        return false;
+    };
+
+    toml::table tbl;
+    try {
+        tbl = toml::parse_file(path);
+    } catch (const toml::parse_error& e) {
+        return fail(std::string("TOML 解析失败: ") + std::string(e.description()) + " @ " + path);
+    }
+
+    const toml::table* proto = detail::Section(tbl, "protocol");
+    if (!proto) return fail("缺少 [protocol] 段");
+    detail::Get(proto, "udp_port", udp_port);
+    detail::Get(proto, "heartbeat_timeout_s", heartbeat_timeout_s);
+    detail::GetBool(proto, "unsupported_as_success", unsupported_as_success);
+    if (udp_port <= 0 || udp_port > 65535) return fail("[protocol].udp_port 非法");
+
+    const toml::table* topics = detail::Section(tbl, "topics");
+    detail::GetStr(topics, "motion_command", topic_motion_command);
+    detail::GetStr(topics, "motion_state", topic_motion_state);
+    detail::GetStr(topics, "joint_states", topic_joint_states);
+    detail::GetStr(topics, "imu", topic_imu);
+
+    const toml::table* tlm = detail::Section(tbl, "telemetry");
+    detail::GetBool(tlm, "enabled", telemetry_enabled);
+    detail::GetBool(tlm, "require_subscription", telemetry_require_subscription);
+    detail::Get(tlm, "basic_hz", telemetry_basic_hz);
+    detail::Get(tlm, "motion_hz", telemetry_motion_hz);
+    detail::Get(tlm, "device_hz", telemetry_device_hz);
+    detail::Get(tlm, "error_hz", telemetry_error_hz);
+
+    if (error) error->clear();
+    return true;
+}
+
+inline std::string RemoteConfig::Summary() const {
+    char buf[256];
+    snprintf(buf, sizeof(buf), "[config] UDP %d, 心跳超时 %.1f s, 未实现项回 0xE00A=%s\n", udp_port,
+             heartbeat_timeout_s, unsupported_as_success ? "否(回 0)" : "是");
+    std::string s = buf;
+    snprintf(buf, sizeof(buf), "[config] 状态上报: %s 基础 %.0f Hz / 运控 %.0f Hz / 设备 %.0f Hz / 异常 %.0f Hz\n",
+             telemetry_enabled ? "开" : "关", telemetry_basic_hz, telemetry_motion_hz, telemetry_device_hz,
+             telemetry_error_hz);
+    s += buf;
+    return s;
+}
+
+}  // namespace invicta_remote

src/invicta_remote/include/invicta_remote/remote_node.h

@@ -37,6 +37,7 @@
 #include "heartbeat.h"
 #include "invicta_msgs/MotionCommand.h"
 #include "invicta_msgs/MotionState.h"
+#include "remote_config.h"
 #include "json.hpp"
 #include "protocol_defs.h"
 #include "udp_server.h"
@@ -48,21 +49,22 @@ using json = nlohmann::json;
 
 class RemoteNode {
 public:
-    explicit RemoteNode(const ros::NodeHandle& pnh) : pnh_(pnh) {
+    explicit RemoteNode(const ros::NodeHandle& pnh) : pnh_(pnh), cfg_(invicta_remote::Config()) {
-        pnh_.param<int>("udp_port", udp_port_, kUdpDefaultPort);
+        // 参数来自本节点自己的 config/invicta_remote.toml
-        pnh_.param<std::string>("topics/motion_command", motion_command_topic_, "motion_command");
+        udp_port_ = cfg_.udp_port;
-        pnh_.param<std::string>("topics/motion_state", motion_state_topic_, "motion_state");
+        motion_command_topic_ = cfg_.topic_motion_command;
-        pnh_.param<std::string>("topics/joint_states", joint_states_topic_, "joint_states");
+        motion_state_topic_ = cfg_.topic_motion_state;
-        pnh_.param<std::string>("topics/imu", imu_topic_, "imu/data");
+        joint_states_topic_ = cfg_.topic_joint_states;
-        pnh_.param<double>("heartbeat_timeout_s", heartbeat_timeout_s_, 3.0);
+        imu_topic_ = cfg_.topic_imu;
-        pnh_.param<bool>("unsupported_as_success", unsupported_as_success_, false);
+        heartbeat_timeout_s_ = cfg_.heartbeat_timeout_s;
+        unsupported_as_success_ = cfg_.unsupported_as_success;
 
-        pnh_.param<bool>("telemetry/enabled", telemetry_enabled_, true);
+        telemetry_enabled_ = cfg_.telemetry_enabled;
-        pnh_.param<bool>("telemetry/require_subscription", require_subscription_, false);
+        require_subscription_ = cfg_.telemetry_require_subscription;
-        pnh_.param<double>("telemetry/basic_hz", basic_hz_, 2.0);
+        basic_hz_ = cfg_.telemetry_basic_hz;
-        pnh_.param<double>("telemetry/motion_hz", motion_hz_, 10.0);
+        motion_hz_ = cfg_.telemetry_motion_hz;
-        pnh_.param<double>("telemetry/device_hz", device_hz_, 2.0);
+        device_hz_ = cfg_.telemetry_device_hz;
-        pnh_.param<double>("telemetry/error_hz", error_hz_, 2.0);
+        error_hz_ = cfg_.telemetry_error_hz;
 
         heartbeat_ = std::make_unique<Heartbeat>(heartbeat_timeout_s_);
         motion_command_pub_ = nh_.advertise<invicta_msgs::MotionCommand>(motion_command_topic_, 1);
@@ -560,6 +562,7 @@ private:
         }
     };
 
+    const invicta_remote::RemoteConfig& cfg_;
     ros::NodeHandle nh_;
     ros::NodeHandle pnh_;
     ros::Publisher motion_command_pub_;

src/invicta_remote/launch/invicta_remote.launch

@@ -1,6 +1,6 @@
 <launch>
-  <!-- 遥控/设备协议节点：UDP + JSON ↔ ROS 话题。参数在节点的私有命名空间下 -->
+  <!-- 遥控/设备协议节点（UDP + JSON）。参数来自本节点自己的 config/invicta_remote.toml -->
   <node pkg="invicta_remote" type="invicta_remote_node" name="invicta_remote" output="screen">
-    <rosparam command="load" file="$(find invicta_remote)/config/invicta_remote.yaml"/>
+    <param name="config_file" value="$(find invicta_remote)/config/invicta_remote.toml"/>
   </node>
 </launch>

src/invicta_remote/package.xml

@@ -12,6 +12,7 @@
   <buildtool_depend>catkin</buildtool_depend>
 
   <depend>roscpp</depend>
+  <depend>roslib</depend>
   <depend>sensor_msgs</depend>
   <depend>invicta_msgs</depend>
 </package>

src/invicta_remote/src/invicta_remote_node.cpp

@@ -2,8 +2,11 @@
  * @file invicta_remote_node.cpp
  * @brief 遥控/设备协议节点入口（UDP + JSON ↔ ROS 话题）
  */
+#include <ros/package.h>
 #include <ros/ros.h>
 
+#include <iostream>
+
 #include "invicta_remote/remote_node.h"
 
 int main(int argc, char** argv) {
@@ -11,6 +14,16 @@ int main(int argc, char** argv) {
     ros::NodeHandle nh;
     ros::NodeHandle pnh("~");
 
+    std::string config_path;
+    pnh.param<std::string>("config_file", config_path, "");
+    if (config_path.empty()) config_path = ros::package::getPath("invicta_remote") + "/config/invicta_remote.toml";
+    std::string err;
+    if (!invicta_remote::Config().Load(config_path, &err)) {
+        ROS_FATAL("[invicta_remote] 配置加载失败：%s（%s）", err.c_str(), config_path.c_str());
+        return 1;
+    }
+    std::cout << invicta_remote::Config().Summary() << std::flush;
+
     invicta_remote::RemoteNode node(pnh);
     node.Start();