initial commit

state_machine/quadruped_wheel/idle_state.hpp

@@ -0,0 +1,207 @@
+/**
+ * @file idle_state.hpp
+ * @brief robot need to confirm sensor input while in idle state
+ * @author DeepRobotics
+ * @version 1.0
+ * @date 2025-11-07
+ * 
+ * @copyright Copyright (c) 2025  DeepRobotics
+ * 
+ */
+#pragma once
+
+
+#include "state_base.h"
+
+namespace qw {
+    class IdleState : public StateBase {
+    private:
+        int joint_normal_flag_ = -1;
+        int imu_normal_flag_ = -1;
+        bool first_enter_flag_ = true;
+        VecXf joint_pos_, joint_vel_, joint_tau_;
+        Vec3f rpy_, acc_, omg_;
+        double enter_state_time_ = 10000.;
+        double last_print_time = 0;
+
+        void GetProprioceptiveData() {
+            joint_pos_ = ri_ptr_->GetJointPosition();
+            joint_vel_ = ri_ptr_->GetJointVelocity();
+            joint_tau_ = ri_ptr_->GetJointTorque();
+            rpy_ = ri_ptr_->GetImuRpy();
+            acc_ = ri_ptr_->GetImuAcc();
+            omg_ = ri_ptr_->GetImuOmega();
+        }
+
+        int JointDataNormalCheck() {
+            if (robot_name_ == RobotName::FireAnt) {
+                if (joint_pos_.size() != 8 || joint_vel_.size() != 8 || joint_tau_.size() != 8) return 7;
+                for (int i = 0; i < 8; ++i) {
+                    if (!std::isfinite(joint_pos_(i))) return 1;
+                    if (!std::isfinite(joint_vel_(i))) return 4;
+                    if (!std::isfinite(joint_tau_(i))) return 8;
+                    if (i % 2 == 0) {
+                        const bool hind = i >= 4;
+                        const float lower = hind ? -2.809f : -2.792f;
+                        const float upper = hind ? 2.792f : 2.809f;
+                        if (joint_pos_(i) < lower - 0.2f || joint_pos_(i) > upper + 0.2f) return 2;
+                        if (std::abs(joint_vel_(i)) > 17.8024f + 0.1f) return 6;
+                    } else if (std::abs(joint_vel_(i)) > cp_ptr_->wheel_vel_limit_ + 0.1f) {
+                        return 5;
+                    }
+                }
+                return 0;
+            }
+            VecXf joint_pos_lower(16), joint_pos_upper(16);
+            Vec4f fl_lower, fl_upper;
+            fl_lower << cp_ptr_->fl_joint_lower_, -INFINITY;
+            fl_upper << cp_ptr_->fl_joint_upper_, INFINITY;
+            Vec4f fr_lower(-fl_upper(0), fl_lower(1), fl_lower(2), -INFINITY);
+            Vec4f fr_upper(-fl_lower(0), fl_upper(1), fl_upper(2), INFINITY);
+            joint_pos_lower << fl_lower, fr_lower, fl_lower, fr_lower;
+            joint_pos_upper << fl_upper, fr_upper, fl_upper, fr_upper;
+            for (int i = 0; i < 16; ++i) {
+                if (std::isnan(joint_pos_(i))){
+                    return 1;
+                }
+                if (joint_pos_(i) > joint_pos_upper(i) + 0.2){
+                    return 2;
+                }
+                if (joint_pos_(i) < joint_pos_lower(i) - 0.2){
+                    return 3;
+                }
+                if (std::isnan(joint_vel_(i))) {
+                    return 4;
+                } else {
+                    if (i % 4 == 3) {
+                        if (joint_vel_(i) > cp_ptr_->wheel_vel_limit_ + 0.1) return 5;
+                    } else {
+                        if (joint_vel_(i) > cp_ptr_->joint_vel_limit_(i % 4) + 0.1) return 6;
+                    }
+                }
+            }
+            return 0;
+        }
+
+        int ImuDataNormalCheck() {
+            for (int i = 0; i < 3; ++i) {
+                if (std::isnan(rpy_(i)) || fabs(rpy_(i)) > M_PI) {
+                    return 1;
+                }
+                if (std::isnan(omg_(i)) || fabs(omg_(i)) > M_PI) {
+                    return 2;
+                }
+            }
+            if (acc_.norm() < 0.1 * gravity || acc_.norm() > 3.0 * gravity) {
+                return 3;
+            }
+            return 0;
+        }
+
+        void DisplayProprioceptiveInfo() {
+            std::cout << "Joint Data: \n";
+            if (robot_name_ == RobotName::FireAnt) {
+                static const char *legs[] = {"FL", "FR", "HL", "HR"};
+                for (int leg = 0; leg < 4; ++leg) {
+                    std::cout << legs[leg] << " knee/wheel pos: ";
+                    PrintEigenVectorf(joint_pos_.segment(2 * leg, 2));
+                }
+                std::cout << "vel: ";
+                PrintEigenVectorf(joint_vel_);
+                std::cout << "tau: ";
+                PrintEigenVectorf(joint_tau_);
+                std::cout << "Imu Data: \n";
+                std::cout << "rpy: " << rpy_.transpose() << std::endl;
+                std::cout << "acc: " << acc_.transpose() << std::endl;
+                std::cout << "omg: " << omg_.transpose() << std::endl;
+                return;
+            }
+            std::cout << "FL pos: ";
+            PrintEigenVectorf(joint_pos_.segment(0, 4));
+            std::cout << "FR pos: ";
+            PrintEigenVectorf(joint_pos_.segment(4, 4));
+            std::cout << "HL pos: ";
+            PrintEigenVectorf(joint_pos_.segment(8, 4));
+            std::cout << "HR pos: ";
+            PrintEigenVectorf(joint_pos_.segment(12, 4));
+            std::cout << "vel: ";
+            PrintEigenVectorf(joint_vel_);
+            std::cout << "tau: ";
+            PrintEigenVectorf(joint_tau_);
+            std::cout << "Imu Data: \n";
+            std::cout << "rpy: " << rpy_.transpose() << std::endl;
+            std::cout << "acc: " << acc_.transpose() << std::endl;
+            std::cout << "omg: " << omg_.transpose() << std::endl;
+        }
+
+        void DisplayAxisValue() {
+            auto cmd = uc_ptr_->GetUserCommand();
+            std::cout << "User Command Input: \n";
+            std::cout << "axis value:  " << cmd->forward_vel_scale << " "
+                      << cmd->side_vel_scale << " "
+                      << cmd->turnning_vel_scale << std::endl;
+            std::cout << "target mode: " << int(cmd->target_mode) << std::endl;
+        }
+
+        void PrintEigenVectorf(const VecXf &vec, int width = 6) {
+            for (int i = 0; i < vec.size(); ++i) {
+                std::cout << std::setw(width)
+                          << std::setprecision(3)
+                          << std::right
+                          << vec(i) << " ";  // 输出矩阵元素
+            }
+            std::cout << "\n";
+        }
+
+
+    public:
+        IdleState(const RobotName &robot_name, const std::string &state_name,
+                  std::shared_ptr<ControllerData> data_ptr) : StateBase(robot_name, state_name, data_ptr) {
+        }
+
+        ~IdleState() {}
+
+        virtual void OnEnter() {
+            StateBase::msfb_.UpdateCurrentState(RobotMotionState::WaitingForStand);
+            enter_state_time_ = ri_ptr_->GetInterfaceTimeStamp();
+        };
+
+        virtual void OnExit() {
+            first_enter_flag_ = false;
+        }
+
+        virtual void Run() {
+            
+            GetProprioceptiveData();
+            joint_normal_flag_ = JointDataNormalCheck();
+            if (((ri_ptr_->GetInterfaceTimeStamp() - last_print_time) > 1)) {
+                DisplayProprioceptiveInfo();
+                DisplayAxisValue();
+                last_print_time = ri_ptr_->GetInterfaceTimeStamp();
+            }
+            MatXf cmd = MatXf::Zero(ri_ptr_->dof_num_, 5);
+            ri_ptr_->SetJointCommand(cmd);
+        }
+
+        virtual bool LoseControlJudge() {
+            return false;
+        }
+
+        virtual StateName GetNextStateName() {
+            if ((joint_normal_flag_ != 0)) {
+                std::cout << "========== warning ========>> "
+                          << "joint status: " << joint_normal_flag_
+                          << std::endl;
+                return StateName::kIdle;
+            }
+            if (uc_ptr_->GetUserCommand()->safe_control_mode != 0) {
+                return StateName::kIdle;
+            }
+
+            if (uc_ptr_->GetUserCommand()->target_mode == uint8_t(RobotMotionState::StandingUp))
+                return StateName::kStandUp;
+            return StateName::kIdle;
+        }
+    };
+
+};

