feat:

* add joint_names to config.yaml
* add ReadTensorFromYaml
* 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

README.md

@@ -172,6 +172,23 @@ Press the **R2** button on the controller to switch the robot to the default sta
     bash kill_cyberdog.sh
     ```
 
+## Add Your Robot
+
+In the following, let ROBOT represent the name of your robot.
+
+1. Create a model package named ROBOT_description in the robots folder. Place the URDF model in the urdf path within the folder and name it ROBOT.urdf. Create a namespace named ROBOT_gazebo in the config folder within the model file for joint configuration.
+2. Place the model file in models/ROBOT.
+3. Add a new field in rl_sar/config.yaml named ROBOT and adjust the parameters, such as changing the model_name to the model file name from the previous step.
+4. Add a new launch file in the rl_sar/launch folder. Refer to other launch files for guidance on modification.
+5. Change ROBOT_NAME to ROBOT in rl_xxx.cpp.
+6. Compile and run.
+7. If the torque of your robot's joints exceeds 50Nm, you need to modify line 180 in `rl_sar/src/unitree_ros/unitree_legged_control/src/joint_controller.cpp` to:
+   ```cpp
+   // calcTorque = computeTorque(currentPos, currentVel, servoCmd);      
+   calcTorque = servoCmd.posStiffness * (servoCmd.pos - currentPos) + servoCmd.velStiffness * (servoCmd.vel - currentVel) + servoCmd.torque;
+   ```
+   This will remove the 50Nm limit.
+
 ## Citation
 
 Please cite the following if you use this code or parts of it:

README_CN.md

@@ -172,6 +172,24 @@ rosrun rl_sar rl_real_a1
     bash kill_cyberdog.sh
     ```
 
+
+## 添加你的机器人
+
+下文中将ROBOT代表机器人名称
+
+1. 在robots文件夹中创建名为ROBOT_description的模型包，将模型的urdf放到文件夹中的urdf路径下并命名为ROBOT.urdf，在模型文件中的config文件夹中创建命名空间为ROBOT_gazebo的关节配置文件
+2. 将模型文件放到models/ROBOT中
+3. 在rl_sar/config.yaml中添加一个新的字段，命名为ROBOT，更改其中参数，如将model_name改为上一步的模型文件名
+4. 在rl_sar/launch文件夹中添加一个新的launch文件，请参考其他launch文件自行修改
+5. 修改rl_xxx.cpp中的ROBOT_NAME为ROBOT
+6. 编译运行
+7. 若您的机器人关节力矩大于50Nm，则需要修改`rl_sar/src/unitree_ros/unitree_legged_control/src/joint_controller.cpp`中180行为：
+   ```cpp
+   // calcTorque = computeTorque(currentPos, currentVel, servoCmd);      
+   calcTorque = servoCmd.posStiffness * (servoCmd.pos - currentPos) + servoCmd.velStiffness * (servoCmd.vel - currentVel) + servoCmd.torque;
+   ```
+   这样会解除50Nm的限制
+
 ## 引用
 
 如果您使用此代码或其部分内容，请引用以下内容：

src/rl_sar/config.yaml

@@ -28,6 +28,10 @@ a1:
                     -0.1000, 0.8000, -1.5000, # FR
                      0.1000, 1.0000, -1.5000, # RL
                     -0.1000, 1.0000, -1.5000] # RR
+  joint_names: ["FL_hip_joint", "FL_thigh_joint", "FL_calf_joint",
+                "FR_hip_joint", "FR_thigh_joint", "FR_calf_joint",
+                "RL_hip_joint", "RL_thigh_joint", "RL_calf_joint",
+                "RR_hip_joint", "RR_thigh_joint", "RR_calf_joint"]
 
 cyberdog1:
   model_name: "model.pt"
@@ -59,3 +63,7 @@ cyberdog1:
                     -0.1000, 0.8000, -1.5000, # FR
                      0.1000, 1.0000, -1.5000, # RL
                     -0.1000, 1.0000, -1.5000] # RR
+  joint_names: ["FL_hip_joint", "FL_thigh_joint", "FL_calf_joint",
+                "FR_hip_joint", "FR_thigh_joint", "FR_calf_joint",
+                "RL_hip_joint", "RL_thigh_joint", "RL_calf_joint",
+                "RR_hip_joint", "RR_thigh_joint", "RR_calf_joint"]

src/rl_sar/include/rl_sim.hpp

@@ -56,7 +56,6 @@ private:
     geometry_msgs::Pose pose;
     geometry_msgs::Twist cmd_vel;
 
-    std::vector<std::string> joint_names;
     std::vector<double> joint_positions;
     std::vector<double> joint_velocities;
     std::vector<double> joint_efforts;
