feat: add cyberdog

README_CN.md

@@ -78,6 +78,8 @@ rosrun teleop_twist_keyboard teleop_twist_keyboard.py
 
 ### 实物
 
+#### Unitree A1
+
 与实物的连接可分为无线与有线形式
 
 * 无线：连接机器人发出的Unitree开头的WIFI **（注意：无线连接可能会出现丢包断联甚至失控，请注意安全）**
@@ -92,6 +94,54 @@ rosrun rl_sar rl_real
 
 按下遥控器的**R2**键让机器人切换到默认站起姿态，按下**R1**键切换到RL控制模式，任意状态按下**L2**切换到最初的趴下姿态。左摇杆上下控制x左右控制yaw，右摇杆左右控制y。
 
+#### Cyberdog1
+
+1. 连接机器人
+
+    将本地PC连接至铁蛋的USB download type-c 接口(位于中间)，等待出现”L4T-README” 弹窗
+
+      ```bash
+      ping 192.168.55.100     #本地PC被分配的ip
+      ssh mi@192.168.55.1     #登录nx应用板 ,密码123
+      athena_version -v #核对当前版本>=1.0.0.94
+      ssh root@192.168.55.233 #登录运动控制板
+      ```
+
+2. 进入电机控制模式
+
+    修改配置开关，激活用户控制模式，运行用户自己的控制器：
+
+      ```bash
+      ssh root@192.168.55.233 #登录运动控制板
+      cd /robot
+      ./initialize.sh #拷贝出厂代码到可读写的开发区（/mnt/UDISK/robot-software），切换到开发者模式，仅需执行一次
+      vi /mnt/UDISK/robot-software/config/user_code_ctrl_mode.txt #切换mode:1(0:默认模式，1用户代码控制电机模式),重启机器人生效
+      ```
+
+
+3. 用户电脑侧部署
+
+    运行在用户pc侧(linux)难以保证实时lcm通信，仅推荐编译验证和简单的位控测试  
+
+      ```bash
+      ping 192.168.55.233 #通过type c线连接Cyberdog的Download接口后，确认通信正常
+      ifconfig | grep -B 1 192.168.55.100 | grep "flags"| cut -d ':' -f1 #获取该ip对应网络设备，一般为usb0
+      sudo ifconfig usb0 multicast #usb0替换为上文获取的168.55.100对应网络设备,并配为多播
+      sudo route add -net 224.0.0.0 netmask 240.0.0.0 dev usb0 #添加路由表，usb0对应替换
+      ```
+    
+    启动控制程序
+    
+    ```bash
+    source devel/setup.bash
+    rosrun rl_sar rl_real
+    ```
+    
+    按下键盘上的**0**键让机器人切换到默认站起姿态，按下**P**键切换到RL控制模式，任意状态按下**1**键切换到最初的趴下姿态。WS控制x，AD控制yaw，JL控制y。
+    
+
+注：lcm通信若不成功，无法正常激活电机控制模式，log提示：Motor control mode has not been activated successfully
+
 ## 引用
 
 如果您使用此代码或其部分内容，请引用以下内容：

src/rl_sar/CMakeLists.txt

@@ -68,4 +68,12 @@ add_executable(rl_real src/rl_real.cpp )
 target_link_libraries(rl_real
   ${catkin_LIBRARIES} ${EXTRA_LIBS} "${TORCH_LIBRARIES}"
   rl observation_buffer
+)
+
+add_subdirectory(library/cyberdog_motor_sdk)
+include_directories(library/cyberdog_motor_sdk/include)
+add_executable(rl_real_cyberdog src/rl_real_cyberdog.cpp )
+target_link_libraries(rl_real_cyberdog
+  ${catkin_LIBRARIES} ${EXTRA_LIBS} "${TORCH_LIBRARIES}"
+  rl observation_buffer cyberdog_motor_sdk
 )

src/rl_sar/include/rl_real_cyberdog.hpp

