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/run_policy/invicta_policy_runner.hpp
+127 -132
View File
@@ -1,88 +1,57 @@
/**
* @file invicta_policy_runner.hpp
* @brief ONNX 策略推理（由上游 M20PolicyRunner 移植）
* @brief ONNX 策略推理。关节数量、顺序、动作尺度、默认姿态、增益全部来自 invicta.toml
*
* 策略与关节顺序、动作尺度、默认关节角绑定，改这些就等于换策略，请一起改。
* 观测 57 维 = 角速度(3) + 投影重力(3) + 速度指令(3) + 关节角(16) + 关节角速度(16) + 上一步动作(16)
* 观测（M20 同款配方，维度 = 9 + 3N）：
* [角速度(3)·omega_scale, 投影重力(3), 速度指令(3), 关节角(N), 关节角速度(N)·dof_vel_scale, 上一步动作(N)]
* N = 关节数量（本构型 8）。轮关节位置是否进观测由 zero_obs_wheels 控制。
*
* 动作 → 指令：非轮关节给位置目标（action·scale + default），轮关节给速度目标。
* 配置里的 action_scale / default_joint_pos / kp / kd 按「策略顺序」（policy.joint_order）给出，
* 这里自动置换到机器人顺序。
*/
#pragma once
#define PI 3.14159265358979323846
#include <onnxruntime_c_api.h>
#include <onnxruntime_cxx_api.h>
#include <array>
#include <algorithm>
#include <cmath>
#include <ctime>
#include <string>
#include <unordered_map>
#include <utility>
#include <vector>
#include "config/deploy_config.h"
#include "policy_runner_base.hpp"
class InvictaPolicyRunner : public PolicyRunnerBase {
private:
VecXf kp_, kd_;
VecXf dof_default_eigen_policy, dof_default_eigen_robot;
Vec3f gravity_direction = Vec3f(0., 0., -1.);
const int motor_num = 16;
const int observation_dim = 57;
const int action_dim = 16;
float agent_timestep = 0.02; ///< 策略决策周期 0.02 s = 50 Hz
VecXf joint_pos_rl = VecXf(action_dim); ///< policy 关节顺序
VecXf joint_vel_rl = VecXf(action_dim);
const std::string policy_path_;
float omega_scale_ = 0.25;
float dof_vel_scale_ = 0.05;
VecXf imu_w_eigen, base_acc_eigen, motor_p_eigen, motor_v_eigen, current_action_eigen, last_action_eigen,
current_observation_, tmp_action_eigen;
RobotAction robot_action;
/// 机器人（话题）关节顺序 = /joint_states 的 name 顺序
std::vector<std::string> robot_order = {"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"};
/// 策略内部关节顺序（12 个腿关节在前，4 个轮在后），与训练导出绑定
std::vector<std::string> policy_order = {
"fl_hipx_joint", "fl_hipy_joint", "fl_knee_joint", "fr_hipx_joint", "fr_hipy_joint", "fr_knee_joint",
"hl_hipx_joint", "hl_hipy_joint", "hl_knee_joint", "hr_hipx_joint", "hr_hipy_joint", "hr_knee_joint",
"fl_wheel_joint", "fr_wheel_joint", "hl_wheel_joint", "hr_wheel_joint",
};
std::vector<float> action_scale_robot = {0.125, 0.25, 0.25, 5, 0.125, 0.25, 0.25, 5,
0.125, 0.25, 0.25, 5, 0.125, 0.25, 0.25, 5};
Ort::SessionOptions session_options_;
Ort::Session session_{nullptr};
Ort::Env env_;
std::vector<int> robot2policy_idx, policy2robot_idx;
const char* input_names_[1] = {"obs"}; // 必须与模型导出时一致
const char* output_names_[1] = {"actions"};
Ort::MemoryInfo memory_info{nullptr};
const std::array<int64_t, 2> input_observationShape = {1, observation_dim};
public:
InvictaPolicyRunner(const std::string& policy_name, const std::string& policy_path)
: PolicyRunnerBase(policy_name),
policy_path_(policy_path),
cfg_(invicta::Config()),
policy_path_(policy_path.empty() ? cfg_.policy_path : policy_path),
env_(ORT_LOGGING_LEVEL_WARNING, "InvictaPolicyRunner"),
session_options_{},
session_{nullptr},
memory_info(Ort::MemoryInfo::CreateCpu(OrtArenaAllocator, OrtMemTypeDefault)) {
dof_default_eigen_policy.setZero(action_dim);
dof_default_eigen_robot.setZero(action_dim);
dof_default_eigen_policy << 0.0, -0.3, 0.6, 0.0, -0.3, 0.6, 0.0, 0.3, -0.6, 0.0, 0.3, -0.6, 0.0, 0.0, 0.0, 0.0;
dof_default_eigen_robot << 0.0, -0.3, 0.6, 0.0, 0.0, -0.3, 0.6, 0.0, 0.0, 0.3, -0.6, 0.0, 0.0, 0.3, -0.6, 0.0;
SetDecimation(4);
action_dim_ = cfg_.action_dim;
obs_dim_ = cfg_.obs_dim;
if (action_dim_ <= 0 || obs_dim_ <= 0) {
throw std::runtime_error("策略维度为 0：配置未加载或 [policy] 段不完整");
}
robot2policy_idx_ = Permutation(cfg_.joint_names, cfg_.policy_joint_order);
policy2robot_idx_ = Permutation(cfg_.policy_joint_order, cfg_.joint_names);
ToRobotOrder(cfg_.action_scale, action_scale_robot_);
ToRobotOrder(cfg_.default_joint_pos, default_robot_);
ToRobotOrder(cfg_.policy_kp, kp_robot_);
ToRobotOrder(cfg_.policy_kd, kd_robot_);
default_policy_.resize(action_dim_);
for (int i = 0; i < action_dim_; ++i) default_policy_(i) = float(cfg_.default_joint_pos[size_t(i)]);
wheel_robot_idx_ = cfg_.WheelIndices();
SetDecimation(cfg_.policy_decimation);
session_options_.SetIntraOpNumThreads(4);
session_options_.SetGraphOptimizationLevel(GraphOptimizationLevel::ORT_ENABLE_EXTENDED);
@@ -90,99 +59,125 @@ public:
std::cerr << "Model file not found: " << policy_path_ << std::endl;
throw std::runtime_error("Model file missing");
}
session_ = Ort::Session(env_, policy_path_.c_str(), session_options_);
kp_ = Vec4f(80, 80, 80, 0.).replicate(4, 1);
kd_ = Vec4f(2, 2, 2, 0.6).replicate(4, 1);
robot2policy_idx = generate_permutation(robot_order, policy_order);
policy2robot_idx = generate_permutation(policy_order, robot_order);
current_observation_.setZero(obs_dim_);
last_action_eigen_.setZero(action_dim_);
tmp_action_eigen_.setZero(action_dim_);
current_action_eigen_.setZero(action_dim_);
joint_pos_rl_.setZero(action_dim_);
joint_vel_rl_.setZero(action_dim_);
robot_action.kp = kp_;
robot_action.kd = kd_;
robot_action.tau_ff = VecXf::Zero(motor_num);
robot_action.goal_joint_pos = VecXf::Zero(motor_num);
robot_action.goal_joint_vel = VecXf::Zero(motor_num);
robot_action_.kp = kp_robot_;
robot_action_.kd = kd_robot_;
robot_action_.tau_ff = VecXf::Zero(action_dim_);
robot_action_.goal_joint_pos = VecXf::Zero(action_dim_);
robot_action_.goal_joint_vel = VecXf::Zero(action_dim_);
current_observation_.setZero(observation_dim);
last_action_eigen.setZero(action_dim);
tmp_action_eigen.setZero(action_dim);
current_action_eigen.setZero(action_dim);
input_observation_shape_ = {1, obs_dim_};
}
~InvictaPolicyRunner() override = default;
static std::vector<int> generate_permutation(const std::vector<std::string>& from,
const std::vector<std::string>& to, int default_index = 0) {
std::unordered_map<std::string, int> idx_map;
for (int i = 0; i < static_cast<int>(from.size()); ++i) idx_map[from[i]] = i;
std::vector<int> perm;
for (const auto& name : to) {
auto it = idx_map.find(name);
perm.push_back(it != idx_map.end() ? it->second : default_index);
}
return perm;
void DisplayPolicyInfo() {
std::cout << "[policy] " << policy_path_ << " obs=" << obs_dim_ << " action=" << action_dim_
<< " decimation=" << decimation_ << std::endl;
}
void DisplayPolicyInfo() {}
void OnEnter() {
run_cnt_ = 0;
cmd_vel_input_.setZero();
last_action_eigen.setZero(action_dim);
tmp_action_eigen.setZero(action_dim);
motor_p_eigen.setZero(12);
motor_v_eigen.setZero(motor_num);
}
VecXf Onnx_infer(VecXf current_observation) {
Ort::Value input_tensor =
Ort::Value::CreateTensor<float>(memory_info, current_observation.data(),
current_observation.size(), input_observationShape.data(),
input_observationShape.size());
std::vector<Ort::Value> inputs;
inputs.emplace_back(std::move(input_tensor));
auto outputs =
session_.Run(Ort::RunOptions{nullptr}, input_names_, inputs.data(), 1, output_names_, 1);
float* action_data = outputs[0].GetTensorMutableData<float>();
Eigen::Map<Eigen::VectorXf> action_map(action_data, action_dim);
return VecXf(action_map);
last_action_eigen_.setZero(action_dim_);
tmp_action_eigen_.setZero(action_dim_);
}
RobotAction getRobotAction(const RobotBasicState& ro, const UserCommand& uc) {
Vec3f base_omega = ro.base_omega * omega_scale_;
Vec3f projected_gravity_vec = ro.base_rot_mat.inverse() * gravity_direction;
Vec3f cmd = Vec3f(uc.forward_vel_scale, uc.side_vel_scale, uc.turnning_vel_scale);
const Vec3f base_omega = ro.base_omega * float(cfg_.omega_scale);
const Vec3f projected_gravity = ro.base_rot_mat.inverse() * gravity_direction_;
const Vec3f cmd(uc.forward_vel_scale, uc.side_vel_scale, uc.turnning_vel_scale);
for (int i = 0; i < action_dim; ++i) {
joint_pos_rl(i) = ro.joint_pos(robot2policy_idx[i]);
joint_vel_rl(i) = ro.joint_vel(robot2policy_idx[i]) * dof_vel_scale_;
for (int i = 0; i < action_dim_; ++i) {
const int r = robot2policy_idx_[size_t(i)]; // 策略第 i 位对应机器人第 r 个关节
joint_pos_rl_(i) = ro.joint_pos(r);
joint_vel_rl_(i) = ro.joint_vel(r) * float(cfg_.dof_vel_scale);
}
joint_pos_rl.segment(12, 4).setZero(); // 轮关节角不进观测
joint_pos_rl -= dof_default_eigen_policy;
current_observation_ << base_omega, projected_gravity_vec, cmd, joint_pos_rl, joint_vel_rl,
last_action_eigen;
current_action_eigen = Onnx_infer(current_observation_);
last_action_eigen = current_action_eigen;
for (int i = 0; i < action_dim; ++i) {
tmp_action_eigen(i) = current_action_eigen(policy2robot_idx[i]);
tmp_action_eigen(i) *= action_scale_robot[i];
if (cfg_.zero_obs_wheels) {
for (int w : wheel_robot_idx_) joint_pos_rl_(robot2policy_idx_[size_t(w)]) = 0.f;
}
tmp_action_eigen += dof_default_eigen_robot;
joint_pos_rl_ -= default_policy_;
for (int i = 0; i < 4; ++i) {
robot_action.goal_joint_pos.segment(i * 4, 3) = tmp_action_eigen.segment(i * 4, 3);
robot_action.goal_joint_vel(i * 4 + 3) = tmp_action_eigen(i * 4 + 3);
current_observation_ << base_omega, projected_gravity, cmd, joint_pos_rl_, joint_vel_rl_, last_action_eigen_;
current_action_eigen_ = OnnxInfer(current_observation_);
last_action_eigen_ = current_action_eigen_;
for (int i = 0; i < action_dim_; ++i) {
tmp_action_eigen_(i) = current_action_eigen_(policy2robot_idx_[size_t(i)]) * action_scale_robot_(i);
}
tmp_action_eigen_ += default_robot_;
for (int i = 0; i < action_dim_; ++i) {
if (cfg_.IsWheel(i)) {
robot_action_.goal_joint_pos(i) = 0.f;
robot_action_.goal_joint_vel(i) = tmp_action_eigen_(i);
} else {
robot_action_.goal_joint_pos(i) = tmp_action_eigen_(i);
robot_action_.goal_joint_vel(i) = 0.f;
}
}
++run_cnt_;
return robot_action;
return robot_action_;
}
private:
static std::vector<int> Permutation(const std::vector<std::string>& from, const std::vector<std::string>& to) {
std::unordered_map<std::string, int> idx;
for (size_t i = 0; i < from.size(); ++i) idx[from[i]] = int(i);
std::vector<int> perm;
for (const auto& n : to) {
auto it = idx.find(n);
perm.push_back(it == idx.end() ? 0 : it->second);
}
return perm;
}
/// 把「策略顺序」的数组搬到「机器人顺序」
void ToRobotOrder(const std::vector<double>& policy_order_values, VecXf& out) {
out.resize(action_dim_);
for (int i = 0; i < action_dim_; ++i) {
out(i) = float(policy_order_values[size_t(robot2policy_idx_[size_t(i)])]);
}
}
VecXf OnnxInfer(VecXf observation) {
Ort::Value input_tensor =
Ort::Value::CreateTensor<float>(memory_info, observation.data(), observation.size(),
input_observation_shape_.data(), input_observation_shape_.size());
std::vector<Ort::Value> inputs;
inputs.emplace_back(std::move(input_tensor));
auto outputs = session_.Run(Ort::RunOptions{nullptr}, input_names_, inputs.data(), 1, output_names_, 1);
float* data = outputs[0].GetTensorMutableData<float>();
Eigen::Map<Eigen::VectorXf> mapped(data, action_dim_);
return VecXf(mapped);
}
const invicta::DeployConfig& cfg_;
const std::string policy_path_;
int action_dim_ = 0, obs_dim_ = 0;
const Vec3f gravity_direction_ = Vec3f(0., 0., -1.);
std::vector<int> robot2policy_idx_, policy2robot_idx_, wheel_robot_idx_;
VecXf action_scale_robot_, default_robot_, kp_robot_, kd_robot_, default_policy_;
VecXf joint_pos_rl_, joint_vel_rl_;
VecXf current_observation_, last_action_eigen_, tmp_action_eigen_, current_action_eigen_;
RobotAction robot_action_;
Ort::SessionOptions session_options_;
Ort::Session session_;
Ort::Env env_;
const char* input_names_[1] = {"obs"}; // 必须与模型导出时一致
const char* output_names_[1] = {"actions"};
Ort::MemoryInfo memory_info;
std::vector<int64_t> input_observation_shape_;
};