fix: support go2 sdk

README.md

@@ -128,13 +128,14 @@ Or press **0** on the keyboard to switch the robot to the default standing posit
 
 #### Unitree Go2
 
-TODO-devel-go2 添加go2的使用说明
+TODO-devel-go2 完善go2的使用说明
-
-Open a new terminal and start the control program
 
+1. Connect one end of the Ethernet cable to the Go2 robot and the other end to the user's computer. Then, enable USB Ethernet on the computer and configure it. The IP address of the onboard computer on the Go2 robot is 192.168.123.161, so the computer's USB Ethernet address should be set to the same network segment as the robot. For example, enter 192.168.123.222 in the "Address" field ("222" can be replaced with another number).
+2. Use the `ifconfig` command to find the name of the network interface for the 123 network segment, such as `enxf8e43b808e06`. In the following steps, replace `<YOUR_NETWORK_INTERFACE>` with the actual network interface name.
+3. Open a new terminal and start the control program:
     ```bash
     source devel/setup.bash
-rosrun rl_sar rl_real_go2
+    rosrun rl_sar rl_real_go2 <YOUR_NETWORK_INTERFACE>
     ```
 
 ### Train the actuator network

README_CN.md

@@ -129,13 +129,14 @@ rosrun rl_sar rl_real_a1
 
 #### Unitree Go2
 
-TODO-devel-go2 添加go2的使用说明
+TODO-devel-go2 完善go2的使用说明
-
-新建终端，启动控制程序
 
+1. 用网线的一端连接Go2机器人，另一端连接用户电脑，并开启电脑的 USB Ethernet 后进行配置。机器狗机载电脑的 IP 地地址为 192.168.123.161，故需将电脑 USB Ethernet 地址设置为与机器狗同一网段，如在 Address 中输入 192.168.123.222 (“222”可以改成其他)。
+2. 通过`ifconfig`命令查看123网段的网卡名字，如`enxf8e43b808e06`，下文用 \<YOUR_NETWORK_INTERFACE\> 代替
+3. 新建终端，启动控制程序
     ```bash
     source devel/setup.bash
-rosrun rl_sar rl_real_go2
+    rosrun rl_sar rl_real_go2 <YOUR_NETWORK_INTERFACE>
     ```
 
 ### 训练执行器网络

src/rl_sar/CMakeLists.txt

@@ -37,16 +37,22 @@ catkin_package(
   rospy
 )
 
+# Unitree A1
 include_directories(library/unitree_legged_sdk_3.2/include)
 link_directories(library/unitree_legged_sdk_3.2/lib)
-# TODO-devel-go2 add go2 sdk
+set(UNITREE_A1_LIBS -pthread unitree_legged_sdk_amd64 lcm)
 
