feat: yaml-cpp

.gitignore

@@ -2,6 +2,4 @@ build
 devel
 logs
 .catkin_tools
-.vscode
+.vscode
-old
-*.pt

README_CN.md

@@ -34,6 +34,20 @@ echo 'export Torch_DIR=/path/to/your/torchlib' >> ~/.bashrc
 sudo apt install ros-noetic-teleop-twist-keyboard
 ```
 
+## 依赖
+
+安装yaml-cpp
+
+```bash
+git clone https://github.com/jbeder/yaml-cpp.git
+cd yaml-cpp && mkdir build && cd build
+cmake -DYAML_BUILD_SHARED_LIBS=on .. && make
+sudo make install
+sudo ldconfig
+```
+
+头文件在/usr/local/include，库文件在/usr/local/lib
+
 ## 编译
 
 自定义代码中的以下两个函数，以适配不同的模型：

src/rl_sar/CMakeLists.txt

@@ -27,6 +27,9 @@ find_package(catkin REQUIRED COMPONENTS
 
 find_package(Python3 COMPONENTS Interpreter Development REQUIRED)
 
+find_package(yaml-cpp REQUIRED)
+include_directories(${YAML_CPP_INCLUDE_DIR})
+
 catkin_package(
     CATKIN_DEPENDS 
     unitree_legged_msgs 
@@ -36,6 +39,9 @@ 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)
 
+add_subdirectory(library/cyberdog_motor_sdk)
+include_directories(library/cyberdog_motor_sdk/include)
+
 include_directories(
     include
     ${catkin_INCLUDE_DIRS}
@@ -61,19 +67,18 @@ 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} "${TORCH_LIBRARIES}"
-  rl observation_buffer
+  rl observation_buffer yaml-cpp
 )
 
-add_executable(rl_real src/rl_real.cpp )
+add_executable(rl_real_a1 src/rl_real_a1.cpp )
-target_link_libraries(rl_real
+target_link_libraries(rl_real_a1
   ${catkin_LIBRARIES} ${EXTRA_LIBS} "${TORCH_LIBRARIES}"
-  rl observation_buffer
+  rl observation_buffer yaml-cpp
 )
 
-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
+  rl observation_buffer cyberdog_motor_sdk yaml-cpp
 )

src/rl_sar/config.yaml

@@ -0,0 +1,45 @@
+a1:
+  num_observations: 45
+  clip_obs: 100.0
+  clip_actions: 100.0
+  damping: 0.5
+  stiffness: 20.0
+  action_scale: 0.25
+  hip_scale_reduction: 0.5
+  num_of_dofs: 12
+  lin_vel_scale: 2.0
+  ang_vel_scale: 0.25
+  dof_pos_scale: 1.0
+  dof_vel_scale: 0.05
+  torque_limits: [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
+  default_dof_pos: [ 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
+
+cyberdog1:
+  num_observations: 45
+  clip_obs: 100.0
+  clip_actions: 100.0
+  damping: 0.5
+  stiffness: 20.0
+  action_scale: 0.25
+  hip_scale_reduction: 0.5
+  num_of_dofs: 12
+  lin_vel_scale: 2.0
+  ang_vel_scale: 0.25
+  dof_pos_scale: 1.0
+  dof_vel_scale: 0.05
+  torque_limits: [24.0, 24.0, 24.0,    
+                  24.0, 24.0, 24.0,
+                  24.0, 24.0, 24.0,
+                  24.0, 24.0, 24.0]
+  #                  hip,    thigh,   calf
+  default_dof_pos: [ 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

src/rl_sar/library/rl/rl.cpp

@@ -1,5 +1,45 @@
 #include "rl.hpp"
 
+void RL::ReadYaml(std::string robot_name)
+{
+	YAML::Node config;
+	try
+	{
+		config = YAML::LoadFile(CONFIG_PATH)[robot_name];
+	} catch(YAML::BadFile &e)
+	{
+
+		std::cout << "The file '" << CONFIG_PATH << "' does not exist" << std::endl;
+		return;
+	}
+
+    this->params.num_observations = config["num_observations"].as<int>();
+    this->params.clip_obs = config["clip_obs"].as<float>();
+    this->params.clip_actions = config["clip_actions"].as<float>();
+    this->params.damping = config["damping"].as<float>();
+    this->params.stiffness = config["stiffness"].as<float>();
+    this->params.d_gains = torch::ones(12) * this->params.damping;
+    this->params.p_gains = torch::ones(12) * this->params.stiffness;
+    this->params.action_scale = config["action_scale"].as<float>();
+    this->params.hip_scale_reduction = config["hip_scale_reduction"].as<float>();
+    this->params.num_of_dofs = config["num_of_dofs"].as<int>();
+    this->params.lin_vel_scale = config["lin_vel_scale"].as<float>();
+    this->params.ang_vel_scale = config["ang_vel_scale"].as<float>();
+    this->params.dof_pos_scale = config["dof_pos_scale"].as<float>();
+    this->params.dof_vel_scale =config["dof_vel_scale"].as<float>();
+    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({{config["torque_limits"][0].as<float>(), config["torque_limits"][1].as<float>(), config["torque_limits"][2].as<float>(),    
+                                                 config["torque_limits"][3].as<float>(), config["torque_limits"][4].as<float>(), config["torque_limits"][5].as<float>(), 
+                                                 config["torque_limits"][6].as<float>(), config["torque_limits"][7].as<float>(), config["torque_limits"][8].as<float>(), 
+                                                 config["torque_limits"][9].as<float>(), config["torque_limits"][10].as<float>(), config["torque_limits"][11].as<float>()}});
+
+    this->params.default_dof_pos = torch::tensor({{config["default_dof_pos"][0].as<float>(), config["default_dof_pos"][1].as<float>(), config["default_dof_pos"][2].as<float>(),    
+                                                  config["default_dof_pos"][3].as<float>(), config["default_dof_pos"][4].as<float>(), config["default_dof_pos"][5].as<float>(), 
+                                                  config["default_dof_pos"][6].as<float>(), config["default_dof_pos"][7].as<float>(), config["default_dof_pos"][8].as<float>(), 
+                                                  config["default_dof_pos"][9].as<float>(), config["default_dof_pos"][10].as<float>(), config["default_dof_pos"][11].as<float>()}});
+}
+
 torch::Tensor RL::QuatRotateInverse(torch::Tensor q, torch::Tensor v)
 {
     c10::IntArrayRef shape = q.sizes();
@@ -23,6 +63,12 @@ void RL::InitObservations()
     this->obs.actions = 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}});
 }
 
+void RL::InitOutputs()
+{
+    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;
+}
+
 torch::Tensor RL::ComputeTorques(torch::Tensor actions)
 {
     torch::Tensor actions_scaled = actions * this->params.action_scale;

src/rl_sar/library/rl/rl.hpp

@@ -8,6 +8,9 @@
 #include "matplotlibcpp.h"
 namespace plt = matplotlibcpp;
 
+#include <yaml-cpp/yaml.h>
+#define CONFIG_PATH CMAKE_CURRENT_SOURCE_DIR "/config.yaml"
+
 struct ModelParams
 {
     int num_observations;
@@ -15,7 +18,7 @@ struct ModelParams
     float stiffness;
     float action_scale;
     float hip_scale_reduction;
-    float num_of_dofs;
+    int num_of_dofs;
     float lin_vel_scale;
     float ang_vel_scale;
     float dof_pos_scale;
@@ -55,6 +58,8 @@ public:
     torch::Tensor ComputePosition(torch::Tensor actions);
     torch::Tensor QuatRotateInverse(torch::Tensor q, torch::Tensor v);
     void InitObservations();
+    void InitOutputs();
+    void ReadYaml(std::string robot_name);
 
 protected:
     // rl module

src/rl_sar/src/rl_real_a1.cpp

@@ -1,55 +1,31 @@
 #include "../include/rl_real.hpp"
 
+#define ROBOT_NAME "a1"
+
 // #define PLOT
 
 RL_Real rl_sar;
 
 RL_Real::RL_Real() : safe(LeggedType::A1), udp(LOWLEVEL)
 {
+    ReadYaml(ROBOT_NAME);
+
     udp.InitCmdData(cmd);
 
     start_time = std::chrono::high_resolution_clock::now();
 
-    std::string actor_path = std::string(CMAKE_CURRENT_SOURCE_DIR) + "/models/base/actor.pt";
+    std::string actor_path = std::string(CMAKE_CURRENT_SOURCE_DIR) + "/models/" + ROBOT_NAME + "/actor.pt";
-    std::string encoder_path = std::string(CMAKE_CURRENT_SOURCE_DIR) + "/models/base/encoder.pt";
+    std::string encoder_path = std::string(CMAKE_CURRENT_SOURCE_DIR) + "/models/" + ROBOT_NAME + "/encoder.pt";
-    std::string vq_path = std::string(CMAKE_CURRENT_SOURCE_DIR) + "/models/base/vq_layer.pt";
+    std::string vq_path = std::string(CMAKE_CURRENT_SOURCE_DIR) + "/models/" + ROBOT_NAME + "/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->InitOutputs();
     
-    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);
 

src/rl_sar/src/rl_real_cyberdog.cpp

@@ -1,53 +1,29 @@
 #include "../include/rl_real_cyberdog.hpp"
 
-#define PLOT
+#define ROBOT_NAME "cyberdog1"
+
+// #define PLOT
 
 RL_Real rl_sar;
 
 RL_Real::RL_Real() : CustomInterface(500)
 {
+    ReadYaml(ROBOT_NAME);
+
     start_time = std::chrono::high_resolution_clock::now();
 
-    std::string actor_path = std::string(CMAKE_CURRENT_SOURCE_DIR) + "/models/cyberdog/actor.pt";
+    std::string actor_path = std::string(CMAKE_CURRENT_SOURCE_DIR) + "/models/" + ROBOT_NAME + "/actor.pt";
-    std::string encoder_path = std::string(CMAKE_CURRENT_SOURCE_DIR) + "/models/cyberdog/encoder.pt";
+    std::string encoder_path = std::string(CMAKE_CURRENT_SOURCE_DIR) + "/models/" + ROBOT_NAME + "/encoder.pt";
-    std::string vq_path = std::string(CMAKE_CURRENT_SOURCE_DIR) + "/models/cyberdog/vq_layer.pt";
+    std::string vq_path = std::string(CMAKE_CURRENT_SOURCE_DIR) + "/models/" + ROBOT_NAME + "/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->InitOutputs();
-    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({{24.0, 24.0, 24.0,    
-                                                 24.0, 24.0, 24.0,
-                                                 24.0, 24.0, 24.0,
-                                                 24.0, 24.0, 24.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);
 

src/rl_sar/src/rl_sim.cpp

@@ -1,9 +1,13 @@
 #include "../include/rl_sim.hpp"
 
+#define ROBOT_NAME "cyberdog1"
+
 // #define PLOT
 
 RL_Sim::RL_Sim()
 {
+    ReadYaml(ROBOT_NAME);
+
     ros::NodeHandle nh;
     start_time = std::chrono::high_resolution_clock::now();
 
@@ -11,46 +15,18 @@ RL_Sim::RL_Sim()
 
     motor_commands.resize(12);
 
-    std::string actor_path = std::string(CMAKE_CURRENT_SOURCE_DIR) + "/models/actor.pt";
+    std::string actor_path = std::string(CMAKE_CURRENT_SOURCE_DIR) + "/models/" + ROBOT_NAME + "/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/" + ROBOT_NAME + "/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/" + ROBOT_NAME + "/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->InitOutputs();
-    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;
     joint_positions = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
     joint_velocities = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
     plot_real_joint_pos.resize(12);