From a8c104ea3547ca43a540e863981f5ff297becd66 Mon Sep 17 00:00:00 2001 From: Aliyah-li <1120540191@qq.com> Date: Thu, 10 Sep 2026 20:48:46 +0800 Subject: [PATCH] Add Lite3 lie down state --- README.md | 17 ++- .../robot/simulation/mujoco_simulation.py | 9 +- interface/user_command/keyboard_interface.hpp | 6 +- .../retroid_gamepad_interface.hpp | 8 +- .../user_command/xbox_gamepad_interface.hpp | 13 +- state_machine/liedown_state.hpp | 134 ++++++++++++++++++ state_machine/rl_control_state.hpp | 2 +- state_machine/rl_control_state_onnx.hpp | 3 +- state_machine/standup_state.hpp | 2 + state_machine/state_machine.hpp | 8 +- types/custom_types.h | 4 +- 11 files changed, 196 insertions(+), 10 deletions(-) create mode 100644 state_machine/liedown_state.hpp diff --git a/README.md b/README.md index 3ae9b5a1..adc3a83e 100644 --- a/README.md +++ b/README.md @@ -47,6 +47,8 @@ tips：可以将仿真器窗口设为始终位于最上层，方便可视化 **键盘控制：** - z： 机器狗站立进入默认状态 - c： 机器狗站立进入rl控制状态 +- x： 机器狗趴下 +- r： 关节阻尼模式 - wasd：前后左右 - qe：顺逆时针旋转 @@ -56,6 +58,7 @@ tips：可以将仿真器窗口设为始终位于最上层，方便可视化 - 左肩键 LB：机器狗站立（对应键盘z） - 右肩键 RB：进入RL控制状态（对应键盘c） +- X：机器狗趴下（对应键盘x） - 左扳机 LT：关节阻尼模式（对应键盘r） - 左摇杆 上下：前后移动（对应键盘ws） - 左摇杆 左右：左右平移（对应键盘ad） @@ -83,7 +86,6 @@ ports = [43897] # scp传输文件 (打开本地电脑终端) scp -r ~/Lite3_rl_deploy ysc@192.168.2.1:~/ - # ssh连接机器狗运动主机以远程开发 #Username Password #ysc ' (a single quote) @@ -105,6 +107,13 @@ make -j 参考https://github.com/DeepRoboticsLab/gamepad +- Y：机器狗站立 +- A：进入RL控制状态 +- X：机器狗趴下 +- 同时按下左右摇杆：关节阻尼模式 +- 左摇杆：前后左右移动 +- 右摇杆：顺逆时针旋转 + ## 模型转换 运行RL训练出的策略文件需要链接onnxruntime库，而onnxruntime支持的模型为.onnx格式，需要手动转换.pt模型为.onnx格式。 @@ -139,6 +148,10 @@ graph LR A(Idle) -->B(StandUp) --> C(RL) C-->D(JointDamping) B-->D +B-->E(LieDown) +C-->E +E-->A +E-->D D-->A ``` @@ -153,6 +166,8 @@ state_machine模块是Lite3在不同的状态之间来回切换，不同的状 4.JointDamping 关节阻尼状态，表示机器狗的关节处于阻尼控制状态 +5.LieDown 趴下状态，表示机器狗从站立状态切换到趴下状态，动作完成后回到Idle + ### interface ```mermaid diff --git a/interface/robot/simulation/mujoco_simulation.py b/interface/robot/simulation/mujoco_simulation.py index 69fd3088..f979e211 100644 --- a/interface/robot/simulation/mujoco_simulation.py +++ b/interface/robot/simulation/mujoco_simulation.py @@ -75,6 +75,13 @@ def __init__(self, model_key: str = MODEL_NAME, self.viewer = None if USE_VIEWER: self.viewer = mujoco.viewer.launch_passive(self.model, self.data) + self._set_viewer_camera() + + def _set_viewer_camera(self): + self.viewer.cam.distance = 3.8 + self.viewer.cam.azimuth = 135 + self.viewer.cam.elevation = -18 + self.viewer.cam.lookat[:] = np.array([0.4, 0.0, 0.45]) def _set_initial_pose(self, key: str): """Set joint positions to match PyBullet initial angles.""" @@ -235,4 +242,4 @@ def _send_robot_state(self, step: int): if __name__ == "__main__": sim = MuJoCoSimulation() - sim.start() \ No newline at end of file + sim.start() diff --git a/interface/user_command/keyboard_interface.hpp b/interface/user_command/keyboard_interface.hpp index da4a249c..5fc25850 100644 --- a/interface/user_command/keyboard_interface.hpp +++ b/interface/user_command/keyboard_interface.hpp @@ -97,9 +97,14 @@ class KeyboardInterface : public UserCommandInterface case RobotMotionState::StandingUp: if(input=='c'){ usr_cmd_.target_mode = int(RobotMotionState::RLControlMode); + }else if(input=='x' || input=='X'){ + usr_cmd_.target_mode = int(RobotMotionState::LieDown); } break; case RobotMotionState::RLControlMode: + if(input=='x' || input=='X'){ + usr_cmd_.target_mode = int(RobotMotionState::LieDown); + } if(input=='w') { usr_cmd_.forward_vel_scale+=AXIS_STEP; forward_time_record = current_time; @@ -147,4 +152,3 @@ class KeyboardInterface : public UserCommandInterface }; - diff --git a/interface/user_command/retroid_gamepad_interface.hpp b/interface/user_command/retroid_gamepad_interface.hpp index c385cd9c..b77a3437 100644 --- a/interface/user_command/retroid_gamepad_interface.hpp +++ b/interface/user_command/retroid_gamepad_interface.hpp @@ -87,6 +87,13 @@ void RetroidGamepadInterface::TransformRetroidToUserCommand(){ case RobotMotionState::StandingUp: if(rt_keys_.A != rt_keys_record_.A){ usr_cmd_.target_mode = int(RobotMotionState::RLControlMode); + }else if(rt_keys_.X != rt_keys_record_.X){ + usr_cmd_.target_mode = int(RobotMotionState::LieDown); + } + break; + case RobotMotionState::RLControlMode: + if(rt_keys_.X != rt_keys_record_.X){ + usr_cmd_.target_mode = int(RobotMotionState::LieDown); } break; @@ -102,4 +109,3 @@ void RetroidGamepadInterface::TransformRetroidToUserCommand(){ } } - diff --git a/interface/user_command/xbox_gamepad_interface.hpp b/interface/user_command/xbox_gamepad_interface.hpp index 391fc23a..b2815652 100644 --- a/interface/user_command/xbox_gamepad_interface.hpp +++ b/interface/user_command/xbox_gamepad_interface.hpp @@ -46,6 +46,7 @@ class XboxGamepadInterface : public UserCommandInterface bool lb_pressed = false; bool rb_pressed = false; bool lt_pressed = false; + bool x_pressed = false; } gp_state_; bool connection_mode_detected_ = false; @@ -166,6 +167,7 @@ class XboxGamepadInterface : public UserCommandInterface bool lb_prev = false; bool rb_prev = false; bool lt_prev = false; + bool x_prev = false; std::cout << "Start Gamepad Listening" << std::endl; @@ -228,6 +230,7 @@ class XboxGamepadInterface : public UserCommandInterface case 1: // B break; case 3: // X + gp_state_.x_pressed = (js_ev.value != 0); break; case 4: // Y break; @@ -249,6 +252,7 @@ class XboxGamepadInterface : public UserCommandInterface case 1: // B break; case 2: // X + gp_state_.x_pressed = (js_ev.value != 0); break; case 3: // Y break; @@ -282,6 +286,9 @@ class XboxGamepadInterface : public UserCommandInterface if(gp_state_.rb_pressed && !rb_prev){ usr_cmd_.target_mode = int(RobotMotionState::RLControlMode); std::cout << "[Gamepad] RB pressed -> RLControlMode" << std::endl; + }else if(gp_state_.x_pressed && !x_prev){ + usr_cmd_.target_mode = int(RobotMotionState::LieDown); + std::cout << "[Gamepad] X pressed -> LieDown" << std::endl; } break; @@ -289,6 +296,9 @@ class XboxGamepadInterface : public UserCommandInterface if(gp_state_.lt_pressed && !lt_prev){ usr_cmd_.target_mode = int(RobotMotionState::JointDamping); std::cout << "[Gamepad] LT pressed -> JointDamping" << std::endl; + }else if(gp_state_.x_pressed && !x_prev){ + usr_cmd_.target_mode = int(RobotMotionState::LieDown); + std::cout << "[Gamepad] X pressed -> LieDown" << std::endl; } double current_time = GetCurrentTimeStamp(); @@ -329,8 +339,9 @@ class XboxGamepadInterface : public UserCommandInterface lb_prev = gp_state_.lb_pressed; rb_prev = gp_state_.rb_pressed; lt_prev = gp_state_.lt_pressed; + x_prev = gp_state_.x_pressed; } std::cout << "Gamepad thread stopped" << std::endl; } -}; \ No newline at end of file +}; diff --git a/state_machine/liedown_state.hpp b/state_machine/liedown_state.hpp new file mode 100644 index 00000000..da5f3348 --- /dev/null +++ b/state_machine/liedown_state.hpp @@ -0,0 +1,134 @@ +/** + * @file liedown_state.hpp + * @brief from stand state to lie down state + */ +#pragma once + +#include "state_base.h" + +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.; + + 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 GetCubicSplinePos(float x0, float v0, float xf, float vf, float t, float T){ + if(t >= T) return xf; + float a, b, c, d; + d = x0; + c = v0; + a = (vf*T - 2*xf + v0*T + 2*x0) / pow(T, 3); + b = (3*xf - vf*T - 2*v0*T - 3*x0) / pow(T, 2); + return a*pow(t, 3)+b*pow(t, 2)+c*t+d; + } + + float GetCubicSplineVel(float x0, float v0, float xf, float vf, float t, float T){ + if(t >= T) return 0; + float a, b, c; + c = v0; + a = (vf*T - 2*xf + v0*T + 2*x0) / pow(T, 3); + b = (3*xf - vf*T - 2*v0*T - 3*x0) / pow(T, 2); + return 3.*a*pow(t, 2) + 2.*b*t + c; + } + + float GetHipYPosByHeight(float h){ + float l1 = cp_ptr_->thigh_len_; + float l2 = cp_ptr_->shank_len_; + 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_; + 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: + LieDownState(const RobotType& robot_type, const std::string& state_name, + std::shared_ptr data_ptr):StateBase(robot_type, state_name, data_ptr){ + goal_joint_pos_ = Vec3f(0., GetHipYPosByHeight(cp_ptr_->pre_height_), + GetKneePosByHeight(cp_ptr_->pre_height_)).replicate(4, 1); + kp_ = cp_ptr_->swing_leg_kp_.replicate(4, 1); + kd_ = cp_ptr_->swing_leg_kd_.replicate(4, 1); + joint_cmd_ = MatXf::Zero(12, 5); + joint_cmd_.col(0) = kp_; + joint_cmd_.col(2) = kd_; + liedown_duration_ = cp_ptr_->stand_duration_; + } + ~LieDownState(){} + + virtual void OnEnter() { + GetRobotJointValue(); + RecordJointData(); + StateBase::msfb_.UpdateCurrentState(RobotMotionState::LieDown); + uc_ptr_->SetMotionStateFeedback(StateBase::msfb_); + }; + + 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;iSetJointCommand(joint_cmd_); + } else if(run_time_ - time_stamp_record_ <= 2.*liedown_duration_){ + joint_cmd_.setZero(); + joint_cmd_.col(2) = kd_; + ri_ptr_->SetJointCommand(joint_cmd_); + } else { + joint_cmd_.setZero(); + ri_ptr_->SetJointCommand(joint_cmd_); + } + } + + virtual bool LoseControlJudge() { + if(uc_ptr_->GetUserCommand().target_mode == int(RobotMotionState::JointDamping)) return true; + return false; + } + + virtual StateName GetNextStateName() { + if(run_time_ - time_stamp_record_ <= 2.*liedown_duration_){ + return StateName::kLieDown; + } + return StateName::kIdle; + } +}; diff --git a/state_machine/rl_control_state.hpp b/state_machine/rl_control_state.hpp index 6e70e270..70fb452d 100644 --- a/state_machine/rl_control_state.hpp +++ b/state_machine/rl_control_state.hpp @@ -125,7 +125,7 @@ class RLControlState : public StateBase } virtual StateName GetNextStateName() { + if(uc_ptr_->GetUserCommand().target_mode == int(RobotMotionState::LieDown)) return StateName::kLieDown; return StateName::kRLControl; } }; - diff --git a/state_machine/rl_control_state_onnx.hpp b/state_machine/rl_control_state_onnx.hpp index 52b1c9fc..30621b4c 100644 --- a/state_machine/rl_control_state_onnx.hpp +++ b/state_machine/rl_control_state_onnx.hpp @@ -133,8 +133,7 @@ class RLControlStateONNX : public StateBase } virtual StateName GetNextStateName() { + if(uc_ptr_->GetUserCommand().target_mode == int(RobotMotionState::LieDown)) return StateName::kLieDown; return StateName::kRLControl; } }; - - diff --git a/state_machine/standup_state.hpp b/state_machine/standup_state.hpp index 68cc9e82..bb4e2eeb 100644 --- a/state_machine/standup_state.hpp +++ b/state_machine/standup_state.hpp @@ -143,6 +143,8 @@ class StandUpState : public StateBase{ if(uc_ptr_->GetUserCommand().target_mode == int(RobotMotionState::RLControlMode)){ return StateName::kRLControl; std::cout << "stand up success" << std::endl; + }else if(uc_ptr_->GetUserCommand().target_mode == int(RobotMotionState::LieDown)){ + return StateName::kLieDown; } } return StateName::kStandUp; diff --git a/state_machine/state_machine.hpp b/state_machine/state_machine.hpp index 57149bd8..d185694a 100644 --- a/state_machine/state_machine.hpp +++ b/state_machine/state_machine.hpp @@ -14,6 +14,7 @@ #include "idle_state.hpp" #include "standup_state.hpp" #include "joint_damping_state.hpp" +#include "liedown_state.hpp" // #ifdef USE_ONNX // #include "rl_control_state_onnx.hpp" @@ -50,6 +51,7 @@ class StateMachine{ std::shared_ptr standup_controller_; std::shared_ptr rl_controller_; std::shared_ptr joint_damping_controller_; + std::shared_ptr liedown_controller_; StateName current_state_name_, next_state_name_; @@ -106,6 +108,9 @@ class StateMachine{ case StateName::kJointDamping:{ return joint_damping_controller_; } + case StateName::kLieDown:{ + return liedown_controller_; + } default:{ std::cerr << "error state name" << std::endl; } @@ -176,6 +181,7 @@ class StateMachine{ joint_damping_controller_ = std::make_shared(robot_type, "joint_damping", data_ptr); + liedown_controller_ = std::make_shared(robot_type, "liedown_state", data_ptr); current_controller_ = idle_controller_; current_state_name_ = kIdle; @@ -221,4 +227,4 @@ class StateMachine{ ri_ptr_->Stop(); } -}; \ No newline at end of file +}; diff --git a/types/custom_types.h b/types/custom_types.h index 6db69d52..9945677e 100644 --- a/types/custom_types.h +++ b/types/custom_types.h @@ -12,6 +12,7 @@ namespace types{ WaitingForStand = 0, StandingUp = 1, JointDamping = 2, + LieDown = 3, RLControlMode = 6, }; @@ -21,6 +22,7 @@ namespace types{ kIdle = 0, kStandUp = 1, kJointDamping = 2, + kLieDown = 3, kRLControl = 6, }; @@ -35,4 +37,4 @@ namespace types{ } }; -#endif \ No newline at end of file +#endif