diff --git a/README.md b/README.md index 5084ced..bb6e78d 100644 --- a/README.md +++ b/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 只传一个路径：``； +不传则默认取本包 `config/` 下的同名文件。TOML 解析用 toml++，以 **header-only 方式**使用（`-DTOML_HEADER_ONLY=1`），CMake 里只有一句 `find_package(tomlplusplus REQUIRED)`——库源码不进仓库，也不引入运行时动态库依赖。 -| | 值 | -|---|---| -| `joint_dir` | `[1,1,-1,1, 1,-1,1,-1, -1,1,-1,1, -1,-1,1,-1]` | -| `joint_offset_deg` | `[-25,-131,160,0, 25,-131,160,0, -25,131,-160,0, 25,131,-160,0]` | +改动节奏：**改 TOML 不用重编**；改各节点的配置头（`deploy_config.h` / `remote_config.h` / `hardware_config.h`）会自动触发该节点重编。 -两条 M20 遗留语义保持原样：轮关节（下标 `%4==3`）指令 `kp` 强制 0（`wheel_kp_zero`）；关节顺序固定为 `fl/fr/hl/hr × hipx/hipy/knee/wheel`，与 `policy.onnx` 绑定。 +两个 TOML 结构坑（已在文件里注释）：① 数组表之后的裸键会归到最后一个元素，所以 `[joint_limits]`/`[leg]` 必须放在 `legs` 段之前；② 整数键要加引号（如 `"4097"`），roslaunch 不接受整数键。 +## 关键约定：硬件约定 vs 模型约定（2 自由度腿） +本构型每条腿只有**摆臂 + 轮**两个关节（4 腿 × 2 = 8 关节），模型约定为： + +- 摆臂角 θ=0 时摆臂竖直向下，θ 增大表示摆臂向外倒开；髋轴到轮轴的高度 h = L·cos(θ)。 +- 站立：h = `[leg].stand_height`（默认 0.48 m，L=0.50 m → ±16.3°）。 +- 趴下：h = `[leg].lie_height`（默认 0.10 m → ±78.5°），**前腿向前倒、后腿向后倒**（由 `[[legs]].splay_sign` 决定，与 M20 相反）。 +- 轮关节指令 `kp` 恒为 0（速度/力矩模式，`wheel_kp_zero`）。 + +硬件约定（电机方向、机械零点）在 `[[legs]]` 里逐关节给： + +```toml +[[legs]] +name = "fl" +splay_sign = 1.0 # 模型姿态：趴下时向前倒 +swing = { dir = 1.0, offset_deg = 0.0 } # 硬件：电机正方向 / 机械零点 +wheel = { dir = 1.0, offset_deg = 0.0 } +``` + +指令换算：`q_cmd = dir · θ_model + offset`，反馈反向（`invicta_interface` 一处完成）。 +第一次上机先按默认值跑，用 idle 打印的关节角与实测对照，再填 `dir`/`offset_deg`。 ## 步态 = RL 模型 厂家协议里的 `GaitParam` 不是简单参数，**每个步态对应一个 RL 模型**： @@ -95,6 +120,27 @@ source devel/setup.bash 依赖：`roscpp`、`roslib`、`sensor_msgs`、`std_msgs` + 自建 `invicta_msgs`。`eigen`、`onnxruntime 1.22`（x86/aarch64）、`policy.onnx` 已 vendor，无需联网。 +### 一次性依赖：toml++（系统安装，源码不入仓库） + +三个节点都用 `find_package(tomlplusplus REQUIRED)` 找它，**以 header-only 方式编译**（实现编进节点里）， +因此目标机不需要额外部署 `libtomlplusplus.so`。 + +```bash +# 方式一：Ubuntu 22.04+ 有官方包 +sudo apt install libtomlplusplus-dev + +# 方式二：目标机是 Ubuntu 20.04（Noetic 基线）或要交叉编译 aarch64 —— 源里没有此包，源码编译安装到系统 +# 源码自行获取（不放进本仓库），例如 tomlplusplus 3.4.0 +cmake -S tomlplusplus -B build -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=/usr/local +cmake --build build -j +sudo cmake --install build # 装头文件 + CMake 配置（tomlplusplusConfig.cmake） + +# 方式三：离线镜像里预装 .deb（只含头文件与 CMake 配置，123 KB） +sudo dpkg -i libtomlplusplus-dev_3.4.0+ds-0.2build2_amd64.deb +``` + +装完 `cmake --find-package ... tomlplusplus` 能找到即可；`catkin_make` 会在 configure 阶段报缺库并停下。 + ## 运行 ```bash @@ -120,29 +166,25 @@ python3 src/invicta_remote/scripts/protocol_udp_test.py --steer 0.3,0,0 --motion ## 已验证到什么程度 -在 `localhost/custom-ros-noetic:v1.0`（Ubuntu 20.04 + ROS Noetic）容器里实测： +在 `localhost/custom-ros-noetic:v1.0`（Ubuntu 20.04 + ROS Noetic）容器里实测（8 关节 + 每节点自己的 TOML）： | 项目 | 结果 | |---|---| -| 四个包 catkin_make | 通过 | -| `ldd` 解析 `libonnxruntime.so.1` | 通过（RPATH 生效） | -| `/joint_states` | 200.001 Hz，关节名/顺序正确 | -| `/joint_commands` | 站立 200 Hz；RL 状态 50.001 Hz，策略输出正常（kp=80，kd=2/0.6） | -| `/motion_state` | 20.000 Hz，RL 时 `motion_state=17`、`gait=4097` | -| 协议：心跳 / 轴指令 / MotionParam | 回 0，状态机正确切换（空闲→站立→RL） | -| 状态上报 `1002/3,4,5,6` | 2 / 10 / 2 / 2 Hz；RL 时 `BasicStatus.MotionState=17`、切步态后 `Gait` 同步变化 | -| 协议：非 RL 下切步态 | 回 `0xE008 Not allowed operation`（57352） | -| 协议：楼梯步态 | 协议节点回 0（已接收），RL 节点拒绝并记录"模型尚未注册" | -| 协议：照明灯 / 导航模式 / 未知命令 | `0xE00A`（57354）/ `0xE00A` / `0xE003`（57347） | -| 手柄实测（真遥控器） | 心跳、摇杆、MotionParam、照明灯、使用模式报文均能收到并正确应答 | - -**未验证**：真实电机与 IMU（`invicta_hardware` 的 `ReadHardware/WriteHardware` 还是 TODO）、实机上报频率、新机器人几何参数、状态上报（未实现）。 +| 四个包 catkin_make | 干净构建通过（`rc=0`） | +| 三份配置加载 | deploy：`dof=8 legs=4 L=0.500 stand=0.480 pre=0.200 lie=0.000`；remote：UDP 30000、上报 2/10/2/2 Hz；hardware：8 关节、`fl: swing can0#1 / wheel can0#3`、电机 P65MAXS/W190S、CAN 2 路 @1M/5M | +| IK 目标角 | 站立 ±16.3°、收腿 ±66.4°、**趴下 ±90.0°（摆臂水平）**，前腿 +、后腿 −（与 M20 相反）✓ | +| 趴下指令实测 | 订阅 `/joint_commands` 采样，四个摆臂峰值均为 **1.5708 rad = 90.0°**，轮关节 0 ✓ | +| `/joint_states` | 200.000 Hz，8 个关节名与配置一致 | +| `/motion_state` | 20.000 Hz | +| 协议 | 心跳回 0；`MotionParam=1` → idle→standup；`MotionParam=4` → standup→liedown | +| 站立后自动进 RL 自检 | 生效（骨架发零位 → `[RL] 暂不进入 RL：关节 fl_swing 未到站立位姿`，正确地拦住） | +**未验证**：真实电机与 IMU（`invicta_hardware` 的 `ReadHardware/WriteHardware` 仍是 TODO）、实机上报频率、**新机器人几何与 `dir`/`offset_deg` 的实测标定**（TOML 里的 0.50 m / 0.48 m 等是待实测替换的占位值）。 ## 与上游 M20 的差异 保留：状态机五状态、ONNX 策略与关节顺序/动作尺度、安全检查、厂家协议的头/JSON 结构与心跳、`eigen`/`onnxruntime`/`policy.onnx`。 -删除或改写：ROS2→ROS1；`drdds` 消息→标准消息 + 3 条自定义；电池订阅（上游死代码）；键盘控制模式；`third_party/gamepad`（未被引用）；Mujoco 仿真与 `M20_description`；Lite3 三个包；`backward.hpp` 死代码；M20 几何硬编码→ROS 参数；原厂控制字→平台无关控制字；**UDP/JSON 协议层从 RL 进程拆到 `invicta_remote`**。 +删除或改写：**16 关节三关节腿 → 8 关节（摆臂 + 轮）2 自由度腿，IK 换成单连杆反解、新增 splay_sign 倒向**；**死参数（机身长宽/髋偏置/轮半径/力矩限制/通用策略参数）全部删除**；ROS2→ROS1；`drdds` 消息→标准消息 + 3 条自定义；电池订阅（上游死代码）；键盘控制模式；`third_party/gamepad`（未被引用）；Mujoco 仿真与 `M20_description`；Lite3 三个包；`backward.hpp` 死代码；M20 几何硬编码→ROS 参数；原厂控制字→平台无关控制字；**UDP/JSON 协议层从 RL 进程拆到 `invicta_remote`**。 ## 已知缺口 diff --git a/src/invicta_deploy/CMakeLists.txt b/src/invicta_deploy/CMakeLists.txt index 2d94e00..a8da0c0 100644 --- a/src/invicta_deploy/CMakeLists.txt +++ b/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} ) diff --git a/src/invicta_deploy/README.md b/src/invicta_deploy/README.md index 65bfe34..5368089 100644 --- a/src/invicta_deploy/README.md +++ b/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`） -| 参数 | 默认 | 说明 | -|---|---|---| -| `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 值 | 换机器人主要改这里 | +本节点不再读 ROS 参数表，也不读别的节点的配置；launch 只传 `config_file`（默认取本包同名文件）。 -启动时会打印实际读到的 `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 -4 fr_hipx 5 fr_hipy 6 fr_knee 7 fr_wheel -8 hl_hipx 9 hl_hipy 10 hl_knee 11 hl_wheel -12 hr_hipx 13 hr_hipy 14 hr_knee 15 hr_wheel +0 fl_swing 1 fl_wheel +2 fr_swing 3 fr_wheel +4 hl_swing 5 hl_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/配置摘要 ``` diff --git a/src/invicta_deploy/config/invicta_deploy.toml b/src/invicta_deploy/config/invicta_deploy.toml new file mode 100644 index 0000000..78cc118 --- /dev/null +++ b/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 diff --git a/src/invicta_deploy/config/invicta_deploy.yaml b/src/invicta_deploy/config/invicta_deploy.yaml deleted file mode 100644 index ff4cf0c..0000000 --- a/src/invicta_deploy/config/invicta_deploy.yaml +++ /dev/null @@ -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） diff --git a/src/invicta_deploy/include/config/deploy_config.h b/src/invicta_deploy/include/config/deploy_config.h new file mode 100644 index 0000000..a92106d --- /dev/null +++ b/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 + +#include +#include +#include +#include +#include +#include + +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 joint_names; + std::vector joints; ///< 顺序同 joint_names + std::vector 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 policy_joint_order; + std::vector action_scale, default_joint_pos, policy_kp, policy_kd; + int action_dim = 0, obs_dim = 0; + + // ---- 步态 → 模型 ---- + uint32_t default_gait = 0x1001; + std::map 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 WheelIndices() const { + std::vector 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 +inline void Get(const toml::table* sect, const char* key, T& out) { + if (!sect) return; + if (auto v = (*sect)[key].value()) out = *v; +} +inline void GetStr(const toml::table* sect, const char* key, std::string& out) { + if (!sect) return; + if (auto v = (*sect)[key].value()) out = *v; +} +inline void GetBool(const toml::table* sect, const char* key, bool& out) { + if (!sect) return; + if (auto v = (*sect)[key].value()) out = *v; +} +inline void GetDoubleArr(const toml::table* sect, const char* key, std::vector& out) { + if (!sect) return; + if (const auto* arr = (*sect)[key].as_array()) { + out.clear(); + for (const auto& e : *arr) + if (auto v = e.value()) out.push_back(*v); + } +} +inline void GetStrArr(const toml::table* sect, const char* key, std::vector& out) { + if (!sect) return; + if (const auto* arr = (*sect)[key].as_array()) { + out.clear(); + for (const auto& e : *arr) + if (auto v = e.value()) 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 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 diff --git a/src/invicta_deploy/interface/robot/hardware/invicta_interface.hpp b/src/invicta_deploy/interface/robot/hardware/invicta_interface.hpp index 89de48b..991ffc9 100644 --- a/src/invicta_deploy/interface/robot/hardware/invicta_interface.hpp +++ b/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 共用， - * 这里只有一台机器人，合成一个类更好读）。差异： - * - ros2/drdds → ROS1 + sensor_msgs/JointState + sensor_msgs/Imu - * - 控制字从原厂驱动器私有值改为 invicta_msgs/control_word.h 的平台无关集合 - * - 删除电池订阅（上游是死代码） - * - 关节方向/零点标定改为 ROS 参数，默认「恒等」，即约定硬件桥接节点已给出 policy 约定下的数据 + * 硬件约定（方向 dir、零点 offset、单位）在本层一次性归一： + * 反馈：q_model = q_robot · dir + offset + * 指令：q_robot = (q_des − offset) · dir + * 上层的状态机与策略只看到「模型约定」下的量（rad / rad/s / Nm）。 + * + * 相比之前：删掉了 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 joint_names_; - std::vector joint_dir_; ///< +1 / -1 - std::vector 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 name2slot_; - uint16_t joint_data_frame_ = 0; - std::vector joint_data_id_; - std::vector joint_data_updated_; ///< 标定用：该关节是否已看到数据变化 - std::vector 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); - driver_status_.resize(dof_num_, 1); + joint_data_id_.resize(size_t(dof_num_), 0); + 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(cmd_topic_, cmd_queue_); - joint_state_sub_ = nh_.subscribe(joints_topic_, state_queue_, &InvictaInterface::JointStateHandler, this); - imu_sub_ = nh_.subscribe(imu_topic_, state_queue_, &InvictaInterface::ImuHandler, this); + cmd_pub_ = nh_.advertise(cfg_.topic_joint_commands, + cfg_.queue_joint_commands); + 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_) { - ResetPositionOffset(); - } else { - std::cout << "[InvictaInterface] auto_calibrate=false，按参数给定的 dir/offset 直接使用\n"; + std::cout << "[InvictaInterface] topics: " << cfg_.topic_joint_states << " / " << cfg_.topic_imu << " -> " + << cfg_.topic_joint_commands << std::endl; + std::cout << "[InvictaInterface] 关节标定（invicta.toml）: "; + 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.position.resize(dof_num_); - msg.velocity.resize(dof_num_); - msg.effort.resize(dof_num_); - msg.kp.resize(dof_num_); - msg.kd.resize(dof_num_); + msg.name = cfg_.joint_names; + msg.position.resize(size_t(dof_num_)); + msg.velocity.resize(size_t(dof_num_)); + msg.effort.resize(size_t(dof_num_)); + msg.kp.resize(size_t(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]; - msg.velocity[i] = input(i, 3) * joint_dir_[i]; - msg.effort[i] = input(i, 4) * joint_dir_[i]; - msg.kp[i] = input(i, 0); + const auto& j = cfg_.Joint(i); + msg.position[i] = (input(i, 1) - Deg2Rad(float(j.offset_deg))) * j.dir; + msg.velocity[i] = input(i, 3) * j.dir; + 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("topics/joint_states", joints_topic_, "joint_states"); - pnh_.param("topics/imu", imu_topic_, "imu/data"); - pnh_.param("topics/joint_commands", cmd_topic_, "joint_commands"); - pnh_.param("queues/joint_commands", cmd_queue_, 1); - pnh_.param("queues/state", state_queue_, 10); - - joint_names_ = DefaultJointNames(); - pnh_.getParam("joint_names", joint_names_); - if (static_cast(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(joint_dir_.size()) != dof_num_) joint_dir_.assign(dof_num_, 1.f); - if (static_cast(pos_offset_deg_.size()) != dof_num_) pos_offset_deg_.assign(dof_num_, 0.f); - - pnh_.param("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("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 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.position.assign(dof_num_, 0.); - msg.velocity.assign(dof_num_, 0.); - msg.effort.assign(dof_num_, 0.); - msg.kp.assign(dof_num_, 0.); - msg.kd.assign(dof_num_, 0.); + msg.name = cfg_.joint_names; + msg.position.assign(size_t(dof_num_), 0.); + msg.velocity.assign(size_t(dof_num_), 0.); + msg.effort.assign(size_t(dof_num_), 0.); + msg.kp.assign(size_t(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"; - } - - 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"; - } - } + std::cout << "[InvictaInterface] 启动控制序列完成 (disable -> error_reset -> enable)" << std::endl; } // ------------------------------------------------------------------ 回调 void JointStateHandler(const sensor_msgs::JointState::ConstPtr& msg) { - if (static_cast(msg->position.size()) != dof_num_ || - static_cast(msg->velocity.size()) != dof_num_ || - static_cast(msg->effort.size()) != dof_num_) { + const size_t n = size_t(dof_num_); + if (msg->position.size() != n || msg->velocity.size() != n || msg->effort.size() != n) { 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(msg->name.size()) == dof_num_; + const bool by_name = msg->name.size() == n; int unknown = 0; - for (int j = 0; j < dof_num_; ++j) { - int i = by_name ? LookupSlot(msg->name[j]) : j; + for (size_t k = 0; k < n; ++k) { + 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]); - joint_vel_(i) = float(msg->velocity[j]) * joint_dir_[i]; - joint_tau_(i) = float(msg->effort[j]) * joint_dir_[i]; + const auto& j = cfg_.Joint(i); + joint_pos_(i) = float(msg->position[k]) * j.dir + Deg2Rad(float(j.offset_deg)); + 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(); + rpy_ = q.toRotationMatrix().eulerAngles(0, 1, 2).cast(); // ZYX，与 RpyToRm 一致 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 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 joint_data_id_; + std::vector 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 diff --git a/src/invicta_deploy/interface/user_command/ros_command_interface.hpp b/src/invicta_deploy/interface/user_command/ros_command_interface.hpp index 5f15f56..085d81d 100644 --- a/src/invicta_deploy/interface/user_command/ros_command_interface.hpp +++ b/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> loaded_gaits, int default_gait) - : loaded_gaits_(std::move(loaded_gaits)), default_gait_(default_gait) { - pnh_.param("topics/motion_command", topic_, "motion_command"); - pnh_.param("queues/motion_command", cmd_queue_, 1); + : loaded_gaits_(std::move(loaded_gaits)), default_gait_(default_gait), cfg_(invicta::Config()) { + topic_ = cfg_.topic_motion_command; + 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> loaded_gaits_; ///< 实际加载成功的步态（由 RLControlState 填充） int default_gait_ = int(kGaitBasic); diff --git a/src/invicta_deploy/launch/invicta_deploy.launch b/src/invicta_deploy/launch/invicta_deploy.launch index 9e5e4e0..229e53c 100644 --- a/src/invicta_deploy/launch/invicta_deploy.launch +++ b/src/invicta_deploy/launch/invicta_deploy.launch @@ -1,14 +1,6 @@ - - - + - - - + diff --git a/src/invicta_deploy/main.cpp b/src/invicta_deploy/main.cpp index 1572679..5941d57 100644 --- a/src/invicta_deploy/main.cpp +++ b/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 #include #include #include +#include +#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("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 fsm = std::make_shared(); fsm->Start(); fsm->Run(); diff --git a/src/invicta_deploy/run_policy/invicta_policy_runner.hpp b/src/invicta_deploy/run_policy/invicta_policy_runner.hpp index ef98af9..023f0bd 100644 --- a/src/invicta_deploy/run_policy/invicta_policy_runner.hpp +++ b/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 * - * 策略与关节顺序、动作尺度、默认关节角绑定，改这些就等于换策略，请一起改。 - * 观测 57 维 = 角速度(3) + 投影重力(3) + 速度指令(3) + 关节角(16) + 关节角速度(16) + 上一步动作(16) + * 观测（M20 同款配方，维度 = 9 + 3N）： + * [角速度(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 #include -#include +#include #include -#include #include #include -#include #include +#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 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 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 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 robot2policy_idx, policy2robot_idx; - - const char* input_names_[1] = {"obs"}; // 必须与模型导出时一致 - const char* output_names_[1] = {"actions"}; - Ort::MemoryInfo memory_info{nullptr}; - const std::array 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); - dof_default_eigen_robot.setZero(action_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; - 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; - SetDecimation(4); + action_dim_ = cfg_.action_dim; + obs_dim_ = cfg_.obs_dim; + if (action_dim_ <= 0 || obs_dim_ <= 0) { + throw std::runtime_error("策略维度为 0：配置未加载或 [policy] 段不完整"); + } + + 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); - policy2robot_idx = generate_permutation(policy_order, robot_order); + current_observation_.setZero(obs_dim_); + 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.kd = kd_; - robot_action.tau_ff = VecXf::Zero(motor_num); - robot_action.goal_joint_pos = VecXf::Zero(motor_num); - robot_action.goal_joint_vel = VecXf::Zero(motor_num); + robot_action_.kp = kp_robot_; + robot_action_.kd = kd_robot_; + robot_action_.tau_ff = VecXf::Zero(action_dim_); + robot_action_.goal_joint_pos = VecXf::Zero(action_dim_); + robot_action_.goal_joint_vel = VecXf::Zero(action_dim_); - current_observation_.setZero(observation_dim); - last_action_eigen.setZero(action_dim); - tmp_action_eigen.setZero(action_dim); - current_action_eigen.setZero(action_dim); + input_observation_shape_ = {1, obs_dim_}; } ~InvictaPolicyRunner() override = default; - static std::vector generate_permutation(const std::vector& from, - const std::vector& to, int default_index = 0) { - std::unordered_map idx_map; - for (int i = 0; i < static_cast(from.size()); ++i) idx_map[from[i]] = i; - - std::vector 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() { + std::cout << "[policy] " << policy_path_ << " obs=" << obs_dim_ << " action=" << action_dim_ + << " decimation=" << decimation_ << std::endl; } - void DisplayPolicyInfo() {} - void OnEnter() { run_cnt_ = 0; cmd_vel_input_.setZero(); - last_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(memory_info, current_observation.data(), - current_observation.size(), input_observationShape.data(), - input_observationShape.size()); - - std::vector 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(); - Eigen::Map action_map(action_data, action_dim); - return VecXf(action_map); + last_action_eigen_.setZero(action_dim_); + tmp_action_eigen_.setZero(action_dim_); } RobotAction getRobotAction(const RobotBasicState& ro, const UserCommand& uc) { - Vec3f base_omega = ro.base_omega * omega_scale_; - Vec3f projected_gravity_vec = ro.base_rot_mat.inverse() * gravity_direction; - Vec3f cmd = Vec3f(uc.forward_vel_scale, uc.side_vel_scale, uc.turnning_vel_scale); + const Vec3f base_omega = ro.base_omega * float(cfg_.omega_scale); + const Vec3f projected_gravity = ro.base_rot_mat.inverse() * gravity_direction_; + const Vec3f cmd(uc.forward_vel_scale, uc.side_vel_scale, uc.turnning_vel_scale); - for (int i = 0; i < action_dim; ++i) { - joint_pos_rl(i) = ro.joint_pos(robot2policy_idx[i]); - joint_vel_rl(i) = ro.joint_vel(robot2policy_idx[i]) * dof_vel_scale_; + for (int i = 0; i < action_dim_; ++i) { + const int r = robot2policy_idx_[size_t(i)]; // 策略第 i 位对应机器人第 r 个关节 + 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(); // 轮关节角不进观测 - - 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]; + if (cfg_.zero_obs_wheels) { + for (int w : wheel_robot_idx_) joint_pos_rl_(robot2policy_idx_[size_t(w)]) = 0.f; } - tmp_action_eigen += dof_default_eigen_robot; + joint_pos_rl_ -= default_policy_; - for (int i = 0; i < 4; ++i) { - robot_action.goal_joint_pos.segment(i * 4, 3) = tmp_action_eigen.segment(i * 4, 3); - robot_action.goal_joint_vel(i * 4 + 3) = tmp_action_eigen(i * 4 + 3); + current_observation_ << base_omega, projected_gravity, cmd, joint_pos_rl_, joint_vel_rl_, last_action_eigen_; + current_action_eigen_ = OnnxInfer(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_[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 Permutation(const std::vector& from, const std::vector& to) { + std::unordered_map idx; + for (size_t i = 0; i < from.size(); ++i) idx[from[i]] = int(i); + std::vector 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& 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(memory_info, observation.data(), observation.size(), + input_observation_shape_.data(), input_observation_shape_.size()); + std::vector 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(); + Eigen::Map 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 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 input_observation_shape_; }; diff --git a/src/invicta_deploy/scripts/smoke_test.sh b/src/invicta_deploy/scripts/smoke_test.sh index 0e9ea92..5b47623 100755 --- a/src/invicta_deploy/scripts/smoke_test.sh +++ b/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 读到 ---" -rosparam get /invicta_deploy/geometry/stand_height -rosparam get /invicta_deploy/calibration/joint_dir +echo "--- 配置是否读到（TOML）---" +grep -aE "^\[config\]" /tmp/dep.log | head -3 echo "--- 节点连接关系 ---" rosnode info /invicta_deploy 2>&1 | sed -n '1,14p' diff --git a/src/invicta_deploy/state_machine/parameters/control_parameters.h b/src/invicta_deploy/state_machine/parameters/control_parameters.h deleted file mode 100644 index 847161b..0000000 --- a/src/invicta_deploy/state_machine/parameters/control_parameters.h +++ /dev/null @@ -1,61 +0,0 @@ -/** - * @file control_parameters.h - * @brief 机器人几何/控制参数。全部来自 ROS 参数，默认值是上游 M20 的取值 - * - * 换成 invicta 自己的机器人时，改 config/invicta_deploy.yaml 即可，不用改代码。 - */ -#pragma once - -#include - -#include - -#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); -}; diff --git a/src/invicta_deploy/state_machine/parameters/invicta_control_parameters.cpp b/src/invicta_deploy/state_machine/parameters/invicta_control_parameters.cpp deleted file mode 100644 index 8f609cf..0000000 --- a/src/invicta_deploy/state_machine/parameters/invicta_control_parameters.cpp +++ /dev/null @@ -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 v; - if (!nh.getParam(key, v) || v.size() != 3) { - return fallback; - } - return Vec3f(static_cast(v[0]), static_cast(v[1]), static_cast(v[2])); -} - -} // namespace - -void ControlParameters::Load(const ros::NodeHandle& nh) { - nh.param("geometry/body_len_x", body_len_x_, 0.3095f * 2.f); - nh.param("geometry/body_len_y", body_len_y_, 0.065f * 2.f); - nh.param("geometry/hip_len", hip_len_, 0.104f); - nh.param("geometry/thigh_len", thigh_len_, 0.25f); - nh.param("geometry/shank_len", shank_len_, 0.25f); - - nh.param("geometry/pre_height", pre_height_, 0.12f); - nh.param("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("timing/stand_duration", stand_duration_, 1.5f); - nh.param("timing/liedown_duration", liedown_duration_, 2.0f); - nh.param("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("rl/auto_enter", rl_auto_enter_, true); - nh.param("rl/enter_delay", rl_enter_delay_, 0.5f); - nh.param("rl/require_pose", rl_require_pose_, true); - nh.param("rl/joint_tolerance", rl_joint_tolerance_, 0.15f); - nh.param("rl/velocity_tolerance", rl_velocity_tolerance_, 1.0f); - nh.param("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("limits/wheel_vel_limit", wheel_vel_limit_, 100.f); - nh.param("geometry/wheel_link_len", wheel_link_len_, 0.040575f); -} diff --git a/src/invicta_deploy/state_machine/quadruped_wheel/idle_state.hpp b/src/invicta_deploy/state_machine/quadruped_wheel/idle_state.hpp index b04bc1f..8ef9b3e 100644 --- a/src/invicta_deploy/state_machine/quadruped_wheel/idle_state.hpp +++ b/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); - Vec4f fl_lower, fl_upper; - fl_lower << cp_ptr_->fl_joint_lower_, -INFINITY; - 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) { + const auto& cfg = invicta::Config(); + for (int i = 0; i < int(joint_pos_.size()); ++i) { + const auto& j = cfg.Joint(i); if (std::isnan(joint_pos_(i))) return 1; - if (joint_pos_(i) > joint_pos_upper(i) + 0.2) return 2; - if (joint_pos_(i) < joint_pos_lower(i) - 0.2) return 3; - if (std::isnan(joint_vel_(i))) return 4; - 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 (!j.is_wheel) { + if (joint_pos_(i) > j.upper + 0.2) return 2; + if (joint_pos_(i) < j.lower - 0.2) return 3; } + 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"; - std::cout << "FL pos: "; - PrintEigenVectorf(joint_pos_.segment(0, 4)); - std::cout << "FR pos: "; - PrintEigenVectorf(joint_pos_.segment(4, 4)); - std::cout << "HL pos: "; - PrintEigenVectorf(joint_pos_.segment(8, 4)); - std::cout << "HR pos: "; - PrintEigenVectorf(joint_pos_.segment(12, 4)); - std::cout << "vel: "; - PrintEigenVectorf(joint_vel_); - 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; + const auto& cfg = invicta::Config(); + std::cout << "Joint Data (rad, rad/s): \n"; + for (size_t l = 0; l < cfg.legs.size(); ++l) { + const int si = cfg.legs[l].swing_index, wi = cfg.legs[l].wheel_index; + std::cout << " " << cfg.legs[l].name << " swing " << std::setw(7) << std::setprecision(3) + << joint_pos_(si) << " (vel " << std::setw(6) << joint_vel_(si) << ") wheel vel " << std::setw(7) + << joint_vel_(wi) << std::endl; + } + std::cout << " tau: " << joint_tau_.transpose() << std::endl; + std::cout << " Imu rpy " << rpy_.transpose() << " | acc " << acc_.transpose() << " | omg " << omg_.transpose() + << std::endl; } void DisplayAxisValue() { auto cmd = uc_ptr_->GetUserCommand(); - std::cout << "User Command Input: \n"; - std::cout << "axis value: " << cmd->forward_vel_scale << " " << cmd->side_vel_scale << " " - << cmd->turnning_vel_scale << std::endl; - std::cout << "target mode: " << int(cmd->target_mode) << std::endl; + std::cout << "User Command: axis " << cmd->forward_vel_scale << " " << cmd->side_vel_scale << " " + << cmd->turnning_vel_scale << " | target_mode " << int(cmd->target_mode) << " | gait 0x" << std::hex + << cmd->gait << std::dec << std::endl; } public: @@ -98,35 +80,30 @@ public: ~IdleState() {} - virtual void OnEnter() { - StateBase::msfb_.UpdateCurrentState(RobotMotionState::WaitingForStand); - } + virtual void OnEnter() { 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)) { - std::cout << "========== warning ========>> " - << "joint status: " << joint_normal_flag_ << std::endl; - return StateName::kIdle; - } - if (uc_ptr_->GetUserCommand()->safe_control_mode != 0) { + if (joint_normal_flag_ != 0) { + std::cout << "========== warning ========>> 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()->target_mode == uint8_t(RobotMotionState::StandingUp)) return StateName::kStandUp; return StateName::kIdle; diff --git a/src/invicta_deploy/state_machine/quadruped_wheel/invicta_state_machine.hpp b/src/invicta_deploy/state_machine/quadruped_wheel/invicta_state_machine.hpp index 9161860..2006e6e 100644 --- a/src/invicta_deploy/state_machine/quadruped_wheel/invicta_state_machine.hpp +++ b/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 #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 idle_controller_; @@ -42,46 +41,37 @@ private: std::shared_ptr 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 - - pnh.param("control_period_ms", control_period_ms_, 5); // 默认 200 Hz - - std::string policy_path; - pnh.param("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("topics/motion_state", state_topic, "motion_state"); - pnh.param("motion_state_report_hz", state_hz, 20.0); - pnh.param("robot_version", version, "INVICTA"); + const auto& cfg = invicta::Config(); + const std::string policy_path = ResolveModelPath(cfg.policy_path); std::map gait_policy_map; - pnh.getParam("gait/policy_map", gait_policy_map); - int default_gait = invicta_msgs::kGaitBasic; - pnh.param("gait/default", default_gait, int(invicta_msgs::kGaitBasic)); + for (const auto& kv : cfg.gait_policies) { + gait_policy_map[std::to_string(kv.first)] = ResolveModelPath(kv.second); + } + const int default_gait = int(cfg.default_gait); auto loaded_gaits = std::make_shared>(); // ---- 指令来源：订阅 /motion_command ---- uc_ptr_ = std::make_shared(loaded_gaits, default_gait); uc_ptr_->SetMotionStateFeedback(&StateBase::msfb_); - // ---- 机器人数据面 + 控制参数 ---- - ri_ptr_ = std::make_shared("invicta", kDofNum); - cp_ptr_ = std::make_shared(pnh); + // ---- 机器人数据面 ---- + ri_ptr_ = std::make_shared(cfg.robot_name, cfg.dof); std::shared_ptr data_ptr = std::make_shared(); data_ptr->ri_ptr = ri_ptr_; data_ptr->uc_ptr = uc_ptr_; - data_ptr->cp_ptr = cp_ptr_; sc_ptr_ = std::make_shared(); 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( - &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 GetStateControllerPtr(StateName state_name) { diff --git a/src/invicta_deploy/state_machine/quadruped_wheel/joint_damping_state.hpp b/src/invicta_deploy/state_machine/quadruped_wheel/joint_damping_state.hpp index 9a732a7..596d973 100644 --- a/src/invicta_deploy/state_machine/quadruped_wheel/joint_damping_state.hpp +++ b/src/invicta_deploy/state_machine/quadruped_wheel/joint_damping_state.hpp @@ -2,13 +2,10 @@ * @file joint_damping_state.hpp * @brief 关节阻尼（过渡态）：缓慢松下 → 机器人着地 → 转入空闲（自由） * - * 设计意图：这不是一个「保持」状态，而是松腿过程。只给 kd、不给位置增益，关节在重力下缓慢下沉； - * 到时后转 Idle（Idle 输出零增益 = 完全自由）。 + * 只给 kd、不给位置增益，关节在重力下缓慢下沉；到时后转 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 data_ptr) : StateBase(state_name, data_ptr) { - Vec4f one_leg_kd; - one_leg_kd << cp_ptr_->swing_leg_kd_, 1.; - VecXf kd_ = one_leg_kd.replicate(4, 1); - joint_cmd_ = MatXf::Zero(16, 5); - joint_cmd_.col(2) = kd_; + const auto& cfg = invicta::Config(); + joint_cmd_ = MatXf::Zero(int(ri_ptr_->dof_num_), 5); + for (size_t l = 0; l < cfg.legs.size(); ++l) { + joint_cmd_(cfg.legs[l].swing_index, 2) = float(cfg.damping_kd_leg); + 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_)) { - return StateName::kJointDamping; - } + if (Elapsed() < invicta::Config().damping_hold_s) return StateName::kJointDamping; return StateName::kIdle; } diff --git a/src/invicta_deploy/state_machine/quadruped_wheel/liedown_state.hpp b/src/invicta_deploy/state_machine/quadruped_wheel/liedown_state.hpp index 2ff371d..00a5948 100644 --- a/src/invicta_deploy/state_machine/quadruped_wheel/liedown_state.hpp +++ b/src/invicta_deploy/state_machine/quadruped_wheel/liedown_state.hpp @@ -1,150 +1,112 @@ /** * @file liedown_state.hpp - * @brief liedown state - * @author DeepRobotics - * @version 1.0 - * @date 2026-02-12 - * - * @copyright Copyright (c) 2025 DeepRobotics - * + * @brief 趴下：摆臂角样条到趴下目标 → 阻尼保持 → 完全放松 + * + * 目标高度、倒向、增益都来自 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_; - double time_stamp_record_, run_time_; - VecXf goal_joint_pos_, kp_, kd_; + VecXf init_swing_, target_swing_; + VecXf current_joint_pos_, current_joint_vel_; + double time_stamp_record_ = 0., run_time_ = 0.; MatXf joint_cmd_; - float liedown_duration_ = 2.; - const float init_hipx_pos_ = Deg2Rad(0.); - const float set_wheel_kd_ = 1.; - - void GetRobotJointValue(){ + 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 data_ptr) : StateBase(state_name, data_ptr) { - init_joint_pos_ = VecXf::Zero(16); - init_joint_vel_ = VecXf::Zero(16); - 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); - goal_joint_pos_(4) = -init_hipx_pos_; goal_joint_pos_(12) = -init_hipx_pos_; + const auto& cfg = invicta::Config(); + 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; - - 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_; + target_swing_.resize(int(n)); + for (size_t l = 0; l < n; ++l) { + target_swing_(int(l)) = float(cfg.SwingAngleForHeight(cfg.lie_height, int(l))); } - ~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); - joint_cmd_.col(0) = kp_; - joint_cmd_.col(2) = kd_; + const auto& cfg = invicta::Config(); 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()); - VecXf planning_joint_vel(current_joint_pos_.rows()); - if(run_time_ - time_stamp_record_ <= liedown_duration_){ - for(int i=0;iSetJointCommand(joint_cmd_); - } else if (run_time_ - time_stamp_record_ <= 2.0 * liedown_duration_){ - joint_cmd_ = MatXf::Zero(16, 5); - joint_cmd_.col(2) = kd_; - - ri_ptr_->SetJointCommand(joint_cmd_); - } else { - joint_cmd_ = MatXf::Zero(16, 5); + if (elapsed <= cfg.liedown_duration_s) { + // ① 样条到趴下姿态（摆臂带速度前馈；轮不给位置，只靠 kd） + for (size_t l = 0; l < cfg.legs.size(); ++l) { + const int si = cfg.legs[l].swing_index; + joint_cmd_(si, 1) = GetCubicSplinePos(init_swing_(int(l)), current_joint_vel_(si), + target_swing_(int(l)), 0.f, float(elapsed), + float(cfg.liedown_duration_s)); + 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; } + + // ② 阻尼保持（摆臂只给 kd），③ 之后全零（完全放松），状态机随后回 idle + MatXf cmd = MatXf::Zero(int(ri_ptr_->dof_num_), 5); + if (elapsed <= cfg.liedown_duration_s + cfg.damping_hold_s) { + for (size_t l = 0; l < cfg.legs.size(); ++l) { + cmd(cfg.legs[l].swing_index, 2) = float(cfg.damping_kd_leg); + cmd(cfg.legs[l].wheel_index, 2) = float(cfg.damping_kd_wheel); + } + } + ri_ptr_->SetJointCommand(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_){ - return StateName::kLieDown; - }else{ - if(uc_ptr_->GetUserCommand()->target_mode == uint8_t(RobotMotionState::StandingUp)){ - return StateName::kStandUp; - } - } + const auto& cfg = invicta::Config(); + if (uc_ptr_->GetUserCommand()->safe_control_mode != 0) return StateName::kJointDamping; + + 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; } }; diff --git a/src/invicta_deploy/state_machine/quadruped_wheel/standup_state.hpp b/src/invicta_deploy/state_machine/quadruped_wheel/standup_state.hpp index dfba0f1..e8955a1 100644 --- a/src/invicta_deploy/state_machine/quadruped_wheel/standup_state.hpp +++ b/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 - * @author DeepRobotics - * @version 1.0 - * @date 2025-11-07 - * - * @copyright Copyright (c) 2025 DeepRobotics - * + * @brief 起立：摆臂角三段样条（当前 → 收腿姿态 → 站立姿态），并保留站立后自动进 RL 的自检 + * + * 运动学：每条腿只有 摆臂 + 轮 两个关节，髋轴→轮轴高度 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_; - double time_stamp_record_, run_time_; - VecXf goal_joint_pos_, kp_, kd_; + VecXf init_swing_, tuck_swing_, stand_swing_; ///< 每条腿的摆臂角（模型约定） + VecXf current_joint_pos_, current_joint_vel_; + double time_stamp_record_ = 0., run_time_ = 0.; MatXf joint_cmd_; - double stand_duration_ = 2.; - VecXf stand_pose_; ///< 站立终点位姿（用于自检） - double last_ready_log_ = -1e9; ///< 自检未通过日志节流 + double tuck_duration_ = 1.5; ///< 第一阶段：收腿 + double raise_duration_ = 1.5; ///< 第二阶段：撑起 + VecXf stand_pose_; ///< 站立终点位姿（用于自检） + double last_ready_log_ = -1e9; bool rl_enter_logged_ = false; - const float init_hipx_pos_ = Deg2Rad(0.); - const float set_wheel_kd_ = 1.; - - void GetRobotJointValue(){ + 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)); - 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; + /// 模型约定的摆臂角（含 splay_sign） + float SwingAngleForHeight(float h, size_t leg) const { + return float(invicta::Config().SwingAngleForHeight(h, int(leg))); } public: StandUpState(const std::string& state_name, std::shared_ptr 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); - goal_joint_pos_(4) = -init_hipx_pos_; goal_joint_pos_(12) = -init_hipx_pos_; - // 后髋膝取反 - 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); + const auto& cfg = invicta::Config(); + const size_t n = cfg.legs.size(); - Vec4f one_leg_kp, one_leg_kd; - one_leg_kp << cp_ptr_->swing_leg_kp_, 0; - one_leg_kd << cp_ptr_->swing_leg_kd_, 0; - kp_ = one_leg_kp.replicate(4, 1); - kd_ = one_leg_kd.replicate(4, 1); - 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); + tuck_swing_.resize(int(n)); + stand_swing_.resize(int(n)); + for (size_t l = 0; l < n; ++l) { + tuck_swing_(int(l)) = SwingAngleForHeight(float(cfg.pre_height), l); + stand_swing_(int(l)) = SwingAngleForHeight(float(cfg.stand_height), l); } - ~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()); - VecXf planning_joint_vel(current_joint_pos_.rows()); - if(run_time_ - time_stamp_record_ <= stand_duration_){ - for(int i=0;idof_num_)); + VecXf vel = VecXf::Zero(int(ri_ptr_->dof_num_)); + VecXf kd = joint_cmd_.col(2); + + if (elapsed <= tuck_duration_) { + // 第一阶段：把腿收到「收腿姿态」，轮自由滚动（kd=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{ - double new_time = run_time_ - time_stamp_record_ - stand_duration_; - float dt = 0.001; - 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_; + } 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); } - joint_cmd_.col(1) = planning_joint_pos; - joint_cmd_.col(3) = planning_joint_vel; - joint_cmd_.col(2) = kd_; + joint_cmd_.col(1) = pos; + joint_cmd_.col(3) = 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_) { - for (int i = 0; i < 16; ++i) { - if (i % 4 == 3) continue; // 轮关节不要求位姿 - if (std::fabs(current_joint_pos_(i) - stand_pose_(i)) > cp_ptr_->rl_joint_tolerance_) { - LogNotReady("关节 " + std::to_string(i) + " 未到站立位姿"); + if (cfg.rl_require_pose) { + for (size_t l = 0; l < cfg.legs.size(); ++l) { + const int si = cfg.legs[l].swing_index; + if (std::fabs(current_joint_pos_(si) - stand_pose_(si)) > cfg.rl_joint_tolerance) { + LogNotReady("关节 " + cfg.joint_names[size_t(si)] + " 未到站立位姿"); return false; } - } - for (int i = 0; i < 16; ++i) { - if (i % 4 == 3) continue; - if (std::fabs(current_joint_vel_(i)) > cp_ptr_->rl_velocity_tolerance_) { - LogNotReady("关节速度过大"); + if (std::fabs(current_joint_vel_(si)) > cfg.rl_velocity_tolerance) { + LogNotReady("关节 " + cfg.joint_names[size_t(si)] + " 速度过大"); 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,28 +180,29 @@ 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; - if(run_time_ - time_stamp_record_ <= 2.*stand_duration_){ - return StateName::kStandUp; - }else{ - if(uc_ptr_->GetUserCommand()->target_mode == uint8_t(RobotMotionState::LieDown)){ - return StateName::kLieDown; - } - const bool user_request = (uc_ptr_->GetUserCommand()->target_mode == - uint8_t(RobotMotionState::RLControlMode)); - if (cp_ptr_->rl_auto_enter_ || user_request) { - if (run_time_ - time_stamp_record_ >= 2. * stand_duration_ + cp_ptr_->rl_enter_delay_ && - ReadyForRl()) { - if (!rl_enter_logged_) { - rl_enter_logged_ = true; - std::cout << "[RL] 站立完成且自检通过，进入 RL 控制" << std::endl; - } - return StateName::kRLControl; + const auto& cfg = invicta::Config(); + if (uc_ptr_->GetUserCommand()->safe_control_mode != 0) return StateName::kJointDamping; + + const double elapsed = run_time_ - time_stamp_record_; + 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 == uint8_t(RobotMotionState::RLControlMode)); + if (cfg.rl_auto_enter || user_request) { + if (elapsed >= ready_time + cfg.rl_enter_delay && ReadyForRl()) { + if (!rl_enter_logged_) { + rl_enter_logged_ = true; + std::cout << "[RL] 站立完成且自检通过，进入 RL 控制" << std::endl; } + return StateName::kRLControl; } } return StateName::kStandUp; diff --git a/src/invicta_deploy/state_machine/state_base.h b/src/invicta_deploy/state_machine/state_base.h index c38ba90..2cfc431 100644 --- a/src/invicta_deploy/state_machine/state_base.h +++ b/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 ri_ptr; std::shared_ptr uc_ptr; - std::shared_ptr 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 data_ptr_; std::shared_ptr ri_ptr_; std::shared_ptr uc_ptr_; - std::shared_ptr cp_ptr_; static MotionStateFeedback msfb_; }; diff --git a/src/invicta_deploy/state_machine/state_machine_base.h b/src/invicta_deploy/state_machine/state_machine_base.h index fbe389b..1d5abab 100644 --- a/src/invicta_deploy/state_machine/state_machine_base.h +++ b/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 uc_ptr_; std::shared_ptr ri_ptr_; - std::shared_ptr cp_ptr_; std::shared_ptr sc_ptr_; std::shared_ptr current_controller_; diff --git a/src/invicta_hardware/CMakeLists.txt b/src/invicta_hardware/CMakeLists.txt index 588861c..7d26cf1 100644 --- a/src/invicta_hardware/CMakeLists.txt +++ b/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}) diff --git a/src/invicta_hardware/README.md b/src/invicta_hardware/README.md index 23c9360..9fab623 100644 --- a/src/invicta_hardware/README.md +++ b/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`） -| 参数 | 默认 | 说明 | -|---|---|---| -| `joint_states_topic` / `imu_topic` / `joint_commands_topic` | `joint_states` / `imu/data` / `joint_commands` | 话题名 | -| `imu_frame_id` | `base_link` | IMU 坐标系 | -| `publish_rate_hz` | 200.0 | 状态发布频率（需 ≥ deploy 的 200 Hz 控制周期） | -| `command_queue` | 1 | 指令只取最新一帧 | -| `joint_names` | 16 个 | 关节顺序 | +| TOML 段 | 内容 | +|---|---| +| `[robot].joint_names` | 关节名与顺序（8 个：`fl/fr/hl/hr × swing/wheel`） | +| `[joint_limits]` | 逐关节限位（下发前自检用） | +| `[[legs]]` ×4 | 每腿 `swing`/`wheel` 的 `dir`、`offset_deg`、`bus`、`canid` | +| `[motor.joint]` / `[motor.wheel]` | 电机量程：`p/v/t/kp/kd` 上下限（P65MAXS / W190S） | +| `[motor.can]` | 总线名、仲裁 ID、1M/5M 比特率、上电使能帧数与间隔 | +| `[motor.gains]` | 台架脚本当前增益（进 RL 前以 `invicta_deploy` 的 `[leg.gains]`/策略为准） | +| `[topics]` `[hardware]` | 本节点话题、发布频率、IMU frame | +启动时打印关节表、每条腿的总线/CANID 与电机量程，便于接线核对。 ## 运行 ```bash diff --git a/src/invicta_hardware/config/invicta_hardware.toml b/src/invicta_hardware/config/invicta_hardware.toml new file mode 100644 index 0000000..1a2c9aa --- /dev/null +++ b/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 # 指令只取最新一帧 diff --git a/src/invicta_hardware/config/invicta_hardware.yaml b/src/invicta_hardware/config/invicta_hardware.yaml deleted file mode 100644 index ed70537..0000000 --- a/src/invicta_hardware/config/invicta_hardware.yaml +++ /dev/null @@ -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] diff --git a/src/invicta_hardware/include/invicta_hardware/hardware_bridge.h b/src/invicta_hardware/include/invicta_hardware/hardware_bridge.h index 516f59d..ff6cca6 100644 --- a/src/invicta_hardware/include/invicta_hardware/hardware_bridge.h +++ b/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²]（机体系）。 diff --git a/src/invicta_hardware/include/invicta_hardware/hardware_config.h b/src/invicta_hardware/include/invicta_hardware/hardware_config.h new file mode 100644 index 0000000..c36a55d --- /dev/null +++ b/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 + +#include +#include +#include + +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 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 joint_names; + std::vector joints; ///< 顺序同 joint_names + std::vector 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 +inline void Get(const toml::table* sect, const char* key, T& out) { + if (!sect) return; + if (auto v = (*sect)[key].value()) out = *v; +} +inline void GetStr(const toml::table* sect, const char* key, std::string& out) { + if (!sect) return; + if (auto v = (*sect)[key].value()) out = *v; +} +inline void GetDoubleArr(const toml::table* sect, const char* key, std::vector& out) { + if (!sect) return; + if (const auto* arr = (*sect)[key].as_array()) { + out.clear(); + for (const auto& e : *arr) + if (auto v = e.value()) out.push_back(*v); + } +} +inline void GetStrArr(const toml::table* sect, const char* key, std::vector& out) { + if (!sect) return; + if (const auto* arr = (*sect)[key].as_array()) { + out.clear(); + for (const auto& e : *arr) + if (auto v = e.value()) 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 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 diff --git a/src/invicta_hardware/launch/invicta_hardware.launch b/src/invicta_hardware/launch/invicta_hardware.launch index c61ff67..aac879b 100644 --- a/src/invicta_hardware/launch/invicta_hardware.launch +++ b/src/invicta_hardware/launch/invicta_hardware.launch @@ -1,6 +1,6 @@ - + - + diff --git a/src/invicta_hardware/package.xml b/src/invicta_hardware/package.xml index 84a516a..5c84835 100644 --- a/src/invicta_hardware/package.xml +++ b/src/invicta_hardware/package.xml @@ -12,6 +12,7 @@ catkin roscpp + roslib sensor_msgs geometry_msgs invicta_msgs diff --git a/src/invicta_hardware/src/invicta_hardware_node.cpp b/src/invicta_hardware/src/invicta_hardware_node.cpp index 195e89d..d530be9 100644 --- a/src/invicta_hardware/src/invicta_hardware_node.cpp +++ b/src/invicta_hardware/src/invicta_hardware_node.cpp @@ -2,10 +2,12 @@ * @file invicta_hardware_node.cpp * @brief 关节/IMU 数据源 + 关节指令执行端（骨架入口） */ +#include #include #include +#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("joint_states_topic", joint_state_topic_, "joint_states"); - pnh.param("imu_topic", imu_topic_, "imu/data"); - pnh.param("joint_commands_topic", command_topic_, "joint_commands"); - pnh.param("imu_frame_id", imu_frame_id_, "base_link"); - pnh.param("publish_rate_hz", publish_rate_hz_, 200.0); - pnh.param("command_queue", command_queue_, 1); + // 参数来自本节点自己的 config/invicta_hardware.toml + const auto& cfg = invicta_hardware::Config(); + joint_state_topic_ = cfg.topic_joint_states; + imu_topic_ = cfg.topic_imu; + command_topic_ = cfg.topic_joint_commands; + 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", - "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_); + joint_names_ = cfg.joint_names; // 顺序必须与 invicta_deploy 一致（同一份配置，天然一致） 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("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); diff --git a/src/invicta_remote/CMakeLists.txt b/src/invicta_remote/CMakeLists.txt index 4c00b9f..b84b314 100644 --- a/src/invicta_remote/CMakeLists.txt +++ b/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 diff --git a/src/invicta_remote/README.md b/src/invicta_remote/README.md index 2854409..c590150 100644 --- a/src/invicta_remote/README.md +++ b/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`） -| 参数 | 默认 | 说明 | -|---|---|---| -| `udp_port` | 30000 | 遥控 App 里配置的端口 | -| `topics/motion_command` | `motion_command` | 发布 `invicta_msgs/MotionCommand` | -| `topics/motion_state` | `motion_state` | 订阅 `invicta_msgs/MotionState` | -| `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 | 四路上报频率 | +| TOML 段 | 内容 | +|---|---| +| `[protocol]` | `udp_port`、`heartbeat_timeout_s`、`unsupported_as_success` | +| `[topics]` | `motion_command` / `motion_state` / `joint_states` / `imu` 话题名 | +| `[telemetry]` | 四路上报的开关与频率（`enabled`、`require_subscription`、`basic_hz`/`motion_hz`/`device_hz`/`error_hz`） | +launch 只传 `config_file`（默认 `$(find invicta_remote)/config/invicta_remote.toml`）。 ## 运行与测试 ```bash diff --git a/src/invicta_remote/config/invicta_remote.toml b/src/invicta_remote/config/invicta_remote.toml new file mode 100644 index 0000000..26a2945 --- /dev/null +++ b/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 异常状态 diff --git a/src/invicta_remote/config/invicta_remote.yaml b/src/invicta_remote/config/invicta_remote.yaml deleted file mode 100644 index bafa187..0000000 --- a/src/invicta_remote/config/invicta_remote.yaml +++ /dev/null @@ -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 diff --git a/src/invicta_remote/include/invicta_remote/remote_config.h b/src/invicta_remote/include/invicta_remote/remote_config.h new file mode 100644 index 0000000..16bcf31 --- /dev/null +++ b/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 + +#include + +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 +inline void Get(const toml::table* sect, const char* key, T& out) { + if (!sect) return; + if (auto v = (*sect)[key].value()) out = *v; +} +inline void GetStr(const toml::table* sect, const char* key, std::string& out) { + if (!sect) return; + if (auto v = (*sect)[key].value()) out = *v; +} +inline void GetBool(const toml::table* sect, const char* key, bool& out) { + if (!sect) return; + if (auto v = (*sect)[key].value()) 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 diff --git a/src/invicta_remote/include/invicta_remote/remote_node.h b/src/invicta_remote/include/invicta_remote/remote_node.h index c7bec0f..b5c3085 100644 --- a/src/invicta_remote/include/invicta_remote/remote_node.h +++ b/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) { - pnh_.param("udp_port", udp_port_, kUdpDefaultPort); - pnh_.param("topics/motion_command", motion_command_topic_, "motion_command"); - pnh_.param("topics/motion_state", motion_state_topic_, "motion_state"); - pnh_.param("topics/joint_states", joint_states_topic_, "joint_states"); - pnh_.param("topics/imu", imu_topic_, "imu/data"); - pnh_.param("heartbeat_timeout_s", heartbeat_timeout_s_, 3.0); - pnh_.param("unsupported_as_success", unsupported_as_success_, false); + explicit RemoteNode(const ros::NodeHandle& pnh) : pnh_(pnh), cfg_(invicta_remote::Config()) { + // 参数来自本节点自己的 config/invicta_remote.toml + udp_port_ = cfg_.udp_port; + motion_command_topic_ = cfg_.topic_motion_command; + motion_state_topic_ = cfg_.topic_motion_state; + joint_states_topic_ = cfg_.topic_joint_states; + imu_topic_ = cfg_.topic_imu; + heartbeat_timeout_s_ = cfg_.heartbeat_timeout_s; + unsupported_as_success_ = cfg_.unsupported_as_success; - pnh_.param("telemetry/enabled", telemetry_enabled_, true); - pnh_.param("telemetry/require_subscription", require_subscription_, false); - pnh_.param("telemetry/basic_hz", basic_hz_, 2.0); - pnh_.param("telemetry/motion_hz", motion_hz_, 10.0); - pnh_.param("telemetry/device_hz", device_hz_, 2.0); - pnh_.param("telemetry/error_hz", error_hz_, 2.0); + telemetry_enabled_ = cfg_.telemetry_enabled; + require_subscription_ = cfg_.telemetry_require_subscription; + basic_hz_ = cfg_.telemetry_basic_hz; + motion_hz_ = cfg_.telemetry_motion_hz; + device_hz_ = cfg_.telemetry_device_hz; + error_hz_ = cfg_.telemetry_error_hz; heartbeat_ = std::make_unique(heartbeat_timeout_s_); motion_command_pub_ = nh_.advertise(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_; diff --git a/src/invicta_remote/launch/invicta_remote.launch b/src/invicta_remote/launch/invicta_remote.launch index e7539d5..e519ed4 100644 --- a/src/invicta_remote/launch/invicta_remote.launch +++ b/src/invicta_remote/launch/invicta_remote.launch @@ -1,6 +1,6 @@ - + - + diff --git a/src/invicta_remote/package.xml b/src/invicta_remote/package.xml index 621cd94..4ccbdd6 100644 --- a/src/invicta_remote/package.xml +++ b/src/invicta_remote/package.xml @@ -12,6 +12,7 @@ catkin roscpp + roslib sensor_msgs invicta_msgs diff --git a/src/invicta_remote/src/invicta_remote_node.cpp b/src/invicta_remote/src/invicta_remote_node.cpp index a3e7cd4..2083da1 100644 --- a/src/invicta_remote/src/invicta_remote_node.cpp +++ b/src/invicta_remote/src/invicta_remote_node.cpp @@ -2,8 +2,11 @@ * @file invicta_remote_node.cpp * @brief 遥控/设备协议节点入口（UDP + JSON ↔ ROS 话题） */ +#include #include +#include + #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("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();