state_machine/quadruped_wheel/joint_damping_state.hpp

@@ -0,0 +1,65 @@
+/**
+ * @file joint_damping_state.hpp
+ * @brief joint passive control state
+ * @author DeepRobotics
+ * @version 1.0
+ * @date 2025-11-07
+ * 
+ * @copyright Copyright (c) 2025  DeepRobotics
+ * 
+ */
+#pragma once
+
+#include "state_base.h"
+
+namespace qw{
+class JointDampingState : public StateBase{
+private:
+    float time_record_, run_time_;
+    MatXf joint_cmd_;
+public:
+    JointDampingState(const RobotName& robot_name, const std::string& state_name, 
+        std::shared_ptr<ControllerData> data_ptr):StateBase(robot_name, state_name, data_ptr){
+            if (robot_name_ == RobotName::FireAnt) {
+                VecXf kd_ = VecXf::Zero(8);
+                for (int leg = 0; leg < 4; ++leg) {
+                    kd_(2 * leg) = 2.0f;
+                    kd_(2 * leg + 1) = 0.6f;
+                }
+                joint_cmd_ = MatXf::Zero(8, 5);
+                joint_cmd_.col(2) = kd_;
+                return;
+            }
+            Vec4f one_leg_kd;
+            one_leg_kd << cp_ptr_->swing_leg_kd_, 1.;
+            VecXf kd_ = one_leg_kd.replicate(4, 1);
+            joint_cmd_ = MatXf::Zero(16, 5);
+            joint_cmd_.col(2) = kd_;
+        }
+    ~JointDampingState(){}
+
+    virtual void OnEnter() {
+        time_record_ = ri_ptr_->GetInterfaceTimeStamp();
+        run_time_ = ri_ptr_->GetInterfaceTimeStamp();
+        msfb_.UpdateCurrentState(RobotMotionState::JointDamping);
+    };
+    virtual void OnExit() {
+
+    }
+    virtual void Run() {
+        run_time_ = ri_ptr_->GetInterfaceTimeStamp();
+        ri_ptr_->SetJointCommand(joint_cmd_);
+    }
+    virtual bool LoseControlJudge() {
+        return false;
+    }
+    virtual StateName GetNextStateName() {
+        if(run_time_ - time_record_ < 3.){
+            return StateName::kJointDamping;
+        }
+        return StateName::kIdle;
+    }
+};
+
+
+};

