caowangrenbo
46bbb06978
标定边界上移到 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 验证通过。
334 lines
14 KiB
C++
334 lines
14 KiB
C++
/**
|
|
* @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 {
|
|
|
|
/// 单个关节：限位（模型帧）。硬件标定 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]
|
|
};
|
|
|
|
/// 一条腿：摆臂 + 轮
|
|
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 里");
|
|
}
|
|
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
|