@@ -0,0 +1,89 @@
+#ifndef RL_REAL_HPP
+#define RL_REAL_HPP
+
+#include "../library/rl/rl.hpp"
+#include "../library/observation_buffer/observation_buffer.hpp"
+#include "unitree_legged_sdk/loop.h"
+#include <boost/bind.hpp>
+#include <CustomInterface.h>
+#include <csignal>
+// #include <signal.h>
+#include <termios.h>
+#include <sys/ioctl.h>
+
+using namespace UNITREE_LEGGED_SDK;
+
+using CyberdogData = Robot_Data;
+using CyberdogCmd = Motor_Cmd;
+
+enum RobotState {
+    STATE_WAITING = 0,
+    STATE_POS_GETUP,
+    STATE_RL_INIT,
+    STATE_RL_RUNNING,
+    STATE_POS_GETDOWN,
+};
+
+struct KeyBoard
+{
+    RobotState robot_state;
+    float x = 0;
+    float y = 0;
+    float yaw = 0;
+};
+
+class RL_Real : public RL, public CustomInterface
+{
+public:
+    RL_Real();
+    ~RL_Real();
+    
+    void RunModel();
+    torch::Tensor Forward() override;
+    torch::Tensor ComputeObservation() override;
+
+    ObservationBuffer history_obs_buf;
+    torch::Tensor history_obs;
+    int motiontime = 0;
+
+    CyberdogData cyberdogData;
+	CyberdogCmd cyberdogCmd;
+    void UserCode();
+	long long count = 0;
+    
+    void RobotControl();
+
+    std::shared_ptr<LoopFunc> loop_control;
+    std::shared_ptr<LoopFunc> loop_rl;
+    std::shared_ptr<LoopFunc> loop_plot;
+
+    float getup_percent = 0.0;
+    float getdown_percent = 0.0;
+    float start_pos[12];
+	float now_pos[12];
+
+    int robot_state = STATE_WAITING;
+
+    std::vector<double> plot_t;
+    std::vector<std::vector<double>> plot_real_joint_pos, plot_target_joint_pos;
+    void Plot();
+
+    void run_keyboard();
+    KeyBoard keyboard;
+    std::thread _keyboardThread;
+private:
+    std::vector<std::string> joint_names;
+    std::vector<double> joint_positions;
+    std::vector<double> joint_velocities;
+
+    int dof_mapping[12] = {3, 4, 5, 0, 1, 2, 9, 10, 11, 6, 7, 8};
+    int hip_scale_reduction_indices[4] = {0, 3, 6, 9};
+
+    std::chrono::high_resolution_clock::time_point start_time;
+
+    // other rl module
+    torch::jit::script::Module encoder;
+    torch::jit::script::Module vq;
+};
+
+#endif

src/rl_sar/src/rl_real.cpp

@@ -10,9 +10,9 @@ RL_Real::RL_Real() : safe(LeggedType::A1), udp(LOWLEVEL)
 
     start_time = std::chrono::high_resolution_clock::now();
 
-    std::string actor_path = std::string(CMAKE_CURRENT_SOURCE_DIR) + "/models/actor.pt";
+    std::string actor_path = std::string(CMAKE_CURRENT_SOURCE_DIR) + "/models/base/actor.pt";
-    std::string encoder_path = std::string(CMAKE_CURRENT_SOURCE_DIR) + "/models/encoder.pt";
+    std::string encoder_path = std::string(CMAKE_CURRENT_SOURCE_DIR) + "/models/base/encoder.pt";
-    std::string vq_path = std::string(CMAKE_CURRENT_SOURCE_DIR) + "/models/vq_layer.pt";
+    std::string vq_path = std::string(CMAKE_CURRENT_SOURCE_DIR) + "/models/base/vq_layer.pt";
 
     this->actor = torch::jit::load(actor_path);
     this->encoder = torch::jit::load(encoder_path);

src/rl_sar/src/rl_real_cyberdog.cpp

