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
@@ -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;