Add gamepad control, lie down state, and RBS buffer I/O

Author: chenchangshuo

Contributors.md

@@ -11,5 +11,5 @@ This is the official list of Deep Robotics SDK Project developers and contributo
 
 * [Bo Peng](https://www.linkedin.com/in/bo-peng-647068272/)
 * [Haokai Dai](https://github.com/HaoKa1)
-* Changshuo Chen
+* [Changshuo Chen](https://github.com/CcShuo)
 * Zhichao Feng

src/M20_sdk_deploy/CMakeLists.txt

@@ -34,6 +34,7 @@ include_directories(
   include/utils
   interface/robot
   interface/user_command
+  interface/network
   state_machine
   run_policy
   ${THIRD_PARTY}/eigen/

src/M20_sdk_deploy/README.md

@@ -107,7 +107,7 @@ python3 src/M20_sdk_deploy/interface/robot/simulation/mujoco_simulation_ros2.py
 
 
 # Sim-to-Real
-This process is almost identical to simulation-simulation. You only need to add the step of connecting to Wi-Fi to transfer data, and then modify the compilation instructions. The default control mode is currently set to keyboard mode. We will be adding controller support in future updates. Stay tuned.
+This process is almost identical to simulation-simulation. You only need to add the step of connecting to Wi-Fi to transfer data, and then modify the compilation instructions.Real-robot control is divided into keyboard mode and gamepad control mode. You need to modify the RemoteCommandType parameter in the main function to select the desired mode.
 
 
 Please first use the OTA upgrade function in the handle settings to upgrade the hardware to version 1.1.7.
@@ -139,11 +139,22 @@ ros2 run m20_sdk_deploy rl_deploy
 
 # exit sdk mode：
 ros2 service call /SDK_MODE drdds/srv/StdSrvInt32 command:\ 0
+```
 
-# keyboard control
-Note: 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.
+### ⌨️ Keyboard Control
+*(Note: 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
 - c： rl control default position
+- x： lie down
 - wasd：forward/leftward/backward/rightward
 - qe：clockwise/counter clockwise
-```
+
+### 🎮 Gamepad Control
+*(Note: When using the gamepad control function, please ensure that the Gamepad APP version is V1.5.11 or higher.)*
+- L1： default position
+- L2： rl control default position
+- R1： lie down
+- R2： joint damping
+- Left joystick：forward/leftward/backward/rightward
+- Right joystick：clockwise/counter clockwise
+

src/M20_sdk_deploy/include/types/common_types.h

@@ -29,58 +29,89 @@
 
 
 namespace types {
-    using Vec3f = Eigen::Vector3f;
-    using Vec3d = Eigen::Vector3d;
-    using Vec4f = Eigen::Vector4f;
-    using Vec4d = Eigen::Vector4d;
-    using VecXf = Eigen::VectorXf;
-    using VecXd = Eigen::VectorXd;
+using Vec3f = Eigen::Vector3f;
+using Vec3d = Eigen::Vector3d;
+using Vec4f = Eigen::Vector4f;
+using Vec4d = Eigen::Vector4d;
+using VecXf = Eigen::VectorXf;
+using VecXd = Eigen::VectorXd;
 
-    using Mat3f = Eigen::Matrix3f;
-    using Mat3d = Eigen::Matrix3d;
-    using MatXf = Eigen::MatrixXf;
-    using MatXd = Eigen::MatrixXd;
+using Mat3f = Eigen::Matrix3f;
+using Mat3d = Eigen::Matrix3d;
+using MatXf = Eigen::MatrixXf;
+using MatXd = Eigen::MatrixXd;
 
-    const float gravity = 9.815;
+const float gravity = 9.815;
 
-    struct RobotBasicState {
-        Vec3f base_rpy;
-        Vec4f base_quat;
-        Mat3f base_rot_mat;
-        Vec3f base_omega;
-        Vec3f base_acc;
-        MatXf flt_base_acc_mat;
-        VecXf joint_pos;
-        VecXf joint_vel;
-        VecXf joint_tau;
-    };
+struct RobotBasicState {
+    Vec3f base_rpy;
+    Vec4f base_quat;
+    Mat3f base_rot_mat;
+    Vec3f base_omega;
+    Vec3f base_acc;
+    MatXf flt_base_acc_mat;
+    VecXf joint_pos;
+    VecXf joint_vel;
+    VecXf joint_tau;
 
-    struct RobotAction {
-        VecXf goal_joint_pos;
-        VecXf goal_joint_vel;
-        VecXf kp;
-        VecXf kd;
-        VecXf tau_ff;
+    RobotBasicState(int dof = 16) {
+        base_rpy.setZero();
+        base_quat.setZero();
+        base_rot_mat.setIdentity();
+        base_omega.setZero();
+        base_acc.setZero();
+        flt_base_acc_mat = Eigen::MatrixXf::Zero(20, 3);
+        joint_pos = VecXf::Zero(dof);
+        joint_vel = VecXf::Zero(dof);
+        joint_tau = VecXf::Zero(dof);
+    }
+};
+
+struct RobotAction {
+    VecXf goal_joint_pos;
+    VecXf goal_joint_vel;
+    VecXf kp;
+    VecXf kd;
+    VecXf tau_ff;
 
-        MatXf ConvertToMat() {
-            MatXf res(goal_joint_pos.rows(), 5);
-            res.col(0) = kp;
-            res.col(1) = goal_joint_pos;
-            res.col(2) = kd;
-            res.col(3) = goal_joint_vel;
-            res.col(4) = tau_ff;
-            return res;
-        }
-    };
+    MatXf ConvertToMat() {
+        MatXf res(goal_joint_pos.rows(), 5);
+        res.col(0) = kp;
+        res.col(1) = goal_joint_pos;
+        res.col(2) = kd;
+        res.col(3) = goal_joint_vel;
+        res.col(4) = tau_ff;
+        return res;
+    }
+};
 
 
-    struct UserCommand {
-        double time_stamp;
+struct UserCommand {
+    double time_stamp;
         uint8_t safe_control_mode; //0 normal, 1 stand, 2 sit, 3 joint damping
-        uint8_t target_mode;
-        float forward_vel_scale = 0;
-        float side_vel_scale = 0;
-        float turnning_vel_scale = 0;
-        float reserved_scale;
-    };
+    uint8_t target_mode;
+    float forward_vel_scale = 0;
+    float side_vel_scale = 0;
+    float turnning_vel_scale = 0;
+    float reserved_scale;
+};
+
+enum FromType {
+    inside = 0,
+    user,
+};
+struct NetInfo {
+    std::string ip;
+    int port;
+    FromType fromType;
+};
+
+struct DataInfo : public NetInfo {
+    const char* buffer;
+    ssize_t len;
+};
+struct CmdInfo : public NetInfo {
+    int type;
+    int command;
 };
+};  // namespace types

src/M20_sdk_deploy/include/types/custom_types.h

@@ -15,6 +15,7 @@ namespace types{
         WaitingForStand = 0,
         StandingUp      = 1,
         JointDamping    = 2,
+        LieDown         = 4,
         RLControlMode   = 6,
     };
 
@@ -23,13 +24,22 @@ namespace types{
         kIdle         = 0,
         kStandUp      = 1,
         kJointDamping = 2,
+        kLieDown      = 4,
         kRLControl    = 6,
     };
 
     enum RemoteCommandType{
         kKeyBoard = 0,
+        kGamepad,
     };
 
+    enum KeyCode {
+        L1,
+        L2,
+        R1,
+        R2,
+        UNKNOWN
+    };
 
     inline std::string GetAbsPath(){
         char buffer[PATH_MAX];

src/M20_sdk_deploy/include/utils/basic_function.hpp

@@ -77,3 +77,22 @@ inline  double GetTimestampMs(){
     clock_gettime(CLOCK_MONOTONIC,&now_timestamp);
     return (now_timestamp.tv_sec-startup_timestamp.tv_sec)*1e3 + (now_timestamp.tv_nsec-startup_timestamp.tv_nsec)/1e6;
 }
+
+inline 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;
+}
+
+inline 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;
+}

src/M20_sdk_deploy/include/utils/thread_pool.hpp

@@ -0,0 +1,64 @@
+/**
+ * @file thread_pool.hpp
+ * @brief thread pool
+ * @author DeepRobotics
+ * @version 1.0
+ * @date 2026-01-07
+ *
+ * @copyright Copyright (c) 2025  DeepRobotics
+ *
+ */
+#pragma once
+#include <atomic>
+#include <condition_variable>
+#include <functional>
+#include <mutex>
+#include <queue>
+#include <thread>
+#include <vector>
+
+class ThreadPool {
+public:
+    ThreadPool(size_t numThreads) : stop(false) {
+        for (size_t i = 0; i < numThreads; ++i) {
+            workers.emplace_back([this]() {
+                while (true) {
+                    std::function<void()> task;
+                    {
+                        std::unique_lock<std::mutex> lock(this->queueMutex);
+                        this->condition.wait(lock, [this] { return this->stop || !this->tasks.empty(); });
+                        if (this->stop && this->tasks.empty()) return;
+                        task = std::move(this->tasks.front());
+                        this->tasks.pop();
+                    }
+                    task();
+                }
+            });
+        }
+    }
+
+    ~ThreadPool() {
+        {
+            std::unique_lock<std::mutex> lock(queueMutex);
+            stop = true;
+        }
+        condition.notify_all();
+        for (std::thread& worker : workers) worker.join();
+    }
+
+    void enqueue(std::function<void()> task) {
+        {
+            std::unique_lock<std::mutex> lock(queueMutex);
+            tasks.emplace(task);
+        }
+        condition.notify_one();
+    }
+
+private:
+    std::vector<std::thread> workers;
+    std::queue<std::function<void()>> tasks;
+
+    std::mutex queueMutex;
+    std::condition_variable condition;
+    std::atomic<bool> stop;
+};