@@ -64,6 +63,9 @@ private:
     int hip_scale_reduction_indices[4] = {0, 3, 6, 9};
 
     std::chrono::high_resolution_clock::time_point start_time;
+
+    void MapData(const std::vector<double>& source_data, std::vector<double>& target_data);
+    std::map<std::string, size_t> sorted_to_original_index;
 };
 
 #endif

src/rl_sar/library/rl/rl.cpp

@@ -1,5 +1,15 @@
 #include "rl.hpp"
 
+torch::Tensor ReadTensorFromYaml(const YAML::Node& node)
+{
+    std::vector<float> values;
+    for(const auto& val : node)
+    {
+        values.push_back(val.as<float>());
+    }
+    return torch::tensor(values).view({1, -1});
+}
+
 void RL::ReadYaml(std::string robot_name)
 {
 	YAML::Node config;
@@ -17,18 +27,6 @@ void RL::ReadYaml(std::string robot_name)
     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.p_gains = torch::tensor({{config["p_gains"][0].as<float>(), config["p_gains"][1].as<float>(), config["p_gains"][2].as<float>(),    
-                                           config["p_gains"][3].as<float>(), config["p_gains"][4].as<float>(), config["p_gains"][5].as<float>(), 
-                                           config["p_gains"][6].as<float>(), config["p_gains"][7].as<float>(), config["p_gains"][8].as<float>(), 
-                                           config["p_gains"][9].as<float>(), config["p_gains"][10].as<float>(), config["p_gains"][11].as<float>()}});
-    this->params.d_gains = torch::tensor({{config["d_gains"][0].as<float>(), config["d_gains"][1].as<float>(), config["d_gains"][2].as<float>(),    
-                                           config["d_gains"][3].as<float>(), config["d_gains"][4].as<float>(), config["d_gains"][5].as<float>(), 
-                                           config["d_gains"][6].as<float>(), config["d_gains"][7].as<float>(), config["d_gains"][8].as<float>(), 
-                                           config["d_gains"][9].as<float>(), config["d_gains"][10].as<float>(), config["d_gains"][11].as<float>()}});
     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>();
@@ -36,17 +34,20 @@ void RL::ReadYaml(std::string robot_name)
     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>()}});
+    this->params.commands_scale = torch::tensor({this->params.lin_vel_scale, this->params.lin_vel_scale, this->params.ang_vel_scale * 2});
+    // 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.p_gains = ReadTensorFromYaml(config["p_gains"]);
+    this->params.d_gains = ReadTensorFromYaml(config["d_gains"]);
+    this->params.torque_limits = ReadTensorFromYaml(config["torque_limits"]);
+    this->params.default_dof_pos = ReadTensorFromYaml(config["default_dof_pos"]);
+    const YAML::Node& joint_names_node = config["joint_names"];
+    for(const auto& name : joint_names_node)
+    {
+        this->params.joint_names.push_back(name.as<std::string>());
+    }
 }
 
 void RL::CSVInit(std::string robot_name)

src/rl_sar/library/rl/rl.hpp

@@ -31,6 +31,7 @@ struct ModelParams
     torch::Tensor p_gains;
     torch::Tensor commands_scale;
     torch::Tensor default_dof_pos;
+    std::vector<std::string> joint_names;
 };
 
 struct Observations

src/rl_sar/src/rl_real_a1.cpp

@@ -3,7 +3,7 @@
 #define ROBOT_NAME "a1"
 
 // #define PLOT
-#define CSV_LOGGER
+// #define CSV_LOGGER
 
 RL_Real rl_sar;
 
@@ -180,7 +180,7 @@ void RL_Real::RobotControl()
         }
     }
 
-    safe.PowerProtect(cmd, state, 7);
+    // safe.PowerProtect(cmd, state, 7);
     udp.SetSend(cmd);
 }
 

src/rl_sar/src/rl_sim.cpp

