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

Initial commit: extract M20 sim2real into ROS1 invicta project

Browse Source 
Source: DeepRoboticsLab/sdk_deploy (BSD-3-Clause), M20 sim2real part only.

Packages:
- invicta_msgs: JointCommandArray, MotionCommand, MotionState; standard messages
  (sensor_msgs/JointState, sensor_msgs/Imu) elsewhere; shared motion value defs.
- invicta_deploy: state machine (idle/standup/rl/joint-damping/lie-down), ONNX
  policy with per-gait models, auto RL entry after standup with readiness checks,
  safety loop, gait->model table; no protocol/network code.
- invicta_remote: vendor basic_server protocol (UDP + JSON, proto.md V1.2.0),
  header/MsgId, 0xE0xx error codes, motion/usage-mode/gait translation, telemetry
  1002/3,4,5,6 (2/10/2/2 Hz); publishes /motion_command, subscribes /motion_state.
- invicta_hardware: joint/IMU bridge skeleton (SocketCAN I/O is TODO), publishes
  sensor_msgs/JointState + sensor_msgs/Imu, consumes /joint_commands.

Verified on ROS Noetic (Ubuntu 20.04) in docker: clean catkin_make, 200 Hz joint
state / 50 Hz RL commands / 20 Hz state report, protocol conformance (MotionParam,
gait preconditions, error codes), telemetry push, and end-to-end switching of two
policy models with a real remote controller.
This commit is contained in:
caowangrenbo
2026-10-21 00:46:29 +08:00
commit cf70b1a1d7
2016 changed files with 489092 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/invicta_deploy/interface/robot/hardware/invicta_interface.hpp
+329
View File
@@ -0,0 +1,329 @@
/**
* @file invicta_interface.hpp
* @brief invicta 的 ROS1 数据面：订阅关节状态与 IMU，发布关节指令
*
* 由上游 M20 的 DdsInterface + M20Interface 合并而来（原来分两层是为了和 Lite3 共用，
* 这里只有一台机器人，合成一个类更好读）。差异：
* - ros2/drdds → ROS1 + sensor_msgs/JointState + sensor_msgs/Imu
* - 控制字从原厂驱动器私有值改为 invicta_msgs/control_word.h 的平台无关集合
* - 删除电池订阅（上游是死代码）
* - 关节方向/零点标定改为 ROS 参数，默认「恒等」，即约定硬件桥接节点已给出 policy 约定下的数据
*/
#pragma once
#include <ros/ros.h>
#include <sensor_msgs/Imu.h>
#include <sensor_msgs/JointState.h>
#include <string>
#include <unordered_map>
#include <vector>
#include "basic_function.hpp"
#include "common_types.h"
#include "invicta_msgs/JointCommandArray.h"
#include "invicta_msgs/control_word.h"
#include "robot_interface.h"
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) {
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);
LoadParams();
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);
}
virtual ~InvictaInterface() {}
// ------------------------------------------------------------------ 接口实现
virtual void Start() {
// 启动控制序列：先关使能，清错误，再使能。此后恒发 kMotorControl。
SendStartupSequence();
if (auto_calibrate_) {
ResetPositionOffset();
} else {
std::cout << "[InvictaInterface] auto_calibrate=false，按参数给定的 dir/offset 直接使用\n";
}
}
virtual void Stop() {}
virtual double GetInterfaceTimeStamp() { return ri_ts_; }
virtual VecXf GetJointPosition() { return joint_pos_; }
virtual VecXf GetJointVelocity() { return joint_vel_; }
virtual VecXf GetJointTorque() { return joint_tau_; }
virtual Vec3f GetImuRpy() { return rpy_; }
virtual Vec3f GetImuAcc() { return acc_; }
virtual Vec3f GetImuOmega() { return omega_body_; }
virtual VecXf GetContactForce() { return VecXf::Zero(4); }
virtual double GetImuTimestamp() { return imu_ts_; }
virtual std::vector<uint16_t> GetJointDataID() { return joint_data_id_; }
virtual std::vector<uint16_t> GetDriverStatusWord() { return driver_status_; }
virtual void SetJointCommand(Eigen::Matrix<float, Eigen::Dynamic, 5> input) {
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_);
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);
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);
}
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.control_word = control_word;
cmd_pub_.publish(msg);
}
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";
}
}
}
// ------------------------------------------------------------------ 回调
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_) {
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(),
msg->effort.size());
return;
}
bool by_name = static_cast<int>(msg->name.size()) == dof_num_;
int unknown = 0;
for (int j = 0; j < dof_num_; ++j) {
int i = by_name ? LookupSlot(msg->name[j]) : j;
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];
}
if (!by_name && !warned_name_mismatch_) {
warned_name_mismatch_ = true;
ROS_WARN("[InvictaInterface] %s 未按约定给出 %d 个 name，按数组下标解析",
joints_topic_.c_str(), dof_num_);
}
if (unknown > 0) {
ROS_WARN_THROTTLE(5.0, "[InvictaInterface] %s 有 %d 个关节名不认识，已忽略",
joints_topic_.c_str(), unknown);
}
for (int i = 0; i < dof_num_; ++i) joint_data_id_[i] = joint_data_frame_;
++joint_data_frame_;
ri_ts_ = msg->header.stamp.toSec();
}
int LookupSlot(const std::string& name) const {
auto it = name2slot_.find(name);
return it == name2slot_.end() ? -1 : it->second;
}
void ImuHandler(const sensor_msgs::Imu::ConstPtr& msg) {
// 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());
}
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());
return;
}
q.normalize();
// eulerAngles(0,1,2) 即 ZYX 顺序，与 basic_function.hpp 的 RpyToRm 一致
rpy_ = q.toRotationMatrix().eulerAngles(0, 1, 2).cast<float>();
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();
}
};
}; // namespace interface
src/invicta_deploy/interface/robot/robot_interface.h
+101
View File
@@ -0,0 +1,101 @@
/**
* @file robot_interface.h
* @brief 机器人控制接口抽象：状态机只依赖这个接口，不关心数据来自话题还是别的通道
*/
#pragma once
#include <atomic>
#include <vector>
#include "common_types.h"
using namespace types;
namespace interface {
class RobotInterface {
public:
/**
* @param robot_name 机器人名（仅用于打印）
* @param dof_num 自由度数量（invicta 固定 16，与 policy.onnx 绑定）
*/
RobotInterface(const std::string& robot_name, int dof_num) : robot_name_(robot_name), dof_num_(dof_num) {
start_flag_ = true;
joint_cmd_ = Eigen::Matrix<float, Eigen::Dynamic, 5>::Zero(dof_num_, 5);
}
virtual ~RobotInterface(){};
std::string robot_name_;
const int dof_num_;
Eigen::Matrix<float, Eigen::Dynamic, 5> joint_cmd_;
std::atomic<bool> start_flag_;
/// 建立通信、做启动标定
virtual void Start() = 0;
/// 停止控制、释放通信
virtual void Stop() = 0;
/// 最近一帧关节数据的时间戳 [s]
virtual double GetInterfaceTimeStamp() = 0;
/// 关节角 [rad]，长度 dof_num_
virtual VecXf GetJointPosition() = 0;
/// 关节角速度 [rad/s]
virtual VecXf GetJointVelocity() = 0;
/// 关节力矩 [Nm]
virtual VecXf GetJointTorque() = 0;
/// 基座 roll-pitch-yaw [rad]
virtual Vec3f GetImuRpy() = 0;
/// 基座加速度 [m/s^2]
virtual Vec3f GetImuAcc() = 0;
/// 基座角速度，机体系 [rad/s]
virtual Vec3f GetImuOmega() = 0;
/**
* @brief 下发关节指令（标准形式）
* torque = kp * (qDes - q) + kd * (vDes - v) + tff
* @param input dof_num_ × 5 矩阵，列为 [kp, qDes, kd, vDes, tff]
*/
virtual void SetJointCommand(Eigen::Matrix<float, Eigen::Dynamic, 5> input) = 0;
virtual MatXf GetJointCommand() { return joint_cmd_; }
/// 接触力（invicta 无足端力传感器，返回零）
virtual VecXf GetContactForce() = 0;
/// IMU 数据时间戳 [s]
virtual double GetImuTimestamp() { return 0.; }
/**
* @brief 关节电机温度 [℃]
*
* invicta 的硬件桥接节点目前不发布温度：sensor_msgs/JointState 没有对应字段。
* 这里返回常温常量，结果是 SafeController 的过温检查恒不触发（保护由硬件急停兜底）。
* 若以后桥接节点通过独立诊断话题提供温度，在此覆盖该函数即可。
*/
virtual VecXf GetMotorTemperture() { return 30. * VecXf::Ones(dof_num_); }
virtual VecXf GetDriverTemperture() { return VecXf::Zero(dof_num_); }
/**
* @brief 驱动器状态字，1 表示正常
*
* 同 GetMotorTemperture：invicta 暂不提供，返回全 1 使 DriveStatus 检查不报警。
*/
virtual std::vector<uint16_t> GetDriverStatusWord() { return std::vector<uint16_t>(dof_num_, 1); }
/**
* @brief 关节数据帧号，用于判断数据是否在更新
*/
virtual std::vector<uint16_t> GetJointDataID() { return std::vector<uint16_t>(dof_num_, 0); }
virtual void RefreshRobotData() {}
};
}; // namespace interface