refactor(config) + feat(model): 每节点一份 TOML 取代 ROS yaml；8 关节两自由度腿与单连杆 IK
配置（节点自治，跨包不共享）： - 新增各节点自己的 config/<节点>.toml（deploy / remote / hardware），只放本节点参数；删除三份 *.yaml - 各节点自带配置结构与加载器（deploy_config.h / remote_config.h / hardware_config.h）， 删除 state_machine/parameters/ 与 ControlParameters - TOML 解析库 toml++ 改为系统安装：find_package(tomlplusplus REQUIRED)，以 header-only 方式编译， 库源码不进仓库、目标机不需部署运行时 .so - launch 只传 config_file；改 TOML 不需重编，改配置头自动触发该节点重编 机器人模型（按本机构型改）： - 16 关节三关节腿 → 8 关节（每腿 摆臂 + 轮） - IK 改为单连杆反解 θ = splay_sign·acos(h/L)；趴下 h=0 → 摆臂水平（±90°） - 步态倒向：趴下时前腿向前、后腿向后（splay_sign 前 +1 / 后 −1），与 M20 相反 - 起立/趴下用样条 + 速度前馈；轮 kp 恒 0；限位改为逐关节查表 - 精简死参数：机身长宽、髋偏置、轮半径、力矩限制、通用策略参数，以及 auto_calibrate 与 ±360° 回绕标定 - 硬件侧参数入库：电机量程（P65MAXS/W190S）、CAN 通道与仲裁 ID、使能帧数/间隔、每腿 bus/canid、台架增益 保留（未改动 HEAD 已有功能）：多步态模型预加载与运行中切换、站立后自动进 RL 与就绪自检、 遥测四路上报、阻尼态单调时钟计时、策略输出 NaN 保护 验证（ROS Noetic 容器，干净构建）： - 四个包 catkin_make rc=0 - /joint_states 200 Hz（8 关节名与配置一致）、/motion_state 20 Hz - IK 目标角：站立 ±16.3°、收腿 ±66.4°、趴下 ±90.0°；趴下指令实测峰值 1.5708 rad - 协议 MotionParam=1/4 触发 idle→standup→liedown；自动进 RL 自检在零反馈下正确拦住 待实测：摆臂长度/站趴高度、各关节 dir/offset_deg；invicta_hardware 的 CAN 读写仍为 TODO
This commit is contained in:
@@ -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}
|
||||
)
|
||||
|
||||
@@ -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/配置摘要
|
||||
```
|
||||
|
||||
@@ -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
|
||||
@@ -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）
|
||||
@@ -0,0 +1,341 @@
|
||||
/**
|
||||
* @file deploy_config.h
|
||||
* @brief invicta_deploy 自己的配置（TOML）：config/invicta_deploy.toml
|
||||
*
|
||||
* 只包含本节点需要的参数（关节表与标定、腿的运动学、状态机时序与增益、策略与步态、
|
||||
* 本节点的话题/队列/控制周期）。本节点不读别的节点的配置文件。
|
||||
*/
|
||||
#pragma once
|
||||
|
||||
#include <toml++/toml.hpp>
|
||||
|
||||
#include <algorithm>
|
||||
#include <cmath>
|
||||
#include <cstdint>
|
||||
#include <map>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
namespace invicta {
|
||||
|
||||
/// 单个关节：限位与硬件标定
|
||||
struct JointSpec {
|
||||
std::string name;
|
||||
bool is_wheel = false;
|
||||
int leg = -1;
|
||||
double lower = 0., upper = 0.; ///< 位置限位 [rad]
|
||||
double vel_limit = 0.; ///< 速度限位 [rad/s]
|
||||
double dir = 1.; ///< 硬件方向 ±1
|
||||
double offset_deg = 0.; ///< 硬件零点偏置 [deg]
|
||||
};
|
||||
|
||||
/// 一条腿：摆臂 + 轮
|
||||
struct LegSpec {
|
||||
std::string name;
|
||||
std::string swing_joint, wheel_joint;
|
||||
double splay_sign = 1.; ///< 模型姿态约定：趴下时摆臂倒向（前腿 +1 向前、后腿 −1 向后）
|
||||
int swing_index = -1, wheel_index = -1;
|
||||
};
|
||||
|
||||
class DeployConfig {
|
||||
public:
|
||||
// ---- 机器人 ----
|
||||
std::string robot_name = "invicta";
|
||||
std::string robot_version = "INVICTA";
|
||||
std::vector<std::string> joint_names;
|
||||
std::vector<JointSpec> joints; ///< 顺序同 joint_names
|
||||
std::vector<LegSpec> legs;
|
||||
int dof = 0;
|
||||
|
||||
// ---- 腿的运动学（2 自由度：摆臂 + 轮）----
|
||||
double swing_link_len = 0.5; ///< L：髋轴 → 轮轴 [m]
|
||||
double wheel_radius = 0.06;
|
||||
double stand_height = 0.48, pre_height = 0.20, lie_height = 0.0;
|
||||
|
||||
// ---- 时序与增益 ----
|
||||
double stand_duration_s = 1.5, pre_hold_s = 0.0, liedown_duration_s = 2.0, damping_hold_s = 3.0;
|
||||
double stand_kp = 200., stand_kd = 4., lie_kp = 300., lie_kd = 4.5;
|
||||
double damping_kd_leg = 4., damping_kd_wheel = 1.;
|
||||
bool wheel_kp_zero = true;
|
||||
|
||||
// ---- 站立后自动进 RL ----
|
||||
bool rl_auto_enter = true;
|
||||
double rl_enter_delay = 0.5;
|
||||
bool rl_require_pose = true;
|
||||
double rl_joint_tolerance = 0.15, rl_velocity_tolerance = 1.0;
|
||||
bool rl_require_imu = true;
|
||||
|
||||
// ---- 策略 ----
|
||||
std::string policy_path; ///< 相对本包目录、或绝对路径
|
||||
int policy_decimation = 4;
|
||||
double omega_scale = 0.25, dof_vel_scale = 0.05;
|
||||
bool zero_obs_wheels = true;
|
||||
std::vector<std::string> policy_joint_order;
|
||||
std::vector<double> action_scale, default_joint_pos, policy_kp, policy_kd;
|
||||
int action_dim = 0, obs_dim = 0;
|
||||
|
||||
// ---- 步态 → 模型 ----
|
||||
uint32_t default_gait = 0x1001;
|
||||
std::map<uint32_t, std::string> gait_policies;
|
||||
|
||||
// ---- 本节点话题与节奏 ----
|
||||
std::string topic_joint_states = "joint_states", topic_imu = "imu/data";
|
||||
std::string topic_joint_commands = "joint_commands";
|
||||
std::string topic_motion_command = "motion_command", topic_motion_state = "motion_state";
|
||||
int control_period_ms = 5;
|
||||
double state_report_hz = 20.0;
|
||||
int queue_motion_command = 1, queue_joint_commands = 1, queue_state = 10;
|
||||
|
||||
bool Load(const std::string& path, std::string* error = nullptr);
|
||||
|
||||
// ---- 查询与运动学 ----
|
||||
int IndexOf(const std::string& name) const {
|
||||
auto it = std::find(joint_names.begin(), joint_names.end(), name);
|
||||
return it == joint_names.end() ? -1 : int(it - joint_names.begin());
|
||||
}
|
||||
const JointSpec& Joint(int i) const { return joints[size_t(i)]; }
|
||||
bool IsWheel(int i) const { return joints[size_t(i)].is_wheel; }
|
||||
std::vector<int> WheelIndices() const {
|
||||
std::vector<int> v;
|
||||
for (int i = 0; i < dof; ++i)
|
||||
if (joints[size_t(i)].is_wheel) v.push_back(i);
|
||||
return v;
|
||||
}
|
||||
/// 模型约定的摆臂角：θ = splay_sign · acos(h / L)
|
||||
double SwingAngleForHeight(double h, int leg) const {
|
||||
const double L = swing_link_len;
|
||||
double c = (L > 1e-9) ? h / L : 1.0;
|
||||
c = std::max(-1.0, std::min(1.0, c));
|
||||
return legs[size_t(leg)].splay_sign * std::acos(c);
|
||||
}
|
||||
double HeightForSwingAngle(double theta) const { return swing_link_len * std::cos(theta); }
|
||||
|
||||
std::string Summary() const;
|
||||
};
|
||||
|
||||
inline DeployConfig& Config() {
|
||||
static DeployConfig cfg;
|
||||
return cfg;
|
||||
}
|
||||
|
||||
// ------------------------------------------------------------------ TOML 读取小工具
|
||||
namespace detail {
|
||||
|
||||
inline const toml::table* Section(const toml::table& t, const char* key) { return t[key].as_table(); }
|
||||
|
||||
template <typename T>
|
||||
inline void Get(const toml::table* sect, const char* key, T& out) {
|
||||
if (!sect) return;
|
||||
if (auto v = (*sect)[key].value<T>()) out = *v;
|
||||
}
|
||||
inline void GetStr(const toml::table* sect, const char* key, std::string& out) {
|
||||
if (!sect) return;
|
||||
if (auto v = (*sect)[key].value<std::string>()) out = *v;
|
||||
}
|
||||
inline void GetBool(const toml::table* sect, const char* key, bool& out) {
|
||||
if (!sect) return;
|
||||
if (auto v = (*sect)[key].value<bool>()) out = *v;
|
||||
}
|
||||
inline void GetDoubleArr(const toml::table* sect, const char* key, std::vector<double>& out) {
|
||||
if (!sect) return;
|
||||
if (const auto* arr = (*sect)[key].as_array()) {
|
||||
out.clear();
|
||||
for (const auto& e : *arr)
|
||||
if (auto v = e.value<double>()) out.push_back(*v);
|
||||
}
|
||||
}
|
||||
inline void GetStrArr(const toml::table* sect, const char* key, std::vector<std::string>& out) {
|
||||
if (!sect) return;
|
||||
if (const auto* arr = (*sect)[key].as_array()) {
|
||||
out.clear();
|
||||
for (const auto& e : *arr)
|
||||
if (auto v = e.value<std::string>()) out.push_back(*v);
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace detail
|
||||
|
||||
inline bool DeployConfig::Load(const std::string& path, std::string* error) {
|
||||
const auto fail = [&](const std::string& msg) {
|
||||
if (error) *error = msg;
|
||||
return false;
|
||||
};
|
||||
|
||||
toml::table tbl;
|
||||
try {
|
||||
tbl = toml::parse_file(path);
|
||||
} catch (const toml::parse_error& e) {
|
||||
return fail(std::string("TOML 解析失败: ") + std::string(e.description()) + " @ " + path);
|
||||
}
|
||||
|
||||
const toml::table* robot = detail::Section(tbl, "robot");
|
||||
if (!robot) return fail("缺少 [robot] 段");
|
||||
detail::GetStr(robot, "name", robot_name);
|
||||
detail::GetStr(robot, "version", robot_version);
|
||||
detail::GetStrArr(robot, "joint_names", joint_names);
|
||||
if (joint_names.empty()) return fail("[robot].joint_names 为空");
|
||||
dof = int(joint_names.size());
|
||||
if (dof % 2 != 0) return fail("joint_names 数量必须是偶数（每腿 摆臂+轮）");
|
||||
|
||||
std::vector<double> lower, upper, vel_limit;
|
||||
const toml::table* limits = detail::Section(tbl, "joint_limits");
|
||||
detail::GetDoubleArr(limits, "lower", lower);
|
||||
detail::GetDoubleArr(limits, "upper", upper);
|
||||
detail::GetDoubleArr(limits, "vel_limit", vel_limit);
|
||||
if (int(lower.size()) != dof || int(upper.size()) != dof || int(vel_limit.size()) != dof) {
|
||||
return fail("[joint_limits] 的 lower/upper/vel_limit 长度必须等于关节数量");
|
||||
}
|
||||
joints.assign(size_t(dof), JointSpec{});
|
||||
for (int i = 0; i < dof; ++i) {
|
||||
joints[size_t(i)].name = joint_names[size_t(i)];
|
||||
joints[size_t(i)].lower = lower[size_t(i)];
|
||||
joints[size_t(i)].upper = upper[size_t(i)];
|
||||
joints[size_t(i)].vel_limit = vel_limit[size_t(i)];
|
||||
}
|
||||
|
||||
legs.clear();
|
||||
if (const auto* leg_arr = tbl["legs"].as_array()) {
|
||||
for (const auto& elem : *leg_arr) {
|
||||
const toml::table* lt = elem.as_table();
|
||||
if (!lt) continue;
|
||||
LegSpec leg;
|
||||
detail::GetStr(lt, "name", leg.name);
|
||||
detail::GetStr(lt, "swing_joint", leg.swing_joint);
|
||||
detail::GetStr(lt, "wheel_joint", leg.wheel_joint);
|
||||
detail::Get(lt, "splay_sign", leg.splay_sign);
|
||||
leg.swing_index = IndexOf(leg.swing_joint);
|
||||
leg.wheel_index = IndexOf(leg.wheel_joint);
|
||||
if (leg.swing_index < 0 || leg.wheel_index < 0) {
|
||||
return fail("[[legs]] " + leg.name + " 的 swing_joint/wheel_joint 不在 joint_names 里");
|
||||
}
|
||||
const toml::table* sw = detail::Section(*lt, "swing");
|
||||
const toml::table* wh = detail::Section(*lt, "wheel");
|
||||
detail::Get(sw, "dir", joints[size_t(leg.swing_index)].dir);
|
||||
detail::Get(sw, "offset_deg", joints[size_t(leg.swing_index)].offset_deg);
|
||||
detail::Get(wh, "dir", joints[size_t(leg.wheel_index)].dir);
|
||||
detail::Get(wh, "offset_deg", joints[size_t(leg.wheel_index)].offset_deg);
|
||||
joints[size_t(leg.swing_index)].leg = int(legs.size());
|
||||
joints[size_t(leg.wheel_index)].leg = int(legs.size());
|
||||
joints[size_t(leg.wheel_index)].is_wheel = true;
|
||||
legs.push_back(leg);
|
||||
}
|
||||
}
|
||||
if (legs.empty()) return fail("缺少 [[legs]] 段");
|
||||
if (int(legs.size()) * 2 != dof) return fail("腿数与关节数不匹配（每腿 2 个关节）");
|
||||
|
||||
const toml::table* gc = detail::Section(tbl, "leg");
|
||||
detail::Get(gc, "swing_link_len", swing_link_len);
|
||||
detail::Get(gc, "wheel_radius", wheel_radius);
|
||||
detail::Get(gc, "stand_height", stand_height);
|
||||
detail::Get(gc, "pre_height", pre_height);
|
||||
detail::Get(gc, "lie_height", lie_height);
|
||||
const toml::table* timing = gc ? detail::Section(*gc, "timing") : nullptr;
|
||||
detail::Get(timing, "stand_duration_s", stand_duration_s);
|
||||
detail::Get(timing, "pre_hold_s", pre_hold_s);
|
||||
detail::Get(timing, "liedown_duration_s", liedown_duration_s);
|
||||
detail::Get(timing, "damping_hold_s", damping_hold_s);
|
||||
const toml::table* gains = gc ? detail::Section(*gc, "gains") : nullptr;
|
||||
detail::Get(gains, "stand_kp", stand_kp);
|
||||
detail::Get(gains, "stand_kd", stand_kd);
|
||||
detail::Get(gains, "lie_kp", lie_kp);
|
||||
detail::Get(gains, "lie_kd", lie_kd);
|
||||
detail::Get(gains, "damping_kd_leg", damping_kd_leg);
|
||||
detail::Get(gains, "damping_kd_wheel", damping_kd_wheel);
|
||||
detail::GetBool(gains, "wheel_kp_zero", wheel_kp_zero);
|
||||
|
||||
if (swing_link_len <= 1e-6) return fail("[leg].swing_link_len 必须 > 0");
|
||||
for (const auto& pair : {std::make_pair("stand_height", stand_height),
|
||||
std::make_pair("pre_height", pre_height),
|
||||
std::make_pair("lie_height", lie_height)}) {
|
||||
if (pair.second > swing_link_len) {
|
||||
return fail(std::string("[leg].") + pair.first + " 大于 swing_link_len，物理上不可达");
|
||||
}
|
||||
}
|
||||
|
||||
const toml::table* rl = detail::Section(tbl, "rl");
|
||||
detail::GetBool(rl, "auto_enter", rl_auto_enter);
|
||||
detail::Get(rl, "enter_delay_s", rl_enter_delay);
|
||||
detail::GetBool(rl, "require_pose", rl_require_pose);
|
||||
detail::Get(rl, "joint_tolerance", rl_joint_tolerance);
|
||||
detail::Get(rl, "velocity_tolerance", rl_velocity_tolerance);
|
||||
detail::GetBool(rl, "require_imu", rl_require_imu);
|
||||
|
||||
if (const toml::table* pol = detail::Section(tbl, "policy")) {
|
||||
detail::GetStr(pol, "path", policy_path);
|
||||
detail::Get(pol, "decimation", policy_decimation);
|
||||
detail::Get(pol, "omega_scale", omega_scale);
|
||||
detail::Get(pol, "dof_vel_scale", dof_vel_scale);
|
||||
detail::GetBool(pol, "zero_obs_wheels", zero_obs_wheels);
|
||||
detail::GetStrArr(pol, "joint_order", policy_joint_order);
|
||||
detail::GetDoubleArr(pol, "action_scale", action_scale);
|
||||
detail::GetDoubleArr(pol, "default_joint_pos", default_joint_pos);
|
||||
detail::GetDoubleArr(pol, "kp", policy_kp);
|
||||
detail::GetDoubleArr(pol, "kd", policy_kd);
|
||||
if (policy_joint_order.empty()) policy_joint_order = joint_names;
|
||||
if (int(policy_joint_order.size()) != dof) return fail("[policy].joint_order 长度必须等于关节数量");
|
||||
for (const auto& n : policy_joint_order) {
|
||||
if (IndexOf(n) < 0) return fail("[policy].joint_order 里的 " + n + " 不在 joint_names 里");
|
||||
}
|
||||
action_dim = dof;
|
||||
for (const auto& arr : {&action_scale, &default_joint_pos, &policy_kp, &policy_kd}) {
|
||||
if (int(arr->size()) != action_dim) {
|
||||
return fail("[policy] 的 action_scale/default_joint_pos/kp/kd 长度必须等于关节数量");
|
||||
}
|
||||
}
|
||||
obs_dim = 9 + 3 * action_dim; // 角速度(3) + 投影重力(3) + 速度指令(3) + 关节角 + 关节速度 + 上一步动作
|
||||
}
|
||||
|
||||
const toml::table* gait = detail::Section(tbl, "gait");
|
||||
detail::Get(gait, "default", default_gait);
|
||||
if (gait) {
|
||||
if (const toml::table* pol = detail::Section(*gait, "policies")) {
|
||||
for (const auto& [k, v] : *pol) {
|
||||
try {
|
||||
gait_policies[uint32_t(std::stoul(std::string(k.str())))] = v.value_or(std::string{});
|
||||
} catch (const std::exception&) {
|
||||
return fail("[gait.policies] 的键必须能转成整数步态值");
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
const toml::table* topics = detail::Section(tbl, "topics");
|
||||
detail::GetStr(topics, "joint_states", topic_joint_states);
|
||||
detail::GetStr(topics, "imu", topic_imu);
|
||||
detail::GetStr(topics, "joint_commands", topic_joint_commands);
|
||||
detail::GetStr(topics, "motion_command", topic_motion_command);
|
||||
detail::GetStr(topics, "motion_state", topic_motion_state);
|
||||
|
||||
const toml::table* control = detail::Section(tbl, "control");
|
||||
detail::Get(control, "period_ms", control_period_ms);
|
||||
detail::Get(control, "state_report_hz", state_report_hz);
|
||||
|
||||
const toml::table* queues = detail::Section(tbl, "queues");
|
||||
detail::Get(queues, "motion_command", queue_motion_command);
|
||||
detail::Get(queues, "joint_commands", queue_joint_commands);
|
||||
detail::Get(queues, "state", queue_state);
|
||||
|
||||
if (error) error->clear();
|
||||
return true;
|
||||
}
|
||||
|
||||
inline std::string DeployConfig::Summary() const {
|
||||
char buf[512];
|
||||
std::string s;
|
||||
snprintf(buf, sizeof(buf), "[config] %s dof=%d legs=%zu L=%.3f m stand=%.3f pre=%.3f lie=%.3f\n",
|
||||
robot_name.c_str(), dof, legs.size(), swing_link_len, stand_height, pre_height, lie_height);
|
||||
s += buf;
|
||||
for (size_t i = 0; i < legs.size(); ++i) {
|
||||
const double sw = SwingAngleForHeight(stand_height, int(i));
|
||||
const double li = SwingAngleForHeight(lie_height, int(i));
|
||||
snprintf(buf, sizeof(buf), "[config] %s splay=%+.0f 站立摆臂=%+.1f° 趴下=%+.1f°\n",
|
||||
legs[i].name.c_str(), legs[i].splay_sign, sw * 180. / M_PI, li * 180. / M_PI);
|
||||
s += buf;
|
||||
}
|
||||
snprintf(buf, sizeof(buf), "[config] policy=%s obs=%d action=%d decimation=%d gait(default)=0x%04X\n",
|
||||
policy_path.c_str(), obs_dim, action_dim, policy_decimation, default_gait);
|
||||
s += buf;
|
||||
return s;
|
||||
}
|
||||
|
||||
} // namespace invicta
|
||||
@@ -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<std::string> joint_names_;
|
||||
std::vector<float> joint_dir_; ///< +1 / -1
|
||||
std::vector<float> pos_offset_deg_; ///< 关节零点偏置 [deg]
|
||||
|
||||
bool auto_calibrate_ = false;
|
||||
float wrap_hip_deg_ = 140.f; ///< 标定时 hipy(k%4==1) 的回绕阈值
|
||||
float wrap_knee_deg_ = 164.f; ///< 标定时 knee(k%4==2) 的回绕阈值
|
||||
bool wheel_kp_zero_ = true; ///< 轮关节 (k%4==3) 强制 kp=0，速度/力矩模式
|
||||
|
||||
// ---- 状态缓存 ----
|
||||
double ri_ts_ = 0., imu_ts_ = 0.;
|
||||
Vec3f omega_body_, rpy_, acc_;
|
||||
VecXf joint_pos_, joint_vel_, joint_tau_;
|
||||
std::unordered_map<std::string, int> name2slot_;
|
||||
uint16_t joint_data_frame_ = 0;
|
||||
std::vector<uint16_t> joint_data_id_;
|
||||
std::vector<bool> joint_data_updated_; ///< 标定用：该关节是否已看到数据变化
|
||||
std::vector<uint16_t> driver_status_;
|
||||
bool warned_name_mismatch_ = false;
|
||||
bool warned_imu_no_orientation_ = false;
|
||||
|
||||
ros::Publisher cmd_pub_;
|
||||
ros::Subscriber joint_state_sub_, imu_sub_;
|
||||
|
||||
public:
|
||||
InvictaInterface(const std::string& robot_name, int dof_num) : RobotInterface(robot_name, dof_num) {
|
||||
InvictaInterface(const std::string& robot_name, int dof_num)
|
||||
: RobotInterface(robot_name, dof_num), cfg_(invicta::Config()) {
|
||||
if (dof_num_ != cfg_.dof) {
|
||||
ROS_WARN("[InvictaInterface] 传入 dof=%d 与配置的 %d 不一致，按配置执行", dof_num_, cfg_.dof);
|
||||
}
|
||||
joint_pos_ = VecXf::Zero(dof_num_);
|
||||
joint_vel_ = VecXf::Zero(dof_num_);
|
||||
joint_tau_ = VecXf::Zero(dof_num_);
|
||||
joint_data_id_.resize(dof_num_, 0);
|
||||
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<invicta_msgs::JointCommandArray>(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<invicta_msgs::JointCommandArray>(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<std::string>("topics/joint_states", joints_topic_, "joint_states");
|
||||
pnh_.param<std::string>("topics/imu", imu_topic_, "imu/data");
|
||||
pnh_.param<std::string>("topics/joint_commands", cmd_topic_, "joint_commands");
|
||||
pnh_.param<int>("queues/joint_commands", cmd_queue_, 1);
|
||||
pnh_.param<int>("queues/state", state_queue_, 10);
|
||||
|
||||
joint_names_ = DefaultJointNames();
|
||||
pnh_.getParam("joint_names", joint_names_);
|
||||
if (static_cast<int>(joint_names_.size()) != dof_num_) {
|
||||
ROS_ERROR("[InvictaInterface] 参数 joint_names 长度 %zu != dof_num %d，使用默认顺序",
|
||||
joint_names_.size(), dof_num_);
|
||||
joint_names_ = DefaultJointNames();
|
||||
}
|
||||
name2slot_.clear();
|
||||
for (int i = 0; i < dof_num_; ++i) {
|
||||
name2slot_[joint_names_[i]] = i;
|
||||
}
|
||||
|
||||
joint_dir_.assign(dof_num_, 1.f);
|
||||
pos_offset_deg_.assign(dof_num_, 0.f);
|
||||
pnh_.getParam("calibration/joint_dir", joint_dir_);
|
||||
pnh_.getParam("calibration/joint_offset_deg", pos_offset_deg_);
|
||||
if (static_cast<int>(joint_dir_.size()) != dof_num_) joint_dir_.assign(dof_num_, 1.f);
|
||||
if (static_cast<int>(pos_offset_deg_.size()) != dof_num_) pos_offset_deg_.assign(dof_num_, 0.f);
|
||||
|
||||
pnh_.param<bool>("calibration/auto_calibrate", auto_calibrate_, false);
|
||||
pnh_.param("calibration/wrap_hip_deg", wrap_hip_deg_, 140.f);
|
||||
pnh_.param("calibration/wrap_knee_deg", wrap_knee_deg_, 164.f);
|
||||
pnh_.param<bool>("wheel_kp_zero", wheel_kp_zero_, true);
|
||||
|
||||
ROS_INFO_STREAM("[InvictaInterface] topics: " << joints_topic_ << " / " << imu_topic_ << " / " << cmd_topic_
|
||||
<< " | auto_calibrate=" << auto_calibrate_
|
||||
<< " wheel_kp_zero=" << wheel_kp_zero_);
|
||||
std::cout << "[InvictaInterface] joint_dir: ";
|
||||
for (float d : joint_dir_) std::cout << d << " ";
|
||||
std::cout << "\n[InvictaInterface] joint_offset_deg: ";
|
||||
for (float o : pos_offset_deg_) std::cout << o << " ";
|
||||
std::cout << std::endl;
|
||||
}
|
||||
|
||||
static std::vector<std::string> DefaultJointNames() {
|
||||
// 与 policy.onnx 的动作/观测顺序一致，见 invicta_deploy/README.md
|
||||
return {"fl_hipx_joint", "fl_hipy_joint", "fl_knee_joint", "fl_wheel_joint",
|
||||
"fr_hipx_joint", "fr_hipy_joint", "fr_knee_joint", "fr_wheel_joint",
|
||||
"hl_hipx_joint", "hl_hipy_joint", "hl_knee_joint", "hl_wheel_joint",
|
||||
"hr_hipx_joint", "hr_hipy_joint", "hr_knee_joint", "hr_wheel_joint"};
|
||||
}
|
||||
|
||||
// ------------------------------------------------------------------ 启动/标定
|
||||
// ------------------------------------------------------------------ 启动序列
|
||||
void PublishRawCommand(uint16_t control_word) {
|
||||
invicta_msgs::JointCommandArray msg;
|
||||
msg.header.stamp = ros::Time::now();
|
||||
msg.name = joint_names_;
|
||||
msg.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<int>(msg->position.size()) != dof_num_ ||
|
||||
static_cast<int>(msg->velocity.size()) != dof_num_ ||
|
||||
static_cast<int>(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<int>(msg->name.size()) == dof_num_;
|
||||
const bool by_name = msg->name.size() == n;
|
||||
int unknown = 0;
|
||||
for (int j = 0; j < dof_num_; ++j) {
|
||||
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<float>();
|
||||
rpy_ = q.toRotationMatrix().eulerAngles(0, 1, 2).cast<float>(); // 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<std::string, int> name2slot_;
|
||||
|
||||
double ri_ts_ = 0., imu_ts_ = 0.;
|
||||
Vec3f omega_body_, rpy_, acc_;
|
||||
VecXf joint_pos_, joint_vel_, joint_tau_;
|
||||
uint16_t joint_data_frame_ = 0;
|
||||
std::vector<uint16_t> joint_data_id_;
|
||||
std::vector<uint16_t> driver_status_;
|
||||
bool warned_name_mismatch_ = false;
|
||||
bool warned_imu_no_orientation_ = false;
|
||||
|
||||
ros::Publisher cmd_pub_;
|
||||
ros::Subscriber joint_state_sub_, imu_sub_;
|
||||
};
|
||||
|
||||
}; // namespace interface
|
||||
|
||||
@@ -21,6 +21,7 @@
|
||||
|
||||
#include "basic_function.hpp"
|
||||
#include "custom_types.h"
|
||||
#include "config/deploy_config.h"
|
||||
#include "invicta_msgs/MotionCommand.h"
|
||||
#include "invicta_msgs/motion_defs.h"
|
||||
#include "user_command_interface.h"
|
||||
@@ -36,9 +37,9 @@ public:
|
||||
* @param default_gait 默认步态（协议 GaitParam）
|
||||
*/
|
||||
RosCommandInterface(std::shared_ptr<std::set<int>> loaded_gaits, int default_gait)
|
||||
: loaded_gaits_(std::move(loaded_gaits)), default_gait_(default_gait) {
|
||||
pnh_.param<std::string>("topics/motion_command", topic_, "motion_command");
|
||||
pnh_.param<int>("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<std::set<int>> loaded_gaits_; ///< 实际加载成功的步态（由 RLControlState 填充）
|
||||
int default_gait_ = int(kGaitBasic);
|
||||
|
||||
|
||||
@@ -1,14 +1,6 @@
|
||||
<launch>
|
||||
<!-- RL 部署节点：状态机 + 策略推理。参数在节点的私有命名空间下。
|
||||
无硬件台架自测时（invicta_hardware 只发零位状态）关掉两个自检，
|
||||
否则站立完成后永远不会自动进入 RL：
|
||||
roslaunch invicta_deploy invicta_deploy.launch rl_require_pose:=false rl_require_imu:=false
|
||||
-->
|
||||
<arg name="rl_require_pose" default="true"/>
|
||||
<arg name="rl_require_imu" default="true"/>
|
||||
<!-- RL 部署节点：状态机 + 策略推理。参数来自本节点自己的 config/invicta_deploy.toml -->
|
||||
<node pkg="invicta_deploy" type="invicta_deploy" name="invicta_deploy" output="screen">
|
||||
<rosparam command="load" file="$(find invicta_deploy)/config/invicta_deploy.yaml"/>
|
||||
<param name="rl/require_pose" value="$(arg rl_require_pose)"/>
|
||||
<param name="rl/require_imu" value="$(arg rl_require_imu)"/>
|
||||
<param name="config_file" value="$(find invicta_deploy)/config/invicta_deploy.toml"/>
|
||||
</node>
|
||||
</launch>
|
||||
|
||||
@@ -1,12 +1,17 @@
|
||||
/**
|
||||
* @file main.cpp
|
||||
* @brief invicta RL deploy 入口：ROS 回调跑在 main 线程，控制线程跑 200 Hz 状态机
|
||||
* @brief invicta RL deploy 入口：先加载 TOML 配置，再跑状态机
|
||||
*
|
||||
* 配置路径：ROS 参数 ~config_file（launch 传入），默认本包 config/invicta_deploy.toml。
|
||||
*/
|
||||
#include <ros/package.h>
|
||||
#include <ros/ros.h>
|
||||
|
||||
#include <iostream>
|
||||
#include <memory>
|
||||
#include <string>
|
||||
|
||||
#include "config/deploy_config.h"
|
||||
#include "quadruped_wheel/invicta_state_machine.hpp"
|
||||
|
||||
using namespace types;
|
||||
@@ -15,8 +20,21 @@ MotionStateFeedback StateBase::msfb_ = MotionStateFeedback();
|
||||
|
||||
int main(int argc, char** argv) {
|
||||
ros::init(argc, argv, "invicta_deploy");
|
||||
std::cout << "Invicta State Machine Start Running" << std::endl;
|
||||
ros::NodeHandle pnh("~");
|
||||
|
||||
std::string config_path;
|
||||
pnh.param<std::string>("config_file", config_path, "");
|
||||
if (config_path.empty()) config_path = ros::package::getPath("invicta_deploy") + "/config/invicta_deploy.toml";
|
||||
|
||||
std::string err;
|
||||
auto& cfg = invicta::Config();
|
||||
if (!cfg.Load(config_path, &err)) {
|
||||
ROS_FATAL("[invicta_deploy] 配置加载失败：%s（%s）", err.c_str(), config_path.c_str());
|
||||
return 1;
|
||||
}
|
||||
std::cout << cfg.Summary() << std::flush;
|
||||
|
||||
std::cout << "Invicta State Machine Start Running" << std::endl;
|
||||
std::shared_ptr<StateMachineBase> fsm = std::make_shared<invicta::InvictaStateMachine>();
|
||||
fsm->Start();
|
||||
fsm->Run();
|
||||
|
||||
@@ -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 <onnxruntime_c_api.h>
|
||||
#include <onnxruntime_cxx_api.h>
|
||||
|
||||
#include <array>
|
||||
#include <algorithm>
|
||||
#include <cmath>
|
||||
#include <ctime>
|
||||
#include <string>
|
||||
#include <unordered_map>
|
||||
#include <utility>
|
||||
#include <vector>
|
||||
|
||||
#include "config/deploy_config.h"
|
||||
#include "policy_runner_base.hpp"
|
||||
|
||||
class InvictaPolicyRunner : public PolicyRunnerBase {
|
||||
private:
|
||||
VecXf kp_, kd_;
|
||||
VecXf dof_default_eigen_policy, dof_default_eigen_robot;
|
||||
Vec3f gravity_direction = Vec3f(0., 0., -1.);
|
||||
|
||||
const int motor_num = 16;
|
||||
const int observation_dim = 57;
|
||||
const int action_dim = 16;
|
||||
float agent_timestep = 0.02; ///< 策略决策周期 0.02 s = 50 Hz
|
||||
|
||||
VecXf joint_pos_rl = VecXf(action_dim); ///< policy 关节顺序
|
||||
VecXf joint_vel_rl = VecXf(action_dim);
|
||||
|
||||
const std::string policy_path_;
|
||||
|
||||
float omega_scale_ = 0.25;
|
||||
float dof_vel_scale_ = 0.05;
|
||||
VecXf imu_w_eigen, base_acc_eigen, motor_p_eigen, motor_v_eigen, current_action_eigen, last_action_eigen,
|
||||
current_observation_, tmp_action_eigen;
|
||||
|
||||
RobotAction robot_action;
|
||||
|
||||
/// 机器人（话题）关节顺序 = /joint_states 的 name 顺序
|
||||
std::vector<std::string> robot_order = {"fl_hipx_joint", "fl_hipy_joint", "fl_knee_joint", "fl_wheel_joint",
|
||||
"fr_hipx_joint", "fr_hipy_joint", "fr_knee_joint", "fr_wheel_joint",
|
||||
"hl_hipx_joint", "hl_hipy_joint", "hl_knee_joint", "hl_wheel_joint",
|
||||
"hr_hipx_joint", "hr_hipy_joint", "hr_knee_joint", "hr_wheel_joint"};
|
||||
|
||||
/// 策略内部关节顺序（12 个腿关节在前，4 个轮在后），与训练导出绑定
|
||||
std::vector<std::string> policy_order = {
|
||||
"fl_hipx_joint", "fl_hipy_joint", "fl_knee_joint", "fr_hipx_joint", "fr_hipy_joint", "fr_knee_joint",
|
||||
"hl_hipx_joint", "hl_hipy_joint", "hl_knee_joint", "hr_hipx_joint", "hr_hipy_joint", "hr_knee_joint",
|
||||
"fl_wheel_joint", "fr_wheel_joint", "hl_wheel_joint", "hr_wheel_joint",
|
||||
};
|
||||
|
||||
std::vector<float> action_scale_robot = {0.125, 0.25, 0.25, 5, 0.125, 0.25, 0.25, 5,
|
||||
0.125, 0.25, 0.25, 5, 0.125, 0.25, 0.25, 5};
|
||||
|
||||
Ort::SessionOptions session_options_;
|
||||
Ort::Session session_{nullptr};
|
||||
Ort::Env env_;
|
||||
std::vector<int> robot2policy_idx, policy2robot_idx;
|
||||
|
||||
const char* input_names_[1] = {"obs"}; // 必须与模型导出时一致
|
||||
const char* output_names_[1] = {"actions"};
|
||||
Ort::MemoryInfo memory_info{nullptr};
|
||||
const std::array<int64_t, 2> input_observationShape = {1, observation_dim};
|
||||
|
||||
public:
|
||||
InvictaPolicyRunner(const std::string& policy_name, const std::string& policy_path)
|
||||
: PolicyRunnerBase(policy_name),
|
||||
policy_path_(policy_path),
|
||||
cfg_(invicta::Config()),
|
||||
policy_path_(policy_path.empty() ? cfg_.policy_path : policy_path),
|
||||
env_(ORT_LOGGING_LEVEL_WARNING, "InvictaPolicyRunner"),
|
||||
session_options_{},
|
||||
session_{nullptr},
|
||||
memory_info(Ort::MemoryInfo::CreateCpu(OrtArenaAllocator, OrtMemTypeDefault)) {
|
||||
dof_default_eigen_policy.setZero(action_dim);
|
||||
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<int> generate_permutation(const std::vector<std::string>& from,
|
||||
const std::vector<std::string>& to, int default_index = 0) {
|
||||
std::unordered_map<std::string, int> idx_map;
|
||||
for (int i = 0; i < static_cast<int>(from.size()); ++i) idx_map[from[i]] = i;
|
||||
|
||||
std::vector<int> perm;
|
||||
for (const auto& name : to) {
|
||||
auto it = idx_map.find(name);
|
||||
perm.push_back(it != idx_map.end() ? it->second : default_index);
|
||||
}
|
||||
return perm;
|
||||
void DisplayPolicyInfo() {
|
||||
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<float>(memory_info, current_observation.data(),
|
||||
current_observation.size(), input_observationShape.data(),
|
||||
input_observationShape.size());
|
||||
|
||||
std::vector<Ort::Value> inputs;
|
||||
inputs.emplace_back(std::move(input_tensor));
|
||||
|
||||
auto outputs =
|
||||
session_.Run(Ort::RunOptions{nullptr}, input_names_, inputs.data(), 1, output_names_, 1);
|
||||
|
||||
float* action_data = outputs[0].GetTensorMutableData<float>();
|
||||
Eigen::Map<Eigen::VectorXf> action_map(action_data, action_dim);
|
||||
return VecXf(action_map);
|
||||
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<int> Permutation(const std::vector<std::string>& from, const std::vector<std::string>& to) {
|
||||
std::unordered_map<std::string, int> idx;
|
||||
for (size_t i = 0; i < from.size(); ++i) idx[from[i]] = int(i);
|
||||
std::vector<int> perm;
|
||||
for (const auto& n : to) {
|
||||
auto it = idx.find(n);
|
||||
perm.push_back(it == idx.end() ? 0 : it->second);
|
||||
}
|
||||
return perm;
|
||||
}
|
||||
|
||||
/// 把「策略顺序」的数组搬到「机器人顺序」
|
||||
void ToRobotOrder(const std::vector<double>& policy_order_values, VecXf& out) {
|
||||
out.resize(action_dim_);
|
||||
for (int i = 0; i < action_dim_; ++i) {
|
||||
out(i) = float(policy_order_values[size_t(robot2policy_idx_[size_t(i)])]);
|
||||
}
|
||||
}
|
||||
|
||||
VecXf OnnxInfer(VecXf observation) {
|
||||
Ort::Value input_tensor =
|
||||
Ort::Value::CreateTensor<float>(memory_info, observation.data(), observation.size(),
|
||||
input_observation_shape_.data(), input_observation_shape_.size());
|
||||
std::vector<Ort::Value> inputs;
|
||||
inputs.emplace_back(std::move(input_tensor));
|
||||
auto outputs = session_.Run(Ort::RunOptions{nullptr}, input_names_, inputs.data(), 1, output_names_, 1);
|
||||
float* data = outputs[0].GetTensorMutableData<float>();
|
||||
Eigen::Map<Eigen::VectorXf> mapped(data, action_dim_);
|
||||
return VecXf(mapped);
|
||||
}
|
||||
|
||||
const invicta::DeployConfig& cfg_;
|
||||
const std::string policy_path_;
|
||||
int action_dim_ = 0, obs_dim_ = 0;
|
||||
const Vec3f gravity_direction_ = Vec3f(0., 0., -1.);
|
||||
|
||||
std::vector<int> robot2policy_idx_, policy2robot_idx_, wheel_robot_idx_;
|
||||
VecXf action_scale_robot_, default_robot_, kp_robot_, kd_robot_, default_policy_;
|
||||
|
||||
VecXf joint_pos_rl_, joint_vel_rl_;
|
||||
VecXf current_observation_, last_action_eigen_, tmp_action_eigen_, current_action_eigen_;
|
||||
RobotAction robot_action_;
|
||||
|
||||
Ort::SessionOptions session_options_;
|
||||
Ort::Session session_;
|
||||
Ort::Env env_;
|
||||
const char* input_names_[1] = {"obs"}; // 必须与模型导出时一致
|
||||
const char* output_names_[1] = {"actions"};
|
||||
Ort::MemoryInfo memory_info;
|
||||
std::vector<int64_t> input_observation_shape_;
|
||||
};
|
||||
|
||||
@@ -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'
|
||||
|
||||
@@ -1,61 +0,0 @@
|
||||
/**
|
||||
* @file control_parameters.h
|
||||
* @brief 机器人几何/控制参数。全部来自 ROS 参数，默认值是上游 M20 的取值
|
||||
*
|
||||
* 换成 invicta 自己的机器人时，改 config/invicta_deploy.yaml 即可，不用改代码。
|
||||
*/
|
||||
#pragma once
|
||||
|
||||
#include <ros/ros.h>
|
||||
|
||||
#include <string>
|
||||
|
||||
#include "common_types.h"
|
||||
#include "custom_types.h"
|
||||
|
||||
using namespace types;
|
||||
|
||||
class ControlParameters {
|
||||
public:
|
||||
explicit ControlParameters(const ros::NodeHandle& nh) { Load(nh); }
|
||||
~ControlParameters() {}
|
||||
|
||||
/// 机身尺寸 [m]
|
||||
float body_len_x_, body_len_y_;
|
||||
float hip_len_, thigh_len_, shank_len_;
|
||||
|
||||
/// 站立高度配置 [m]
|
||||
float pre_height_, stand_height_;
|
||||
|
||||
/// 单腿摆动相 PD 增益
|
||||
Vec3f swing_leg_kp_, swing_leg_kd_;
|
||||
|
||||
/// 腿关节位置限制 [rad]（左前腿；右侧镜像）
|
||||
Vec3f fl_joint_lower_, fl_joint_upper_;
|
||||
|
||||
/// 关节速度限制 [rad/s]、力矩限制 [Nm]
|
||||
Vec3f joint_vel_limit_;
|
||||
Vec3f torque_limit_;
|
||||
|
||||
/// 站立/卧倒时长 [s]
|
||||
float stand_duration_ = 1.5;
|
||||
float liedown_duration_ = 2.0;
|
||||
|
||||
/// 关节阻尼时长 [s]：缓慢松下 → 机器人着地 → 转入空闲（自由）
|
||||
float joint_damping_duration_ = 3.0;
|
||||
|
||||
// ---- 站立完成后自动进入 RL（原厂机制：模型推理健康 + 姿态检查通过即切换，不等指令）----
|
||||
bool rl_auto_enter_ = true;
|
||||
float rl_enter_delay_ = 0.5f; ///< 到达站立位姿后再稳定多久 [s]
|
||||
bool rl_require_pose_ = true; ///< 是否检查关节角已到站立位姿（无硬件台架要关掉）
|
||||
float rl_joint_tolerance_ = 0.15f; ///< 关节角容差 [rad]
|
||||
float rl_velocity_tolerance_ = 1.0f; ///< 关节速度容差 [rad/s]
|
||||
bool rl_require_imu_ = true; ///< 是否检查 IMU 数据有效
|
||||
|
||||
/// 轮关节速度限制与轮半径相关长度 [m]
|
||||
float wheel_vel_limit_ = 100;
|
||||
float wheel_link_len_ = 0.040575;
|
||||
|
||||
private:
|
||||
void Load(const ros::NodeHandle& nh);
|
||||
};
|
||||
@@ -1,58 +0,0 @@
|
||||
/**
|
||||
* @file invicta_control_parameters.cpp
|
||||
* @brief 从 ROS 参数读取控制参数，默认值为上游 M20
|
||||
*/
|
||||
#include "parameters/control_parameters.h"
|
||||
|
||||
namespace {
|
||||
|
||||
Vec3f GetVec3(const ros::NodeHandle& nh, const std::string& key, const Vec3f& fallback) {
|
||||
std::vector<double> v;
|
||||
if (!nh.getParam(key, v) || v.size() != 3) {
|
||||
return fallback;
|
||||
}
|
||||
return Vec3f(static_cast<float>(v[0]), static_cast<float>(v[1]), static_cast<float>(v[2]));
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
void ControlParameters::Load(const ros::NodeHandle& nh) {
|
||||
nh.param<float>("geometry/body_len_x", body_len_x_, 0.3095f * 2.f);
|
||||
nh.param<float>("geometry/body_len_y", body_len_y_, 0.065f * 2.f);
|
||||
nh.param<float>("geometry/hip_len", hip_len_, 0.104f);
|
||||
nh.param<float>("geometry/thigh_len", thigh_len_, 0.25f);
|
||||
nh.param<float>("geometry/shank_len", shank_len_, 0.25f);
|
||||
|
||||
nh.param<float>("geometry/pre_height", pre_height_, 0.12f);
|
||||
nh.param<float>("geometry/stand_height", stand_height_, 0.48f);
|
||||
|
||||
swing_leg_kp_ = GetVec3(nh, "gains/swing_leg_kp", Vec3f(200., 200., 200.));
|
||||
swing_leg_kd_ = GetVec3(nh, "gains/swing_leg_kd", Vec3f(4., 4., 4.));
|
||||
|
||||
fl_joint_lower_ = GetVec3(nh, "limits/fl_joint_lower", Vec3f(-0.4363, -2.443, -2.758));
|
||||
fl_joint_upper_ = GetVec3(nh, "limits/fl_joint_upper", Vec3f(0.6109, 2.443, 2.758));
|
||||
joint_vel_limit_ = GetVec3(nh, "limits/joint_vel_limit", Vec3f(22.4, 22.4, 22.4));
|
||||
torque_limit_ = GetVec3(nh, "limits/torque_limit", Vec3f(76.4, 76.4, 76.4));
|
||||
|
||||
nh.param<float>("timing/stand_duration", stand_duration_, 1.5f);
|
||||
nh.param<float>("timing/liedown_duration", liedown_duration_, 2.0f);
|
||||
nh.param<float>("timing/joint_damping_duration", joint_damping_duration_, 3.0f);
|
||||
if (joint_damping_duration_ < 0.1f || joint_damping_duration_ > 30.f) {
|
||||
ROS_WARN("[ControlParameters] timing/joint_damping_duration=%.2f 超出 [0.1, 30] s，回退到 3.0 s",
|
||||
joint_damping_duration_);
|
||||
joint_damping_duration_ = 3.0f;
|
||||
}
|
||||
ROS_INFO("[ControlParameters] joint_damping_duration = %.2f s", joint_damping_duration_);
|
||||
|
||||
nh.param<bool>("rl/auto_enter", rl_auto_enter_, true);
|
||||
nh.param<float>("rl/enter_delay", rl_enter_delay_, 0.5f);
|
||||
nh.param<bool>("rl/require_pose", rl_require_pose_, true);
|
||||
nh.param<float>("rl/joint_tolerance", rl_joint_tolerance_, 0.15f);
|
||||
nh.param<float>("rl/velocity_tolerance", rl_velocity_tolerance_, 1.0f);
|
||||
nh.param<bool>("rl/require_imu", rl_require_imu_, true);
|
||||
ROS_INFO("[ControlParameters] 站立后自动进 RL: %s（delay %.2f s, require_pose=%d, require_imu=%d）",
|
||||
rl_auto_enter_ ? "开" : "关", rl_enter_delay_, int(rl_require_pose_), int(rl_require_imu_));
|
||||
|
||||
nh.param<float>("limits/wheel_vel_limit", wheel_vel_limit_, 100.f);
|
||||
nh.param<float>("geometry/wheel_link_len", wheel_link_len_, 0.040575f);
|
||||
}
|
||||
@@ -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;
|
||||
|
||||
@@ -1,8 +1,9 @@
|
||||
/**
|
||||
* @file invicta_state_machine.hpp
|
||||
* @brief 状态机装配：运动指令来源（ROS 话题）+ 数据面 + 五个状态 + 安全检查 + 状态回报
|
||||
* @brief 状态机装配：指令来源（ROS 话题）+ 数据面 + 五个状态 + 安全检查 + 状态回报
|
||||
*
|
||||
* 遥控协议（UDP/JSON）已拆到 invicta_remote 节点，本节点只消费 /motion_command：
|
||||
* 所有参数来自 invicta::Config()（invicta.toml）；模型文件留在本包 policy/ 下，
|
||||
* 配置里的相对路径在这里解析为「包目录 + 相对路径」。
|
||||
* invicta_remote --/motion_command--> invicta_deploy --/motion_state--> invicta_remote
|
||||
*/
|
||||
#pragma once
|
||||
@@ -17,6 +18,7 @@
|
||||
#include <string>
|
||||
|
||||
#include "hardware/invicta_interface.hpp"
|
||||
#include "config/deploy_config.h"
|
||||
#include "motion_state_reporter.hpp"
|
||||
#include "quadruped_wheel/idle_state.hpp"
|
||||
#include "quadruped_wheel/joint_damping_state.hpp"
|
||||
@@ -29,9 +31,6 @@
|
||||
|
||||
namespace invicta {
|
||||
|
||||
/// 关节数量，与 policy.onnx 绑定（M20 是 4 腿 × 4 关节）
|
||||
constexpr int kDofNum = 16;
|
||||
|
||||
class InvictaStateMachine : public StateMachineBase {
|
||||
private:
|
||||
std::shared_ptr<StateBase> idle_controller_;
|
||||
@@ -42,46 +41,37 @@ private:
|
||||
|
||||
std::shared_ptr<interface::MotionStateReporter> state_reporter_;
|
||||
|
||||
/// 模型路径：绝对路径原样使用；相对路径基于 invicta_deploy 包目录（模型跟着 RL 包走）
|
||||
static std::string ResolveModelPath(const std::string& path) {
|
||||
if (path.empty() || path.front() == '/') return path;
|
||||
return ros::package::getPath("invicta_deploy") + "/" + path;
|
||||
}
|
||||
|
||||
public:
|
||||
InvictaStateMachine() : StateMachineBase() {}
|
||||
~InvictaStateMachine() {}
|
||||
|
||||
void Start() {
|
||||
ros::NodeHandle pnh("~"); // 参数都在 ~ 下，见 config/invicta_deploy.yaml
|
||||
|
||||
pnh.param<int>("control_period_ms", control_period_ms_, 5); // 默认 200 Hz
|
||||
|
||||
std::string policy_path;
|
||||
pnh.param<std::string>("policy_path", policy_path, "");
|
||||
if (policy_path.empty()) {
|
||||
policy_path = ros::package::getPath("invicta_deploy") + "/policy/policy.onnx";
|
||||
}
|
||||
|
||||
std::string state_topic = "motion_state";
|
||||
double state_hz = 20.0;
|
||||
std::string version = "INVICTA";
|
||||
pnh.param<std::string>("topics/motion_state", state_topic, "motion_state");
|
||||
pnh.param<double>("motion_state_report_hz", state_hz, 20.0);
|
||||
pnh.param<std::string>("robot_version", version, "INVICTA");
|
||||
const auto& cfg = invicta::Config();
|
||||
|
||||
const std::string policy_path = ResolveModelPath(cfg.policy_path);
|
||||
std::map<std::string, std::string> gait_policy_map;
|
||||
pnh.getParam("gait/policy_map", gait_policy_map);
|
||||
int default_gait = invicta_msgs::kGaitBasic;
|
||||
pnh.param<int>("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<std::set<int>>();
|
||||
|
||||
// ---- 指令来源：订阅 /motion_command ----
|
||||
uc_ptr_ = std::make_shared<interface::RosCommandInterface>(loaded_gaits, default_gait);
|
||||
uc_ptr_->SetMotionStateFeedback(&StateBase::msfb_);
|
||||
|
||||
// ---- 机器人数据面 + 控制参数 ----
|
||||
ri_ptr_ = std::make_shared<interface::InvictaInterface>("invicta", kDofNum);
|
||||
cp_ptr_ = std::make_shared<ControlParameters>(pnh);
|
||||
// ---- 机器人数据面 ----
|
||||
ri_ptr_ = std::make_shared<interface::InvictaInterface>(cfg.robot_name, cfg.dof);
|
||||
|
||||
std::shared_ptr<ControllerData> data_ptr = std::make_shared<ControllerData>();
|
||||
data_ptr->ri_ptr = ri_ptr_;
|
||||
data_ptr->uc_ptr = uc_ptr_;
|
||||
data_ptr->cp_ptr = cp_ptr_;
|
||||
|
||||
sc_ptr_ = std::make_shared<SafeController>();
|
||||
sc_ptr_->SetRobotDataSource(ri_ptr_);
|
||||
@@ -98,18 +88,19 @@ public:
|
||||
current_state_name_ = kIdle;
|
||||
next_state_name_ = kIdle;
|
||||
|
||||
std::cout << "control period: " << control_period_ms_ << " ms, policy: " << policy_path << std::endl;
|
||||
std::cout << "control period: " << cfg.control_period_ms << " ms, 默认模型: " << policy_path << std::endl;
|
||||
|
||||
// ---- 状态回报（供 invicta_remote 判前提 + 将来做状态上报）----
|
||||
// ---- 状态回报（供 invicta_remote 判前提 + 状态上报）----
|
||||
state_reporter_ = std::make_shared<interface::MotionStateReporter>(
|
||||
&StateBase::msfb_, uc_ptr_->GetUserCommand(), state_topic, state_hz, version);
|
||||
&StateBase::msfb_, uc_ptr_->GetUserCommand(), cfg.topic_motion_state, cfg.state_report_hz,
|
||||
cfg.robot_version);
|
||||
|
||||
ri_ptr_->Start();
|
||||
uc_ptr_->Start();
|
||||
sc_ptr_->Start();
|
||||
state_reporter_->Start();
|
||||
current_controller_->OnEnter();
|
||||
std::cout << "state report: " << state_topic << " @ " << state_hz << " Hz" << std::endl;
|
||||
std::cout << "state report: " << cfg.topic_motion_state << " @ " << cfg.state_report_hz << " Hz" << std::endl;
|
||||
}
|
||||
|
||||
std::shared_ptr<StateBase> GetStateControllerPtr(StateName state_name) {
|
||||
|
||||
@@ -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<ControllerData> 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;
|
||||
}
|
||||
|
||||
|
||||
@@ -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<ControllerData> 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;i<current_joint_pos_.rows();++i){
|
||||
planning_joint_pos(i) = GetCubicSplinePos(init_joint_pos_(i), init_joint_vel_(i), goal_joint_pos_(i), 0,
|
||||
run_time_ - time_stamp_record_, liedown_duration_);
|
||||
planning_joint_vel(i) = GetCubicSplineVel(init_joint_pos_(i), init_joint_vel_(i), goal_joint_pos_(i), 0,
|
||||
run_time_ - time_stamp_record_, liedown_duration_);
|
||||
if(i%4==3){
|
||||
planning_joint_vel(i) = 0.0;
|
||||
}
|
||||
}
|
||||
const double elapsed = run_time_ - time_stamp_record_;
|
||||
|
||||
joint_cmd_.col(1) = planning_joint_pos;
|
||||
joint_cmd_.col(3) = planning_joint_vel;
|
||||
joint_cmd_.col(2) = kd_;
|
||||
ri_ptr_->SetJointCommand(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;
|
||||
}
|
||||
};
|
||||
|
||||
@@ -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<ControllerData> data_ptr)
|
||||
: StateBase(state_name, data_ptr) {
|
||||
goal_joint_pos_ = Vec4f(init_hipx_pos_, GetHipYPosByHeight(cp_ptr_->pre_height_), GetKneePosByHeight(cp_ptr_->pre_height_), 0).replicate(4, 1);
|
||||
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;i<current_joint_pos_.rows();++i){
|
||||
planning_joint_pos(i) = GetCubicSplinePos(init_joint_pos_(i), init_joint_vel_(i), goal_joint_pos_(i), 0,
|
||||
run_time_ - time_stamp_record_, stand_duration_);
|
||||
planning_joint_vel(i) = GetCubicSplineVel(init_joint_pos_(i), init_joint_vel_(i), goal_joint_pos_(i), 0,
|
||||
run_time_ - time_stamp_record_, stand_duration_);
|
||||
if(i%4==3){
|
||||
kd_(i) = 0;
|
||||
}
|
||||
const auto& cfg = invicta::Config();
|
||||
const double elapsed = run_time_ - time_stamp_record_;
|
||||
|
||||
VecXf pos = VecXf::Zero(int(ri_ptr_->dof_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;
|
||||
|
||||
@@ -8,7 +8,7 @@
|
||||
#include "common_types.h"
|
||||
#include "custom_types.h"
|
||||
#include "motion_state_feedback.hpp"
|
||||
#include "parameters/control_parameters.h"
|
||||
#include "config/deploy_config.h"
|
||||
#include "robot_interface.h"
|
||||
#include "user_command_interface.h"
|
||||
|
||||
@@ -18,7 +18,6 @@ using namespace interface;
|
||||
struct ControllerData {
|
||||
std::shared_ptr<RobotInterface> ri_ptr;
|
||||
std::shared_ptr<UserCommandInterface> uc_ptr;
|
||||
std::shared_ptr<ControlParameters> cp_ptr;
|
||||
};
|
||||
|
||||
class StateBase {
|
||||
@@ -27,7 +26,6 @@ public:
|
||||
: state_name_(state_name), data_ptr_(data_ptr) {
|
||||
ri_ptr_ = data_ptr_->ri_ptr;
|
||||
uc_ptr_ = data_ptr_->uc_ptr;
|
||||
cp_ptr_ = data_ptr_->cp_ptr;
|
||||
std::memset(&msfb_, 0, sizeof(msfb_));
|
||||
}
|
||||
|
||||
@@ -53,7 +51,6 @@ public:
|
||||
std::shared_ptr<ControllerData> data_ptr_;
|
||||
std::shared_ptr<RobotInterface> ri_ptr_;
|
||||
std::shared_ptr<UserCommandInterface> uc_ptr_;
|
||||
std::shared_ptr<ControlParameters> cp_ptr_;
|
||||
|
||||
static MotionStateFeedback msfb_;
|
||||
};
|
||||
|
||||
@@ -100,7 +100,7 @@ public:
|
||||
}
|
||||
|
||||
virtual void RunThread() {
|
||||
set_timer.time_init(control_period_ms_);
|
||||
set_timer.time_init(invicta::Config().control_period_ms);
|
||||
startTime = set_timer.get_start_time();
|
||||
|
||||
while (ros::ok()) {
|
||||
@@ -140,7 +140,6 @@ public:
|
||||
|
||||
std::shared_ptr<UserCommandInterface> uc_ptr_;
|
||||
std::shared_ptr<RobotInterface> ri_ptr_;
|
||||
std::shared_ptr<ControlParameters> cp_ptr_;
|
||||
std::shared_ptr<SafeController> sc_ptr_;
|
||||
|
||||
std::shared_ptr<StateBase> current_controller_;
|
||||
|
||||
@@ -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})
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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 # 指令只取最新一帧
|
||||
@@ -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]
|
||||
@@ -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²]（机体系）。
|
||||
|
||||
@@ -0,0 +1,258 @@
|
||||
/**
|
||||
* @file hardware_config.h
|
||||
* @brief invicta_hardware 自己的配置（TOML）：config/invicta_hardware.toml
|
||||
*
|
||||
* 只包含本节点需要的参数：关节表与硬件标定、每条腿挂的总线/CANID、电机与 CAN 参数、
|
||||
* 本节点发布/订阅的话题与频率。
|
||||
*/
|
||||
#pragma once
|
||||
|
||||
#include <toml++/toml.hpp>
|
||||
|
||||
#include <algorithm>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
namespace invicta_hardware {
|
||||
|
||||
/// 单个关节：限位与硬件标定
|
||||
struct JointSpec {
|
||||
std::string name;
|
||||
bool is_wheel = false;
|
||||
int leg = -1;
|
||||
double lower = 0., upper = 0.; ///< 位置限位 [rad]（下发前自检用）
|
||||
double vel_limit = 0.; ///< 速度限位 [rad/s]
|
||||
double dir = 1.; ///< 硬件方向 ±1
|
||||
double offset_deg = 0.; ///< 硬件零点偏置 [deg]
|
||||
};
|
||||
|
||||
/// 一条腿在总线上的位置
|
||||
struct LegSpec {
|
||||
std::string name;
|
||||
std::string swing_joint, wheel_joint;
|
||||
std::string swing_bus, wheel_bus;
|
||||
int swing_canid = 0, wheel_canid = 0;
|
||||
int swing_index = -1, wheel_index = -1;
|
||||
};
|
||||
|
||||
/// 电机量程（MIT 类协议的 p/v/t/kp/kd 上限）
|
||||
struct MotorSpec {
|
||||
std::string model;
|
||||
double p_max = 12.5, p_min = -12.5;
|
||||
double v_max = 0., v_min = 0.;
|
||||
double t_max = 0., t_min = 0.;
|
||||
double kp_max = 0., kd_max = 0.;
|
||||
};
|
||||
|
||||
struct CanSpec {
|
||||
std::vector<std::string> channels;
|
||||
double bitrate = 1000000, data_bitrate = 5000000;
|
||||
int arbitration_id = 0;
|
||||
int enable_frames = 5;
|
||||
double enable_interval_s = 0.02;
|
||||
};
|
||||
|
||||
class HardwareConfig {
|
||||
public:
|
||||
// ---- 关节表与标定 ----
|
||||
std::vector<std::string> joint_names;
|
||||
std::vector<JointSpec> joints; ///< 顺序同 joint_names
|
||||
std::vector<LegSpec> legs;
|
||||
int dof = 0;
|
||||
|
||||
// ---- 电机与总线 ----
|
||||
MotorSpec motor_joint, motor_wheel;
|
||||
CanSpec can;
|
||||
double bench_joint_kp = 150., bench_joint_kd = 2., bench_wheel_kd = 0.5;
|
||||
|
||||
// ---- 本节点话题与频率 ----
|
||||
std::string topic_joint_states = "joint_states", topic_imu = "imu/data";
|
||||
std::string topic_joint_commands = "joint_commands";
|
||||
double publish_rate_hz = 200.0;
|
||||
std::string imu_frame_id = "base_link";
|
||||
int command_queue = 1;
|
||||
|
||||
bool Load(const std::string& path, std::string* error = nullptr);
|
||||
|
||||
int IndexOf(const std::string& name) const {
|
||||
auto it = std::find(joint_names.begin(), joint_names.end(), name);
|
||||
return it == joint_names.end() ? -1 : int(it - joint_names.begin());
|
||||
}
|
||||
const JointSpec& Joint(int i) const { return joints[size_t(i)]; }
|
||||
bool IsWheel(int i) const { return joints[size_t(i)].is_wheel; }
|
||||
|
||||
std::string Summary() const;
|
||||
};
|
||||
|
||||
inline HardwareConfig& Config() {
|
||||
static HardwareConfig cfg;
|
||||
return cfg;
|
||||
}
|
||||
|
||||
namespace detail {
|
||||
|
||||
inline const toml::table* Section(const toml::table& t, const char* key) { return t[key].as_table(); }
|
||||
|
||||
template <typename T>
|
||||
inline void Get(const toml::table* sect, const char* key, T& out) {
|
||||
if (!sect) return;
|
||||
if (auto v = (*sect)[key].value<T>()) out = *v;
|
||||
}
|
||||
inline void GetStr(const toml::table* sect, const char* key, std::string& out) {
|
||||
if (!sect) return;
|
||||
if (auto v = (*sect)[key].value<std::string>()) out = *v;
|
||||
}
|
||||
inline void GetDoubleArr(const toml::table* sect, const char* key, std::vector<double>& out) {
|
||||
if (!sect) return;
|
||||
if (const auto* arr = (*sect)[key].as_array()) {
|
||||
out.clear();
|
||||
for (const auto& e : *arr)
|
||||
if (auto v = e.value<double>()) out.push_back(*v);
|
||||
}
|
||||
}
|
||||
inline void GetStrArr(const toml::table* sect, const char* key, std::vector<std::string>& out) {
|
||||
if (!sect) return;
|
||||
if (const auto* arr = (*sect)[key].as_array()) {
|
||||
out.clear();
|
||||
for (const auto& e : *arr)
|
||||
if (auto v = e.value<std::string>()) out.push_back(*v);
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace detail
|
||||
|
||||
inline bool HardwareConfig::Load(const std::string& path, std::string* error) {
|
||||
const auto fail = [&](const std::string& msg) {
|
||||
if (error) *error = msg;
|
||||
return false;
|
||||
};
|
||||
|
||||
toml::table tbl;
|
||||
try {
|
||||
tbl = toml::parse_file(path);
|
||||
} catch (const toml::parse_error& e) {
|
||||
return fail(std::string("TOML 解析失败: ") + std::string(e.description()) + " @ " + path);
|
||||
}
|
||||
|
||||
const toml::table* robot = detail::Section(tbl, "robot");
|
||||
if (!robot) return fail("缺少 [robot] 段");
|
||||
detail::GetStrArr(robot, "joint_names", joint_names);
|
||||
if (joint_names.empty()) return fail("[robot].joint_names 为空");
|
||||
dof = int(joint_names.size());
|
||||
|
||||
std::vector<double> lower, upper, vel_limit;
|
||||
const toml::table* limits = detail::Section(tbl, "joint_limits");
|
||||
detail::GetDoubleArr(limits, "lower", lower);
|
||||
detail::GetDoubleArr(limits, "upper", upper);
|
||||
detail::GetDoubleArr(limits, "vel_limit", vel_limit);
|
||||
if (int(lower.size()) != dof || int(upper.size()) != dof || int(vel_limit.size()) != dof) {
|
||||
return fail("[joint_limits] 的 lower/upper/vel_limit 长度必须等于关节数量");
|
||||
}
|
||||
joints.assign(size_t(dof), JointSpec{});
|
||||
for (int i = 0; i < dof; ++i) {
|
||||
joints[size_t(i)].name = joint_names[size_t(i)];
|
||||
joints[size_t(i)].lower = lower[size_t(i)];
|
||||
joints[size_t(i)].upper = upper[size_t(i)];
|
||||
joints[size_t(i)].vel_limit = vel_limit[size_t(i)];
|
||||
}
|
||||
|
||||
legs.clear();
|
||||
if (const auto* leg_arr = tbl["legs"].as_array()) {
|
||||
for (const auto& elem : *leg_arr) {
|
||||
const toml::table* lt = elem.as_table();
|
||||
if (!lt) continue;
|
||||
LegSpec leg;
|
||||
detail::GetStr(lt, "name", leg.name);
|
||||
detail::GetStr(lt, "swing_joint", leg.swing_joint);
|
||||
detail::GetStr(lt, "wheel_joint", leg.wheel_joint);
|
||||
leg.swing_index = IndexOf(leg.swing_joint);
|
||||
leg.wheel_index = IndexOf(leg.wheel_joint);
|
||||
if (leg.swing_index < 0 || leg.wheel_index < 0) {
|
||||
return fail("[[legs]] " + leg.name + " 的 swing_joint/wheel_joint 不在 joint_names 里");
|
||||
}
|
||||
const toml::table* sw = detail::Section(*lt, "swing");
|
||||
const toml::table* wh = detail::Section(*lt, "wheel");
|
||||
detail::Get(sw, "dir", joints[size_t(leg.swing_index)].dir);
|
||||
detail::Get(sw, "offset_deg", joints[size_t(leg.swing_index)].offset_deg);
|
||||
detail::GetStr(sw, "bus", leg.swing_bus);
|
||||
detail::Get(sw, "canid", leg.swing_canid);
|
||||
detail::Get(wh, "dir", joints[size_t(leg.wheel_index)].dir);
|
||||
detail::Get(wh, "offset_deg", joints[size_t(leg.wheel_index)].offset_deg);
|
||||
detail::GetStr(wh, "bus", leg.wheel_bus);
|
||||
detail::Get(wh, "canid", leg.wheel_canid);
|
||||
joints[size_t(leg.swing_index)].leg = int(legs.size());
|
||||
joints[size_t(leg.wheel_index)].leg = int(legs.size());
|
||||
joints[size_t(leg.wheel_index)].is_wheel = true;
|
||||
legs.push_back(leg);
|
||||
}
|
||||
}
|
||||
if (legs.empty()) return fail("缺少 [[legs]] 段");
|
||||
|
||||
if (const toml::table* mj = detail::Section(tbl, "motor")) {
|
||||
const auto load_motor = [](const toml::table* m, MotorSpec& out) {
|
||||
detail::GetStr(m, "model", out.model);
|
||||
detail::Get(m, "p_max", out.p_max);
|
||||
detail::Get(m, "p_min", out.p_min);
|
||||
detail::Get(m, "v_max", out.v_max);
|
||||
detail::Get(m, "v_min", out.v_min);
|
||||
detail::Get(m, "t_max", out.t_max);
|
||||
detail::Get(m, "t_min", out.t_min);
|
||||
detail::Get(m, "kp_max", out.kp_max);
|
||||
detail::Get(m, "kd_max", out.kd_max);
|
||||
};
|
||||
load_motor(detail::Section(*mj, "joint"), motor_joint);
|
||||
load_motor(detail::Section(*mj, "wheel"), motor_wheel);
|
||||
|
||||
if (const toml::table* c = detail::Section(*mj, "can")) {
|
||||
detail::GetStrArr(c, "channels", can.channels);
|
||||
detail::Get(c, "bitrate", can.bitrate);
|
||||
detail::Get(c, "data_bitrate", can.data_bitrate);
|
||||
detail::Get(c, "arbitration_id", can.arbitration_id);
|
||||
detail::Get(c, "enable_frames", can.enable_frames);
|
||||
detail::Get(c, "enable_interval_s", can.enable_interval_s);
|
||||
}
|
||||
const toml::table* mg = detail::Section(*mj, "gains");
|
||||
detail::Get(mg, "bench_joint_kp", bench_joint_kp);
|
||||
detail::Get(mg, "bench_joint_kd", bench_joint_kd);
|
||||
detail::Get(mg, "bench_wheel_kd", bench_wheel_kd);
|
||||
}
|
||||
|
||||
const toml::table* topics = detail::Section(tbl, "topics");
|
||||
detail::GetStr(topics, "joint_states", topic_joint_states);
|
||||
detail::GetStr(topics, "imu", topic_imu);
|
||||
detail::GetStr(topics, "joint_commands", topic_joint_commands);
|
||||
|
||||
const toml::table* hw = detail::Section(tbl, "hardware");
|
||||
detail::Get(hw, "publish_rate_hz", publish_rate_hz);
|
||||
detail::GetStr(hw, "imu_frame_id", imu_frame_id);
|
||||
detail::Get(hw, "command_queue", command_queue);
|
||||
if (publish_rate_hz <= 0.) return fail("[hardware].publish_rate_hz 必须 > 0");
|
||||
|
||||
if (error) error->clear();
|
||||
return true;
|
||||
}
|
||||
|
||||
inline std::string HardwareConfig::Summary() const {
|
||||
char buf[512];
|
||||
std::string s;
|
||||
snprintf(buf, sizeof(buf), "[config] %d 关节，%zu 条腿，状态发布 %.0f Hz，IMU frame=%s\n", dof, legs.size(),
|
||||
publish_rate_hz, imu_frame_id.c_str());
|
||||
s += buf;
|
||||
for (const auto& leg : legs) {
|
||||
snprintf(buf, sizeof(buf), "[config] %s: swing %s(%s#%d) wheel %s(%s#%d)\n", leg.name.c_str(),
|
||||
leg.swing_joint.c_str(), leg.swing_bus.c_str(), leg.swing_canid, leg.wheel_joint.c_str(),
|
||||
leg.wheel_bus.c_str(), leg.wheel_canid);
|
||||
s += buf;
|
||||
}
|
||||
snprintf(buf, sizeof(buf), "[config] 电机: 关节 %s(p±%.1f v±%.1f t±%.0f) 轮 %s(p±%.1f v±%.1f t±%.0f)\n",
|
||||
motor_joint.model.c_str(), motor_joint.p_max, motor_joint.v_max, motor_joint.t_max,
|
||||
motor_wheel.model.c_str(), motor_wheel.p_max, motor_wheel.v_max, motor_wheel.t_max);
|
||||
s += buf;
|
||||
snprintf(buf, sizeof(buf), "[config] CAN: %zu 路 %s.. @%.1fM/%.1fM, 使能 %d 帧 x %.3f s\n",
|
||||
can.channels.size(), can.channels.empty() ? "-" : can.channels.front().c_str(), can.bitrate / 1e6,
|
||||
can.data_bitrate / 1e6, can.enable_frames, can.enable_interval_s);
|
||||
s += buf;
|
||||
return s;
|
||||
}
|
||||
|
||||
} // namespace invicta_hardware
|
||||
@@ -1,6 +1,6 @@
|
||||
<launch>
|
||||
<!-- 关节/IMU 数据源 + 关节指令执行端（骨架）。参数在节点的私有命名空间下 -->
|
||||
<!-- 关节/IMU 数据源 + 关节指令执行端（骨架）。参数来自本节点自己的 config/invicta_hardware.toml -->
|
||||
<node pkg="invicta_hardware" type="invicta_hardware_node" name="invicta_hardware" output="screen">
|
||||
<rosparam command="load" file="$(find invicta_hardware)/config/invicta_hardware.yaml"/>
|
||||
<param name="config_file" value="$(find invicta_hardware)/config/invicta_hardware.toml"/>
|
||||
</node>
|
||||
</launch>
|
||||
|
||||
@@ -12,6 +12,7 @@
|
||||
<buildtool_depend>catkin</buildtool_depend>
|
||||
|
||||
<depend>roscpp</depend>
|
||||
<depend>roslib</depend>
|
||||
<depend>sensor_msgs</depend>
|
||||
<depend>geometry_msgs</depend>
|
||||
<depend>invicta_msgs</depend>
|
||||
|
||||
@@ -2,10 +2,12 @@
|
||||
* @file invicta_hardware_node.cpp
|
||||
* @brief 关节/IMU 数据源 + 关节指令执行端（骨架入口）
|
||||
*/
|
||||
#include <ros/package.h>
|
||||
#include <ros/ros.h>
|
||||
|
||||
#include <thread>
|
||||
|
||||
#include "invicta_hardware/hardware_config.h"
|
||||
#include "invicta_hardware/hardware_bridge.h"
|
||||
#include "invicta_msgs/control_word.h"
|
||||
|
||||
@@ -28,18 +30,16 @@ HardwareBridge::HardwareBridge(const ros::NodeHandle& pnh) : nh_() {
|
||||
}
|
||||
|
||||
void HardwareBridge::LoadParams(const ros::NodeHandle& pnh) {
|
||||
pnh.param<std::string>("joint_states_topic", joint_state_topic_, "joint_states");
|
||||
pnh.param<std::string>("imu_topic", imu_topic_, "imu/data");
|
||||
pnh.param<std::string>("joint_commands_topic", command_topic_, "joint_commands");
|
||||
pnh.param<std::string>("imu_frame_id", imu_frame_id_, "base_link");
|
||||
pnh.param<double>("publish_rate_hz", publish_rate_hz_, 200.0);
|
||||
pnh.param<int>("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<std::string>("config_file", config_path, "");
|
||||
if (config_path.empty()) config_path = ros::package::getPath("invicta_hardware") + "/config/invicta_hardware.toml";
|
||||
std::string err;
|
||||
if (!invicta_hardware::Config().Load(config_path, &err)) {
|
||||
ROS_FATAL("[invicta_hardware] 配置加载失败：%s（%s）", err.c_str(), config_path.c_str());
|
||||
return 1;
|
||||
}
|
||||
std::cout << invicta_hardware::Config().Summary() << std::flush;
|
||||
|
||||
invicta_hardware::HardwareBridge bridge(pnh);
|
||||
std::thread loop_thread(&invicta_hardware::HardwareBridge::Run, &bridge);
|
||||
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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 异常状态
|
||||
@@ -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
|
||||
@@ -0,0 +1,114 @@
|
||||
/**
|
||||
* @file remote_config.h
|
||||
* @brief invicta_remote 自己的配置（TOML）：config/invicta_remote.toml
|
||||
*
|
||||
* 只包含本节点需要的参数：UDP 端口与协议开关、本节点话题、状态上报频率。
|
||||
*/
|
||||
#pragma once
|
||||
|
||||
#include <toml++/toml.hpp>
|
||||
|
||||
#include <string>
|
||||
|
||||
namespace invicta_remote {
|
||||
|
||||
class RemoteConfig {
|
||||
public:
|
||||
// ---- 协议 ----
|
||||
int udp_port = 30000;
|
||||
double heartbeat_timeout_s = 3.0;
|
||||
bool unsupported_as_success = false;
|
||||
|
||||
// ---- 本节点话题 ----
|
||||
std::string topic_motion_command = "motion_command";
|
||||
std::string topic_motion_state = "motion_state";
|
||||
std::string topic_joint_states = "joint_states";
|
||||
std::string topic_imu = "imu/data";
|
||||
|
||||
// ---- 状态上报（1002/*）----
|
||||
bool telemetry_enabled = true;
|
||||
bool telemetry_require_subscription = false;
|
||||
double telemetry_basic_hz = 2.0, telemetry_motion_hz = 10.0;
|
||||
double telemetry_device_hz = 2.0, telemetry_error_hz = 2.0;
|
||||
|
||||
bool Load(const std::string& path, std::string* error = nullptr);
|
||||
|
||||
std::string Summary() const;
|
||||
};
|
||||
|
||||
inline RemoteConfig& Config() {
|
||||
static RemoteConfig cfg;
|
||||
return cfg;
|
||||
}
|
||||
|
||||
namespace detail {
|
||||
|
||||
inline const toml::table* Section(const toml::table& t, const char* key) { return t[key].as_table(); }
|
||||
|
||||
template <typename T>
|
||||
inline void Get(const toml::table* sect, const char* key, T& out) {
|
||||
if (!sect) return;
|
||||
if (auto v = (*sect)[key].value<T>()) out = *v;
|
||||
}
|
||||
inline void GetStr(const toml::table* sect, const char* key, std::string& out) {
|
||||
if (!sect) return;
|
||||
if (auto v = (*sect)[key].value<std::string>()) out = *v;
|
||||
}
|
||||
inline void GetBool(const toml::table* sect, const char* key, bool& out) {
|
||||
if (!sect) return;
|
||||
if (auto v = (*sect)[key].value<bool>()) out = *v;
|
||||
}
|
||||
|
||||
} // namespace detail
|
||||
|
||||
inline bool RemoteConfig::Load(const std::string& path, std::string* error) {
|
||||
const auto fail = [&](const std::string& msg) {
|
||||
if (error) *error = msg;
|
||||
return false;
|
||||
};
|
||||
|
||||
toml::table tbl;
|
||||
try {
|
||||
tbl = toml::parse_file(path);
|
||||
} catch (const toml::parse_error& e) {
|
||||
return fail(std::string("TOML 解析失败: ") + std::string(e.description()) + " @ " + path);
|
||||
}
|
||||
|
||||
const toml::table* proto = detail::Section(tbl, "protocol");
|
||||
if (!proto) return fail("缺少 [protocol] 段");
|
||||
detail::Get(proto, "udp_port", udp_port);
|
||||
detail::Get(proto, "heartbeat_timeout_s", heartbeat_timeout_s);
|
||||
detail::GetBool(proto, "unsupported_as_success", unsupported_as_success);
|
||||
if (udp_port <= 0 || udp_port > 65535) return fail("[protocol].udp_port 非法");
|
||||
|
||||
const toml::table* topics = detail::Section(tbl, "topics");
|
||||
detail::GetStr(topics, "motion_command", topic_motion_command);
|
||||
detail::GetStr(topics, "motion_state", topic_motion_state);
|
||||
detail::GetStr(topics, "joint_states", topic_joint_states);
|
||||
detail::GetStr(topics, "imu", topic_imu);
|
||||
|
||||
const toml::table* tlm = detail::Section(tbl, "telemetry");
|
||||
detail::GetBool(tlm, "enabled", telemetry_enabled);
|
||||
detail::GetBool(tlm, "require_subscription", telemetry_require_subscription);
|
||||
detail::Get(tlm, "basic_hz", telemetry_basic_hz);
|
||||
detail::Get(tlm, "motion_hz", telemetry_motion_hz);
|
||||
detail::Get(tlm, "device_hz", telemetry_device_hz);
|
||||
detail::Get(tlm, "error_hz", telemetry_error_hz);
|
||||
|
||||
if (error) error->clear();
|
||||
return true;
|
||||
}
|
||||
|
||||
inline std::string RemoteConfig::Summary() const {
|
||||
char buf[256];
|
||||
snprintf(buf, sizeof(buf), "[config] UDP %d, 心跳超时 %.1f s, 未实现项回 0xE00A=%s\n", udp_port,
|
||||
heartbeat_timeout_s, unsupported_as_success ? "否(回 0)" : "是");
|
||||
std::string s = buf;
|
||||
snprintf(buf, sizeof(buf), "[config] 状态上报: %s 基础 %.0f Hz / 运控 %.0f Hz / 设备 %.0f Hz / 异常 %.0f Hz\n",
|
||||
telemetry_enabled ? "开" : "关", telemetry_basic_hz, telemetry_motion_hz, telemetry_device_hz,
|
||||
telemetry_error_hz);
|
||||
s += buf;
|
||||
return s;
|
||||
}
|
||||
|
||||
} // namespace invicta_remote
|
||||
@@ -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<int>("udp_port", udp_port_, kUdpDefaultPort);
|
||||
pnh_.param<std::string>("topics/motion_command", motion_command_topic_, "motion_command");
|
||||
pnh_.param<std::string>("topics/motion_state", motion_state_topic_, "motion_state");
|
||||
pnh_.param<std::string>("topics/joint_states", joint_states_topic_, "joint_states");
|
||||
pnh_.param<std::string>("topics/imu", imu_topic_, "imu/data");
|
||||
pnh_.param<double>("heartbeat_timeout_s", heartbeat_timeout_s_, 3.0);
|
||||
pnh_.param<bool>("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<bool>("telemetry/enabled", telemetry_enabled_, true);
|
||||
pnh_.param<bool>("telemetry/require_subscription", require_subscription_, false);
|
||||
pnh_.param<double>("telemetry/basic_hz", basic_hz_, 2.0);
|
||||
pnh_.param<double>("telemetry/motion_hz", motion_hz_, 10.0);
|
||||
pnh_.param<double>("telemetry/device_hz", device_hz_, 2.0);
|
||||
pnh_.param<double>("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>(heartbeat_timeout_s_);
|
||||
motion_command_pub_ = nh_.advertise<invicta_msgs::MotionCommand>(motion_command_topic_, 1);
|
||||
@@ -560,6 +562,7 @@ private:
|
||||
}
|
||||
};
|
||||
|
||||
const invicta_remote::RemoteConfig& cfg_;
|
||||
ros::NodeHandle nh_;
|
||||
ros::NodeHandle pnh_;
|
||||
ros::Publisher motion_command_pub_;
|
||||
|
||||
@@ -1,6 +1,6 @@
|
||||
<launch>
|
||||
<!-- 遥控/设备协议节点：UDP + JSON ↔ ROS 话题。参数在节点的私有命名空间下 -->
|
||||
<!-- 遥控/设备协议节点（UDP + JSON）。参数来自本节点自己的 config/invicta_remote.toml -->
|
||||
<node pkg="invicta_remote" type="invicta_remote_node" name="invicta_remote" output="screen">
|
||||
<rosparam command="load" file="$(find invicta_remote)/config/invicta_remote.yaml"/>
|
||||
<param name="config_file" value="$(find invicta_remote)/config/invicta_remote.toml"/>
|
||||
</node>
|
||||
</launch>
|
||||
|
||||
@@ -12,6 +12,7 @@
|
||||
<buildtool_depend>catkin</buildtool_depend>
|
||||
|
||||
<depend>roscpp</depend>
|
||||
<depend>roslib</depend>
|
||||
<depend>sensor_msgs</depend>
|
||||
<depend>invicta_msgs</depend>
|
||||
</package>
|
||||
|
||||
@@ -2,8 +2,11 @@
|
||||
* @file invicta_remote_node.cpp
|
||||
* @brief 遥控/设备协议节点入口（UDP + JSON ↔ ROS 话题）
|
||||
*/
|
||||
#include <ros/package.h>
|
||||
#include <ros/ros.h>
|
||||
|
||||
#include <iostream>
|
||||
|
||||
#include "invicta_remote/remote_node.h"
|
||||
|
||||
int main(int argc, char** argv) {
|
||||
@@ -11,6 +14,16 @@ int main(int argc, char** argv) {
|
||||
ros::NodeHandle nh;
|
||||
ros::NodeHandle pnh("~");
|
||||
|
||||
std::string config_path;
|
||||
pnh.param<std::string>("config_file", config_path, "");
|
||||
if (config_path.empty()) config_path = ros::package::getPath("invicta_remote") + "/config/invicta_remote.toml";
|
||||
std::string err;
|
||||
if (!invicta_remote::Config().Load(config_path, &err)) {
|
||||
ROS_FATAL("[invicta_remote] 配置加载失败：%s（%s）", err.c_str(), config_path.c_str());
|
||||
return 1;
|
||||
}
|
||||
std::cout << invicta_remote::Config().Summary() << std::flush;
|
||||
|
||||
invicta_remote::RemoteNode node(pnh);
|
||||
node.Start();
|
||||
|
||||
|
||||
Reference in New Issue
Block a user