@@ -9,6 +9,15 @@ RL_Sim::RL_Sim()
 {
     ReadYaml(ROBOT_NAME);
 
+    // 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_names = params.joint_names;
+    std::sort(sorted_joint_names.begin(), sorted_joint_names.end());
+    for(size_t i = 0; i < params.joint_names.size(); ++i)
+    {
+        sorted_to_original_index[sorted_joint_names[i]] = i;
+    }
+
     ros::NodeHandle nh;
     start_time = std::chrono::high_resolution_clock::now();
 
@@ -34,17 +43,10 @@ RL_Sim::RL_Sim()
 
     nh.param<std::string>("ros_namespace", ros_namespace, "");
 
-    joint_names = {
-        "FL_hip_joint", "FL_thigh_joint", "FL_calf_joint",
-        "FR_hip_joint", "FR_thigh_joint", "FR_calf_joint",
-        "RL_hip_joint", "RL_thigh_joint", "RL_calf_joint",
-        "RR_hip_joint", "RR_thigh_joint", "RR_calf_joint",
-    };
-
     for (int i = 0; i < 12; ++i)
     {
-        torque_publishers[joint_names[i]] = nh.advertise<unitree_legged_msgs::MotorCmd>(
-            ros_namespace + joint_names[i].substr(0, joint_names[i].size() - 6) + "_controller/command", 10);
+        torque_publishers[params.joint_names[i]] = nh.advertise<unitree_legged_msgs::MotorCmd>(
+            ros_namespace + params.joint_names[i].substr(0, params.joint_names[i].size() - 6) + "_controller/command", 10);
     }
 
     model_state_subscriber_ = nh.subscribe<gazebo_msgs::ModelStates>(
@@ -93,7 +95,7 @@ void RL_Sim::RobotControl()
         // motor_commands[i].tau = output_torques[0][i].item<double>();
         motor_commands[i].tau = 0;
 
-        torque_publishers[joint_names[i]].publish(motor_commands[i]);
+        torque_publishers[params.joint_names[i]].publish(motor_commands[i]);
     }
 }
 
@@ -109,46 +111,35 @@ void RL_Sim::CmdvelCallback(const geometry_msgs::Twist::ConstPtr &msg)
     cmd_vel = *msg;
 }
 
+void RL_Sim::MapData(const std::vector<double>& source_data, std::vector<double>& target_data)
+{
+    for(size_t i = 0; i < source_data.size(); ++i)
+    {
+        target_data[i] = source_data[sorted_to_original_index[params.joint_names[i]]];
+    }
+}
+
 void RL_Sim::JointStatesCallback(const sensor_msgs::JointState::ConstPtr &msg)
 {
-    joint_positions = msg->position;
-    joint_velocities = msg->velocity;
-    joint_efforts = msg->effort;
+    MapData(msg->position, joint_positions);
+    MapData(msg->velocity, joint_velocities);
+    MapData(msg->effort, joint_efforts);
 }
 
 void RL_Sim::RunModel()
 {
-    // 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", 
-    //     vel.angular.x, vel.angular.y, vel.angular.z);
-    // printf("%f, %f, %f, %f\n", 
-    //     pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w);
-    // printf("%f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f\n", 
-    //     joint_positions[1], joint_positions[2], joint_positions[0],
-    //     joint_positions[4], joint_positions[5], joint_positions[3],
-    //     joint_positions[7], joint_positions[8], joint_positions[6],
-    //     joint_positions[10], joint_positions[11], joint_positions[9]);
-    // printf("%f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f\n", 
-    //     joint_velocities[1], joint_velocities[2], joint_velocities[0],
-    //     joint_velocities[4], joint_velocities[5], joint_velocities[3],
-    //     joint_velocities[7], joint_velocities[8], joint_velocities[6],
-    //     joint_velocities[10], joint_velocities[11], joint_velocities[9]);
-
     // this->obs.lin_vel = torch::tensor({{vel.linear.x, vel.linear.y, vel.linear.z}});
     this->obs.ang_vel = torch::tensor({{vel.angular.x, vel.angular.y, vel.angular.z}});
     this->obs.commands = torch::tensor({{cmd_vel.linear.x, cmd_vel.linear.y, cmd_vel.angular.z}});
     this->obs.base_quat = torch::tensor({{pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w}});
-    this->obs.dof_pos = torch::tensor({{joint_positions[1], joint_positions[2], joint_positions[0],
-                                        joint_positions[4], joint_positions[5], joint_positions[3],
-                                        joint_positions[7], joint_positions[8], joint_positions[6],
-                                        joint_positions[10], joint_positions[11], joint_positions[9]}});
-    this->obs.dof_vel = torch::tensor({{joint_velocities[1], joint_velocities[2], joint_velocities[0],
-                                        joint_velocities[4], joint_velocities[5], joint_velocities[3],
-                                        joint_velocities[7], joint_velocities[8], joint_velocities[6],
-                                        joint_velocities[10], joint_velocities[11], joint_velocities[9]}});
+    this->obs.dof_pos = torch::tensor({{joint_positions[0], joint_positions[1], joint_positions[2],
+                                        joint_positions[3], joint_positions[4], joint_positions[5],
+                                        joint_positions[6], joint_positions[7], joint_positions[8],
+                                        joint_positions[9], joint_positions[10], joint_positions[11]}});
+    this->obs.dof_vel = torch::tensor({{joint_velocities[0], joint_velocities[1], joint_velocities[2],
+                                        joint_velocities[3], joint_velocities[4], joint_velocities[5],
+                                        joint_velocities[6], joint_velocities[7], joint_velocities[8],
+                                        joint_velocities[9], joint_velocities[10], joint_velocities[11]}});
 
     torch::Tensor actions = this->Forward();