diff --git a/README.md b/README.md index a0b6f71..c670854 100644 --- a/README.md +++ b/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: diff --git a/README_CN.md b/README_CN.md index b214cc2..3d8160d 100644 --- a/README_CN.md +++ b/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的限制 + ## 引用 如果您使用此代码或其部分内容，请引用以下内容： diff --git a/src/rl_sar/config.yaml b/src/rl_sar/config.yaml index 2e21776..bf51578 100644 --- a/src/rl_sar/config.yaml +++ b/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" @@ -58,4 +62,8 @@ cyberdog1: default_dof_pos: [ 0.1000, 0.8000, -1.5000, # FL -0.1000, 0.8000, -1.5000, # FR 0.1000, 1.0000, -1.5000, # RL - -0.1000, 1.0000, -1.5000] # RR \ No newline at end of file + -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"] diff --git a/src/rl_sar/include/rl_sim.hpp b/src/rl_sar/include/rl_sim.hpp index 4dc21af..7d1be50 100644 --- a/src/rl_sar/include/rl_sim.hpp +++ b/src/rl_sar/include/rl_sim.hpp @@ -56,7 +56,6 @@ private: geometry_msgs::Pose pose; geometry_msgs::Twist cmd_vel; - std::vector joint_names; std::vector joint_positions; std::vector joint_velocities; std::vector 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& source_data, std::vector& target_data); + std::map sorted_to_original_index; }; #endif \ No newline at end of file diff --git a/src/rl_sar/library/rl/rl.cpp b/src/rl_sar/library/rl/rl.cpp index 9acb4cd..fd931ea 100644 --- a/src/rl_sar/library/rl/rl.cpp +++ b/src/rl_sar/library/rl/rl.cpp @@ -1,5 +1,15 @@ #include "rl.hpp" +torch::Tensor ReadTensorFromYaml(const YAML::Node& node) +{ + std::vector values; + for(const auto& val : node) + { + values.push_back(val.as()); + } + 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(); this->params.clip_obs = config["clip_obs"].as(); this->params.clip_actions = config["clip_actions"].as(); - // this->params.damping = config["damping"].as(); - // this->params.stiffness = config["stiffness"].as(); - // 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(), config["p_gains"][1].as(), config["p_gains"][2].as(), - config["p_gains"][3].as(), config["p_gains"][4].as(), config["p_gains"][5].as(), - config["p_gains"][6].as(), config["p_gains"][7].as(), config["p_gains"][8].as(), - config["p_gains"][9].as(), config["p_gains"][10].as(), config["p_gains"][11].as()}}); - this->params.d_gains = torch::tensor({{config["d_gains"][0].as(), config["d_gains"][1].as(), config["d_gains"][2].as(), - config["d_gains"][3].as(), config["d_gains"][4].as(), config["d_gains"][5].as(), - config["d_gains"][6].as(), config["d_gains"][7].as(), config["d_gains"][8].as(), - config["d_gains"][9].as(), config["d_gains"][10].as(), config["d_gains"][11].as()}}); this->params.action_scale = config["action_scale"].as(); this->params.hip_scale_reduction = config["hip_scale_reduction"].as(); this->params.num_of_dofs = config["num_of_dofs"].as(); @@ -36,17 +34,20 @@ void RL::ReadYaml(std::string robot_name) this->params.ang_vel_scale = config["ang_vel_scale"].as(); this->params.dof_pos_scale = config["dof_pos_scale"].as(); this->params.dof_vel_scale =config["dof_vel_scale"].as(); - 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(), config["torque_limits"][1].as(), config["torque_limits"][2].as(), - config["torque_limits"][3].as(), config["torque_limits"][4].as(), config["torque_limits"][5].as(), - config["torque_limits"][6].as(), config["torque_limits"][7].as(), config["torque_limits"][8].as(), - config["torque_limits"][9].as(), config["torque_limits"][10].as(), config["torque_limits"][11].as()}}); - - this->params.default_dof_pos = torch::tensor({{config["default_dof_pos"][0].as(), config["default_dof_pos"][1].as(), config["default_dof_pos"][2].as(), - config["default_dof_pos"][3].as(), config["default_dof_pos"][4].as(), config["default_dof_pos"][5].as(), - config["default_dof_pos"][6].as(), config["default_dof_pos"][7].as(), config["default_dof_pos"][8].as(), - config["default_dof_pos"][9].as(), config["default_dof_pos"][10].as(), config["default_dof_pos"][11].as()}}); + 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(); + // this->params.stiffness = config["stiffness"].as(); + // 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()); + } } void RL::CSVInit(std::string robot_name) diff --git a/src/rl_sar/library/rl/rl.hpp b/src/rl_sar/library/rl/rl.hpp index e174bb2..b68a484 100644 --- a/src/rl_sar/library/rl/rl.hpp +++ b/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 joint_names; }; struct Observations diff --git a/src/rl_sar/models/cyberdog1/model.pt b/src/rl_sar/models/cyberdog1/model.pt new file mode 100644 index 0000000..94351de Binary files /dev/null and b/src/rl_sar/models/cyberdog1/model.pt differ diff --git a/src/rl_sar/src/rl_real_a1.cpp b/src/rl_sar/src/rl_real_a1.cpp index 10256a6..e0dd322 100644 --- a/src/rl_sar/src/rl_real_a1.cpp +++ b/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); } diff --git a/src/rl_sar/src/rl_sim.cpp b/src/rl_sar/src/rl_sim.cpp index c84c7aa..04e5318 100644 --- a/src/rl_sar/src/rl_sim.cpp +++ b/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 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("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( - ros_namespace + joint_names[i].substr(0, joint_names[i].size() - 6) + "_controller/command", 10); + torque_publishers[params.joint_names[i]] = nh.advertise( + ros_namespace + params.joint_names[i].substr(0, params.joint_names[i].size() - 6) + "_controller/command", 10); } model_state_subscriber_ = nh.subscribe( @@ -93,7 +95,7 @@ void RL_Sim::RobotControl() // motor_commands[i].tau = output_torques[0][i].item(); 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& source_data, std::vector& 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::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(); diff --git a/src/rl_sar/worlds/stairs.world b/src/rl_sar/worlds/stairs.world index 5963133..615ee47 100644 --- a/src/rl_sar/worlds/stairs.world +++ b/src/rl_sar/worlds/stairs.world @@ -9,15 +9,15 @@ 0 0 -9.81 - quick - 50 - 1.3 + quick + 50 + 1.3 - 0.0 - 0.2 - 10.0 - 0.001 + 0.0 + 0.2 + 10.0 + 0.001