diff --git a/src/Lite3_sdk_deploy/README.md b/src/Lite3_sdk_deploy/README.md
index 3839829b..4a734e33 100644
--- a/src/Lite3_sdk_deploy/README.md
+++ b/src/Lite3_sdk_deploy/README.md
@@ -106,13 +106,15 @@ source install/setup.bash
python3 src/Lite3_sdk_deploy/interface/robot/simulation/mujoco_simulation_ros2.py
```
-### Control (Terminal 2)
+### Control (Terminal 1)
**Note:**
> - Right click simulator window and select "always on top"
> - When the robot dog stands up, it may become stuck due to self-collision in the simulation. This is not a bug; please try again.
-> - z： default position
+> - z： default position / stand up from lie down
> - c： rl control default position
+> - x： lie down
+> - r： joint damping
> - wasd：forward/leftward/backward/rightward
> - qe：clockwise/counter clockwise
@@ -166,14 +168,18 @@ source install/setup.bash
ros2 run lite3_sdk_deploy rl_deploy
```
**keyboard control:**
-> - z： default position
+> - z： default position / stand up from lie down
> - c： rl control default position
+> - x： lie down
+> - r： joint damping
> - wasd：forward/leftward/backward/rightward
> - qe：clockwise/counter clockwise
**gamepad control:**
-> - Y： default position
+> - Y： default position / stand up from lie down
> - A： rl control default position
+> - X： lie down
+> - Press both joystick buttons：joint damping
> - Left joystick：forward/leftward/backward/rightward
> - Right joystick：clockwise/counter clockwise
@@ -196,4 +202,4 @@ Verified cross-host cases:
**Warning: The ROS 2 version in Lite3 is foxy (Compatible with Ubuntu 20.04). If the ROS 2 version in your host computer is not foxy, communication will not work properly (ROS 2 does not officially support cross-version communication).**
We add an additional shadow subscriber to solve this problem temporarily.
-If you know how to solve it better, welcome to submit an issue or a pull request.
\ No newline at end of file
+If you know how to solve it better, welcome to submit an issue or a pull request.
diff --git a/src/Lite3_sdk_deploy/include/types/custom_types.h b/src/Lite3_sdk_deploy/include/types/custom_types.h
index 648581e1..55b14928 100644
--- a/src/Lite3_sdk_deploy/include/types/custom_types.h
+++ b/src/Lite3_sdk_deploy/include/types/custom_types.h
@@ -15,6 +15,7 @@ namespace types{
WaitingForStand = 0,
StandingUp = 1,
JointDamping = 2,
+ LieDown = 3,
RLControlMode = 6,
};
@@ -23,6 +24,7 @@ namespace types{
kIdle = 0,
kStandUp = 1,
kJointDamping = 2,
+ kLieDown = 3,
kRLControl = 6,
};
diff --git a/src/Lite3_sdk_deploy/interface/robot/simulation/mujoco_simulation_ros2.py b/src/Lite3_sdk_deploy/interface/robot/simulation/mujoco_simulation_ros2.py
index 29dc3159..37d47238 100644
--- a/src/Lite3_sdk_deploy/interface/robot/simulation/mujoco_simulation_ros2.py
+++ b/src/Lite3_sdk_deploy/interface/robot/simulation/mujoco_simulation_ros2.py
@@ -102,6 +102,7 @@ def __init__(self,
self.viewer = None
if USE_VIEWER:
self.viewer = mujoco.viewer.launch_passive(self.model, self.data)
+ self._configure_viewer_camera()
def _set_initial_pose(self, key: str):
"""关节位置设置为与 PyBullet 脚本一致的初始角度"""
@@ -112,6 +113,13 @@ def _set_initial_pose(self, key: str):
self.data.qpos[:] = qpos0
mujoco.mj_forward(self.model, self.data)
+ def _configure_viewer_camera(self):
+ self.viewer.cam.type = mujoco.mjtCamera.mjCAMERA_FREE
+ self.viewer.cam.lookat[:] = np.array([0.4, 0.0, 0.45])
+ self.viewer.cam.distance = 2.4
+ self.viewer.cam.azimuth = 135.0
+ self.viewer.cam.elevation = -25.0
+
def _cmd_callback(self, msg: JointsDataCmd):
"""Convert received (published) positions/velocities to internal (raw)"""
if len(msg.data.joints_data) not in (12, 16):
@@ -274,4 +282,4 @@ def _publish_robot_state(self, step: int):
sim_node = MuJoCoSimulationNode()
sim_node.start()
sim_node.destroy_node()
- rclpy.shutdown()
\ No newline at end of file
+ rclpy.shutdown()
diff --git a/src/Lite3_sdk_deploy/interface/user_command/keyboard_interface.hpp b/src/Lite3_sdk_deploy/interface/user_command/keyboard_interface.hpp
index 9166141b..2b3bbe51 100644
--- a/src/Lite3_sdk_deploy/interface/user_command/keyboard_interface.hpp
+++ b/src/Lite3_sdk_deploy/interface/user_command/keyboard_interface.hpp
@@ -101,10 +101,19 @@ class KeyboardInterface : public UserCommandInterface
usr_cmd_->target_mode = uint8_t(RobotMotionState::StandingUp);
std::cout << "[MODE] Standing Up\n";
}
+ else if (k == 'z' && msfb_->GetCurrentState() == RobotMotionState::LieDown) {
+ usr_cmd_->target_mode = uint8_t(RobotMotionState::StandingUp);
+ std::cout << "[MODE] Standing Up\n";
+ }
else if (k == 'c' && msfb_->GetCurrentState() == RobotMotionState::StandingUp) {
usr_cmd_->target_mode = uint8_t(RobotMotionState::RLControlMode);
std::cout << "[MODE] RL Control\n";
}
+ else if (k == 'x' && (msfb_->GetCurrentState() == RobotMotionState::StandingUp
+ || msfb_->GetCurrentState() == RobotMotionState::RLControlMode)) {
+ usr_cmd_->target_mode = uint8_t(RobotMotionState::LieDown);
+ std::cout << "[MODE] Lie Down\n";
+ }
}
void keyboard_loop()
@@ -116,7 +125,7 @@ class KeyboardInterface : public UserCommandInterface
<< "╚════════════════════════════════════════════════╝\n"
<< " Movement: W/S (forward/back) A/D (left/right)\n"
<< " Rotation: Q (CCW) E (CW)\n"
- << " Mode: R (damping) Z (stand) C (control)\n"
+ << " Mode: R (damping) Z (stand) C (control) X (lie down)\n"
<< "\n";
char ch;
@@ -130,7 +139,7 @@ class KeyboardInterface : public UserCommandInterface
char k = std::tolower(static_cast(ch));
// Handle mode commands
- if (k == 'r' || k == 'z' || k == 'c') {
+ if (k == 'r' || k == 'z' || k == 'c' || k == 'x') {
process_mode_command(k);
continue;
}
@@ -227,4 +236,4 @@ class KeyboardInterface : public UserCommandInterface
<< " side=" << max_side_
<< " yaw=" << max_yaw_ << "\n";
}
-};
\ No newline at end of file
+};
diff --git a/src/Lite3_sdk_deploy/interface/user_command/keyboard_interface_sim.hpp b/src/Lite3_sdk_deploy/interface/user_command/keyboard_interface_sim.hpp
index 9b953e10..e419bec4 100644
--- a/src/Lite3_sdk_deploy/interface/user_command/keyboard_interface_sim.hpp
+++ b/src/Lite3_sdk_deploy/interface/user_command/keyboard_interface_sim.hpp
@@ -98,7 +98,8 @@ class KeyboardInterface : public UserCommandInterface
usr_cmd_->target_mode = uint8_t(RobotMotionState::JointDamping);
std::cout << "[MODE] Joint Damping\n";
}
- else if (keycode == KEY_Z && msfb_->GetCurrentState() == RobotMotionState::WaitingForStand) {
+ else if (keycode == KEY_Z && (msfb_->GetCurrentState() == RobotMotionState::WaitingForStand
+ || msfb_->GetCurrentState() == RobotMotionState::LieDown)) {
usr_cmd_->target_mode = uint8_t(RobotMotionState::StandingUp);
std::cout << "[MODE] Standing Up\n";
}
@@ -106,6 +107,11 @@ class KeyboardInterface : public UserCommandInterface
usr_cmd_->target_mode = uint8_t(RobotMotionState::RLControlMode);
std::cout << "[MODE] RL Control\n";
}
+ else if (keycode == KEY_X && (msfb_->GetCurrentState() == RobotMotionState::StandingUp
+ || msfb_->GetCurrentState() == RobotMotionState::RLControlMode)) {
+ usr_cmd_->target_mode = uint8_t(RobotMotionState::LieDown);
+ std::cout << "[MODE] Lie Down\n";
+ }
}
bool init_all_keyboards()
@@ -170,7 +176,7 @@ class KeyboardInterface : public UserCommandInterface
<< "╚════════════════════════════════════════════════╝\n"
<< " Hold W/S/A/D/Q/E → velocity ramps up\n"
<< " Release key → instant stop on that axis\n"
- << " Modes: R (damping) Z (stand) C (RL control)\n\n";
+ << " Modes: R (damping) Z (stand) C (RL control) X (lie down)\n\n";
struct input_event ev;
@@ -210,7 +216,7 @@ class KeyboardInterface : public UserCommandInterface
// Mode keys (only on real press)
if (ev.value == 1) {
- if (ev.code == KEY_R || ev.code == KEY_Z || ev.code == KEY_C) {
+ if (ev.code == KEY_R || ev.code == KEY_Z || ev.code == KEY_C || ev.code == KEY_X) {
process_mode_key(ev.code);
}
if (ev.code == KEY_ESC) {
@@ -281,4 +287,4 @@ class KeyboardInterface : public UserCommandInterface
<< " side:" << max_side_
<< " yaw:" << max_yaw_ << "\n";
}
-};
\ No newline at end of file
+};
diff --git a/src/Lite3_sdk_deploy/interface/user_command/retroid_gamepad_interface.hpp b/src/Lite3_sdk_deploy/interface/user_command/retroid_gamepad_interface.hpp
index f1a5f581..821d6bf0 100644
--- a/src/Lite3_sdk_deploy/interface/user_command/retroid_gamepad_interface.hpp
+++ b/src/Lite3_sdk_deploy/interface/user_command/retroid_gamepad_interface.hpp
@@ -93,6 +93,8 @@ class RetroidGamepadInterface : public UserCommandInterface {
bool Y_last = (last_buttons_ & BIT_Y) != 0;
bool A_pressed = (msg->buttons & BIT_A) != 0;
bool A_last = (last_buttons_ & BIT_A) != 0;
+ bool X_pressed = (msg->buttons & BIT_X) != 0;
+ bool X_last = (last_buttons_ & BIT_X) != 0;
bool left_axis_button = (msg->buttons & BIT_LEFT_AXIS_BUTTON) != 0;
bool right_axis_button = (msg->buttons & BIT_RIGHT_AXIS_BUTTON) != 0;
bool left_axis_button_last = (last_buttons_ & BIT_LEFT_AXIS_BUTTON) != 0;
@@ -111,6 +113,21 @@ class RetroidGamepadInterface : public UserCommandInterface {
if (A_pressed && !A_last) {
usr_cmd_->target_mode = uint8_t(RobotMotionState::RLControlMode);
RCLCPP_INFO(node_->get_logger(), "Mode: RL Control");
+ } else if (X_pressed && !X_last) {
+ usr_cmd_->target_mode = uint8_t(RobotMotionState::LieDown);
+ RCLCPP_INFO(node_->get_logger(), "Mode: Lie Down");
+ }
+ break;
+ case RobotMotionState::RLControlMode:
+ if (X_pressed && !X_last) {
+ usr_cmd_->target_mode = uint8_t(RobotMotionState::LieDown);
+ RCLCPP_INFO(node_->get_logger(), "Mode: Lie Down");
+ }
+ break;
+ case RobotMotionState::LieDown:
+ if (Y_pressed && !Y_last) {
+ usr_cmd_->target_mode = uint8_t(RobotMotionState::StandingUp);
+ RCLCPP_INFO(node_->get_logger(), "Mode: Standing Up");
}
break;
default:
diff --git a/src/Lite3_sdk_deploy/state_machine/parameters/control_parameters.h b/src/Lite3_sdk_deploy/state_machine/parameters/control_parameters.h
index 7a558dd9..ddec8bc9 100644
--- a/src/Lite3_sdk_deploy/state_machine/parameters/control_parameters.h
+++ b/src/Lite3_sdk_deploy/state_machine/parameters/control_parameters.h
@@ -65,10 +65,14 @@ class ControlParameters
*/
float stand_duration_ = 1.5;
+ /**
+ * @brief lie down duration
+ */
+ float liedown_duration_ = 2.0;
+
/**
* @brief policy path
*/
std::string common_policy_path_;
Vec3f common_policy_p_gain_, common_policy_d_gain_;
};
-
diff --git a/src/Lite3_sdk_deploy/state_machine/quadruped/liedown_state.hpp b/src/Lite3_sdk_deploy/state_machine/quadruped/liedown_state.hpp
new file mode 100644
index 00000000..f5057aca
--- /dev/null
+++ b/src/Lite3_sdk_deploy/state_machine/quadruped/liedown_state.hpp
@@ -0,0 +1,155 @@
+/**
+ * @file liedown_state.hpp
+ * @brief from stand state to lie down state
+ * @author DeepRobotics
+ * @version 1.0
+ * @date 2026-02-12
+ *
+ * @copyright Copyright (c) 2025 DeepRobotics
+ *
+ */
+#pragma once
+
+#include "state_base.h"
+
+namespace q{
+class LieDownState : 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_;
+ float liedown_duration_ = 2.;
+
+ const float init_hipx_pos_ = Deg2Rad(0.);
+
+ 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 RobotName& robot_name, const std::string& state_name,
+ std::shared_ptr data_ptr):StateBase(robot_name, state_name, data_ptr){
+ goal_joint_pos_ = Vec3f(init_hipx_pos_, GetHipYPosByHeight(0.03), GetKneePosByHeight(0.03)).replicate(4, 1);
+
+ Vec3f one_leg_kp, one_leg_kd;
+ one_leg_kp << cp_ptr_->swing_leg_kp_;
+ one_leg_kd << cp_ptr_->swing_leg_kd_;
+ kp_ = one_leg_kp.replicate(4, 1);
+ kd_ = one_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_->liedown_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.0 * liedown_duration_){
+ joint_cmd_ = MatXf::Zero(12, 5);
+ joint_cmd_.col(2) = kd_;
+ ri_ptr_->SetJointCommand(joint_cmd_);
+ } else {
+ joint_cmd_ = MatXf::Zero(12, 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;
+ }
+};
+
+};
diff --git a/src/Lite3_sdk_deploy/state_machine/quadruped/q_state_machine.hpp b/src/Lite3_sdk_deploy/state_machine/quadruped/q_state_machine.hpp
index f6782527..8294eb15 100644
--- a/src/Lite3_sdk_deploy/state_machine/quadruped/q_state_machine.hpp
+++ b/src/Lite3_sdk_deploy/state_machine/quadruped/q_state_machine.hpp
@@ -16,6 +16,7 @@
#include "quadruped/standup_state.hpp"
#include "quadruped/joint_damping_state.hpp"
#include "quadruped/rl_control_state.hpp"
+#include "quadruped/liedown_state.hpp"
#include "keyboard_interface.hpp"
#include "retroid_gamepad_interface.hpp"
#include "hardware/lite3_interface.hpp"
@@ -29,6 +30,7 @@ class QStateMachine : public StateMachineBase{
std::shared_ptr standup_controller_;
std::shared_ptr rl_controller_;
std::shared_ptr joint_damping_controller_;
+ std::shared_ptr liedown_controller_;
std::shared_ptr joints_data_shadow_subscriber_;
// std::shared_ptr car_move_controller_;
@@ -79,6 +81,7 @@ class QStateMachine : public StateMachineBase{
standup_controller_ = std::make_shared(robot_name_, "standup_state", data_ptr);
rl_controller_ = std::make_shared(robot_name_, "rl_control", data_ptr);
joint_damping_controller_ = std::make_shared(robot_name_, "joint_damping", data_ptr);
+ liedown_controller_ = std::make_shared(robot_name_, "liedown_state", data_ptr);
current_controller_ = idle_controller_;
current_state_name_ = kIdle;
@@ -111,6 +114,9 @@ class QStateMachine : public StateMachineBase{
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_;
diff --git a/src/Lite3_sdk_deploy/state_machine/quadruped/rl_control_state.hpp b/src/Lite3_sdk_deploy/state_machine/quadruped/rl_control_state.hpp
index 5451f0e1..2adc94a2 100644
--- a/src/Lite3_sdk_deploy/state_machine/quadruped/rl_control_state.hpp
+++ b/src/Lite3_sdk_deploy/state_machine/quadruped/rl_control_state.hpp
@@ -132,7 +132,9 @@ namespace q {
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;
}
};
-};
\ No newline at end of file
+};
diff --git a/src/Lite3_sdk_deploy/state_machine/quadruped/standup_state.hpp b/src/Lite3_sdk_deploy/state_machine/quadruped/standup_state.hpp
index 48152e05..bbf52191 100644
--- a/src/Lite3_sdk_deploy/state_machine/quadruped/standup_state.hpp
+++ b/src/Lite3_sdk_deploy/state_machine/quadruped/standup_state.hpp
@@ -151,6 +151,8 @@ class StandUpState : public StateBase{
}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;