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:
@@ -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;
|
||||
|
||||
Reference in New Issue
Block a user