From 6df0191152246a9e7652e342ef21fae4b48d8cca Mon Sep 17 00:00:00 2001 From: fan-ziqi Date: Mon, 8 Apr 2024 10:41:39 +0800 Subject: [PATCH] feat: add pd params --- src/rl_sar/config.yaml | 50 ++++++++++++++++--------- src/rl_sar/include/rl_real_cyberdog.hpp | 8 ---- src/rl_sar/library/rl/rl.cpp | 16 ++++++-- src/rl_sar/src/rl_real_a1.cpp | 8 +++- src/rl_sar/src/rl_real_cyberdog.cpp | 14 ++++--- src/rl_sar/src/rl_sim.cpp | 6 ++- 6 files changed, 62 insertions(+), 40 deletions(-) diff --git a/src/rl_sar/config.yaml b/src/rl_sar/config.yaml index 3f11009..31bde80 100644 --- a/src/rl_sar/config.yaml +++ b/src/rl_sar/config.yaml @@ -2,8 +2,16 @@ a1: num_observations: 45 clip_obs: 100.0 clip_actions: 100.0 - damping: 0.5 - stiffness: 20.0 + # damping: 0.5 + # stiffness: 20.0 + p_gains: [20, 20, 20, # FL + 20, 20, 20, # FR + 20, 20, 20, # RL + 20, 20, 20] # RR + d_gains: [0.5, 0.5, 0.5, # FL + 0.5, 0.5, 0.5, # FR + 0.5, 0.5, 0.5, # RL + 0.5, 0.5, 0.5] # RR action_scale: 0.25 hip_scale_reduction: 0.5 num_of_dofs: 12 @@ -11,22 +19,29 @@ a1: 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 + torque_limits: [20.0, 55.0, 55.0, # FL + 20.0, 55.0, 55.0, # FR + 20.0, 55.0, 55.0, # RL + 20.0, 55.0, 55.0] # RR 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 + 0.1000, 1.0000, -1.5000, # RL + -0.1000, 1.0000, -1.5000] # RR cyberdog1: num_observations: 45 clip_obs: 100.0 clip_actions: 100.0 - damping: 0.5 - stiffness: 20.0 + # damping: 0.5 + # stiffness: 20.0 + p_gains: [30, 40, 50, # FL + 30, 40, 50, # FR + 30, 40, 50, # RL + 30, 40, 50] # RR + d_gains: [0.5, 0.5, 0.5, # FL + 0.5, 0.5, 0.5, # FR + 0.5, 0.5, 0.5, # RL + 0.5, 0.5, 0.5] # RR action_scale: 0.25 hip_scale_reduction: 0.5 num_of_dofs: 12 @@ -34,12 +49,11 @@ cyberdog1: 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 + torque_limits: [24.0, 24.0, 24.0, # FL + 24.0, 24.0, 24.0, # FR + 24.0, 24.0, 24.0, # RL + 24.0, 24.0, 24.0] # RR 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 \ No newline at end of file + 0.1000, 1.0000, -1.5000, # RL + -0.1000, 1.0000, -1.5000] # RR \ No newline at end of file diff --git a/src/rl_sar/include/rl_real_cyberdog.hpp b/src/rl_sar/include/rl_real_cyberdog.hpp index 766acf2..630b195 100644 --- a/src/rl_sar/include/rl_real_cyberdog.hpp +++ b/src/rl_sar/include/rl_real_cyberdog.hpp @@ -79,14 +79,6 @@ private: int dof_mapping[12] = {3, 4, 5, 0, 1, 2, 9, 10, 11, 6, 7, 8}; int hip_scale_reduction_indices[4] = {0, 3, 6, 9}; - float Kp[13] = {25, 40, 30, //FL - 25, 40, 30, //FR - 25, 40, 30, //RL - 25, 40, 30};//RR - float Kd[13] = {0.8, 1.5, 1.0, - 0.8, 1.5, 1.0, - 0.8, 1.5, 1.0, - 0.8, 1.5, 1.0}; std::chrono::high_resolution_clock::time_point start_time; diff --git a/src/rl_sar/library/rl/rl.cpp b/src/rl_sar/library/rl/rl.cpp index b3be31d..f93e9d3 100644 --- a/src/rl_sar/library/rl/rl.cpp +++ b/src/rl_sar/library/rl/rl.cpp @@ -16,10 +16,18 @@ 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.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(); diff --git a/src/rl_sar/src/rl_real_a1.cpp b/src/rl_sar/src/rl_real_a1.cpp index 6b874e1..eaf83ca 100644 --- a/src/rl_sar/src/rl_real_a1.cpp +++ b/src/rl_sar/src/rl_real_a1.cpp @@ -120,6 +120,7 @@ void RL_Real::RobotControl() { robot_state = STATE_RL_RUNNING; this->InitObservations(); + this->InitOutputs(); printf("\nstart rl loop\n"); loop_rl->start(); } @@ -133,8 +134,10 @@ void RL_Real::RobotControl() // cmd.motorCmd[i].q = 0; cmd.motorCmd[i].q = output_dof_pos[0][dof_mapping[i]].item(); cmd.motorCmd[i].dq = 0; - cmd.motorCmd[i].Kp = params.stiffness; - cmd.motorCmd[i].Kd = params.damping; + // cmd.motorCmd[i].Kp = params.stiffness; + // cmd.motorCmd[i].Kd = params.damping; + cmd.motorCmd[i].Kp = params.p_gains[0][dof_mapping[i]].item(); + cmd.motorCmd[i].Kd = params.d_gains[0][dof_mapping[i]].item(); // cmd.motorCmd[i].tau = output_torques[0][dof_mapping[i]].item(); cmd.motorCmd[i].tau = 0; } @@ -170,6 +173,7 @@ void RL_Real::RobotControl() { robot_state = STATE_WAITING; this->InitObservations(); + this->InitOutputs(); printf("\nstop rl loop\n"); loop_rl->shutdown(); } diff --git a/src/rl_sar/src/rl_real_cyberdog.cpp b/src/rl_sar/src/rl_real_cyberdog.cpp index 2a95cd7..31c5e88 100644 --- a/src/rl_sar/src/rl_real_cyberdog.cpp +++ b/src/rl_sar/src/rl_real_cyberdog.cpp @@ -2,7 +2,7 @@ #define ROBOT_NAME "cyberdog1" -// #define PLOT +#define PLOT RL_Real rl_sar; @@ -15,7 +15,7 @@ RL_Real::RL_Real() : CustomInterface(500) 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/" + ROBOT_NAME + "/encoder.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); @@ -131,6 +131,7 @@ void RL_Real::RobotControl() { robot_state = STATE_RL_RUNNING; this->InitObservations(); + this->InitOutputs(); printf("\nstart rl loop\n"); loop_rl->start(); } @@ -143,10 +144,10 @@ void RL_Real::RobotControl() // cyberdogCmd.q_des[i] = 0; cyberdogCmd.q_des[i] = output_dof_pos[0][dof_mapping[i]].item(); cyberdogCmd.qd_des[i] = 0; - cyberdogCmd.kp_des[i] = params.stiffness; - cyberdogCmd.kd_des[i] = params.damping; - // cyberdogCmd.kp_des[i] = Kp[dof_mapping[i]]; - // cyberdogCmd.kd_des[i] = Kd[dof_mapping[i]]; + // cyberdogCmd.kp_des[i] = params.stiffness; + // cyberdogCmd.kd_des[i] = params.damping; + cyberdogCmd.kp_des[i] = params.p_gains[0][dof_mapping[i]].item(); + cyberdogCmd.kd_des[i] = params.d_gains[0][dof_mapping[i]].item(); // cyberdogCmd.tau_des[i] = output_torques[0][dof_mapping[i]].item(); cyberdogCmd.tau_des[i] = 0; } @@ -182,6 +183,7 @@ void RL_Real::RobotControl() { robot_state = STATE_WAITING; this->InitObservations(); + this->InitOutputs(); printf("\nstop rl loop\n"); loop_rl->shutdown(); } diff --git a/src/rl_sar/src/rl_sim.cpp b/src/rl_sar/src/rl_sim.cpp index 30566c3..298031d 100644 --- a/src/rl_sar/src/rl_sim.cpp +++ b/src/rl_sar/src/rl_sim.cpp @@ -84,8 +84,10 @@ void RL_Sim::RobotControl() motor_commands[i].mode = 0x0A; motor_commands[i].q = output_dof_pos[0][i].item(); motor_commands[i].dq = 0; - motor_commands[i].Kp = params.stiffness; - motor_commands[i].Kd = params.damping; + // motor_commands[i].Kp = params.stiffness; + // motor_commands[i].Kd = params.damping; + motor_commands[i].Kp = params.p_gains[0][i].item(); + motor_commands[i].Kd = params.d_gains[0][i].item(); // motor_commands[i].tau = output_torques[0][i].item(); motor_commands[i].tau = 0;