LinkNexus/huoyi_deploy
20
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

refactor(config) + feat(model): 每节点一份 TOML 取代 ROS yaml；8 关节两自由度腿与单连杆 IK

Browse Source 
配置（节点自治，跨包不共享）：
- 新增各节点自己的 config/<节点>.toml（deploy / remote / hardware），只放本节点参数；删除三份 *.yaml
- 各节点自带配置结构与加载器（deploy_config.h / remote_config.h / hardware_config.h），
  删除 state_machine/parameters/ 与 ControlParameters
- TOML 解析库 toml++ 改为系统安装：find_package(tomlplusplus REQUIRED)，以 header-only 方式编译，
  库源码不进仓库、目标机不需部署运行时 .so
- launch 只传 config_file；改 TOML 不需重编，改配置头自动触发该节点重编

机器人模型（按本机构型改）：
- 16 关节三关节腿 → 8 关节（每腿 摆臂 + 轮）
- IK 改为单连杆反解 θ = splay_sign·acos(h/L)；趴下 h=0 → 摆臂水平（±90°）
- 步态倒向：趴下时前腿向前、后腿向后（splay_sign 前 +1 / 后 −1），与 M20 相反
- 起立/趴下用样条 + 速度前馈；轮 kp 恒 0；限位改为逐关节查表
- 精简死参数：机身长宽、髋偏置、轮半径、力矩限制、通用策略参数，以及 auto_calibrate 与 ±360° 回绕标定
- 硬件侧参数入库：电机量程（P65MAXS/W190S）、CAN 通道与仲裁 ID、使能帧数/间隔、每腿 bus/canid、台架增益

保留（未改动 HEAD 已有功能）：多步态模型预加载与运行中切换、站立后自动进 RL 与就绪自检、
遥测四路上报、阻尼态单调时钟计时、策略输出 NaN 保护

验证（ROS Noetic 容器，干净构建）：
- 四个包 catkin_make rc=0
- /joint_states 200 Hz（8 关节名与配置一致）、/motion_state 20 Hz
- IK 目标角：站立 ±16.3°、收腿 ±66.4°、趴下 ±90.0°；趴下指令实测峰值 1.5708 rad
- 协议 MotionParam=1/4 触发 idle→standup→liedown；自动进 RL 自检在零反馈下正确拦住

待实测：摆臂长度/站趴高度、各关节 dir/offset_deg；invicta_hardware 的 CAN 读写仍为 TODO
This commit is contained in:
caowangrenbo
2026-10-21 21:39:49 +08:00
parent cf70b1a1d7
commit a173383756
39 changed files with 1715 additions and 1088 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/invicta_deploy/include/config/deploy_config.h
+341
View File
@@ -0,0 +1,341 @@
/**
* @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 {
/// 单个关节：限位与硬件标定
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]
};
/// 一条腿：摆臂 + 轮
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 里");
}
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;
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