state_machine/quadruped_wheel/liedown_state.hpp

@@ -0,0 +1,170 @@
+/**
+ * @file liedown_state.hpp
+ * @brief liedown state
+ * @author DeepRobotics
+ * @version 1.0
+ * @date 2026-02-12
+ * 
+ * @copyright Copyright (c) 2025  DeepRobotics
+ * 
+ */
+#pragma once
+
+#include "state_base.h"
+
+namespace qw{
+class LieDownState : public StateBase{
+private:
+    VecXf init_joint_pos_, init_joint_vel_, current_joint_pos_, current_joint_vel_;
+    float time_stamp_record_, run_time_;
+    VecXf goal_joint_pos_, kp_, kd_;
+    MatXf joint_cmd_;
+    float liedown_duration_ = 2.;
+
+    const float init_hipx_pos_ = Deg2Rad(0.);
+    const float set_wheel_kd_ = 1.;
+
+    void GetRobotJointValue(){
+        current_joint_pos_ = ri_ptr_->GetJointPosition();
+        current_joint_vel_ = ri_ptr_->GetJointVelocity();
+        run_time_ = ri_ptr_->GetInterfaceTimeStamp();
+    }
+
+    void RecordJointData(){
+        init_joint_pos_ = current_joint_pos_;
+        init_joint_vel_ = current_joint_vel_;
+        time_stamp_record_ = run_time_;
+    }
+    
+    float GetHipYPosByHeight(float h){
+        float l1 = cp_ptr_->thigh_len_;
+        float l2 = cp_ptr_->shank_len_;
+        float default_pos = (cp_ptr_->fl_joint_lower_(1)+cp_ptr_->fl_joint_upper_(1)) / 2.;
+        if(fabs(h) >= l1 + l2) {
+            std::cerr << "error height input" << std::endl;
+            return 0;
+        }
+        float theta = -acos((l1*l1+h*h-l2*l2)/(2.*h*l1));
+        LimitNumber(theta, cp_ptr_->fl_joint_lower_(1), cp_ptr_->fl_joint_upper_(1));
+        return theta;
+    }
+
+    float GetKneePosByHeight(float h){
+        float l1 = cp_ptr_->thigh_len_;
+        float l2 = cp_ptr_->shank_len_;
+        float default_pos = (cp_ptr_->fl_joint_lower_(2)+cp_ptr_->fl_joint_upper_(2)) / 2.;
+        if(fabs(h) >= l1 + l2) {
+            std::cerr << "error height input" << std::endl;
+            return 0;
+        }
+        float theta = M_PI-acos((l1*l1+l2*l2-h*h)/(2*l1*l2));
+        LimitNumber(theta, cp_ptr_->fl_joint_lower_(2), cp_ptr_->fl_joint_upper_(2));
+        return theta;
+    }
+
+public:
+    LieDownState(const RobotName& robot_name, const std::string& state_name, 
+        std::shared_ptr<ControllerData> data_ptr):StateBase(robot_name, state_name, data_ptr){
+            if (robot_name_ == RobotName::FireAnt) {
+                init_joint_pos_ = VecXf::Zero(8);
+                init_joint_vel_ = VecXf::Zero(8);
+                current_joint_pos_ = VecXf::Zero(8);
+                current_joint_vel_ = VecXf::Zero(8);
+                goal_joint_pos_ = VecXf::Zero(8);
+                goal_joint_pos_ << 1.35f, 0.0f, 1.35f, 0.0f, -1.35f, 0.0f, -1.35f, 0.0f;
+                kp_ = VecXf::Zero(8);
+                kd_ = VecXf::Zero(8);
+                for (int leg = 0; leg < 4; ++leg) {
+                    kp_(2 * leg) = 80.0f;
+                    kd_(2 * leg) = 2.0f;
+                    kd_(2 * leg + 1) = 0.6f;
+                }
+                liedown_duration_ = cp_ptr_->liedown_duration_;
+                return;
+            }
+            init_joint_pos_ = VecXf::Zero(16); 
+            init_joint_vel_ = VecXf::Zero(16);
+            current_joint_pos_ = VecXf::Zero(16);
+            current_joint_vel_ = VecXf::Zero(16);
+            
+            goal_joint_pos_ = Vec4f(init_hipx_pos_, GetHipYPosByHeight(0.03), GetKneePosByHeight(0.03), 0).replicate(4, 1);
+            goal_joint_pos_(4) = -init_hipx_pos_; goal_joint_pos_(12) = -init_hipx_pos_;
+
+            goal_joint_pos_(9) = -goal_joint_pos_(9);
+            goal_joint_pos_(10) = -goal_joint_pos_(10);
+            goal_joint_pos_(13) = -goal_joint_pos_(13);
+            goal_joint_pos_(14) = -goal_joint_pos_(14);
+
+            Vec4f one_leg_kp, one_leg_kd;
+
+            one_leg_kp << 300, 300, 300, 0;
+            one_leg_kd << 4.5, 4.5, 4.5, 3; //2; //0;
+            kp_ = one_leg_kp.replicate(4, 1);
+            kd_ = one_leg_kd.replicate(4, 1);
+            liedown_duration_ = cp_ptr_->liedown_duration_;
+        }
+    ~LieDownState(){}
+
+    virtual void OnEnter() {
+        joint_cmd_ = MatXf::Zero(ri_ptr_->dof_num_, 5);
+        joint_cmd_.col(0) = kp_;
+        joint_cmd_.col(2) = kd_;
+        GetRobotJointValue();
+        RecordJointData();  
+        StateBase::msfb_.UpdateCurrentState(RobotMotionState::LieDown);
+    };
+
+    virtual void OnExit() {
+    }
+
+    virtual void Run() {
+        GetRobotJointValue();
+        VecXf planning_joint_pos(current_joint_pos_.rows());
+        VecXf planning_joint_vel(current_joint_pos_.rows());
+        if(run_time_ - time_stamp_record_ <= liedown_duration_){
+            for(int i=0;i<current_joint_pos_.rows();++i){
+                planning_joint_pos(i) = GetCubicSplinePos(init_joint_pos_(i), init_joint_vel_(i), goal_joint_pos_(i), 0, 
+                                                run_time_ - time_stamp_record_, liedown_duration_);
+                planning_joint_vel(i) = GetCubicSplineVel(init_joint_pos_(i), init_joint_vel_(i), goal_joint_pos_(i), 0, 
+                                                run_time_ - time_stamp_record_, liedown_duration_);
+                if((robot_name_ == RobotName::FireAnt && i % 2 == 1) ||
+                   (robot_name_ == RobotName::M20 && i % 4 == 3)){
+                    planning_joint_vel(i) = 0.0;
+                }
+            }
+
+            joint_cmd_.col(1) = planning_joint_pos;
+            joint_cmd_.col(3) = planning_joint_vel;
+            joint_cmd_.col(2) = kd_;
+            ri_ptr_->SetJointCommand(joint_cmd_);
+        } else if (run_time_ - time_stamp_record_ <=  2.0 * liedown_duration_){
+            joint_cmd_ = MatXf::Zero(ri_ptr_->dof_num_, 5);
+            joint_cmd_.col(2) = kd_;
+            
+            ri_ptr_->SetJointCommand(joint_cmd_);
+        } else {
+            joint_cmd_ = MatXf::Zero(ri_ptr_->dof_num_, 5);
+            ri_ptr_->SetJointCommand(joint_cmd_);
+        }
+    }
+    virtual bool LoseControlJudge() {
+        if (uc_ptr_->GetUserCommand()->target_mode == uint8_t(RobotMotionState::JointDamping)) return true;
+        return false;
+    }
+    virtual StateName GetNextStateName() {
+        if(uc_ptr_->GetUserCommand()->safe_control_mode!=0){
+            return StateName::kJointDamping;
+        } 
+        
+        if(run_time_ - time_stamp_record_ <= 2.*liedown_duration_){
+            return StateName::kLieDown;
+        }else{
+            if(uc_ptr_->GetUserCommand()->target_mode == uint8_t(RobotMotionState::StandingUp)){
+                return StateName::kStandUp;
+            }
+        }
+        return StateName::kLieDown;
+    }
+};
+
+};

