feat: unitree go2 sim and real support (#27)

* feat: unitree go2 sim and real support * feat: remove the `use_history` field, add `observations_history` to support history observations, and adjust the related logic to handle the case of empty history observations * feat: fix coordinate bug in ang_vel * fix: simplified handling of historical observation parameters, ensuring initialization of the observation history buffer in non-empty cases. --------- Co-authored-by: fan-ziqi <[email protected]>
fan-ziqi · Oct 15, 2024 · b356b61 · b356b61
1 parent 8b6a2d6
commit b356b61
Show file tree

Hide file tree

Showing 750 changed files with 125,122 additions and 141 deletions.
diff --git a/CONTRIBUTORS.md b/CONTRIBUTORS.md
@@ -0,0 +1,22 @@
+# rl_sar Developers and Contributors
+
+This is the official list of rl_sar Project developers and contributors.
+
+To see the full list of contributors, please check the revision history in the source control.
+
+Guidelines for modifications:
+
+* Please keep the lists sorted alphabetically.
+* Names should be added to this file as: *individual names* or *organizations*.
+
+## Developers
+
+* Ziqi Fan
+
+## Contributors
+
+* Jijie Huang
+
+## Acknowledgements
+
+* Unitree Robotics
diff --git a/README.md b/README.md
@@ -108,7 +108,7 @@ Control:
 
 ### Real Robots
 
-**Example: Unitree A1**
+#### Unitree A1
 
 Unitree A1 can be connected using both wireless and wired methods:
 
@@ -126,9 +126,22 @@ Press the **R2** button on the controller to switch the robot to the default sta
 
 Or press **0** on the keyboard to switch the robot to the default standing position, press **P** to switch to RL control mode, and press **1** in any state to switch to the initial lying position. WS controls x-axis, AD controls yaw, and JL controls y-axis.
 
+#### Unitree Go2
+
+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 <YOUR_NETWORK_INTERFACE>
+    ```
+4. Go2 supports both joy and keyboard control, using the same method as mentioned above for A1.
+
 ### Train the actuator network
 
-1. Uncomment `#define CSV_LOGGER` in the top of `rl_real.cpp`. You can also modify the corresponding part in the simulation program to collect simulation data for testing the training process.
+Take A1 as an example below
+
+1. Uncomment `#define CSV_LOGGER` in the top of `rl_real_a1.cpp`. You can also modify the corresponding part in the simulation program to collect simulation data for testing the training process.
 2. Run the control program, and the program will log all data after execution.
 3. Stop the control program and start training the actuator network. Note that `rl_sar/src/rl_sar/models/` is omitted before the following paths.
     ```bash
@@ -150,9 +163,11 @@ In the following text, `<ROBOT>` represents the name of the robot
 5. If you need to run simulations, modify the launch files as needed by referring to those in the `rl_sar/src/rl_sar/launch` directory.
 6. If you need to run on the physical robot, modify the file `rl_sar/src/rl_sar/src/rl_real_a1.cpp` as needed.
 
-## Reference
+## Contributing
+
+Wholeheartedly welcome contributions from the community to make this framework mature and useful for everyone. These may happen as bug reports, feature requests, or code contributions.
 
-[unitree_ros](https://github.com/unitreerobotics/unitree_ros)
+[List of contributors](CONTRIBUTORS.md)
 
 ## Citation
 

diff --git a/README_CN.md b/README_CN.md
@@ -109,7 +109,7 @@ source devel/setup.bash
 
 ### 真实机器人
 
-**示例：Unitree A1**
+#### Unitree A1
 
 与Unitree A1连接可以使用无线与有线两种方式
 
@@ -127,9 +127,22 @@ rosrun rl_sar rl_real_a1
 
 或者按下键盘上的**0**键让机器人切换到默认站起姿态，按下**P**键切换到RL控制模式，任意状态按下**1**键切换到最初的趴下姿态。WS控制x，AD控制yaw，JL控制y。
 
+#### Unitree 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 <YOUR_NETWORK_INTERFACE>
+    ```
+4. Go2支持手柄与键盘控制，方法与上面a1相同
+
 ### 训练执行器网络
 
-1. 取消注释`rl_real.cpp`中最上面的`#define CSV_LOGGER`，你也可以在仿真程序中修改对应部分采集仿真数据用来测试训练过程。
+下面拿A1举例
+
+1. 取消注释`rl_real_a1.cpp`中最上面的`#define CSV_LOGGER`，你也可以在仿真程序中修改对应部分采集仿真数据用来测试训练过程。
 2. 运行控制程序，程序会在执行后记录所有数据。
 3. 停止控制程序，开始训练执行器网络。注意，下面的路径前均省略了`rl_sar/src/rl_sar/models/`。
     ```bash
@@ -151,9 +164,11 @@ rosrun rl_sar rl_real_a1
 5. 若需要运行仿真，则参考`rl_sar/src/rl_sar/launch`路径下的launch文件自行修改
 6. 若需要运行实物，则参考`rl_sar/src/rl_sar/src/rl_real_a1.cpp`文件自行修改
 
-## 参考
+## 贡献
+
+衷心欢迎社区的贡献，以使这个框架更加成熟和对所有人有用。贡献可以是bug报告、功能请求或代码贡献。
 
-[unitree_ros](https://github.com/unitreerobotics/unitree_ros)
+[贡献者名单](CONTRIBUTORS.md)
 
 ## 引用
 

diff --git a/src/rl_sar/CMakeLists.txt b/src/rl_sar/CMakeLists.txt
@@ -31,24 +31,31 @@ find_package(catkin REQUIRED COMPONENTS
 
 find_package(Python3 COMPONENTS Interpreter Development REQUIRED)
 
-# find_package(yaml-cpp REQUIRED)
-link_directories(/usr/local/lib)
-include_directories(${YAML_CPP_INCLUDE_DIR})
-
 catkin_package(
   CATKIN_DEPENDS
   robot_joint_controller
   rospy
 )
 
+link_directories(/usr/local/lib)
+include_directories(${YAML_CPP_INCLUDE_DIR})
+
+# Unitree A1
 include_directories(library/unitree_legged_sdk_3.2/include)
 link_directories(library/unitree_legged_sdk_3.2/lib)
-set(EXTRA_LIBS -pthread libunitree_legged_sdk_amd64.so lcm)
+set(UNITREE_A1_LIBS -pthread unitree_legged_sdk_amd64 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
@@ -71,13 +78,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} ${UNITREE_GO2_LIBS}
   rl_sdk observation_buffer yaml-cpp
 )
 

diff --git a/src/rl_sar/include/rl_real_go2.hpp b/src/rl_sar/include/rl_real_go2.hpp
@@ -0,0 +1,109 @@
+#ifndef RL_REAL_HPP
+#define RL_REAL_HPP
+
+#include "rl_sdk.hpp"
+#include "observation_buffer.hpp"
+#include "loop.hpp"
+#include <unitree/robot/channel/channel_publisher.hpp>
+#include <unitree/robot/channel/channel_subscriber.hpp>
+#include <unitree/idl/go2/LowState_.hpp>
+#include <unitree/idl/go2/LowCmd_.hpp>
+#include <unitree/idl/go2/WirelessController_.hpp>
+#include <unitree/robot/client/client.hpp>
+#include <unitree/common/time/time_tool.hpp>
+#include <unitree/common/thread/thread.hpp>
+#include <unitree/robot/go2/robot_state/robot_state_client.hpp>
+#include <csignal>
+
+#include "matplotlibcpp.h"
+namespace plt = matplotlibcpp;
+
+using namespace unitree::common;
+using namespace unitree::robot;
+using namespace unitree::robot::go2;
+#define TOPIC_LOWCMD "rt/lowcmd"
+#define TOPIC_LOWSTATE "rt/lowstate"
+#define TOPIC_JOYSTICK "rt/wirelesscontroller"
+constexpr double PosStopF = (2.146E+9f);
+constexpr double VelStopF = (16000.0f);
+
+// 遥控器键值联合体
+typedef union
+{
+    struct
+    {
+        uint8_t R1 : 1;
+        uint8_t L1 : 1;
+        uint8_t start : 1;
+        uint8_t select : 1;
+        uint8_t R2 : 1;
+        uint8_t L2 : 1;
+        uint8_t F1 : 1;
+        uint8_t F2 : 1;
+        uint8_t A : 1;
+        uint8_t B : 1;
+        uint8_t X : 1;
+        uint8_t Y : 1;
+        uint8_t up : 1;
+        uint8_t right : 1;
+        uint8_t down : 1;
+        uint8_t left : 1;
+    } components;
+    uint16_t value;
+} xKeySwitchUnion;
+
+class RL_Real : public RL
+{
+public:
+    RL_Real();
+    ~RL_Real();
+
+private:
+    // rl functions
+    torch::Tensor Forward() override;
+    void GetState(RobotState<double> *state) override;
+    void SetCommand(const RobotCommand<double> *command) override;
+    void RunModel();
+    void RobotControl();
+
+    // history buffer
+    ObservationBuffer history_obs_buf;
+    torch::Tensor history_obs;
+
+    // loop
+    std::shared_ptr<LoopFunc> loop_keyboard;
+    std::shared_ptr<LoopFunc> loop_control;
+    std::shared_ptr<LoopFunc> loop_rl;
+    std::shared_ptr<LoopFunc> loop_plot;
+
+    // plot
+    const int plot_size = 100;
+    std::vector<int> plot_t;
+    std::vector<std::vector<double>> plot_real_joint_pos, plot_target_joint_pos;
+    void Plot();
+
+    // unitree interface
+    void InitRobotStateClient();
+    void InitLowCmd();
+    int QueryServiceStatus(const std::string &serviceName);
+    uint32_t Crc32Core(uint32_t *ptr, uint32_t len);
+    void LowStateMessageHandler(const void *messages);
+    void JoystickHandler(const void *message);
+    RobotStateClient rsc;
+    unitree_go::msg::dds_::LowCmd_ unitree_low_command{};
+    unitree_go::msg::dds_::LowState_ unitree_low_state{};
+    unitree_go::msg::dds_::WirelessController_ joystick{};
+    ChannelPublisherPtr<unitree_go::msg::dds_::LowCmd_> lowcmd_publisher;
+    ChannelSubscriberPtr<unitree_go::msg::dds_::LowState_> lowstate_subscriber;
+    ChannelSubscriberPtr<unitree_go::msg::dds_::WirelessController_> joystick_subscriber;
+    xKeySwitchUnion unitree_joy;
+
+    // others
+    int motiontime = 0;
+    std::vector<double> mapped_joint_positions;
+    std::vector<double> mapped_joint_velocities;
+    int command_mapping[12] = {3, 4, 5, 0, 1, 2, 9, 10, 11, 6, 7, 8};
+    int state_mapping[12] = {3, 4, 5, 0, 1, 2, 9, 10, 11, 6, 7, 8};
+};
+
+#endif // RL_REAL_HPP
diff --git a/src/rl_sar/launch/gazebo_go2_isaacgym.launch b/src/rl_sar/launch/gazebo_go2_isaacgym.launch
@@ -0,0 +1,54 @@
+<launch>
+    <arg name="wname" default="stairs"/>
+    <arg name="rname" default="go2"/>
+    <param name="robot_name" type="str" value="$(arg rname)_isaacgym"/>
+    <param name="ros_namespace" type="str" value="/$(arg rname)_gazebo/"/>
+    <param name="gazebo_model_name" type="str" value="$(arg rname)_gazebo"/>
+    <arg name="robot_path" value="(find $(arg rname)_description)"/>
+    <arg name="dollar" value="$"/>
+
+    <arg name="paused" default="true"/>
+    <arg name="use_sim_time" default="true"/>
+    <arg name="gui" default="true"/>
+    <arg name="headless" default="false"/>
+    <arg name="debug" default="false"/>
+    <!-- Debug mode will hung up the robot, use "true" or "false" to switch it. -->
+    <arg name="user_debug" default="false"/>
+
+    <include file="$(find gazebo_ros)/launch/empty_world.launch">
+        <arg name="world_name" value="$(find rl_sar)/worlds/$(arg wname).world"/>
+        <arg name="debug" value="$(arg debug)"/>
+        <arg name="gui" value="$(arg gui)"/>
+        <arg name="paused" value="$(arg paused)"/>
+        <arg name="use_sim_time" value="$(arg use_sim_time)"/>
+        <arg name="headless" value="$(arg headless)"/>
+    </include>
+
+    <!-- Load the URDF into the ROS Parameter Server -->
+    <param name="robot_description"
+           command="$(find xacro)/xacro --inorder '$(arg dollar)$(arg robot_path)/xacro/robot.xacro'
+           DEBUG:=$(arg user_debug)"/>
+
+    <!-- Run a python script to the send a service call to gazebo_ros to spawn a URDF robot -->
+    <!-- Set trunk and joint positions at startup -->
+    <node pkg="gazebo_ros" type="spawn_model" name="urdf_spawner" respawn="false" output="screen"
+          args="-urdf -z 0.6 -model $(arg rname)_gazebo -param robot_description"/>
+
+    <!-- Load joint controller configurations from YAML file to parameter server -->
+    <rosparam file="$(arg dollar)$(arg robot_path)/config/robot_control.yaml" command="load"/>
+
+    <!-- load the controllers -->
+    <node pkg="controller_manager" type="spawner" name="controller_spawner" respawn="false"
+          output="screen" ns="/$(arg rname)_gazebo" args="joint_state_controller
+          FL_hip_controller FL_thigh_controller FL_calf_controller
+          FR_hip_controller FR_thigh_controller FR_calf_controller
+          RL_hip_controller RL_thigh_controller RL_calf_controller
+          RR_hip_controller RR_thigh_controller RR_calf_controller "/>
+
+    <!-- convert joint states to TF transforms for rviz, etc -->
+    <node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher"
+          respawn="false" output="screen">
+        <remap from="/joint_states" to="/$(arg rname)_gazebo/joint_states"/>
+    </node>
+
+</launch>
diff --git a/src/rl_sar/library/rl_sdk/rl_sdk.cpp b/src/rl_sar/library/rl_sdk/rl_sdk.cpp
@@ -21,9 +21,19 @@ torch::Tensor RL::ComputeObservation()
         {
             obs_list.push_back(this->obs.lin_vel * this->params.lin_vel_scale);
         }
-        else if (observation == "ang_vel")
+        /*
+            The first argument of the QuatRotateInverse function is the quaternion representing the robot's orientation, and the second argument is in the world coordinate system. The function outputs the value of the second argument in the body coordinate system.
+            In IsaacGym, the coordinate system for angular velocity is in the world coordinate system. During training, the angular velocity in the observation uses QuatRotateInverse to transform the coordinate system to the body coordinate system.
+            In Gazebo, the coordinate system for angular velocity is also in the world coordinate system, so QuatRotateInverse is needed to transform the coordinate system to the body coordinate system.
+            In some real robots like Unitree, if the coordinate system for the angular velocity is already in the body coordinate system, no transformation is necessary.
+            Forgetting to perform the transformation or performing it multiple times may cause controller crashes when the rotation reaches 180 degrees.
+        */
+        else if (observation == "ang_vel_body")
+        {
+            obs_list.push_back(this->obs.ang_vel * this->params.ang_vel_scale);
+        }
+        else if (observation == "ang_vel_world")
         {
-            // obs_list.push_back(this->obs.ang_vel * this->params.ang_vel_scale); // TODO is QuatRotateInverse necessery?
             obs_list.push_back(this->QuatRotateInverse(this->obs.base_quat, this->obs.ang_vel, this->params.framework) * this->params.ang_vel_scale);
         }
         else if (observation == "gravity_vec")
@@ -421,11 +431,18 @@ void RL::ReadYaml(std::string robot_name)
     this->params.framework = config["framework"].as<std::string>();
     int rows = config["rows"].as<int>();
     int cols = config["cols"].as<int>();
-    this->params.use_history = config["use_history"].as<bool>();
     this->params.dt = config["dt"].as<double>();
     this->params.decimation = config["decimation"].as<int>();
     this->params.num_observations = config["num_observations"].as<int>();
     this->params.observations = ReadVectorFromYaml<std::string>(config["observations"]);
+    if (config["observations_history"].IsNull())
+    {
+        this->params.observations_history = {};
+    }
+    else
+    {
+        this->params.observations_history = ReadVectorFromYaml<int>(config["observations_history"]);
+    }
     this->params.clip_obs = config["clip_obs"].as<double>();
     if (config["clip_actions_lower"].IsNull() && config["clip_actions_upper"].IsNull())
     {

diff --git a/src/rl_sar/library/rl_sdk/rl_sdk.hpp b/src/rl_sar/library/rl_sdk/rl_sdk.hpp
@@ -72,11 +72,11 @@ struct ModelParams
 {
     std::string model_name;
     std::string framework;
-    bool use_history;
     double dt;
     int decimation;
     int num_observations;
     std::vector<std::string> observations;
+    std::vector<int> observations_history;
     double damping;
     double stiffness;
     double action_scale;