-set(EXTRA_LIBS -pthread libunitree_legged_sdk_amd64.so lcm)
+# Unitree Go2
+include_directories(library/unitree_sdk2/include)
+link_directories(library/unitree_sdk2/lib/x86_64)
+include_directories(library/unitree_sdk2/thirdparty/include)
+include_directories(library/unitree_sdk2/thirdparty/include/ddscxx)
+link_directories(library/unitree_sdk2/thirdparty/lib/x86_64)
+set(UNITREE_GO2_LIBS -pthread unitree_sdk2 ddsc ddscxx)
 
 include_directories(
   include
   ${catkin_INCLUDE_DIRS}
-  ${unitree_legged_sdk_INCLUDE_DIRS}
   library/matplotlibcpp
   library/observation_buffer
   library/rl_sdk
@@ -69,19 +75,19 @@ set_property(TARGET observation_buffer PROPERTY CXX_STANDARD 14)
 
 add_executable(rl_sim src/rl_sim.cpp )
 target_link_libraries(rl_sim
-  ${catkin_LIBRARIES} ${EXTRA_LIBS}
+  ${catkin_LIBRARIES} -pthread
   rl_sdk observation_buffer yaml-cpp
 )
 
 add_executable(rl_real_a1 src/rl_real_a1.cpp )
 target_link_libraries(rl_real_a1
-  ${catkin_LIBRARIES} ${EXTRA_LIBS}
+  ${catkin_LIBRARIES} ${UNITREE_A1_LIBS}
   rl_sdk observation_buffer yaml-cpp
 )
 
 add_executable(rl_real_go2 src/rl_real_go2.cpp )
 target_link_libraries(rl_real_go2
-  ${catkin_LIBRARIES} ${EXTRA_LIBS}
+  ${catkin_LIBRARIES} ${UNITREE_GO2_LIBS}
   rl_sdk observation_buffer yaml-cpp
 )
 

src/rl_sar/include/rl_real_go2.hpp

@@ -11,7 +11,7 @@
 #include <unitree/common/time/time_tool.hpp>
 #include <unitree/common/thread/thread.hpp>
 #include <unitree/robot/go2/robot_state/robot_state_client.hpp>
-#include "unitree_joystick.h" // TODO-devel-go2 这里调用的是a1的，go2有自己的键盘接口吗
+// #include "unitree_joystick.h" // TODO-devel-go2 go2键盘接口
 #include <csignal>
 
 #include "matplotlibcpp.h"
@@ -28,7 +28,7 @@ constexpr double VelStopF = (16000.0f);
 class RL_Real : public RL
 {
 public:
-    RL_Real();
+    RL_Real(const std::string &network_interface);
     ~RL_Real();
 
 private:
@@ -68,7 +68,7 @@ private:
     unitree_go::msg::dds_::LowState_ unitree_low_state{}; // default init
     ChannelPublisherPtr<unitree_go::msg::dds_::LowCmd_> lowcmd_publisher; // publisher
     ChannelSubscriberPtr<unitree_go::msg::dds_::LowState_> lowstate_subscriber; //subscriber
-    xRockerBtnDataStruct unitree_joy;
+    // xRockerBtnDataStruct unitree_joy; // TODO-devel-go2 go2键盘接口
 
     // others
     int motiontime = 0;

src/rl_sar/src/rl_real_a1.cpp

@@ -3,8 +3,6 @@
 // #define PLOT
 // #define CSV_LOGGER
 
-RL_Real rl_sar;
-
 RL_Real::RL_Real() : unitree_safe(UNITREE_LEGGED_SDK::LeggedType::A1), unitree_udp(UNITREE_LEGGED_SDK::LOWLEVEL)
 {
     // read params from yaml
@@ -19,17 +17,15 @@ RL_Real::RL_Real() : unitree_safe(UNITREE_LEGGED_SDK::LeggedType::A1), unitree_u
         }
     }
 
-    // history
-    if (!this->params.observations_history.empty())
-    {
-        this->history_obs_buf = ObservationBuffer(1, this->params.num_observations, this->params.observations_history.size());
-    }
-
     // init robot
     this->unitree_udp.InitCmdData(this->unitree_low_command);
 
     // init rl
     torch::autograd::GradMode::set_enabled(false);
+    if (!this->params.observations_history.empty())
+    {
+        this->history_obs_buf = ObservationBuffer(1, this->params.num_observations, this->params.observations_history.size());
+    }
     this->InitObservations();
     this->InitOutputs();
     this->InitControl();
@@ -242,6 +238,8 @@ int main(int argc, char **argv)
 {
     signal(SIGINT, signalHandler);
 
+    RL_Real rl_sar;
+
     while (1)
     {
         sleep(10);

src/rl_sar/src/rl_real_go2.cpp

@@ -3,9 +3,7 @@
 // #define PLOT
 // #define CSV_LOGGER
 
-RL_Real rl_sar;
+RL_Real::RL_Real(const std::string &network_interface)
-
-void RL_Real::RL_Real()
 {
     // read params from yaml
     this->robot_name = "go2_isaacgym";
@@ -19,18 +17,13 @@ void RL_Real::RL_Real()
         }
     }
 
-    // history
-    if (!this->params.observations_history.empty())
-    {
-        this->history_obs_buf = ObservationBuffer(1, this->params.num_observations, this->params.observations_history.size());
-    }
-
     // init robot
+    ChannelFactory::Instance()->Init(0, network_interface);
     this->InitRobotStateClient();
-    while (rl_sar.QueryServiceStatus("sport_mode"))
+    while (this->QueryServiceStatus("sport_mode"))
     {
         std::cout << "Try to deactivate the service: " << "sport_mode" << std::endl;
-        rl_sar.ActivateService("sport_mode", 0);
+        this->ActivateService("sport_mode", 0);
         sleep(1);
     }
     this->InitLowCmd();
@@ -45,6 +38,10 @@ void RL_Real::RL_Real()
 
     // init rl
     torch::autograd::GradMode::set_enabled(false);
+    if (!this->params.observations_history.empty())
+    {
+        this->history_obs_buf = ObservationBuffer(1, this->params.num_observations, this->params.observations_history.size());
+    }
     this->InitObservations();
     this->InitOutputs();
     this->InitControl();
@@ -89,21 +86,21 @@ RL_Real::~RL_Real()
 
 void RL_Real::GetState(RobotState<double> *state)
 {
-    // TODO-devel-mutex 加锁
+    // TODO-devel-go2 go2键盘接口
-    memcpy(&this->unitree_joy, &this->unitree_low_state.wireless_remote()[0], 40);
+    // memcpy(&this->unitree_joy, &this->unitree_low_state.wireless_remote()[0], 40);
 
-    if ((int)this->unitree_joy.btn.components.R2 == 1)
+    // if ((int)this->unitree_joy.btn.components.R2 == 1)
-    {
+    // {
-        this->control.control_state = STATE_POS_GETUP;
+    //     this->control.control_state = STATE_POS_GETUP;
-    }
+    // }
-    else if ((int)this->unitree_joy.btn.components.R1 == 1)
+    // else if ((int)this->unitree_joy.btn.components.R1 == 1)
-    {
+    // {
-        this->control.control_state = STATE_RL_INIT;
+    //     this->control.control_state = STATE_RL_INIT;
-    }
+    // }
-    else if ((int)this->unitree_joy.btn.components.L2 == 1)
+    // else if ((int)this->unitree_joy.btn.components.L2 == 1)
-    {
+    // {
-        this->control.control_state = STATE_POS_GETDOWN;
+    //     this->control.control_state = STATE_POS_GETDOWN;
-    }
+    // }
 
     if (this->params.framework == "isaacgym")
     {
@@ -348,6 +345,14 @@ int main(int argc, char **argv)
 {
     signal(SIGINT, signalHandler);
 
+    if (argc < 2)
+    {
+        std::cout << "Usage: " << argv[0] << " networkInterface" << std::endl;
+        exit(-1);
+    }
+
+    RL_Real rl_sar(argv[1]);
+
     while (1)
     {
         sleep(10);

src/rl_sar/src/rl_sim.cpp

@@ -18,13 +18,6 @@ RL_Sim::RL_Sim()
             observation = "ang_vel_world";
         }
     }
-
-    // history
-    if (this->params.observations_history.size() != 0)
-    {
-        this->history_obs_buf = ObservationBuffer(1, this->params.num_observations, this->params.observations_history.size());
-    }
-
     // Due to the fact that the robot_state_publisher sorts the joint names alphabetically,
     // the mapping table is established according to the order defined in the YAML file
     std::vector<std::string> sorted_joint_controller_names = this->params.joint_controller_names;
@@ -37,8 +30,12 @@ RL_Sim::RL_Sim()
     this->mapped_joint_velocities = std::vector<double>(this->params.num_of_dofs, 0.0);
     this->mapped_joint_efforts = std::vector<double>(this->params.num_of_dofs, 0.0);
 
-    // init
+    // init rl
     torch::autograd::GradMode::set_enabled(false);
+    if (this->params.observations_history.size() != 0)
+    {
+        this->history_obs_buf = ObservationBuffer(1, this->params.num_observations, this->params.observations_history.size());
+    }
     this->joint_publishers_commands.resize(this->params.num_of_dofs);
     this->InitObservations();
     this->InitOutputs();