state_machine/quadruped_wheel/qw_state_machine.hpp

@@ -0,0 +1,134 @@
+/**
+ * @file quadruped_state_machine.hpp
+ * @brief for robot to switch control state by user command input
+ * @author DeepRobotics
+ * @version 1.0
+ * @date 2025-11-07
+ * 
+ * @copyright Copyright (c) 2025  DeepRobotics
+ * 
+ */
+#pragma once
+
+#include "state_base.h"
+#include "state_machine_base.h"
+#include "quadruped_wheel/idle_state.hpp"
+#include "quadruped_wheel/standup_state.hpp"
+#include "quadruped_wheel/joint_damping_state.hpp"
+#include "quadruped_wheel/rl_control_state.hpp"
+#include "quadruped_wheel/liedown_state.hpp"
+#include "keyboard_interface.hpp"
+#include "hardware/m20_interface.hpp"
+#include "hardware/fire_ant_interface.hpp"
+#include "udp_server.hpp"
+
+namespace qw{
+class QwStateMachine : public StateMachineBase{
+private:
+
+    std::shared_ptr<StateBase> idle_controller_;
+    std::shared_ptr<StateBase> standup_controller_;
+    std::shared_ptr<StateBase> rl_controller_;
+    std::shared_ptr<StateBase> joint_damping_controller_;
+    std::shared_ptr<StateBase> car_move_controller_;
+    std::shared_ptr<StateBase> liedown_controller_;
+
+    std::shared_ptr<UdpServer> udp_server_;
+
+public:
+    const RobotName robot_name_;
+    const RemoteCommandType remote_cmd_type_;
+
+    QwStateMachine(RobotName robot_name, RemoteCommandType rct):StateMachineBase(RobotType::QuadrupedWheel),
+    robot_name_(robot_name),
+    remote_cmd_type_(rct) {}
+    ~QwStateMachine(){}
+
+    void Start(){
+        if(remote_cmd_type_ == RemoteCommandType::kKeyBoard){
+            uc_ptr_ = std::make_shared<KeyboardInterface>(robot_name_);
+        }else if(remote_cmd_type_ == RemoteCommandType::kGamepad){
+            auto gp_ptr = std::make_shared<GamepadInterface>(robot_name_);
+            udp_server_ = std::make_shared<UdpServer>(gp_ptr.get());
+            uc_ptr_ = gp_ptr;
+        }else{
+            std::cerr << "error user command interface! " << std::endl;
+            exit(0);
+        }
+        uc_ptr_->SetMotionStateFeedback(&StateBase::msfb_);
+
+        if(robot_name_ == RobotName::M20){
+   
+            ri_ptr_ = std::make_shared<M20Interface>("M20");
+            
+            cp_ptr_ = std::make_shared<ControlParameters>(robot_name_);
+        } else if (robot_name_ == RobotName::FireAnt) {
+            ri_ptr_ = std::make_shared<FireAntInterface>("FireAnt");
+            cp_ptr_ = std::make_shared<ControlParameters>(robot_name_);
+        } else {
+            throw std::runtime_error("Unsupported robot type");
+        }
+
+        std::shared_ptr<ControllerData> data_ptr = std::make_shared<ControllerData>();
+        data_ptr->ri_ptr = ri_ptr_;
+        data_ptr->uc_ptr = uc_ptr_;
+        data_ptr->cp_ptr = cp_ptr_;
+
+        sc_ptr_ = std::make_shared<SafeController>(QuadrupedWheel, "");
+        sc_ptr_->SetRobotDataSource(ri_ptr_);
+        sc_ptr_->SetUserCommandDataSource(uc_ptr_);
+
+        idle_controller_ = std::make_shared<IdleState>(robot_name_, "idle_state", data_ptr);
+        standup_controller_ = std::make_shared<StandUpState>(robot_name_, "standup_state", data_ptr);
+        rl_controller_ = std::make_shared<RLControlState>(robot_name_, "rl_control", data_ptr);
+        joint_damping_controller_ = std::make_shared<JointDampingState>(robot_name_, "joint_damping", data_ptr);
+        liedown_controller_ = std::make_shared<LieDownState>(robot_name_, "liedown_state", data_ptr);
+
+        current_controller_ = idle_controller_;
+        current_state_name_ = kIdle;
+        next_state_name_ = kIdle;
+
+        ri_ptr_->Start();
+        uc_ptr_->Start();
+        sc_ptr_->Start();
+        current_controller_->OnEnter();
+    }
+
+
+    std::shared_ptr<StateBase> GetStateControllerPtr(StateName state_name){
+        switch(state_name){
+            case StateName::kInvalid:{
+                return nullptr;
+            }
+            case StateName::kIdle:{
+                return idle_controller_;
+            }
+            case StateName::kStandUp:{
+                return standup_controller_;
+            }
+            case StateName::kRLControl:{
+                return rl_controller_;
+            }
+            case StateName::kJointDamping:{
+                return joint_damping_controller_;
+            }
+            case StateName::kLieDown:
+            {
+                return liedown_controller_;
+            }
+            default:{
+                std::cerr << "error state name" << std::endl;
+                return joint_damping_controller_;
+            }
+        }
+        return nullptr;
+    }
+
+    void Stop(){
+        sc_ptr_->Stop();
+        uc_ptr_->Stop();
+        ri_ptr_->Stop();
+    }
+
+};
+};

