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

标定边界上移到 invicta_hardware:
- raw↔model 的 dir/offset 由 hardware 独家施加(话题收发都是模型帧),
  deploy 的 InvictaInterface 退化为透传,invicta_deploy.toml 不再有 dir/offset_deg。
  这样 [joint_limits](模型帧)与被限幅的指令同帧,/joint_states 也是模型角。

关节选择(部分接入 / 单腿调试):
- JointCommandArray 的 name 即本次要控制的关节子集(空 = 全部已配置关节)。
- 未选中的关节写「零力矩自由」块,而不是留全 0——按协议 (0,0,0,0,0) 会解码成
  P_MIN/V_MIN/T_MIN,其中 tau=T_MIN(摆臂 -100 N·m)是危险的前馈力矩。
- 新增 control_word kSetZero(6) -> 0xEF,只作用于选中的摆臂(轮毂不支持,跳过)。

调试脚本 invicta_hardware/scripts/hw_debug_test.py:
- 只起 hardware 节点调试:monitor/damping/angle/wheel/offset/zero 六种模式 + --joints。
- 限幅参数硬编码;发指令时默认回显模型角。

构建:toml++ 改为 vendored single-header(三个包各 third_party/tomlplusplus/toml++/toml.hpp),
   去掉 find_package(tomlplusplus) 与 -DTOML_HEADER_ONLY(单头自带 header-only)。

已在容器(localhost/custom-ros-noetic)中全新 catkin_make + smoke_test 验证通过。
This commit is contained in:
2026-10-23 13:44:27 +08:00
parent 594885b9e8
commit 46bbb06978
24 changed files with 54455 additions and 129 deletions
@@ -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;