Initial commit: extract M20 sim2real into ROS1 invicta project

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.
2026-10-21 00:46:29 +08:00
commit cf70b1a1d7
2016 changed files with 489092 additions and 0 deletions
@@ -0,0 +1,188 @@
+/**
+ * @file invicta_policy_runner.hpp
+ * @brief ONNX 策略推理（由上游 M20PolicyRunner 移植）
+ *
+ * 策略与关节顺序、动作尺度、默认关节角绑定，改这些就等于换策略，请一起改。
+ * 观测 57 维 = 角速度(3) + 投影重力(3) + 速度指令(3) + 关节角(16) + 关节角速度(16) + 上一步动作(16)
+ */
+#pragma once
+#define PI 3.14159265358979323846
+
+#include <onnxruntime_c_api.h>
+#include <onnxruntime_cxx_api.h>
+
+#include <array>
+#include <cmath>
+#include <ctime>
+#include <string>
+#include <unordered_map>
+#include <utility>
+#include <vector>
+
+#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),
+          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);
+        session_options_.SetIntraOpNumThreads(4);
+        session_options_.SetGraphOptimizationLevel(GraphOptimizationLevel::ORT_ENABLE_EXTENDED);
+
+        if (access(policy_path_.c_str(), F_OK) != 0) {
+            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);
+
+        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);
+
+        current_observation_.setZero(observation_dim);
+        last_action_eigen.setZero(action_dim);
+        tmp_action_eigen.setZero(action_dim);
+        current_action_eigen.setZero(action_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() {}
+
+    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);
+    }
+
+    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);
+
+        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_;
+        }
+        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];
+        }
+        tmp_action_eigen += dof_default_eigen_robot;
+
+        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);
+        }
+
+        ++run_cnt_;
+        return robot_action;
+    }
+};