state_machine/quadruped_wheel/rl_control_state.hpp

@@ -0,0 +1,159 @@
+/**
+ * @file rl_control_state.hpp
+ * @brief rl policy runnning state for quadruped-wheel robot
+ * @author DeepRobotics
+ * @version 1.0
+ * @date 2025-11-07
+ * 
+ * @copyright Copyright (c) 2025  DeepRobotics
+ * 
+ */
+#pragma once
+#include "state_base.h"
+#include "policy_runner_base.hpp"
+#include "m20_policy_runner.hpp"
+#include "fire_ant_policy_runner.hpp"
+#include "robot_interface.h"
+#include "user_command_interface.h"
+#include "json.hpp"
+#include "basic_function.hpp"
+
+namespace qw {
+    class RLControlState : public StateBase {
+    private:
+        RobotBasicState rbs_[2];
+        std::atomic<int> rbs_write_index_{0};
+        int getrbsReadIndex() const { return 1 - rbs_write_index_.load(std::memory_order_acquire); }
+
+        std::atomic<int> state_run_cnt_{0};
+
+        std::shared_ptr<PolicyRunnerBase> policy_ptr_;
+        std::shared_ptr<M20PolicyRunner> m20_policy_;
+        std::shared_ptr<FireAntPolicyRunner> fire_ant_policy_;
+
+        std::thread run_policy_thread_;
+        std::atomic<bool> start_flag_{true};
+
+        float policy_cost_time_ = 1;
+
+        Eigen::MatrixXf acc_rot = Eigen::MatrixXf::Zero(20, 3);
+        int acc_rot_count = 0;
+
+        void UpdateRobotObservation() {
+            int write_idx = rbs_write_index_.load(std::memory_order_relaxed);
+            RobotBasicState& buffer = rbs_[write_idx];
+
+            buffer.base_rpy = ri_ptr_->GetImuRpy();
+            buffer.base_rot_mat = RpyToRm(buffer.base_rpy);
+            buffer.base_omega = ri_ptr_->GetImuOmega();
+            buffer.base_acc = ri_ptr_->GetImuAcc();
+            buffer.base_lin_vel = ri_ptr_->GetBaseLinearVelocity();
+            buffer.joint_pos = ri_ptr_->GetJointPosition();
+            buffer.joint_vel = ri_ptr_->GetJointVelocity();
+            buffer.joint_tau = ri_ptr_->GetJointTorque();
+            buffer.contact_force = ri_ptr_->GetContactForce();
+
+            // 储存
+            buffer.flt_base_acc_mat.row(acc_rot_count) = buffer.base_acc.transpose();
+            acc_rot_count += 1;
+            acc_rot_count = acc_rot_count % 20;
+
+            rbs_write_index_.store(1 - write_idx,  std::memory_order_release);
+        }
+
+        void PolicyRunner() {
+            int run_cnt_record = -1;
+            while (start_flag_) {
+                const int state_run_cnt = state_run_cnt_.load(std::memory_order_acquire);
+                if (state_run_cnt % policy_ptr_->decimation_ == 0 && state_run_cnt != run_cnt_record) {
+                    timespec start_timestamp, end_timestamp;
+                    clock_gettime(CLOCK_MONOTONIC, &start_timestamp);
+                    auto ra = policy_ptr_->getRobotAction(rbs_[getrbsReadIndex()], *(uc_ptr_->GetUserCommand()));
+                    
+                    MatXf res = ra.ConvertToMat();
+
+                    ri_ptr_->SetJointCommand(res);
+                    run_cnt_record = state_run_cnt;
+                    clock_gettime(CLOCK_MONOTONIC, &end_timestamp);
+                    policy_cost_time_ = (end_timestamp.tv_sec - start_timestamp.tv_sec) * 1e3
+                                        + (end_timestamp.tv_nsec - start_timestamp.tv_nsec) / 1e6;
+
+                }
+                std::this_thread::sleep_for(std::chrono::microseconds(100));
+            }
+        }
+
+    public:
+        RLControlState(const RobotName &robot_name, const std::string &state_name,
+                       std::shared_ptr<ControllerData> data_ptr) : StateBase(robot_name, state_name, data_ptr) {
+            rbs_[0] = RobotBasicState(ri_ptr_->dof_num_);
+            rbs_[1] = RobotBasicState(ri_ptr_->dof_num_);
+            if (robot_name_ == RobotName::M20) {
+                namespace fs = std::filesystem;
+                fs::path base = fs::path(__FILE__).parent_path();
+                auto model_path = fs::canonical(base / ".." / ".." / "policy" / "policy.onnx");
+                m20_policy_ = std::make_shared<M20PolicyRunner>("m20_policy", model_path.string());
+                policy_ptr_ = m20_policy_;
+            } else if (robot_name_ == RobotName::FireAnt) {
+                namespace fs = std::filesystem;
+                fs::path base = fs::path(__FILE__).parent_path();
+                auto model_path = fs::canonical(base / ".." / ".." / "policy" / "fire_ant_policy.onnx");
+                fire_ant_policy_ = std::make_shared<FireAntPolicyRunner>("fire_ant_stage2", model_path.string());
+                policy_ptr_ = fire_ant_policy_;
+            }
+
+            if (!policy_ptr_) {
+                std::cerr << "error policy" << std::endl;
+                exit(0);
+            }
+            policy_ptr_->DisplayPolicyInfo();
+        }
+
+        ~RLControlState() {
+            start_flag_ = false;
+            if (run_policy_thread_.joinable()) run_policy_thread_.join();
+        }
+
+        virtual void OnEnter() {
+            state_run_cnt_.store(-1, std::memory_order_release);
+            start_flag_ = true;
+            policy_ptr_->OnEnter();
+            run_policy_thread_ = std::thread(std::bind(&RLControlState::PolicyRunner, this));
+            StateBase::msfb_.UpdateCurrentState(RobotMotionState::RLControlMode);
+        };
+
+        virtual void OnExit() {
+            start_flag_ = false;
+            if (run_policy_thread_.joinable()) run_policy_thread_.join();
+            state_run_cnt_.store(-1, std::memory_order_release);
+        }
+
+        virtual void Run() {
+            UpdateRobotObservation();
+            state_run_cnt_.fetch_add(1, std::memory_order_release);
+        }
+
+        virtual bool LoseControlJudge() {
+            if (uc_ptr_->GetUserCommand()->target_mode == uint8_t(RobotMotionState::JointDamping)) return true;
+            return PostureUnsafeCheck();
+        }
+
+        bool PostureUnsafeCheck() {
+            // Vec3f rpy = ri_ptr_->GetImuRpy();
+            // if(rpy(0) > 30./180*M_PI || rpy(1) > 45./180*M_PI){
+            //     std::cout << "posture value: " << 180./M_PI*rpy.transpose() << std::endl;
+            //     return true;
+            // }
+            return false;
+        }
+
+        virtual StateName GetNextStateName() {
+            if (uc_ptr_->GetUserCommand()->safe_control_mode != 0) 
+                return StateName::kJointDamping;
+            if (uc_ptr_->GetUserCommand()->target_mode == uint8_t(RobotMotionState::LieDown))
+                return StateName::kLieDown;
+            
+            return StateName::kRLControl;
+        }
+    };
+};

