feat(hardware): 标定归硬件；按 name 选关节 + kSetZero；toml++ 改 vendored
标定边界上移到 invicta_hardware： - raw↔model 的 dir/offset 由 hardware 独家施加（话题收发都是模型帧）， deploy 的 InvictaInterface 退化为透传，invicta_deploy.toml 不再有 dir/offset_deg。 这样 [joint_limits]（模型帧）与被限幅的指令同帧，/joint_states 也是模型角。 关节选择（部分接入 / 单腿调试）： - JointCommandArray 的 name 即本次要控制的关节子集（空 = 全部已配置关节）。 - 未选中的关节写「零力矩自由」块，而不是留全 0——按协议 (0,0,0,0,0) 会解码成 P_MIN/V_MIN/T_MIN，其中 tau=T_MIN（摆臂 -100 N·m）是危险的前馈力矩。 - 新增 control_word kSetZero(6) -> 0xEF，只作用于选中的摆臂（轮毂不支持，跳过）。 调试脚本 invicta_hardware/scripts/hw_debug_test.py： - 只起 hardware 节点调试：monitor/damping/angle/wheel/offset/zero 六种模式 + --joints。 - 限幅参数硬编码；发指令时默认回显模型角。 构建：toml++ 改为 vendored single-header（三个包各 third_party/tomlplusplus/toml++/toml.hpp）， 去掉 find_package(tomlplusplus) 与 -DTOML_HEADER_ONLY（单头自带 header-only）。 已在容器（localhost/custom-ros-noetic）中全新 catkin_make + smoke_test 验证通过。
This commit is contained in:
@@ -31,12 +31,9 @@ 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)
|
||||
# TOML 解析库：single-header，vendored 在 third_party/tomlplusplus/toml++/toml.hpp
|
||||
# 单头版本本身就是 header-only（自带 TOML_HEADER_ONLY），实现直接编进本节点：无 find_package、无运行时库依赖
|
||||
include_directories(${CMAKE_CURRENT_SOURCE_DIR}/third_party/tomlplusplus)
|
||||
|
||||
set(THIRD_PARTY ${CMAKE_CURRENT_SOURCE_DIR}/third_party)
|
||||
set(ORT_ROOT ${THIRD_PARTY}/onnxruntime/${BUILD_PLATFORM})
|
||||
|
||||
@@ -110,7 +110,7 @@ gait:
|
||||
| `[robot]` | 关节名与顺序（8 个）、机型标识 |
|
||||
| `[joint_limits]` | 逐关节位置/速度限位（顺序同 joint_names） |
|
||||
| `[leg]` `[leg.timing]` `[leg.gains]` | 2 自由度腿的几何（摆臂长度、站/收腿/趴下高度）、时序、各状态 PD 增益 |
|
||||
| `[[legs]]` ×4 | 每腿的摆臂/轮关节名、`splay_sign`（倒向）、`dir`/`offset_deg`（硬件标定） |
|
||||
| `[[legs]]` ×4 | 每腿的摆臂/轮关节名、`splay_sign`（模型倒向）。硬件标定 `dir`/`offset_deg` 在 invicta_hardware |
|
||||
| `[rl]` | 站立完成后自动进 RL 的开关与自检阈值 |
|
||||
| `[policy]` | 模型路径、decimation、观测缩放、关节顺序、动作尺度、默认姿态、kp/kd |
|
||||
| `[gait.policies]` | 步态 → 模型文件（空 = 该步态不可用） |
|
||||
|
||||
@@ -62,42 +62,34 @@ velocity_tolerance = 1.0 # 摆臂角速度容差 [rad/s]
|
||||
require_imu = true # 是否检查 IMU 数据有效
|
||||
|
||||
# ------------------------------------------------------------------ 每腿标定
|
||||
# 摆臂指令角 = dir · θ_model + offset_deg，θ_model = splay_sign · acos(h/L)
|
||||
# 模型约定：摆臂角 θ_model = splay_sign · acos(h/L)。
|
||||
# splay_sign：模型姿态约定。趴下/收腿时摆臂往哪边倒——本题要求「前腿向前、后腿向后」，
|
||||
# 即前腿 +1、后腿 −1（与 M20 相反）
|
||||
# dir ：硬件约定，电机正方向与模型正方向的关系（±1）
|
||||
# offset_deg：机械零点偏置 [deg]，第一次上机按 idle 打印角与实测对照后填写
|
||||
# 硬件标定（电机方向 dir、机械零点 offset_deg）**不在本文件**：raw↔model 的换算由
|
||||
# invicta_hardware 独家承担（见该节点 config 与 README「标定边界」），本节点只处理模型量。
|
||||
[[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]
|
||||
|
||||
@@ -18,15 +18,13 @@
|
||||
|
||||
namespace invicta {
|
||||
|
||||
/// 单个关节：限位与硬件标定
|
||||
/// 单个关节：限位（模型帧）。硬件标定 dir/offset 在 invicta_hardware，见该节点 README
|
||||
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]
|
||||
};
|
||||
|
||||
/// 一条腿：摆臂 + 轮
|
||||
@@ -208,12 +206,6 @@ inline bool DeployConfig::Load(const std::string& path, std::string* error) {
|
||||
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;
|
||||
|
||||
@@ -1,15 +1,14 @@
|
||||
/**
|
||||
* @file invicta_interface.hpp
|
||||
* @brief ROS1 数据面：订阅关节状态与 IMU，发布关节指令。关节表、话题、标定全部来自 invicta.toml
|
||||
* @brief ROS1 数据面：订阅关节状态与 IMU，发布关节指令。关节表、话题来自 invicta_deploy.toml
|
||||
*
|
||||
* 硬件约定（方向 dir、零点 offset、单位）在本层一次性归一：
|
||||
* 反馈：q_model = q_robot · dir + offset
|
||||
* 指令：q_robot = (q_des − offset) · dir
|
||||
* 上层的状态机与策略只看到「模型约定」下的量（rad / rad/s / Nm）。
|
||||
* 本层只做**模型帧**的透传：/joint_states 与 /joint_commands 上都是模型量（rad / rad/s / Nm）。
|
||||
* 硬件的方向 `dir` 与零点 `offset` 由 invicta_hardware 在 raw↔model 的唯一边界处施加
|
||||
* （见 invicta_hardware/README.md「标定边界」），deploy 不再参与标定。
|
||||
* 上层状态机与策略只看到模型约定下的量。
|
||||
*
|
||||
* 相比之前：删掉了 auto_calibrate / ±360° 回绕标定（那是 M20 三关节腿的绝对编码器问题，
|
||||
* 本构型摆臂行程只有 ±80°，无回绕；标定统一由 invicta.toml 的 dir/offset 表达），
|
||||
* 轮关节判定也从 i%4==3 改为按配置的 is_wheel。
|
||||
* 本构型摆臂行程只有 ±80°，无回绕），也删掉了本地 dir/offset 换算（已上移到 hardware）。
|
||||
*/
|
||||
#pragma once
|
||||
|
||||
@@ -59,12 +58,7 @@ public:
|
||||
SendStartupSequence();
|
||||
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;
|
||||
std::cout << "[InvictaInterface] 模型帧透传：dir/offset 由 invicta_hardware 施加" << std::endl;
|
||||
}
|
||||
|
||||
virtual void Stop() {}
|
||||
@@ -93,9 +87,10 @@ public:
|
||||
msg.kd.resize(size_t(dof_num_));
|
||||
for (int i = 0; i < dof_num_; ++i) {
|
||||
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;
|
||||
// 模型帧透传：不做 dir/offset 换算（帽子在 invicta_hardware）
|
||||
msg.position[i] = input(i, 1);
|
||||
msg.velocity[i] = input(i, 3);
|
||||
msg.effort[i] = input(i, 4);
|
||||
msg.kp[i] = (cfg_.wheel_kp_zero && j.is_wheel) ? 0.f : input(i, 0);
|
||||
msg.kd[i] = input(i, 2);
|
||||
}
|
||||
@@ -157,10 +152,10 @@ protected:
|
||||
++unknown;
|
||||
continue;
|
||||
}
|
||||
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;
|
||||
// 模型帧透传：不做 dir/offset 换算（帽子在 invicta_hardware）
|
||||
joint_pos_(i) = float(msg->position[k]);
|
||||
joint_vel_(i) = float(msg->velocity[k]);
|
||||
joint_tau_(i) = float(msg->effort[k]);
|
||||
}
|
||||
if (!by_name && !warned_name_mismatch_) {
|
||||
warned_name_mismatch_ = true;
|
||||
|
||||
@@ -3,7 +3,7 @@
|
||||
* @brief 待机：按 invicta.toml 的逐关节限位检查本体数据，等指令
|
||||
*
|
||||
* 相比 M20：不再有「每腿 4 关节 / 下标 %4」的假设，限位改成逐关节查表；
|
||||
* 左右/前后的差异已经在配置的标定（dir/offset）里归一。
|
||||
* 左右/前后的镜像已经在 invicta_hardware 的标定（dir/offset）里归一。
|
||||
*/
|
||||
#pragma once
|
||||
|
||||
|
||||
@@ -7,7 +7,7 @@
|
||||
* 摆臂倒向由 splay_sign 决定：本题要求趴下/收腿时「前腿向前、后腿向后」（与 M20 相反）。
|
||||
*
|
||||
* 与上游 M20 的差别：不再有「每腿 4 关节、i%4==3 是轮、外摆钉在 0、后腿髋膝取反」这些假设，
|
||||
* 左右/前后的镜像已经在配置的标定（dir/offset）里归一。
|
||||
* 左右/前后的镜像已经在 invicta_hardware 的标定（dir/offset）里归一。
|
||||
*/
|
||||
#pragma once
|
||||
|
||||
|
||||
@@ -0,0 +1,13 @@
|
||||
# toml++ (vendored)
|
||||
|
||||
这三个节点各自用到的 TOML 解析库，以 **single-header / header-only** 方式编进节点
|
||||
（无 `find_package`，目标机也不需要额外 `.so`）。
|
||||
|
||||
- 上游：https://github.com/marzer/tomlplusplus
|
||||
- 版本：v3.4.0（单头文件 `toml.hpp`）
|
||||
- 取用：`toml.hpp`（amalgamated single header）。单头版本本身就是 header-only——
|
||||
「drop it in, there is no step two」——文件内部在未定义时自动 `#define TOML_HEADER_ONLY 1`，
|
||||
**不需要任何编译宏**。放在 `toml++/toml.hpp` 是为了匹配上游 `#include <toml++/toml.hpp>`
|
||||
的路径，源码无需改动。
|
||||
|
||||
升级：下载新的 `toml.hpp` 覆盖本目录下 `toml++/toml.hpp` 即可（三个包各一份，保持同版本）。
|
||||
+17897
File diff suppressed because it is too large
Load Diff
@@ -21,12 +21,9 @@ catkin_package(
|
||||
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)
|
||||
# TOML 解析库：single-header，vendored 在 third_party/tomlplusplus/toml++/toml.hpp
|
||||
# 单头版本本身就是 header-only（自带 TOML_HEADER_ONLY），实现直接编进本节点：无 find_package、无运行时库依赖
|
||||
include_directories(${CMAKE_CURRENT_SOURCE_DIR}/third_party/tomlplusplus)
|
||||
|
||||
include_directories(
|
||||
include
|
||||
|
||||
@@ -34,6 +34,20 @@
|
||||
| `kMotorControl`(3) | 发运行块；**未使能时拒发**（手册 §9.1 要求先使能再运行） |
|
||||
| `kErrorReset`(4) | 本协议没有独立清错指令（手册 §6：故障后靠重新使能/重启），按"重发使能"处理 |
|
||||
| `kGetStatus`(5) | 不需要发帧：状态随每条指令自动返回 |
|
||||
| `kSetZero`(6) | `0xFF×9 + 0xEF`：把当前机械位置写成模组零位。**只作用于指令 `name` 选中的关节**，且只发摆臂（轮毂不支持，手册 §4.4）。之后该位姿反馈角≈0，请把对应 `offset_deg` 置 0 |
|
||||
|
||||
### 关节选择（部分接入 / 单腿调试）
|
||||
|
||||
`JointCommandArray` 的 **`name` 列表就是本次要控制的关节子集**：
|
||||
|
||||
- `name` 为空 → 全部已配置关节（deploy 一直填全部，行为不变）。
|
||||
- `name` 是子集 → **只驱动/置零这些关节**；不在子集里的关节会被写入一个**零力矩「自由」块**（`kp=kd=0`、`tau=0`），既不驱动也不保持，也不参与置零。
|
||||
- ⚠ 这里**不能**简单地把未选中的块留成全 0：按协议 `(0,0,0,0,0)` 会解码成 `P_MIN/V_MIN/T_MIN`，其中 `tau=T_MIN`（摆臂 −100 N·m）是危险的前馈力矩。所以显式写零力矩块。
|
||||
- 数组长度（`position`/`velocity`/`effort`/`kp`/`kd`）**仍必须是全部关节数**，只有 `name` 决定选择范围。
|
||||
- `kEnable`/`kDisable`/`kErrorReset` 仍作用于全部已配置关节（总线级动作）；只有运行帧与 `kSetZero` 按 `name` 选择。
|
||||
- 控制字只在**变化**时落地：要对另一个子集重新置零，先把控制字切到别的值（如 `kGetStatus`）再切回 `kSetZero`。
|
||||
|
||||
这样台架上只接一条腿也能单独控制、单独置零，不必把电机全接上。
|
||||
|
||||
同一路总线上最多 4 个关节（CANID 1~4），合成一帧下发：Byte0-1 是 MSGID `0x0100`，每个 CANID 占
|
||||
10 字节，块起点 `2 + 10×(CANID−1)`，长度按手册裁剪表（本机 4 个 ID → 48 字节）。
|
||||
@@ -44,7 +58,9 @@
|
||||
- **边界和限幅是两件事**：`[motor.*]` 的 p/v/t 是**归一化边界**——只决定 CAN 帧里 `0~65535` 映射到多少物理量（编码/解码必须用同一套，手册 §5/§9.6），不是运行限幅；`[joint_limits]` 才是唯一实际生效的**限幅**。下发顺序是「先按限幅夹紧 → 再归一化编码」。加载时会校验限幅不超过边界，超了直接报错，不靠编码饱和兜着。
|
||||
- **kp/kd 不是策略算的**，来自 deploy 的 `[policy].kp/kd` 与 `[leg.gains]`（用户配置）。越界时本节点会夹到 `[0, kp_max]` / `[0, kd_max]` 并打警告——那种日志说明配置写错了，不要靠饱和兜着。
|
||||
- 前馈力矩按 `[joint_limits].torque_limit`（默认取厂家峰值扭矩）夹紧，超过也告警。
|
||||
- **本节点收发的都是原始电机量**（电机坐标系/电机单位），`dir`/`offset_deg` 由 `invicta_deploy` 的 `InvictaInterface` 施加。如果改成由本节点施加，必须同时把 deploy 的 `[legs].dir/offset` 置为恒等——否则 `dir=±1` 会二次相乘抵消、`offset_deg` 会翻倍。
|
||||
- **本节点是 raw↔model 的唯一边界**：话题上收发的都是**模型帧**（模型坐标、rad）。
|
||||
`[[legs]]` 的 `dir`/`offset_deg` 由本节点施加（`q_model = q_motor·dir + offset`，反向 `q_motor = (q_model − offset)·dir`；速度/力矩只乘 `dir`，不含 offset）。
|
||||
`invicta_deploy` 侧不再做任何标定，只处理模型量。这样 `[joint_limits]`（模型帧限幅）与被限幅的指令同帧，不会错位；任何看 `/joint_states` 的消费者拿到的也是有物理意义的角度。
|
||||
- IMU：姿态四元数（**弧度**）、角速度 `[rad/s]`（机体系）、线加速度 `[m/s²]`（机体系）。⚠️ 目前**还没有 IMU 数据源**，本节点发的是单位四元数，等硬件接上再改。
|
||||
- 温度取自反馈帧 Byte8：`T = raw − 40`（0~255 ↔ −40~215 ℃）；电压 Byte10 直接是伏。
|
||||
- 状态码 Byte9：bit0 使能，bit1~6 故障（过压/过流/过温/过速/编码器/欠压）。故障位出现时本节点会打 ERROR 日志，并原样放进 `/joint_diagnostics`。
|
||||
@@ -74,8 +90,8 @@ sudo ip link set can1 up type can bitrate 1000000 dbitrate 5000000 fd on
|
||||
| TOML 段 | 内容 |
|
||||
|---|---|
|
||||
| `[robot].joint_names` | 关节名与顺序（8 个：`fl/fr/hl/hr × swing/wheel`） |
|
||||
| `[joint_limits]` | 人为限幅：位置 `[rad]`、速度 `[rad/s]`、前馈力矩 `[N·m]`（下发前夹紧） |
|
||||
| `[[legs]]` ×4 | 每腿 `swing`/`wheel` 的 `bus`、`canid`（`dir`/`offset_deg` 目前由 deploy 施加） |
|
||||
| `[joint_limits]` | 人为限幅（模型帧）：位置 `[rad]`、速度 `[rad/s]`、前馈力矩 `[N·m]`（下发前夹紧） |
|
||||
| `[[legs]]` ×4 | 每腿 `swing`/`wheel` 的 `dir`、`offset_deg`（硬件标定，本节点施加）、`bus`、`canid` |
|
||||
| `[motor.joint]` / `[motor.wheel]` | 归一化边界（0~65535 ↔ 物理量），编码/解码必须一致（P65MAX-S / W190-S） |
|
||||
| `[motor.can]` | 总线名、仲裁 ID、1M/5M、上电使能帧数与间隔 |
|
||||
| `[motor.gains]` | `bench_*` 仅作参考（实际 kp/kd 由指令给）；`timeout_*_kd` 是看门狗阻尼 |
|
||||
|
||||
@@ -15,6 +15,8 @@ joint_names = [
|
||||
|
||||
# ------------------------------------------------------------------ 关节限位（唯一实际生效的限幅）
|
||||
# 顺序与 joint_names 一致。这里才是「限幅」：机械/使用上的可用范围，下发前按它夹紧。
|
||||
# 限幅与话题上的指令/反馈同为**模型帧**（本节点是 raw↔model 的唯一边界，见 [[legs]]），
|
||||
# 所以夹紧窗口天然自洽（不会出现「拿模型帧限幅去夹电机帧指令」）。
|
||||
# 与 [motor.*] 的关系：那里是「归一化边界」，只决定 0~65535 映射到多少物理量，不是限幅。
|
||||
# 加载时会校验本节限幅不超过归一化边界；超了直接报错，不靠编码饱和悄悄压回去。
|
||||
# 轮关节：轮毂不支持位置控制（手册 §4.4），lower/upper 是占位值，只用 vel_limit；
|
||||
@@ -28,9 +30,12 @@ vel_limit = [ 22.40, 100.0, 22.40, 100.0, 22.40, 100.0, 22.40, 100.0] #
|
||||
torque_limit = [75.0, 28.68, 75.0, 28.68, 75.0, 28.68, 75.0, 28.68]
|
||||
|
||||
# ------------------------------------------------------------------ 每腿的硬件标定与总线
|
||||
# 指令换算：q_motor = (q_model − offset_deg) · dir；反馈反向。
|
||||
# 本节点是 raw↔model 的唯一边界，收到/发出的都是模型帧；本节的 dir/offset_deg 由本节点施加：
|
||||
# 指令换算：q_motor = (q_model − offset_deg) · dir；反馈反向 q_model = q_motor · dir + offset_deg。
|
||||
# velocity/torque 只乘 dir（不含 offset）。
|
||||
# dir ：电机正方向与模型正方向的关系（±1，左右镜像、前后镜像都在这里）
|
||||
# offset_deg：机械零点偏置 [deg]（模型零位 = 摆臂竖直向下时读到的电机角）
|
||||
# offset_deg：机械零点偏置 [deg]（模型零位 = 摆臂竖直向下时读到的电机角）；
|
||||
# 用调试脚本的置零（0xEF，control_word=kSetZero）把当前位姿写成零位后可置 0
|
||||
# bus/canid ：该关节挂在哪条 CANFD 总线、CANID 是多少
|
||||
# 注意：TOML 里数组表之后的裸键会归到最后一个元素，本节必须放在 motor 段之前
|
||||
[[legs]]
|
||||
|
||||
@@ -11,11 +11,17 @@
|
||||
* 反馈不需要主动查询：模组对每条指令都回一帧状态（手册 §3.2），所以本节点
|
||||
* 「发了指令就有反馈」，读回与写入天然同频。
|
||||
*
|
||||
* 与 invicta_deploy 的分工（重要）：
|
||||
* 本节点在话题上收发的都是**原始电机量**（电机坐标系、电机单位），
|
||||
* dir / offset 由 invicta_deploy 的 InvictaInterface 施加（它已经这么做了）。
|
||||
* 如果改成由本节点施加，必须同时把 deploy 的 [legs].dir/offset 置为恒等，
|
||||
* 否则 dir=±1 会二次相乘抵消、offset_deg 会翻倍。
|
||||
* 标定边界（重要）：
|
||||
* 本节点是 raw ↔ model 的**唯一**换算边界。话题上收发的都是**模型帧**（模型坐标、rad）：
|
||||
* 反馈：q_model = q_motor·dir + offset
|
||||
* 指令：q_motor = (q_model − offset)·dir
|
||||
* dir/offset 取自本节点自己的 config/invicta_hardware.toml（[[legs]] 的 swing/wheel）。
|
||||
* invicta_deploy 侧不再做任何标定，只处理模型量（其 toml 不再有 dir/offset_deg）。
|
||||
*
|
||||
* 关节选择（部分接入/单腿调试）：
|
||||
* 指令 JointCommandArray 的 name 列表即本次要控制的关节子集（空 = 全部已配置关节）。
|
||||
* 不在子集里的关节**不写指令块**（其块保持 0 → 电机自由），不会被驱动也不参与置零。
|
||||
* kSetZero 同样只作用于子集里的摆臂。数组长度仍必须是全部关节数（position/… 各 8 个）。
|
||||
*/
|
||||
#pragma once
|
||||
|
||||
@@ -68,11 +74,18 @@ private:
|
||||
void PublishState();
|
||||
void PublishDiagnostics();
|
||||
|
||||
/// 控制字变化：使能/失能按语义落成 CAN 帧
|
||||
void HandleControlWord(uint16_t word);
|
||||
/// 控制字变化：使能/失能按语义落成 CAN 帧；include 是本次指令选中的关节（见 WriteHardware）
|
||||
void HandleControlWord(uint16_t word, const std::vector<uint8_t>* include);
|
||||
|
||||
/// 向所有总线各发若干帧「9×0xFF + code」指令块
|
||||
void SendCodeAll(uint8_t code, int frames = 1, double interval_s = 0.0);
|
||||
/**
|
||||
* 向所有总线各发若干帧「9×0xFF + code」指令块。
|
||||
* include 为空指针 = 所有已配置关节；否则只有 include[joint] != 0 的关节才写入指令块。
|
||||
*/
|
||||
void SendCodeAll(uint8_t code, int frames = 1, double interval_s = 0.0,
|
||||
const std::vector<uint8_t>* include = nullptr);
|
||||
|
||||
/// 由指令里的 name 列表算出关节选择掩码（长度 = 关节数；空列表 = 全选）
|
||||
std::vector<uint8_t> SelectionMask(const std::vector<std::string>& names, bool* had_unknown) const;
|
||||
|
||||
/// 指令超时：kp=0、速度 0、只留阻尼，防止上游挂掉后一直重发最后一帧
|
||||
bool SendTimeoutFrame();
|
||||
@@ -122,7 +135,7 @@ private:
|
||||
std::vector<BusRuntime> buses_;
|
||||
bool ready_ = false;
|
||||
|
||||
// ---- 状态缓存（原始电机量）----
|
||||
// ---- 状态缓存（模型帧：已施加 dir/offset）----
|
||||
std::vector<double> joint_position_, joint_velocity_, joint_effort_;
|
||||
std::vector<double> temperature_, voltage_;
|
||||
std::vector<uint8_t> driver_status_, last_status_;
|
||||
@@ -133,6 +146,7 @@ private:
|
||||
|
||||
std::atomic<bool> running_{false};
|
||||
bool warned_no_command_ = false;
|
||||
bool warned_unknown_joint_ = false; ///< 指令 name 里有不认识的关节名，只警告一次
|
||||
};
|
||||
|
||||
} // namespace invicta_hardware
|
||||
|
||||
@@ -21,11 +21,18 @@ 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 torque_limit = 0.; ///< 前馈力矩限幅 [N·m]（0 = 不额外限制）
|
||||
double dir = 1.; ///< 硬件方向 ±1
|
||||
double offset_deg = 0.; ///< 硬件零点偏置 [deg]
|
||||
double lower = 0., upper = 0.; ///< 位置限位 [rad]（模型帧；下发前自检用）
|
||||
double vel_limit = 0.; ///< 速度限位 [rad/s]（模型帧，幅值）
|
||||
double torque_limit = 0.; ///< 前馈力矩限幅 [N·m]（模型帧，幅值；0 = 不额外限制）
|
||||
double dir = 1.; ///< 硬件方向 ±1（电机正方向 vs 模型正方向）
|
||||
double offset_deg = 0.; ///< 硬件零点偏置 [deg]（模型零位对应的电机角）
|
||||
|
||||
/// 模型帧 → 电机帧：q_motor = (q_model − offset)·dir
|
||||
double ModelToMotorPos(double q_model) const {
|
||||
return (q_model - offset_deg * M_PI / 180.0) * dir;
|
||||
}
|
||||
/// 电机帧 → 模型帧：q_model = q_motor·dir + offset
|
||||
double MotorToModelPos(double q_motor) const { return q_motor * dir + offset_deg * M_PI / 180.0; }
|
||||
};
|
||||
|
||||
/// 一条腿在总线上的位置
|
||||
|
||||
+503
@@ -0,0 +1,503 @@
|
||||
#!/usr/bin/env python3
|
||||
# -*- coding: utf-8 -*-
|
||||
"""
|
||||
invicta_hardware 单节点调试脚本
|
||||
|
||||
只启动 hardware 节点（可 --start-node 代劳），直接往 /joint_commands 发
|
||||
JointCommandArray，用来在台架上单独验证：
|
||||
|
||||
damping 阻尼模式：kp=0、kd 取阻尼值、速度给 0，只抵抗外力不保持位置
|
||||
angle 摆臂角度：把 4 条摆臂打到指定角度（按硬编码限幅夹紧），轮子给 0 速
|
||||
wheel 轮组速度：给 4 个轮子指定转速（按硬编码上限夹紧），摆臂阻尼
|
||||
offset 标定核对：把摆臂打到模型 0 位（默认，可用 --swing 指定对照角），
|
||||
看回显模型角是否等于指令——相等即说明 hardware 的 dir/offset 自洽
|
||||
zero 置零：把当前机械位置写成模组零位（0xEF），仅摆臂（轮毂不支持）
|
||||
monitor 只监听，不发任何指令
|
||||
|
||||
角度回显：发指令时默认每 ~0.2 s 打印一次各关节模型角（rad/deg）。阻尼模式下也照常打印，
|
||||
可以边看角度边手动掰腿。
|
||||
|
||||
⚠ 话题上收发都是**模型帧**：raw↔model 的 dir/offset 换算由 hardware 节点独家承担
|
||||
（见 invicta_hardware/README.md「标定边界」），本脚本不再自己做换算。
|
||||
|
||||
限幅等参数全部硬编码在本文件，不读任何 toml。
|
||||
|
||||
用法（需先 roscore）：
|
||||
# 另开终端先起节点：
|
||||
rosrun invicta_hardware invicta_hardware_node # 有 CAN
|
||||
rosrun invicta_hardware invicta_hardware_node _require_can:=false # 无 CAN 干跑
|
||||
# 或让脚本代起（--require-can / --no-can）：
|
||||
python3 hw_debug_test.py --start-node --no-can --mode damping
|
||||
python3 hw_debug_test.py --start-node --mode damping
|
||||
python3 hw_debug_test.py --mode angle --swing 0.5 --swing-vel 0.5
|
||||
python3 hw_debug_test.py --mode wheel --wheel 3.0
|
||||
python3 hw_debug_test.py --mode offset --swing 0.5
|
||||
python3 hw_debug_test.py --mode zero
|
||||
# 只接了一条腿时：只控制/置零这条腿的关节
|
||||
python3 hw_debug_test.py --mode damping --joints fl_swing,fl_wheel
|
||||
python3 hw_debug_test.py --mode zero --joints fl_swing
|
||||
python3 hw_debug_test.py --mode monitor
|
||||
python3 hw_debug_test.py --mode damping --duration 10
|
||||
"""
|
||||
|
||||
from __future__ import print_function
|
||||
|
||||
import argparse
|
||||
import atexit
|
||||
import math
|
||||
import signal
|
||||
import subprocess
|
||||
import sys
|
||||
import time
|
||||
|
||||
import rospy
|
||||
from sensor_msgs.msg import JointState
|
||||
from std_msgs.msg import Header
|
||||
|
||||
try:
|
||||
from invicta_msgs.msg import JointCommandArray, JointDiagnostics
|
||||
except ImportError as exc: # pragma: no cover - 环境没 source devel 时给出人话提示
|
||||
sys.stderr.write(
|
||||
"无法 import invicta_msgs：%s\n"
|
||||
"请先 source /opt/ros/noetic/setup.bash 和本工作空间的 devel/setup.bash，并确保已 catkin_make。\n" % exc
|
||||
)
|
||||
sys.exit(1)
|
||||
|
||||
# 控制字取值，见 invicta_msgs/control_word.h
|
||||
K_DISABLE = 1
|
||||
K_ENABLE = 2
|
||||
K_MOTOR_CONTROL = 3
|
||||
K_ERROR_RESET = 4
|
||||
K_GET_STATUS = 5
|
||||
K_SET_ZERO = 6
|
||||
|
||||
# =============================================================================
|
||||
# 硬编码参数（不读 toml）
|
||||
# =============================================================================
|
||||
JOINT_NAMES = [
|
||||
"fl_swing", "fl_wheel",
|
||||
"fr_swing", "fr_wheel",
|
||||
"hl_swing", "hl_wheel",
|
||||
"hr_swing", "hr_wheel",
|
||||
]
|
||||
SWING_SLOTS = [0, 2, 4, 6]
|
||||
WHEEL_SLOTS = [1, 3, 5, 7]
|
||||
|
||||
# 标定边界：raw↔model 的 dir/offset 换算**由 hardware 节点独家承担**，
|
||||
# 所以本脚本在 /joint_commands 与 /joint_states 上看到的都是**模型帧**，不再自己换算。
|
||||
# 核对标定 = 发一个模型角 → 看回显模型角是否等于它（angle/offset 模式）。
|
||||
|
||||
# 限幅（hardware 节点里 [joint_limits] 的值，这里同步一份）
|
||||
SWING_ANGLE_MIN = -1.40 # [rad]
|
||||
SWING_ANGLE_MAX = 1.40 # [rad]
|
||||
SWING_VEL_LIMIT = 22.4 # [rad/s]
|
||||
SWING_TAU_LIMIT = 75.0 # [N·m]
|
||||
WHEEL_VEL_LIMIT = 100.0 # [rad/s] 轮组工作上限（额定 81.2 的余量）
|
||||
WHEEL_TAU_LIMIT = 28.68 # [N·m]
|
||||
KP_MAX = 500.0
|
||||
KD_MAX = 5.0
|
||||
|
||||
# 默认增益
|
||||
DEFAULT_KP = 150.0 # 摆臂位置环（台架值）
|
||||
DEFAULT_KD = 2.0 # 摆臂速度环
|
||||
DEFAULT_DAMP_JOINT_KD = 4.0 # 阻尼模式摆臂 kd
|
||||
DEFAULT_DAMP_WHEEL_KD = 1.0 # 阻尼模式轮组 kd
|
||||
|
||||
TOPIC_JOINT_STATES = "/joint_states"
|
||||
TOPIC_JOINT_COMMANDS = "/joint_commands"
|
||||
TOPIC_DIAGNOSTICS = "/joint_diagnostics"
|
||||
|
||||
DEFAULT_PUB_HZ = 200.0
|
||||
DEFAULT_SHOW_HZ = 5.0
|
||||
|
||||
|
||||
# =============================================================================
|
||||
# 工具
|
||||
# =============================================================================
|
||||
def clamp(v, lo, hi, what, warns):
|
||||
if v > hi:
|
||||
warns.append("%s=%.3f 超上限 %.3f，已夹紧" % (what, v, hi))
|
||||
return hi
|
||||
if v < lo:
|
||||
warns.append("%s=%.3f 超下限 %.3f，已夹紧" % (what, v, lo))
|
||||
return lo
|
||||
return v
|
||||
|
||||
|
||||
def parse_list(text, n, default):
|
||||
"""把 '0.5' 或 '0.5,0.6,0.7,0.8' 解析成 n 个浮点。"""
|
||||
if text is None:
|
||||
return [float(default)] * n
|
||||
parts = [p.strip() for p in str(text).split(",") if p.strip() != ""]
|
||||
if len(parts) == 1:
|
||||
return [float(parts[0])] * n
|
||||
if len(parts) != n:
|
||||
raise ValueError("需要 1 个或 %d 个值，收到 %d 个：%s" % (n, len(parts), text))
|
||||
return [float(p) for p in parts]
|
||||
|
||||
|
||||
# =============================================================================
|
||||
# 主体
|
||||
# =============================================================================
|
||||
class HardwareDebug(object):
|
||||
def __init__(self, args):
|
||||
self.args = args
|
||||
self.n = len(JOINT_NAMES)
|
||||
self.name2slot = {nm: i for i, nm in enumerate(JOINT_NAMES)}
|
||||
|
||||
self.state_pos = [None] * self.n # 模型角（hardware 已换算）
|
||||
self.state_vel = [None] * self.n
|
||||
self.state_stamp = 0.0
|
||||
self.state_ok = False
|
||||
self.diag_text = {}
|
||||
|
||||
# 本次要控制的关节子集（指令 name 列表）。空 = 全部 8 个。
|
||||
self.selected_names = list(args.joints) if args.joints else list(JOINT_NAMES)
|
||||
self.selected_mask = [nm in self.selected_names for nm in JOINT_NAMES]
|
||||
|
||||
self.enabled = False
|
||||
self.stop_requested = False
|
||||
|
||||
self.pub = rospy.Publisher(TOPIC_JOINT_COMMANDS, JointCommandArray, queue_size=1)
|
||||
rospy.Subscriber(TOPIC_JOINT_STATES, JointState, self._on_state, queue_size=1)
|
||||
rospy.Subscriber(TOPIC_DIAGNOSTICS, JointDiagnostics, self._on_diag, queue_size=1)
|
||||
|
||||
# ------------------------------------------------------------ 回调
|
||||
def _on_state(self, msg):
|
||||
get = {}
|
||||
for k, nm in enumerate(msg.name):
|
||||
if nm in self.name2slot:
|
||||
get[self.name2slot[nm]] = k
|
||||
for i, nm in enumerate(JOINT_NAMES):
|
||||
if i in get:
|
||||
k = get[i]
|
||||
if k < len(msg.position):
|
||||
self.state_pos[i] = msg.position[k]
|
||||
if k < len(msg.velocity):
|
||||
self.state_vel[i] = msg.velocity[k]
|
||||
elif i < len(msg.position):
|
||||
# 没给 name 就按数组下标
|
||||
self.state_pos[i] = msg.position[i]
|
||||
if i < len(msg.velocity):
|
||||
self.state_vel[i] = msg.velocity[i]
|
||||
self.state_stamp = msg.header.stamp.to_sec()
|
||||
self.state_ok = True
|
||||
|
||||
def _on_diag(self, msg):
|
||||
for k, nm in enumerate(msg.name):
|
||||
if k < len(msg.temperature):
|
||||
t = msg.temperature[k]
|
||||
v = msg.voltage[k] if k < len(msg.voltage) else float("nan")
|
||||
s = msg.status[k] if k < len(msg.status) else 0
|
||||
self.diag_text[nm] = "%.0f℃ %.1fV st=0x%02X" % (t, v, s)
|
||||
|
||||
# ------------------------------------------------------------ 发指令
|
||||
def publish(self, control_word, kp, kd, pos, vel, tau):
|
||||
msg = JointCommandArray()
|
||||
msg.header = Header(stamp=rospy.Time.now())
|
||||
# name = 本次要控制的关节子集；数组长度仍是全部 8 个（hardware 要求），
|
||||
# 未选中的位置清零/增益置 0，做到既不驱动也不保持。
|
||||
msg.name = list(self.selected_names)
|
||||
msg.position = [pos[i] if self.selected_mask[i] else 0.0 for i in range(self.n)]
|
||||
msg.velocity = [vel[i] if self.selected_mask[i] else 0.0 for i in range(self.n)]
|
||||
msg.effort = [tau[i] if self.selected_mask[i] else 0.0 for i in range(self.n)]
|
||||
msg.kp = [kp[i] if self.selected_mask[i] else 0.0 for i in range(self.n)]
|
||||
msg.kd = [kd[i] if self.selected_mask[i] else 0.0 for i in range(self.n)]
|
||||
msg.control_word = control_word
|
||||
self.pub.publish(msg)
|
||||
|
||||
def startup_sequence(self):
|
||||
"""disable → error_reset → enable，之后再走 motor control。"""
|
||||
print("[dbg] 使能序列 disable → error_reset → enable ...")
|
||||
zero = [0.0] * self.n
|
||||
for word in (K_DISABLE, K_ERROR_RESET, K_ENABLE):
|
||||
for _ in range(3):
|
||||
self.publish(word, zero, zero, zero, zero, zero)
|
||||
rospy.sleep(0.02)
|
||||
# 等 hardware 的 Run 循环把控制字处理掉
|
||||
rospy.sleep(0.15)
|
||||
self.enabled = True
|
||||
|
||||
# ------------------------------------------------------------ 打印
|
||||
def show_header(self):
|
||||
cols = "%-10s %10s %10s %10s" % ("joint", "model_rad", "model_deg", "model_vel")
|
||||
print("\n" + cols)
|
||||
print("-" * len(cols))
|
||||
|
||||
def show_once(self):
|
||||
if not self.state_ok:
|
||||
print("[dbg] 还没有 /joint_states 反馈（hardware 节点起了吗？CAN 通吗？）")
|
||||
return
|
||||
self.show_header()
|
||||
for i, nm in enumerate(JOINT_NAMES):
|
||||
m = self.state_pos[i]
|
||||
if m is None:
|
||||
continue
|
||||
v = self.state_vel[i]
|
||||
v = 0.0 if v is None else v
|
||||
extra = self.diag_text.get(nm, "")
|
||||
print("%-10s %10.4f %10.2f %10.3f %s" % (nm, m, math.degrees(m), v, extra))
|
||||
age = rospy.get_time() - self.state_stamp
|
||||
print(" (反馈 %s, 龄期 %.3fs)" % ("OK" if self.state_ok else "无", age))
|
||||
|
||||
# ------------------------------------------------------------ 模式
|
||||
def build_cmd(self, t, phase):
|
||||
"""返回 (kp, kd, pos, vel, tau) 或 None（monitor）。"""
|
||||
a = self.args
|
||||
n = self.n
|
||||
kp = [0.0] * n
|
||||
kd = [0.0] * n
|
||||
pos = [0.0] * n
|
||||
vel = [0.0] * n
|
||||
tau = [0.0] * n
|
||||
warns = []
|
||||
|
||||
if a.mode == "damping":
|
||||
for i in SWING_SLOTS:
|
||||
kd[i] = a.damp_joint_kd
|
||||
for i in WHEEL_SLOTS:
|
||||
kd[i] = a.damp_wheel_kd
|
||||
return kp, kd, pos, vel, tau, warns
|
||||
|
||||
if a.mode in ("angle", "offset"):
|
||||
# 摆臂：位置环；轮子：阻尼 + 0 速。指令即模型帧，hardware 负责换算到电机帧。
|
||||
kp_s = clamp(a.kp, 0.0, KP_MAX, "kp", warns)
|
||||
kd_s = clamp(a.kd, 0.0, KD_MAX, "kd", warns)
|
||||
vel_s = clamp(a.swing_vel, -SWING_VEL_LIMIT, SWING_VEL_LIMIT, "swing_vel", warns)
|
||||
for leg_i, i in enumerate(SWING_SLOTS):
|
||||
target = a.swing[leg_i] # 模型角 [rad]
|
||||
target = clamp(target, SWING_ANGLE_MIN, SWING_ANGLE_MAX, "swing%s" % JOINT_NAMES[i], warns)
|
||||
if a.ramp > 0.0:
|
||||
frac = min(1.0, t / a.ramp)
|
||||
target = target * frac
|
||||
kp[i] = kp_s
|
||||
kd[i] = kd_s
|
||||
vel[i] = vel_s
|
||||
pos[i] = target
|
||||
for i in WHEEL_SLOTS:
|
||||
kd[i] = a.damp_wheel_kd
|
||||
return kp, kd, pos, vel, tau, warns
|
||||
|
||||
if a.mode == "wheel":
|
||||
kd_w = clamp(a.kd, 0.0, KD_MAX, "kd", warns)
|
||||
for leg_i, i in enumerate(WHEEL_SLOTS):
|
||||
target = clamp(a.wheel[leg_i], -WHEEL_VEL_LIMIT, WHEEL_VEL_LIMIT,
|
||||
"wheel%s" % JOINT_NAMES[i], warns)
|
||||
if a.ramp > 0.0:
|
||||
target = target * min(1.0, t / a.ramp)
|
||||
kd[i] = kd_w
|
||||
vel[i] = target
|
||||
for i in SWING_SLOTS:
|
||||
kd[i] = a.damp_joint_kd
|
||||
return kp, kd, pos, vel, tau, warns
|
||||
|
||||
return None
|
||||
|
||||
# ------------------------------------------------------------ 置零
|
||||
def do_set_zero(self):
|
||||
"""把当前机械位置写成模组零位（0xEF）。只作用于 --joints 选中的关节；轮毂不支持。"""
|
||||
swings = [nm for nm in self.selected_names if nm in [JOINT_NAMES[i] for i in SWING_SLOTS]]
|
||||
print("\n" + "=" * 60)
|
||||
print(" 置零（control_word=kSetZero -> 0xEF）")
|
||||
print(" 目标关节: %s" % (", ".join(swings) if swings else "（无摆臂，将被忽略）"))
|
||||
print(" ⚠ 请确认这些摆臂已处在**机械零位**姿态（参考：竖直向下）。")
|
||||
print(" ⚠ 只对摆臂生效，轮毂不支持置零（hardware 节点会自动跳过）。")
|
||||
print(" ⚠ 置零后该位姿的反馈角≈0；请把 hardware toml 的 offset_deg 置 0（或据此重标）。")
|
||||
print("=" * 60)
|
||||
if not swings:
|
||||
print("[dbg] 选中的关节里没有摆臂，置零不会有任何效果；用 --joints 指定摆臂名。")
|
||||
return
|
||||
zero = [0.0] * self.n
|
||||
# hardware 只在控制字「变化」时落地一次。先发 kGetStatus（无副作用）再发 kSetZero，
|
||||
# 保证即便节点进程被复用也能触发 5→6 的变化。
|
||||
for _ in range(3):
|
||||
self.publish(K_GET_STATUS, zero, zero, zero, zero, zero)
|
||||
rospy.sleep(0.02)
|
||||
for _ in range(3):
|
||||
self.publish(K_SET_ZERO, zero, zero, zero, zero, zero)
|
||||
rospy.sleep(0.02)
|
||||
rospy.sleep(0.4) # 等模组把零位写进去、反馈归零
|
||||
print("\n[dbg] 已发置零。下面看反馈模型角（offset 设为 0 时，此处应≈0）：")
|
||||
for _ in range(8):
|
||||
self.show_once()
|
||||
rospy.sleep(0.35)
|
||||
|
||||
# ------------------------------------------------------------ 主循环
|
||||
def run(self):
|
||||
a = self.args
|
||||
print("[dbg] 模式 = %s，发布 %s @ %.0f Hz，回显 %.0f Hz" %
|
||||
(a.mode, TOPIC_JOINT_COMMANDS, a.pub_hz, a.show_hz))
|
||||
print("[dbg] 话题上收发都是模型帧；dir/offset 由 hardware 节点施加（本脚本不换算）")
|
||||
|
||||
# 等订阅者出现（ROS1 发布无 latch，太早发时序帧会丢）
|
||||
for _ in range(200):
|
||||
if self.pub.get_num_connections() > 0 or rospy.is_shutdown():
|
||||
break
|
||||
rospy.sleep(0.05)
|
||||
if self.pub.get_num_connections() == 0:
|
||||
print("[dbg] 警告：%s 还没有订阅者，确认 hardware 节点在跑" % TOPIC_JOINT_COMMANDS)
|
||||
|
||||
if a.mode == "zero":
|
||||
self.do_set_zero()
|
||||
return
|
||||
|
||||
if a.mode in ("angle", "offset") and not any(self.selected_mask[i] for i in SWING_SLOTS):
|
||||
print("[dbg] 警告：--joints 里没有摆臂，angle/offset 模式不会驱动任何关节")
|
||||
if a.mode == "wheel" and not any(self.selected_mask[i] for i in WHEEL_SLOTS):
|
||||
print("[dbg] 警告：--joints 里没有轮组，wheel 模式不会驱动任何关节")
|
||||
|
||||
if a.mode != "monitor":
|
||||
self.startup_sequence()
|
||||
|
||||
pub_period = 1.0 / a.pub_hz
|
||||
show_period = 1.0 / a.show_hz
|
||||
t0 = rospy.get_time()
|
||||
last_show = 0.0
|
||||
last_warn_t = 0.0
|
||||
r = rospy.Rate(a.pub_hz)
|
||||
|
||||
while not rospy.is_shutdown() and not self.stop_requested:
|
||||
t = rospy.get_time() - t0
|
||||
if a.duration > 0.0 and t >= a.duration:
|
||||
print("[dbg] 到达 --duration %.1fs，停止指令" % a.duration)
|
||||
break
|
||||
|
||||
if a.mode != "monitor":
|
||||
built = self.build_cmd(t, None)
|
||||
if built is not None:
|
||||
kp, kd, pos, vel, tau, warns = built
|
||||
# 限幅提示节流
|
||||
if warns and (rospy.get_time() - last_warn_t) > 2.0:
|
||||
last_warn_t = rospy.get_time()
|
||||
for w in warns:
|
||||
print("[dbg][限幅] " + w)
|
||||
self.publish(K_MOTOR_CONTROL, kp, kd, pos, vel, tau)
|
||||
|
||||
if (rospy.get_time() - last_show) >= show_period:
|
||||
last_show = rospy.get_time()
|
||||
self.show_once()
|
||||
|
||||
r.sleep()
|
||||
|
||||
self.safe_stop()
|
||||
|
||||
def safe_stop(self):
|
||||
if self.args.mode in ("monitor", "zero"):
|
||||
return
|
||||
print("\n[dbg] 收尾：先转阻尼 0.3s，再失能 ...")
|
||||
zero = [0.0] * self.n
|
||||
kd = [self.args.damp_joint_kd if i in SWING_SLOTS else self.args.damp_wheel_kd for i in range(self.n)]
|
||||
kp = zero
|
||||
end = time.time() + 0.3
|
||||
while time.time() < end and not rospy.is_shutdown():
|
||||
self.publish(K_MOTOR_CONTROL, kp, kd, zero, zero, zero)
|
||||
rospy.sleep(0.005)
|
||||
for _ in range(3):
|
||||
self.publish(K_DISABLE, zero, zero, zero, zero, zero)
|
||||
rospy.sleep(0.02)
|
||||
print("[dbg] 已失能，退出")
|
||||
|
||||
|
||||
# =============================================================================
|
||||
# 启动 hardware 节点（可选）
|
||||
# =============================================================================
|
||||
def start_node(args):
|
||||
require_can = "true" if args.require_can else "false"
|
||||
cmd = ["roslaunch", "invicta_hardware", "invicta_hardware.launch", "require_can:=" + require_can]
|
||||
print("[dbg] 代起节点：%s" % " ".join(cmd))
|
||||
proc = subprocess.Popen(cmd)
|
||||
atexit.register(lambda: proc.poll() is None and proc.terminate())
|
||||
rospy.sleep(3.0) # 等节点把话题挂上
|
||||
return proc
|
||||
|
||||
|
||||
# =============================================================================
|
||||
# CLI
|
||||
# =============================================================================
|
||||
def parse_args(argv):
|
||||
p = argparse.ArgumentParser(
|
||||
description="invicta_hardware 单节点调试脚本（限幅参数硬编码）",
|
||||
formatter_class=argparse.RawDescriptionHelpFormatter,
|
||||
)
|
||||
p.add_argument("--mode", choices=["monitor", "damping", "angle", "wheel", "offset", "zero"], default="monitor",
|
||||
help="monitor=只监听；damping=阻尼；angle=摆臂角度；wheel=轮速；offset=标定核对；zero=置零（默认 monitor）")
|
||||
p.add_argument("--swing", default=None,
|
||||
help="摆臂目标模型角 [rad]，单个或逗号分隔 4 个（fl,fr,hl,hr）。angle/offset 模式用")
|
||||
p.add_argument("--swing-vel", dest="swing_vel", type=float, default=0.0,
|
||||
help="摆臂目标角速度 [rad/s]，kd 会跟踪它（默认 0；>0 时不适合位置保持）")
|
||||
p.add_argument("--wheel", default=None,
|
||||
help="轮组目标转速 [rad/s]，单个或逗号分隔 4 个（fl,fr,hl,hr）。wheel 模式用")
|
||||
p.add_argument("--joints", default=None,
|
||||
help="只控制/置零这些关节（逗号分隔，如 fl_swing,fl_wheel）；默认全部 8 个。"
|
||||
"对应指令 name 列表；不在列表里的关节不驱动、不置零")
|
||||
p.add_argument("--kp", type=float, default=DEFAULT_KP, help="摆臂位置增益（默认 %.0f）" % DEFAULT_KP)
|
||||
p.add_argument("--kd", type=float, default=DEFAULT_KD, help="速度增益（摆臂角度模式/轮速模式，默认 %.1f）" % DEFAULT_KD)
|
||||
p.add_argument("--damp-joint-kd", dest="damp_joint_kd", type=float, default=DEFAULT_DAMP_JOINT_KD,
|
||||
help="阻尼模式摆臂 kd（默认 %.1f）" % DEFAULT_DAMP_JOINT_KD)
|
||||
p.add_argument("--damp-wheel-kd", dest="damp_wheel_kd", type=float, default=DEFAULT_DAMP_WHEEL_KD,
|
||||
help="阻尼模式轮组 kd（默认 %.1f）" % DEFAULT_DAMP_WHEEL_KD)
|
||||
p.add_argument("--ramp", type=float, default=0.5, help="从 0 斜坡到目标的时长 [s]，0=直接给（默认 0.5）")
|
||||
p.add_argument("--rate", dest="pub_hz", type=float, default=DEFAULT_PUB_HZ, help="指令发布频率 [Hz]（默认 200）")
|
||||
p.add_argument("--show-hz", dest="show_hz", type=float, default=DEFAULT_SHOW_HZ, help="角度回显频率 [Hz]（默认 5）")
|
||||
p.add_argument("--duration", type=float, default=0.0, help="运行时长 [s]，0=直到 Ctrl-C（默认 0）")
|
||||
p.add_argument("--start-node", dest="start_node", action="store_true", help="用 roslaunch 代起 hardware 节点")
|
||||
p.add_argument("--require-can", dest="require_can", action="store_true", default=True,
|
||||
help="代起节点时 require_can:=true（默认）")
|
||||
p.add_argument("--no-can", dest="require_can", action="store_false",
|
||||
help="代起节点时 require_can:=false（无 CAN 干跑）")
|
||||
args = p.parse_args(argv)
|
||||
|
||||
# 预解析列表参数
|
||||
args.swing = parse_list(args.swing, 4, 0.0)
|
||||
args.wheel = parse_list(args.wheel, 4, 0.0)
|
||||
|
||||
# --joints：空格/逗号分隔的关节名，校验必须是已知关节
|
||||
if args.joints:
|
||||
names = [s.strip() for s in str(args.joints).replace(" ", ",").split(",") if s.strip()]
|
||||
bad = [s for s in names if s not in JOINT_NAMES]
|
||||
if bad:
|
||||
p.error("--joints 里有未知关节名：%s（可选：%s）" % (",".join(bad), ",".join(JOINT_NAMES)))
|
||||
# 去重、按关节表顺序排好
|
||||
args.joints = [nm for nm in JOINT_NAMES if nm in names]
|
||||
else:
|
||||
args.joints = None
|
||||
|
||||
if args.mode in ("angle", "offset") and args.swing == [0.0] * 4:
|
||||
print("[dbg] 提示：angle/offset 模式未给 --swing，目标角为 0（= 模型零位）")
|
||||
if args.mode == "wheel" and args.wheel == [0.0] * 4:
|
||||
print("[dbg] 提示：wheel 模式未给 --wheel，目标转速为 0")
|
||||
return args
|
||||
|
||||
|
||||
def main(argv):
|
||||
args = parse_args(argv)
|
||||
rospy.init_node("invicta_hw_debug", anonymous=True)
|
||||
|
||||
node_proc = start_node(args) if args.start_node else None
|
||||
dbg = HardwareDebug(args)
|
||||
|
||||
def _sigint(_sig, _frm):
|
||||
dbg.stop_requested = True
|
||||
signal.signal(signal.SIGINT, _sigint)
|
||||
|
||||
print("=" * 70)
|
||||
print(" invicta_hardware 调试脚本")
|
||||
print(" 关节顺序: " + ", ".join(JOINT_NAMES))
|
||||
print(" 控制关节: " + ", ".join(args.joints) if args.joints else " 控制关节: 全部 8 个")
|
||||
print(" 话题为模型帧（dir/offset 由 hardware 节点施加）")
|
||||
print(" 硬编码限幅: 摆臂角 [%.2f, %.2f] rad, 摆臂 vel %.1f, tau %.1f; 轮 vel %.1f, tau %.2f" %
|
||||
(SWING_ANGLE_MIN, SWING_ANGLE_MAX, SWING_VEL_LIMIT, SWING_TAU_LIMIT, WHEEL_VEL_LIMIT, WHEEL_TAU_LIMIT))
|
||||
print("=" * 70)
|
||||
|
||||
try:
|
||||
dbg.run()
|
||||
except rospy.ROSInterruptException:
|
||||
dbg.safe_stop()
|
||||
finally:
|
||||
if node_proc is not None and node_proc.poll() is None:
|
||||
node_proc.terminate()
|
||||
return 0
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
sys.exit(main(sys.argv[1:]))
|
||||
@@ -155,8 +155,29 @@ void HardwareBridge::CommandCallback(const invicta_msgs::JointCommandArray::Cons
|
||||
.count();
|
||||
}
|
||||
|
||||
// ---------------------------------------------------------------- 关节选择
|
||||
std::vector<uint8_t> HardwareBridge::SelectionMask(const std::vector<std::string>& names, bool* had_unknown) const {
|
||||
const size_t n = joint_names_.size();
|
||||
std::vector<uint8_t> mask(n, 1); // 空列表 = 全选
|
||||
if (had_unknown) *had_unknown = false;
|
||||
if (names.empty()) return mask;
|
||||
|
||||
std::fill(mask.begin(), mask.end(), uint8_t(0));
|
||||
int unknown = 0;
|
||||
for (const auto& nm : names) {
|
||||
const int i = Config().IndexOf(nm);
|
||||
if (i < 0) {
|
||||
++unknown;
|
||||
continue;
|
||||
}
|
||||
mask[size_t(i)] = 1;
|
||||
}
|
||||
if (unknown > 0 && had_unknown) *had_unknown = true;
|
||||
return mask;
|
||||
}
|
||||
|
||||
// ---------------------------------------------------------------- 控制字 → CAN
|
||||
void HardwareBridge::HandleControlWord(uint16_t word) {
|
||||
void HardwareBridge::HandleControlWord(uint16_t word, const std::vector<uint8_t>* include) {
|
||||
switch (word) {
|
||||
case invicta_msgs::kDisable:
|
||||
SendCodeAll(kCodeDisable, 1);
|
||||
@@ -188,13 +209,34 @@ void HardwareBridge::HandleControlWord(uint16_t word) {
|
||||
// 每条指令都会回 12 字节状态帧，无需专门请求
|
||||
ROS_INFO("[invicta_hardware] control_word -> kGetStatus：状态随每条指令自动返回，无需请求");
|
||||
break;
|
||||
case invicta_msgs::kSetZero: {
|
||||
// 把当前机械位置写成模组零位（0xEF）。只作用于指令 name 选中的关节子集，
|
||||
// 且跳过轮毂（不支持置零，手册 §4.4）。
|
||||
const size_t n = joint_names_.size();
|
||||
std::vector<uint8_t> mask(n, 1);
|
||||
if (include) mask = *include;
|
||||
std::string sel;
|
||||
for (size_t i = 0; i < n; ++i) {
|
||||
if (Config().IsWheel(int(i))) mask[i] = 0;
|
||||
if (mask[i]) sel += (sel.empty() ? "" : ",") + joint_names_[i];
|
||||
}
|
||||
if (sel.empty()) {
|
||||
ROS_WARN("[invicta_hardware] kSetZero：选中的关节里没有可置零的摆臂，忽略");
|
||||
break;
|
||||
}
|
||||
SendCodeAll(kCodeZero, 1, 0.0, &mask);
|
||||
ROS_WARN("[invicta_hardware] control_word -> kSetZero：已发置零（0xEF）到 [%s]；"
|
||||
"此后这些摆臂该位姿的反馈角度约为 0，请把对应 offset_deg 设为 0（或据此重标）",
|
||||
sel.c_str());
|
||||
break;
|
||||
}
|
||||
default:
|
||||
ROS_WARN("[invicta_hardware] 未知 control_word=%u，忽略", word);
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
void HardwareBridge::SendCodeAll(uint8_t code, int frames, double interval_s) {
|
||||
void HardwareBridge::SendCodeAll(uint8_t code, int frames, double interval_s, const std::vector<uint8_t>* include) {
|
||||
std::lock_guard<std::mutex> lock(can_mutex_);
|
||||
for (int f = 0; f < std::max(1, frames); ++f) {
|
||||
for (auto& bus : buses_) {
|
||||
@@ -203,8 +245,19 @@ void HardwareBridge::SendCodeAll(uint8_t code, int frames, double interval_s) {
|
||||
WriteU16(payload, kMsgId);
|
||||
bool any = false;
|
||||
for (int canid = 1; canid <= bus.max_canid; ++canid) {
|
||||
if (bus.canid_to_joint[size_t(canid)] < 0) continue;
|
||||
PackCodeBlock(payload + 2 + kBlockBytes * (canid - 1), code);
|
||||
const int joint = bus.canid_to_joint[size_t(canid)];
|
||||
if (joint < 0) continue;
|
||||
uint8_t* blk = payload + 2 + kBlockBytes * (canid - 1);
|
||||
if (include && !(*include)[size_t(joint)]) {
|
||||
// 未选中的关节：写「自由」运行块（kp=kd=0、tau=0，即零力矩）。
|
||||
// ⚠ 绝不能留全 0：(0,0,0,0,0) 按协议会解码成 P_MIN/V_MIN/T_MIN，
|
||||
// 其中 tau=T_MIN（摆臂 −100 N·m）会是危险的前馈力矩。
|
||||
PackRunBlock(blk, 0.0, 0.0, 0.0, 0.0, 0.0,
|
||||
Config().IsWheel(joint) ? wheel_range_ : joint_range_);
|
||||
any = true;
|
||||
continue;
|
||||
}
|
||||
PackCodeBlock(blk, code);
|
||||
any = true;
|
||||
}
|
||||
if (!any) continue;
|
||||
@@ -232,11 +285,14 @@ bool HardwareBridge::SendTimeoutFrame() {
|
||||
for (int canid = 1; canid <= bus.max_canid; ++canid) {
|
||||
const int joint = bus.canid_to_joint[size_t(canid)];
|
||||
if (joint < 0) continue;
|
||||
const bool is_wheel = Config().IsWheel(joint);
|
||||
const auto& spec = Config().Joint(joint);
|
||||
const bool is_wheel = spec.is_wheel;
|
||||
const MotorRange& range = is_wheel ? wheel_range_ : joint_range_;
|
||||
const double kd = is_wheel ? timeout_wheel_kd_ : timeout_joint_kd_;
|
||||
// kp=0、速度与力矩都给 0：只留阻尼抵抗外力，不保持位置
|
||||
PackRunBlock(payload + 2 + kBlockBytes * (canid - 1), 0.0, 0.0, 0.0, kd, 0.0, range);
|
||||
// kp=0、速度与力矩都给 0：只留阻尼抵抗外力，不保持位置。
|
||||
// kp=0 时位置指令不产生力矩，但仍按模型帧 0 换算成电机帧下发，避免读者误会。
|
||||
const double pos_motor = spec.ModelToMotorPos(0.0);
|
||||
PackRunBlock(payload + 2 + kBlockBytes * (canid - 1), pos_motor, 0.0, 0.0, kd, 0.0, range);
|
||||
any = true;
|
||||
}
|
||||
if (!any) continue;
|
||||
@@ -277,9 +333,11 @@ bool HardwareBridge::ReadHardware() {
|
||||
continue;
|
||||
}
|
||||
|
||||
joint_position_[size_t(joint)] = fb.position;
|
||||
joint_velocity_[size_t(joint)] = fb.velocity;
|
||||
joint_effort_[size_t(joint)] = fb.torque;
|
||||
// 电机帧 → 模型帧：位置含 offset+dir，速度/力矩只翻符号（M20 参考同此约定）
|
||||
const auto& spec = Config().Joint(joint);
|
||||
joint_position_[size_t(joint)] = spec.MotorToModelPos(fb.position);
|
||||
joint_velocity_[size_t(joint)] = fb.velocity * spec.dir;
|
||||
joint_effort_[size_t(joint)] = fb.torque * spec.dir;
|
||||
temperature_[size_t(joint)] = fb.temperature;
|
||||
voltage_[size_t(joint)] = fb.voltage;
|
||||
driver_status_[size_t(joint)] = fb.status;
|
||||
@@ -311,13 +369,22 @@ bool HardwareBridge::WriteHardware() {
|
||||
cmd = latest_cmd_;
|
||||
}
|
||||
|
||||
// 控制字变化在这里处理（循环线程），不阻塞订阅回调
|
||||
if (cmd.control_word != applied_control_word_) {
|
||||
HandleControlWord(cmd.control_word);
|
||||
applied_control_word_ = cmd.control_word;
|
||||
const size_t n = joint_names_.size();
|
||||
|
||||
// 指令 name 列表 = 本次要控制的关节子集（空 = 全部已配置关节）。数组长度仍必须是 n。
|
||||
bool had_unknown = false;
|
||||
const std::vector<uint8_t> selected = SelectionMask(cmd.name, &had_unknown);
|
||||
if (had_unknown && !warned_unknown_joint_) {
|
||||
warned_unknown_joint_ = true;
|
||||
ROS_WARN("[invicta_hardware] %s 的 name 里有不认识的关节名，已忽略（只控制能识别的）",
|
||||
command_topic_.c_str());
|
||||
}
|
||||
|
||||
const size_t n = joint_names_.size();
|
||||
// 控制字变化在这里处理（循环线程），不阻塞订阅回调
|
||||
if (cmd.control_word != applied_control_word_) {
|
||||
HandleControlWord(cmd.control_word, &selected);
|
||||
applied_control_word_ = cmd.control_word;
|
||||
}
|
||||
if (cmd.position.size() != n || cmd.velocity.size() != n || cmd.effort.size() != n || cmd.kp.size() != n ||
|
||||
cmd.kd.size() != n) {
|
||||
ROS_WARN_THROTTLE(5.0, "[invicta_hardware] 指令数组长度与 %zu 个关节不匹配，丢弃该帧", n);
|
||||
@@ -364,13 +431,22 @@ bool HardwareBridge::WriteHardware() {
|
||||
for (int canid = 1; canid <= bus.max_canid; ++canid) {
|
||||
const int joint = bus.canid_to_joint[size_t(canid)];
|
||||
if (joint < 0) continue;
|
||||
uint8_t* blk = payload + 2 + kBlockBytes * (canid - 1);
|
||||
const MotorRange& range = Config().IsWheel(joint) ? wheel_range_ : joint_range_;
|
||||
// 未被 name 选中的关节：写「自由」运行块（零力矩），即不驱动也不保持。
|
||||
// ⚠ 不能留全 0（会解码成 T_MIN 力矩，见 SendCodeAll 注释）。
|
||||
if (!selected[size_t(joint)]) {
|
||||
PackRunBlock(blk, 0.0, 0.0, 0.0, 0.0, 0.0, range);
|
||||
any = true;
|
||||
continue;
|
||||
}
|
||||
|
||||
const auto& spec = Config().Joint(joint);
|
||||
const MotorRange& range = Config().IsWheel(joint) ? wheel_range_ : joint_range_;
|
||||
const std::string& name = joint_names_[size_t(joint)];
|
||||
const size_t j = size_t(joint);
|
||||
|
||||
// 先按人为限幅夹紧，再交给编码；[motor.*] 只是归一化边界，不参与取最小
|
||||
// 先按人为限幅夹紧（限幅与指令同为模型帧，见 [joint_limits]），再换算到电机帧编码。
|
||||
// [motor.*] 只是归一化边界，不参与取最小。
|
||||
const double pos = std::min(std::max(cmd.position[j], spec.lower), spec.upper);
|
||||
const double vlim = spec.vel_limit > 0. ? spec.vel_limit : std::abs(range.v_max);
|
||||
const double vel = std::min(std::max(cmd.velocity[j], -vlim), vlim);
|
||||
@@ -381,6 +457,10 @@ bool HardwareBridge::WriteHardware() {
|
||||
tau, tlim);
|
||||
tau = std::min(std::max(tau, -tlim), tlim);
|
||||
}
|
||||
// 模型帧 → 电机帧：位置 q_motor=(q_model−offset)·dir，速度/力矩只乘 dir
|
||||
const double pos_motor = spec.ModelToMotorPos(pos);
|
||||
const double vel_motor = vel * spec.dir;
|
||||
const double tau_motor = tau * spec.dir;
|
||||
|
||||
// kp/kd 来自配置（[policy].kp/kd、[leg.gains]），不是策略算出来的：
|
||||
// 越界会被编码器静默饱和，这里显式夹紧并把配置错误报出来
|
||||
@@ -397,7 +477,7 @@ bool HardwareBridge::WriteHardware() {
|
||||
kd = std::min(std::max(kd, 0.0), range.kd_max);
|
||||
}
|
||||
|
||||
PackRunBlock(payload + 2 + kBlockBytes * (canid - 1), pos, vel, kp, kd, tau, range);
|
||||
PackRunBlock(blk, pos_motor, vel_motor, kp, kd, tau_motor, range);
|
||||
any = true;
|
||||
}
|
||||
if (!any) continue;
|
||||
|
||||
@@ -0,0 +1,13 @@
|
||||
# toml++ (vendored)
|
||||
|
||||
这三个节点各自用到的 TOML 解析库，以 **single-header / header-only** 方式编进节点
|
||||
（无 `find_package`，目标机也不需要额外 `.so`）。
|
||||
|
||||
- 上游：https://github.com/marzer/tomlplusplus
|
||||
- 版本：v3.4.0（单头文件 `toml.hpp`）
|
||||
- 取用：`toml.hpp`（amalgamated single header）。单头版本本身就是 header-only——
|
||||
「drop it in, there is no step two」——文件内部在未定义时自动 `#define TOML_HEADER_ONLY 1`，
|
||||
**不需要任何编译宏**。放在 `toml++/toml.hpp` 是为了匹配上游 `#include <toml++/toml.hpp>`
|
||||
的路径，源码无需改动。
|
||||
|
||||
升级：下载新的 `toml.hpp` 覆盖本目录下 `toml++/toml.hpp` 即可（三个包各一份，保持同版本）。
|
||||
File diff suppressed because it is too large
Load Diff
@@ -18,6 +18,7 @@ enum JointControlWord : uint16_t {
|
||||
kMotorControl = 3, ///< 正常控制：tau = kp*(q_des - q) + kd*(dq_des - dq) + tau_ff
|
||||
kErrorReset = 4, ///< 清除驱动错误
|
||||
kGetStatus = 5, ///< 请求驱动器状态（诊断用，当前 RL 层不主动发）
|
||||
kSetZero = 6, ///< 把关节当前位置设为机械零位（标定用；轮毂电机不支持，节点会跳过）
|
||||
};
|
||||
|
||||
} // namespace invicta_msgs
|
||||
|
||||
@@ -20,12 +20,9 @@ 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)
|
||||
# TOML 解析库：single-header，vendored 在 third_party/tomlplusplus/toml++/toml.hpp
|
||||
# 单头版本本身就是 header-only（自带 TOML_HEADER_ONLY），实现直接编进本节点：无 find_package、无运行时库依赖
|
||||
include_directories(${CMAKE_CURRENT_SOURCE_DIR}/third_party/tomlplusplus)
|
||||
|
||||
include_directories(
|
||||
include
|
||||
|
||||
@@ -0,0 +1,13 @@
|
||||
# toml++ (vendored)
|
||||
|
||||
这三个节点各自用到的 TOML 解析库，以 **single-header / header-only** 方式编进节点
|
||||
（无 `find_package`，目标机也不需要额外 `.so`）。
|
||||
|
||||
- 上游：https://github.com/marzer/tomlplusplus
|
||||
- 版本：v3.4.0（单头文件 `toml.hpp`）
|
||||
- 取用：`toml.hpp`（amalgamated single header）。单头版本本身就是 header-only——
|
||||
「drop it in, there is no step two」——文件内部在未定义时自动 `#define TOML_HEADER_ONLY 1`，
|
||||
**不需要任何编译宏**。放在 `toml++/toml.hpp` 是为了匹配上游 `#include <toml++/toml.hpp>`
|
||||
的路径，源码无需改动。
|
||||
|
||||
升级：下载新的 `toml.hpp` 覆盖本目录下 `toml++/toml.hpp` 即可（三个包各一份，保持同版本）。
|
||||
+17897
File diff suppressed because it is too large
Load Diff
Reference in New Issue
Block a user