@@ -0,0 +1,392 @@
+#include "../include/rl_real_cyberdog.hpp"
+
+// #define PLOT
+
+RL_Real rl_sar;
+
+RL_Real::RL_Real() : CustomInterface(500)
+{
+    start_time = std::chrono::high_resolution_clock::now();
+
+    std::string actor_path = std::string(CMAKE_CURRENT_SOURCE_DIR) + "/models/cyberdog/actor.pt";
+    std::string encoder_path = std::string(CMAKE_CURRENT_SOURCE_DIR) + "/models/cyberdog/encoder.pt";
+    std::string vq_path = std::string(CMAKE_CURRENT_SOURCE_DIR) + "/models/cyberdog/vq_layer.pt";
+
+    this->actor = torch::jit::load(actor_path);
+    this->encoder = torch::jit::load(encoder_path);
+    this->vq = torch::jit::load(vq_path);
+    this->InitObservations();
+    
+    this->params.num_observations = 45;
+    this->params.clip_obs = 100.0;
+    this->params.clip_actions = 100.0;
+    this->params.damping = 0.5;
+    this->params.stiffness = 20;
+    this->params.d_gains = torch::ones(12) * this->params.damping;
+    this->params.p_gains = torch::ones(12) * this->params.stiffness;
+    this->params.action_scale = 0.25;
+    this->params.hip_scale_reduction = 0.5;
+    this->params.num_of_dofs = 12;
+    this->params.lin_vel_scale = 2.0;
+    this->params.ang_vel_scale = 0.25;
+    this->params.dof_pos_scale = 1.0;
+    this->params.dof_vel_scale = 0.05;
+    this->params.commands_scale = torch::tensor({this->params.lin_vel_scale, this->params.lin_vel_scale, this->params.ang_vel_scale});
+    
+    this->params.torque_limits = torch::tensor({{20.0, 55.0, 55.0,    
+                                                 20.0, 55.0, 55.0,
+                                                 20.0, 55.0, 55.0,
+                                                 20.0, 55.0, 55.0}});
+
+    //                                              hip,    thigh,   calf
+    this->params.default_dof_pos = torch::tensor({{ 0.1000, 0.8000, -1.5000,   // FL
+                                                   -0.1000, 0.8000, -1.5000,   // FR
+                                                    0.1000, 1.0000, -1.5000,   // RR
+                                                   -0.1000, 1.0000, -1.5000}});// RL
+
+    this->history_obs_buf = ObservationBuffer(1, this->params.num_observations, 6);
+
+    output_torques = torch::tensor({{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}});
+    output_dof_pos = params.default_dof_pos;
+    plot_real_joint_pos.resize(12);
+    plot_target_joint_pos.resize(12);
+
+    loop_control = std::make_shared<LoopFunc>("loop_control", 0.002,    boost::bind(&RL_Real::RobotControl, this));
+    loop_rl      = std::make_shared<LoopFunc>("loop_rl"     , 0.02 ,    boost::bind(&RL_Real::RunModel,     this));
+
+    loop_control->start();
+    
+#ifdef PLOT
+    loop_plot    = std::make_shared<LoopFunc>("loop_plot"   , 0.002,    boost::bind(&RL_Real::Plot,         this));
+    loop_plot->start();
+#endif
+    _keyboardThread = std::thread(&RL_Real::run_keyboard, this);
+}
+
+RL_Real::~RL_Real()
+{
+    loop_control->shutdown();
+    loop_rl->shutdown();
+#ifdef PLOT
+    loop_plot->shutdown();
+#endif
+    printf("exit\n");
+}
+
+void RL_Real::RobotControl()
+{
+    std::cout << "robot_state" << keyboard.robot_state
+              << " x" << keyboard.x << " y" << keyboard.y << " yaw" << keyboard.yaw
+              << "\r";
+    motiontime++;
+
+    // waiting
+    if(robot_state == STATE_WAITING)
+    {
+        for(int i = 0; i < 12; ++i)
+        {
+            cyberdogCmd.q_des[i] = cyberdogData.q[i];
+        }
+        if(keyboard.robot_state == STATE_POS_GETUP)
+        {
+            keyboard.robot_state = STATE_WAITING;
+            getup_percent = 0.0;
+            for(int i = 0; i < 12; ++i)
+            {
+                now_pos[i] = cyberdogData.q[i];
+                start_pos[i] = now_pos[i];
+            }
+            robot_state = STATE_POS_GETUP;
+        }
+    }
+    // stand up (position control)
+    else if(robot_state == STATE_POS_GETUP)
+    {
+        if(getup_percent != 1)
+        {
+            getup_percent += 1 / 1000.0;
+            getup_percent = getup_percent > 1 ? 1 : getup_percent;
+            for(int i = 0; i < 12; ++i)
+            {
+                cyberdogCmd.q_des[i] = (1 - getup_percent) * now_pos[i] + getup_percent * params.default_dof_pos[0][dof_mapping[i]].item<double>();
+                cyberdogCmd.qd_des[i] = 0;
+                cyberdogCmd.kp_des[i] = 50;
+                cyberdogCmd.kd_des[i] = 3;
+                cyberdogCmd.tau_des[i] = 0;
+            }
+            printf("getting up %.3f%%\r", getup_percent*100.0);
+        }
+        if(keyboard.robot_state == STATE_RL_INIT)
+        {
+            keyboard.robot_state = STATE_WAITING;
+            robot_state = STATE_RL_INIT;
+        }
+        else if(keyboard.robot_state == STATE_POS_GETDOWN)
+        {
+            keyboard.robot_state = STATE_WAITING;
+            getdown_percent = 0.0;
+            for(int i = 0; i < 12; ++i)
+            {
+                now_pos[i] = cyberdogData.q[i];
+            }
+            robot_state = STATE_POS_GETDOWN;
+        }
+    }
+    // init obs and start rl loop
+    else if(robot_state == STATE_RL_INIT)
+    {
+        if(getup_percent == 1)
+        {
+            robot_state = STATE_RL_RUNNING;
+            this->InitObservations();
+            printf("\nstart rl loop\n");
+            loop_rl->start();
+        }
+    }
+    // rl loop
+    else if(robot_state == STATE_RL_RUNNING)
+    {
+        for(int i = 0; i < 12; ++i)
+        {
+            // cyberdogCmd.q_des[i] = 0;
+            cyberdogCmd.q_des[i] = output_dof_pos[0][dof_mapping[i]].item<double>();
+            cyberdogCmd.qd_des[i] = 0;
+            cyberdogCmd.kp_des[i] = params.stiffness;
+            cyberdogCmd.kd_des[i] = params.damping;
+            // cyberdogCmd.tau_des[i] = output_torques[0][dof_mapping[i]].item<double>();
+            cyberdogCmd.tau_des[i] = 0;
+        }
+        if(keyboard.robot_state == STATE_POS_GETDOWN)
+        {
+            keyboard.robot_state = STATE_WAITING;
+            getdown_percent = 0.0;
+            for(int i = 0; i < 12; ++i)
+            {
+                now_pos[i] = cyberdogData.q[i];
+            }
+            robot_state = STATE_POS_GETDOWN;
+        }
+    }
+    // get down (position control)
+    else if(robot_state == STATE_POS_GETDOWN)
+    {
+        if(getdown_percent != 1)
+        {
+            getdown_percent += 1 / 1000.0;
+            getdown_percent = getdown_percent > 1 ? 1 : getdown_percent;
+            for(int i = 0; i < 12; ++i)
+            {
+                cyberdogCmd.q_des[i] = (1 - getdown_percent) * now_pos[i] + getdown_percent * start_pos[i];
+                cyberdogCmd.qd_des[i] = 0;
+                cyberdogCmd.kp_des[i] = 50;
+                cyberdogCmd.kd_des[i] = 3;
+                cyberdogCmd.tau_des[i] = 0;
+            }
+            printf("getting down %.3f%%\r", getdown_percent*100.0);
+        }
+        if(getdown_percent == 1)
+        {
+            robot_state = STATE_WAITING;
+            this->InitObservations();
+            printf("\nstop rl loop\n");
+            loop_rl->shutdown();
+        }
+    }
+}
+
+void RL_Real::UserCode()
+{
+	cyberdogData = robot_data;
+	motor_cmd = cyberdogCmd;
+}
+
+static bool kbhit()
+{
+    termios term;
+    tcgetattr(0, &term);
+    
+    termios term2 = term;
+    term2.c_lflag &= ~ICANON;
+    tcsetattr(0, TCSANOW, &term2);
+    
+    int byteswaiting;
+    ioctl(0, FIONREAD, &byteswaiting);
+    
+    tcsetattr(0, TCSANOW, &term);
+    
+    return byteswaiting > 0;
+}
+void RL_Real::run_keyboard()
+{
+    int c;
+    // Check for keyboard input
+    while(true)
+    {
+        if(kbhit())
+        {
+            c = fgetc(stdin);
+            switch(c)
+            {
+                case '0':
+                    keyboard.robot_state = STATE_POS_GETUP;
+                    break;
+                case 'p':
+                    keyboard.robot_state = STATE_RL_INIT;
+                    break;
+                case '1':
+                    keyboard.robot_state = STATE_POS_GETDOWN;
+                    break;
+                case 'q':
+                    break;
+                case 'w':
+                    keyboard.x += 0.1;
+                    break;
+                case 's':
+                    keyboard.x -= 0.1;
+                    break;
+                case 'a':
+                    keyboard.yaw += 0.1;
+                    break;
+                case 'd':
+                    keyboard.yaw -= 0.1;
+                    break;
+                case 'i':
+                    break;
+                case 'k':
+                    break;
+                case 'j':
+                    keyboard.y += 0.1;
+                    break;
+                case 'l':
+                    keyboard.y -= 0.1;
+                    break;
+                case ' ':
+                    keyboard.x = 0;
+                    keyboard.y = 0;
+                    keyboard.yaw = 0;
+                    break;
+                default:
+                    break;
+            }
+        }
+        usleep(10000);
+    } 
+}
+
+void RL_Real::RunModel()
+{
+    if(robot_state == STATE_RL_RUNNING)
+    {
+        // auto duration = std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::high_resolution_clock::now() - start_time).count();
+        // std::cout << "Execution time: " << duration << " microseconds" << std::endl;
+        // start_time = std::chrono::high_resolution_clock::now();
+
+        // printf("%f, %f, %f\n", 
+        //     cyberdogData.omega[0], cyberdogData.omega[1], cyberdogData.omega[2]);
+        // printf("%f, %f, %f, %f\n", 
+        //     cyberdogData.quat[1], cyberdogData.quat[2], cyberdogData.quat[3], cyberdogData.quat[0]);
+        // printf("%f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f\n", 
+        //     cyberdogData.q[3], cyberdogData.q[4], cyberdogData.q[5],
+        //     cyberdogData.q[0], cyberdogData.q[1], cyberdogData.q[2],
+        //     cyberdogData.q[9], cyberdogData.q[10], cyberdogData.q[11],
+        //     cyberdogData.q[6], cyberdogData.q[7], cyberdogData.q[8]);
+        // printf("%f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f\n", 
+        //     cyberdogData.qd[3], cyberdogData.qd[4], cyberdogData.qd[5],
+        //     cyberdogData.qd[0], cyberdogData.qd[1], cyberdogData.qd[2],
+        //     cyberdogData.qd[9], cyberdogData.qd[10], cyberdogData.qd[11],
+        //     cyberdogData.qd[6], cyberdogData.qd[7], cyberdogData.qd[8]);
+        
+        this->obs.ang_vel = torch::tensor({{cyberdogData.omega[0], cyberdogData.omega[1], cyberdogData.omega[2]}});
+        this->obs.commands = torch::tensor({{keyboard.x, keyboard.y, keyboard.yaw}});
+        this->obs.base_quat = torch::tensor({{cyberdogData.quat[1], cyberdogData.quat[2], cyberdogData.quat[3], cyberdogData.quat[0]}});
+        this->obs.dof_pos = torch::tensor({{cyberdogData.q[3], cyberdogData.q[4], cyberdogData.q[5],
+                                            cyberdogData.q[0], cyberdogData.q[1], cyberdogData.q[2],
+                                            cyberdogData.q[9], cyberdogData.q[10], cyberdogData.q[11],
+                                            cyberdogData.q[6], cyberdogData.q[7], cyberdogData.q[8]}});
+        this->obs.dof_vel = torch::tensor({{cyberdogData.qd[3], cyberdogData.qd[4], cyberdogData.qd[5],
+                                            cyberdogData.qd[0], cyberdogData.qd[1], cyberdogData.qd[2],
+                                            cyberdogData.qd[9], cyberdogData.qd[10], cyberdogData.qd[11],
+                                            cyberdogData.qd[6], cyberdogData.qd[7], cyberdogData.qd[8]}});
+        
+        torch::Tensor actions = this->Forward();
+
+        for (int i : hip_scale_reduction_indices)
+        {
+            actions[0][i] *= this->params.hip_scale_reduction;
+        }
+
+        output_torques = this->ComputeTorques(actions);
+        output_dof_pos = this->ComputePosition(actions);
+    }
+    
+}
+
+torch::Tensor RL_Real::ComputeObservation()
+{
+    torch::Tensor obs = torch::cat({// this->QuatRotateInverse(this->obs.base_quat, this->obs.lin_vel) * this->params.lin_vel_scale,
+                                    this->QuatRotateInverse(this->obs.base_quat, this->obs.ang_vel) * this->params.ang_vel_scale,
+                                    this->QuatRotateInverse(this->obs.base_quat, this->obs.gravity_vec),
+                                    this->obs.commands * this->params.commands_scale,
+                                    (this->obs.dof_pos - this->params.default_dof_pos) * this->params.dof_pos_scale,
+                                    this->obs.dof_vel * this->params.dof_vel_scale,
+                                    this->obs.actions
+                                    },1);
+    obs = torch::clamp(obs, -this->params.clip_obs, this->params.clip_obs);
+    return obs;
+}
+
+torch::Tensor RL_Real::Forward()
+{
+    torch::Tensor obs = this->ComputeObservation();
+
+    history_obs_buf.insert(obs);
+    history_obs = history_obs_buf.get_obs_vec({0, 1, 2, 3, 4, 5});
+
+    torch::Tensor encoding = this->encoder.forward({history_obs}).toTensor();
+
+    torch::Tensor z = this->vq.forward({encoding}).toTensor();
+
+    torch::Tensor actor_input = torch::cat({obs, z}, 1);
+
+    torch::Tensor action = this->actor.forward({actor_input}).toTensor();
+
+    this->obs.actions = action;
+    torch::Tensor clamped = torch::clamp(action, -this->params.clip_actions, this->params.clip_actions);
+
+    return clamped;
+}
+
+void RL_Real::Plot()
+{
+    plot_t.push_back(motiontime);
+    plt::cla();
+    plt::clf();
+    for(int i = 0; i < 12; ++i)
+    {
+        plot_real_joint_pos[i].push_back(cyberdogData.q[i]);
+        plot_target_joint_pos[i].push_back(cyberdogCmd.q_des[i]);
+        plt::subplot(4, 3, i+1);
+        plt::named_plot("_real_joint_pos", plot_t, plot_real_joint_pos[i], "r");
+        plt::named_plot("_target_joint_pos", plot_t, plot_target_joint_pos[i], "b");
+        plt::xlim(motiontime-10000, motiontime);
+    }
+    // plt::legend();
+    plt::pause(0.0001);
+}
+
+void signalHandler(int signum)
+{
+    exit(0);
+}
+
+int main(int argc, char **argv)
+{
+    signal(SIGINT, signalHandler);
+
+    while(1)
+    {
+        sleep(10);
+    };
+
+    return 0;
+}