state_machine/quadruped_wheel/standup_state.hpp

@@ -0,0 +1,198 @@
+/**
+ * @file standup_state.hpp
+ * @brief from sit state to stand state
+ * @author DeepRobotics
+ * @version 1.0
+ * @date 2025-11-07
+ * 
+ * @copyright Copyright (c) 2025  DeepRobotics
+ * 
+ */
+#pragma once
+
+#include "state_base.h"
+
+namespace qw{
+class StandUpState : public StateBase{
+private:
+    VecXf init_joint_pos_, init_joint_vel_, current_joint_pos_, current_joint_vel_;
+    double time_stamp_record_, run_time_;
+    VecXf goal_joint_pos_, kp_, kd_;
+    MatXf joint_cmd_;
+    double stand_duration_ = 2.;
+
+    const float init_hipx_pos_ = Deg2Rad(0.);
+    const float set_wheel_kd_ = 1.;
+
+    void GetRobotJointValue(){
+        current_joint_pos_ = ri_ptr_->GetJointPosition();
+        current_joint_vel_ = ri_ptr_->GetJointVelocity();
+        run_time_ = ri_ptr_->GetInterfaceTimeStamp();
+    }
+
+    void RecordJointData(){
+        init_joint_pos_ = current_joint_pos_;
+        init_joint_vel_ = current_joint_vel_;
+        time_stamp_record_ = run_time_;
+    }
+
+    float GetHipYPosByHeight(float h){
+        float l1 = cp_ptr_->thigh_len_;
+        float l2 = cp_ptr_->shank_len_;
+        float default_pos = (cp_ptr_->fl_joint_lower_(1)+cp_ptr_->fl_joint_upper_(1)) / 2.;
+        if(fabs(h) >= l1 + l2) {
+            std::cerr << "error height input" << std::endl;
+            return 0;
+        }
+        float theta = -acos((l1*l1+h*h-l2*l2)/(2.*h*l1));
+        theta = LimitNumber(theta, cp_ptr_->fl_joint_lower_(1), cp_ptr_->fl_joint_upper_(1));
+        return theta;
+    }
+
+    float GetKneePosByHeight(float h){
+        float l1 = cp_ptr_->thigh_len_;
+        float l2 = cp_ptr_->shank_len_;
+        float default_pos = (cp_ptr_->fl_joint_lower_(2)+cp_ptr_->fl_joint_upper_(2)) / 2.;
+        if(fabs(h) >= l1 + l2) {
+            std::cerr << "error height input" << std::endl;
+            return 0;
+        }
+        float theta = M_PI-acos((l1*l1+l2*l2-h*h)/(2*l1*l2));
+        theta = LimitNumber(theta, cp_ptr_->fl_joint_lower_(2), cp_ptr_->fl_joint_upper_(2));
+        return theta;
+    }
+
+public:
+    StandUpState(const RobotName& robot_name, const std::string& state_name, 
+        std::shared_ptr<ControllerData> data_ptr):StateBase(robot_name, state_name, data_ptr){
+            if (robot_name_ == RobotName::FireAnt) {
+                goal_joint_pos_ = VecXf::Zero(8);
+                goal_joint_pos_ << 0.5f, 0.0f, 0.5f, 0.0f, -0.5f, 0.0f, -0.5f, 0.0f;
+                kp_ = VecXf::Zero(8);
+                kd_ = VecXf::Zero(8);
+                for (int leg = 0; leg < 4; ++leg) {
+                    kp_(2 * leg) = 80.0f;
+                    kd_(2 * leg) = 2.0f;
+                    kd_(2 * leg + 1) = 0.6f;
+                }
+                joint_cmd_ = MatXf::Zero(8, 5);
+                joint_cmd_.col(0) = kp_;
+                joint_cmd_.col(2) = kd_;
+                stand_duration_ = cp_ptr_->stand_duration_;
+                return;
+            }
+            goal_joint_pos_ = Vec4f(init_hipx_pos_, GetHipYPosByHeight(cp_ptr_->pre_height_), GetKneePosByHeight(cp_ptr_->pre_height_), 0).replicate(4, 1);
+            goal_joint_pos_(4) = -init_hipx_pos_; goal_joint_pos_(12) = -init_hipx_pos_;
+            // 后髋膝取反
+            goal_joint_pos_(9) = -goal_joint_pos_(9);
+            goal_joint_pos_(10) = -goal_joint_pos_(10);
+            goal_joint_pos_(13) = -goal_joint_pos_(13);
+            goal_joint_pos_(14) = -goal_joint_pos_(14);
+
+            Vec4f one_leg_kp, one_leg_kd;
+            one_leg_kp << cp_ptr_->swing_leg_kp_, 0;
+            one_leg_kd << cp_ptr_->swing_leg_kd_, 0;
+            kp_ = one_leg_kp.replicate(4, 1);
+            kd_ = one_leg_kd.replicate(4, 1);
+            joint_cmd_ = MatXf::Zero(16, 5);
+            joint_cmd_.col(0) = kp_;
+            joint_cmd_.col(2) = kd_;
+            stand_duration_ = cp_ptr_->stand_duration_;
+        }
+    ~StandUpState(){}
+
+    virtual void OnEnter() {
+        GetRobotJointValue();
+        RecordJointData();
+        StateBase::msfb_.UpdateCurrentState(RobotMotionState::StandingUp);
+    };
+    virtual void OnExit() {
+    }
+    virtual void Run() {
+        GetRobotJointValue();
+        VecXf planning_joint_pos(current_joint_pos_.rows());
+        VecXf planning_joint_vel(current_joint_pos_.rows());
+        if (robot_name_ == RobotName::FireAnt) {
+            const double elapsed = run_time_ - time_stamp_record_;
+            for (int i = 0; i < current_joint_pos_.rows(); ++i) {
+                planning_joint_pos(i) = GetCubicSplinePos(
+                    init_joint_pos_(i), init_joint_vel_(i), goal_joint_pos_(i), 0.0f,
+                    elapsed, stand_duration_);
+                planning_joint_vel(i) = GetCubicSplineVel(
+                    init_joint_pos_(i), init_joint_vel_(i), goal_joint_pos_(i), 0.0f,
+                    elapsed, stand_duration_);
+                if (i % 2 == 1) planning_joint_vel(i) = 0.0f;
+            }
+            joint_cmd_.col(1) = planning_joint_pos;
+            joint_cmd_.col(3) = planning_joint_vel;
+            ri_ptr_->SetJointCommand(joint_cmd_);
+            return;
+        }
+        if(run_time_ - time_stamp_record_ <= stand_duration_){
+            for(int i=0;i<current_joint_pos_.rows();++i){
+                planning_joint_pos(i) = GetCubicSplinePos(init_joint_pos_(i), init_joint_vel_(i), goal_joint_pos_(i), 0, 
+                                                run_time_ - time_stamp_record_, stand_duration_);
+                planning_joint_vel(i) = GetCubicSplineVel(init_joint_pos_(i), init_joint_vel_(i), goal_joint_pos_(i), 0, 
+                                                run_time_ - time_stamp_record_, stand_duration_);
+                if(i%4==3){
+                    kd_(i) = 0;
+                }
+            }
+            
+        }else{
+            float new_time = run_time_ - time_stamp_record_ - stand_duration_;
+            float dt = 0.001;
+            float plan_height = GetCubicSplinePos(cp_ptr_->pre_height_, 0, cp_ptr_->stand_height_, 0, 
+                                                new_time, stand_duration_);
+            float plan_height_next = GetCubicSplinePos(cp_ptr_->pre_height_, 0, cp_ptr_->stand_height_, 0, 
+                                                new_time+dt, stand_duration_);
+            float hipy_pos = GetHipYPosByHeight(plan_height);
+            float hipy_vel = (GetHipYPosByHeight(plan_height_next) - hipy_pos) / dt;
+            float knee_pos = GetKneePosByHeight(plan_height);
+            float knee_vel = (GetKneePosByHeight(plan_height_next) - knee_pos) / dt;
+            planning_joint_pos = Vec4f(init_hipx_pos_, hipy_pos, knee_pos, 0).replicate(4, 1);
+            planning_joint_pos(4) = -init_hipx_pos_; planning_joint_pos(12) = -init_hipx_pos_;
+
+            planning_joint_vel = Vec4f(0, hipy_vel, knee_vel, 0).replicate(4, 1);
+
+            // 后髋膝取反
+            planning_joint_pos(9) = -planning_joint_pos(9);
+            planning_joint_pos(10) = -planning_joint_pos(10);
+            planning_joint_pos(13) = -planning_joint_pos(13);
+            planning_joint_pos(14) = -planning_joint_pos(14);
+            planning_joint_vel(9) = -planning_joint_vel(9);
+            planning_joint_vel(10) = -planning_joint_vel(10);
+            planning_joint_vel(13) = -planning_joint_vel(13);
+            planning_joint_vel(14) = -planning_joint_vel(14);
+
+            for(int i=3;i<16;i+=4){
+                kd_(i) = set_wheel_kd_;
+            }
+        }
+
+        joint_cmd_.col(1) = planning_joint_pos;
+        joint_cmd_.col(3) = planning_joint_vel;
+        joint_cmd_.col(2) = kd_;
+        ri_ptr_->SetJointCommand(joint_cmd_);
+    }
+    virtual bool LoseControlJudge() {
+        if(uc_ptr_->GetUserCommand()->target_mode == uint8_t(RobotMotionState::JointDamping)) return true;
+        return false;
+    }
+    virtual StateName GetNextStateName() {
+        if(uc_ptr_->GetUserCommand()->safe_control_mode!=0) return StateName::kJointDamping;
+        const double ready_time = robot_name_ == RobotName::FireAnt ? stand_duration_ : 2.0 * stand_duration_;
+        if(run_time_ - time_stamp_record_ <= ready_time){
+            return StateName::kStandUp;
+        }else{
+            if(uc_ptr_->GetUserCommand()->target_mode == uint8_t(RobotMotionState::RLControlMode)){
+                return StateName::kRLControl;
+            }else if(uc_ptr_->GetUserCommand()->target_mode == uint8_t(RobotMotionState::LieDown)){
+                return StateName::kLieDown;
+            }
+        }
+        return StateName::kStandUp;
+    }
+};
+
+};