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/interface/robot/hardware/invicta_interface.hpp
+83 -195
View File
@@ -1,13 +1,15 @@
/**
* @file invicta_interface.hpp
* @brief invicta 的 ROS1 数据面：订阅关节状态与 IMU，发布关节指令
* @brief ROS1 数据面：订阅关节状态与 IMU，发布关节指令。关节表、话题、标定全部来自 invicta.toml
*
* 由上游 M20 的 DdsInterface + M20Interface 合并而来（原来分两层是为了和 Lite3 共用，
* 这里只有一台机器人，合成一个类更好读）。差异：
* - ros2/drdds → ROS1 + sensor_msgs/JointState + sensor_msgs/Imu
* - 控制字从原厂驱动器私有值改为 invicta_msgs/control_word.h 的平台无关集合
* - 删除电池订阅（上游是死代码）
* - 关节方向/零点标定改为 ROS 参数，默认「恒等」，即约定硬件桥接节点已给出 policy 约定下的数据
* 硬件约定（方向 dir、零点 offset、单位）在本层一次性归一：
* 反馈：q_model = q_robot · dir + offset
* 指令：q_robot = (q_des − offset) · dir
* 上层的状态机与策略只看到「模型约定」下的量（rad / rad/s / Nm）。
*
* 相比之前：删掉了 auto_calibrate / ±360° 回绕标定（那是 M20 三关节腿的绝对编码器问题，
* 本构型摆臂行程只有 ±80°，无回绕；标定统一由 invicta.toml 的 dir/offset 表达），
* 轮关节判定也从 i%4==3 改为按配置的 is_wheel。
*/
#pragma once
@@ -21,6 +23,7 @@
#include "basic_function.hpp"
#include "common_types.h"
#include "config/deploy_config.h"
#include "invicta_msgs/JointCommandArray.h"
#include "invicta_msgs/control_word.h"
#include "robot_interface.h"
@@ -28,68 +31,40 @@
namespace interface {
class InvictaInterface : public RobotInterface {
protected:
ros::NodeHandle nh_; ///< 话题用：相对节点命名空间（默认 /joint_states 等）
ros::NodeHandle pnh_{"~"}; ///< 参数用：私有命名空间 ~topics/... ~calibration/...
// ---- 话题与队列 ----
std::string joints_topic_ = "joint_states";
std::string imu_topic_ = "imu/data";
std::string cmd_topic_ = "joint_commands";
int cmd_queue_ = 1; ///< 指令流只关心最新值，队列不堆积
int state_queue_ = 10; ///< 状态流按 200 Hz 设计，队列给足以吸收抖动
// ---- 关节约定 ----
std::vector<std::string> joint_names_;
std::vector<float> joint_dir_; ///< +1 / -1
std::vector<float> pos_offset_deg_; ///< 关节零点偏置 [deg]
bool auto_calibrate_ = false;
float wrap_hip_deg_ = 140.f; ///< 标定时 hipy(k%4==1) 的回绕阈值
float wrap_knee_deg_ = 164.f; ///< 标定时 knee(k%4==2) 的回绕阈值
bool wheel_kp_zero_ = true; ///< 轮关节 (k%4==3) 强制 kp=0，速度/力矩模式
// ---- 状态缓存 ----
double ri_ts_ = 0., imu_ts_ = 0.;
Vec3f omega_body_, rpy_, acc_;
VecXf joint_pos_, joint_vel_, joint_tau_;
std::unordered_map<std::string, int> name2slot_;
uint16_t joint_data_frame_ = 0;
std::vector<uint16_t> joint_data_id_;
std::vector<bool> joint_data_updated_; ///< 标定用：该关节是否已看到数据变化
std::vector<uint16_t> driver_status_;
bool warned_name_mismatch_ = false;
bool warned_imu_no_orientation_ = false;
ros::Publisher cmd_pub_;
ros::Subscriber joint_state_sub_, imu_sub_;
public:
InvictaInterface(const std::string& robot_name, int dof_num) : RobotInterface(robot_name, dof_num) {
InvictaInterface(const std::string& robot_name, int dof_num)
: RobotInterface(robot_name, dof_num), cfg_(invicta::Config()) {
if (dof_num_ != cfg_.dof) {
ROS_WARN("[InvictaInterface] 传入 dof=%d 与配置的 %d 不一致，按配置执行", dof_num_, cfg_.dof);
}
joint_pos_ = VecXf::Zero(dof_num_);
joint_vel_ = VecXf::Zero(dof_num_);
joint_tau_ = VecXf::Zero(dof_num_);
joint_data_id_.resize(dof_num_, 0);
driver_status_.resize(dof_num_, 1);
joint_data_id_.resize(size_t(dof_num_), 0);
driver_status_.assign(size_t(dof_num_), 1);
LoadParams();
for (int i = 0; i < dof_num_; ++i) name2slot_[cfg_.joint_names[size_t(i)]] = i;
cmd_pub_ = nh_.advertise<invicta_msgs::JointCommandArray>(cmd_topic_, cmd_queue_);
joint_state_sub_ = nh_.subscribe(joints_topic_, state_queue_, &InvictaInterface::JointStateHandler, this);
imu_sub_ = nh_.subscribe(imu_topic_, state_queue_, &InvictaInterface::ImuHandler, this);
cmd_pub_ = nh_.advertise<invicta_msgs::JointCommandArray>(cfg_.topic_joint_commands,
cfg_.queue_joint_commands);
joint_state_sub_ = nh_.subscribe(cfg_.topic_joint_states, cfg_.queue_state,
&InvictaInterface::JointStateHandler, this);
imu_sub_ = nh_.subscribe(cfg_.topic_imu, cfg_.queue_state, &InvictaInterface::ImuHandler, this);
}
virtual ~InvictaInterface() {}
// ------------------------------------------------------------------ 接口实现
virtual void Start() {
// 启动控制序列：先关使能，清错误，再使能。此后恒发 kMotorControl。
SendStartupSequence();
if (auto_calibrate_) {
ResetPositionOffset();
} else {
std::cout << "[InvictaInterface] auto_calibrate=false，按参数给定的 dir/offset 直接使用\n";
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;
}
virtual void Stop() {}
@@ -110,22 +85,20 @@ public:
joint_cmd_ = input;
invicta_msgs::JointCommandArray msg;
msg.header.stamp = ros::Time::now();
msg.name = joint_names_;
msg.position.resize(dof_num_);
msg.velocity.resize(dof_num_);
msg.effort.resize(dof_num_);
msg.kp.resize(dof_num_);
msg.kd.resize(dof_num_);
msg.name = cfg_.joint_names;
msg.position.resize(size_t(dof_num_));
msg.velocity.resize(size_t(dof_num_));
msg.effort.resize(size_t(dof_num_));
msg.kp.resize(size_t(dof_num_));
msg.kd.resize(size_t(dof_num_));
for (int i = 0; i < dof_num_; ++i) {
msg.position[i] = (input(i, 1) - Deg2Rad(pos_offset_deg_[i])) * joint_dir_[i];
msg.velocity[i] = input(i, 3) * joint_dir_[i];
msg.effort[i] = input(i, 4) * joint_dir_[i];
msg.kp[i] = input(i, 0);
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;
msg.kp[i] = (cfg_.wheel_kp_zero && j.is_wheel) ? 0.f : input(i, 0);
msg.kd[i] = input(i, 2);
}
if (wheel_kp_zero_) {
for (int i = 3; i < dof_num_; i += 4) msg.kp[i] = 0.;
}
msg.control_word = invicta_msgs::kMotorControl;
cmd_pub_.publish(msg);
}
@@ -133,161 +106,61 @@ public:
virtual void RefreshRobotData() {}
protected:
// ------------------------------------------------------------------ 参数
void LoadParams() {
pnh_.param<std::string>("topics/joint_states", joints_topic_, "joint_states");
pnh_.param<std::string>("topics/imu", imu_topic_, "imu/data");
pnh_.param<std::string>("topics/joint_commands", cmd_topic_, "joint_commands");
pnh_.param<int>("queues/joint_commands", cmd_queue_, 1);
pnh_.param<int>("queues/state", state_queue_, 10);
joint_names_ = DefaultJointNames();
pnh_.getParam("joint_names", joint_names_);
if (static_cast<int>(joint_names_.size()) != dof_num_) {
ROS_ERROR("[InvictaInterface] 参数 joint_names 长度 %zu != dof_num %d，使用默认顺序",
joint_names_.size(), dof_num_);
joint_names_ = DefaultJointNames();
}
name2slot_.clear();
for (int i = 0; i < dof_num_; ++i) {
name2slot_[joint_names_[i]] = i;
}
joint_dir_.assign(dof_num_, 1.f);
pos_offset_deg_.assign(dof_num_, 0.f);
pnh_.getParam("calibration/joint_dir", joint_dir_);
pnh_.getParam("calibration/joint_offset_deg", pos_offset_deg_);
if (static_cast<int>(joint_dir_.size()) != dof_num_) joint_dir_.assign(dof_num_, 1.f);
if (static_cast<int>(pos_offset_deg_.size()) != dof_num_) pos_offset_deg_.assign(dof_num_, 0.f);
pnh_.param<bool>("calibration/auto_calibrate", auto_calibrate_, false);
pnh_.param("calibration/wrap_hip_deg", wrap_hip_deg_, 140.f);
pnh_.param("calibration/wrap_knee_deg", wrap_knee_deg_, 164.f);
pnh_.param<bool>("wheel_kp_zero", wheel_kp_zero_, true);
ROS_INFO_STREAM("[InvictaInterface] topics: " << joints_topic_ << " / " << imu_topic_ << " / " << cmd_topic_
<< " | auto_calibrate=" << auto_calibrate_
<< " wheel_kp_zero=" << wheel_kp_zero_);
std::cout << "[InvictaInterface] joint_dir: ";
for (float d : joint_dir_) std::cout << d << " ";
std::cout << "\n[InvictaInterface] joint_offset_deg: ";
for (float o : pos_offset_deg_) std::cout << o << " ";
std::cout << std::endl;
}
static std::vector<std::string> DefaultJointNames() {
// 与 policy.onnx 的动作/观测顺序一致，见 invicta_deploy/README.md
return {"fl_hipx_joint", "fl_hipy_joint", "fl_knee_joint", "fl_wheel_joint",
"fr_hipx_joint", "fr_hipy_joint", "fr_knee_joint", "fr_wheel_joint",
"hl_hipx_joint", "hl_hipy_joint", "hl_knee_joint", "hl_wheel_joint",
"hr_hipx_joint", "hr_hipy_joint", "hr_knee_joint", "hr_wheel_joint"};
}
// ------------------------------------------------------------------ 启动/标定
// ------------------------------------------------------------------ 启动序列
void PublishRawCommand(uint16_t control_word) {
invicta_msgs::JointCommandArray msg;
msg.header.stamp = ros::Time::now();
msg.name = joint_names_;
msg.position.assign(dof_num_, 0.);
msg.velocity.assign(dof_num_, 0.);
msg.effort.assign(dof_num_, 0.);
msg.kp.assign(dof_num_, 0.);
msg.kd.assign(dof_num_, 0.);
msg.name = cfg_.joint_names;
msg.position.assign(size_t(dof_num_), 0.);
msg.velocity.assign(size_t(dof_num_), 0.);
msg.effort.assign(size_t(dof_num_), 0.);
msg.kp.assign(size_t(dof_num_), 0.);
msg.kd.assign(size_t(dof_num_), 0.);
msg.control_word = control_word;
cmd_pub_.publish(msg);
}
/// 使能时序：disable → error reset → enable；此后运行期恒为 motor control
void SendStartupSequence() {
const uint16_t seq[] = {invicta_msgs::kDisable, invicta_msgs::kErrorReset, invicta_msgs::kEnable};
for (uint16_t word : seq) {
PublishRawCommand(word);
ros::Duration(0.01).sleep();
}
std::cout << "[InvictaInterface] 启动控制序列完成 (disable -> error_reset -> enable)\n";
}
bool IsDataUpdatedFinished() {
bool res = true;
for (int i = 0; i < dof_num_; ++i) res = res && joint_data_updated_[i];
return res;
}
/**
* @brief 上电标定：等待 16 个关节数据都发生更新，并按阈值修正 ±360° 回绕
*
* 只有在硬件桥接节点上报的是「原始编码器角 + M20 约定」时才需要打开
* （calibration/auto_calibrate=true）。若桥接节点已经给出 policy 约定的角度，
* 保持 false。
*/
void ResetPositionOffset() {
joint_data_updated_.assign(dof_num_, false);
SetJointCommand(MatXf::Zero(dof_num_, 5));
ros::Duration(0.1).sleep();
VecXf last = GetJointPosition();
int cnt = 0;
while (!IsDataUpdatedFinished()) {
++cnt;
ros::spinOnce();
VecXf cur = GetJointPosition();
for (int i = 0; i < dof_num_; ++i) {
if (!joint_data_updated_[i] && cur(i) != last(i) && !std::isnan(cur(i))) {
joint_data_updated_[i] = true;
std::cout << "joint " << i << " data updated at " << cnt << " cnt!" << std::endl;
}
}
last = cur;
ros::Duration(0.001).sleep();
if (cnt > 10000) {
ROS_ERROR("[InvictaInterface] 关节数据未全部更新，标定失败，继续用参数值");
return;
}
}
VecXf cur = GetJointPosition();
for (int k = 0; k < dof_num_; ++k) {
float th = (k % 4 == 1) ? wrap_hip_deg_ : wrap_knee_deg_;
if (k % 4 == 3) continue; // 轮关节不做回绕
if (cur(k) < -th / 180. * M_PI) {
pos_offset_deg_[k] += 360.;
std::cout << "joint " << k << " offset -> " << pos_offset_deg_[k] << "\n";
} else if (cur(k) > th / 180. * M_PI) {
pos_offset_deg_[k] -= 360.;
std::cout << "joint " << k << " offset -> " << pos_offset_deg_[k] << "\n";
}
}
std::cout << "[InvictaInterface] 启动控制序列完成 (disable -> error_reset -> enable)" << std::endl;
}
// ------------------------------------------------------------------ 回调
void JointStateHandler(const sensor_msgs::JointState::ConstPtr& msg) {
if (static_cast<int>(msg->position.size()) != dof_num_ ||
static_cast<int>(msg->velocity.size()) != dof_num_ ||
static_cast<int>(msg->effort.size()) != dof_num_) {
const size_t n = size_t(dof_num_);
if (msg->position.size() != n || msg->velocity.size() != n || msg->effort.size() != n) {
ROS_WARN_THROTTLE(5.0, "[InvictaInterface] %s 长度应为 %d，收到 pos=%zu vel=%zu eff=%zu，丢帧",
joints_topic_.c_str(), dof_num_, msg->position.size(), msg->velocity.size(),
cfg_.topic_joint_states.c_str(), dof_num_, msg->position.size(), msg->velocity.size(),
msg->effort.size());
return;
}
bool by_name = static_cast<int>(msg->name.size()) == dof_num_;
const bool by_name = msg->name.size() == n;
int unknown = 0;
for (int j = 0; j < dof_num_; ++j) {
int i = by_name ? LookupSlot(msg->name[j]) : j;
for (size_t k = 0; k < n; ++k) {
const int i = by_name ? LookupSlot(msg->name[k]) : int(k);
if (i < 0) {
++unknown;
continue;
}
joint_pos_(i) = float(msg->position[j]) * joint_dir_[i] + Deg2Rad(pos_offset_deg_[i]);
joint_vel_(i) = float(msg->velocity[j]) * joint_dir_[i];
joint_tau_(i) = float(msg->effort[j]) * joint_dir_[i];
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;
}
if (!by_name && !warned_name_mismatch_) {
warned_name_mismatch_ = true;
ROS_WARN("[InvictaInterface] %s 未按约定给出 %d 个 name，按数组下标解析",
joints_topic_.c_str(), dof_num_);
cfg_.topic_joint_states.c_str(), dof_num_);
}
if (unknown > 0) {
ROS_WARN_THROTTLE(5.0, "[InvictaInterface] %s 有 %d 个关节名不认识，已忽略",
joints_topic_.c_str(), unknown);
cfg_.topic_joint_states.c_str(), unknown);
}
for (int i = 0; i < dof_num_; ++i) joint_data_id_[i] = joint_data_frame_;
@@ -301,29 +174,44 @@ protected:
}
void ImuHandler(const sensor_msgs::Imu::ConstPtr& msg) {
// orientation_covariance[0] == -1 表示「不提供姿态」，此时不能拿它当有效姿态用
// orientation_covariance[0] == -1 表示「不提供姿态」
if (msg->orientation_covariance[0] == -1.0) {
if (!warned_imu_no_orientation_) {
warned_imu_no_orientation_ = true;
ROS_WARN("[InvictaInterface] %s 没有提供 orientation（covariance=-1），姿态观测无效",
imu_topic_.c_str());
cfg_.topic_imu.c_str());
}
return;
}
Eigen::Quaternionf q(float(msg->orientation.w), float(msg->orientation.x), float(msg->orientation.y),
float(msg->orientation.z));
if (q.norm() < 1e-6f) {
ROS_WARN_THROTTLE(5.0, "[InvictaInterface] %s 四元数模长近零，忽略该帧", imu_topic_.c_str());
ROS_WARN_THROTTLE(5.0, "[InvictaInterface] %s 四元数模长近零，忽略该帧", cfg_.topic_imu.c_str());
return;
}
q.normalize();
// eulerAngles(0,1,2) 即 ZYX 顺序，与 basic_function.hpp 的 RpyToRm 一致
rpy_ = q.toRotationMatrix().eulerAngles(0, 1, 2).cast<float>();
rpy_ = q.toRotationMatrix().eulerAngles(0, 1, 2).cast<float>(); // ZYX，与 RpyToRm 一致
omega_body_ << msg->angular_velocity.x, msg->angular_velocity.y, msg->angular_velocity.z;
acc_ << msg->linear_acceleration.x, msg->linear_acceleration.y, msg->linear_acceleration.z;
imu_ts_ = msg->header.stamp.toSec();
}
protected:
const invicta::DeployConfig& cfg_;
ros::NodeHandle nh_;
std::unordered_map<std::string, int> name2slot_;
double ri_ts_ = 0., imu_ts_ = 0.;
Vec3f omega_body_, rpy_, acc_;
VecXf joint_pos_, joint_vel_, joint_tau_;
uint16_t joint_data_frame_ = 0;
std::vector<uint16_t> joint_data_id_;
std::vector<uint16_t> driver_status_;
bool warned_name_mismatch_ = false;
bool warned_imu_no_orientation_ = false;
ros::Publisher cmd_pub_;
ros::Subscriber joint_state_sub_, imu_sub_;
};
}; // namespace interface
src/invicta_deploy/interface/user_command/ros_command_interface.hpp
+5 -3
View File
@@ -21,6 +21,7 @@
#include "basic_function.hpp"
#include "custom_types.h"
#include "config/deploy_config.h"
#include "invicta_msgs/MotionCommand.h"
#include "invicta_msgs/motion_defs.h"
#include "user_command_interface.h"
@@ -36,9 +37,9 @@ public:
* @param default_gait 默认步态（协议 GaitParam）
*/
RosCommandInterface(std::shared_ptr<std::set<int>> loaded_gaits, int default_gait)
: loaded_gaits_(std::move(loaded_gaits)), default_gait_(default_gait) {
pnh_.param<std::string>("topics/motion_command", topic_, "motion_command");
pnh_.param<int>("queues/motion_command", cmd_queue_, 1);
: loaded_gaits_(std::move(loaded_gaits)), default_gait_(default_gait), cfg_(invicta::Config()) {
topic_ = cfg_.topic_motion_command;
cmd_queue_ = cfg_.queue_motion_command;
sub_ = nh_.subscribe(topic_, cmd_queue_, &RosCommandInterface::CommandCallback, this);
}
@@ -183,6 +184,7 @@ private:
std::string topic_ = "motion_command";
int cmd_queue_ = 1;
const invicta::DeployConfig& cfg_;
std::shared_ptr<std::set<int>> loaded_gaits_; ///< 实际加载成功的步态（由 RLControlState 填充）
int default_gait_ = int(kGaitBasic);