From 9155e6383b995c767367def789a9235eb8d7ac54 Mon Sep 17 00:00:00 2001 From: fan-ziqi Date: Sat, 25 May 2024 17:13:24 +0800 Subject: [PATCH] feat: add clip_actions_upper/lower --- src/rl_sar/config.yaml | 64 +++++++++++++++++----------- src/rl_sar/library/rl_sdk/rl_sdk.cpp | 3 +- src/rl_sar/library/rl_sdk/rl_sdk.hpp | 3 +- src/rl_sar/src/rl_real_a1.cpp | 14 +++--- src/rl_sar/src/rl_sim.cpp | 12 +++--- 5 files changed, 57 insertions(+), 39 deletions(-) diff --git a/src/rl_sar/config.yaml b/src/rl_sar/config.yaml index c2589a8..136c78a 100644 --- a/src/rl_sar/config.yaml +++ b/src/rl_sar/config.yaml @@ -1,41 +1,53 @@ a1: +# order: +# FL +# FR +# RL +# RR model_name: "model_0522.pt" num_observations: 45 clip_obs: 100.0 - clip_actions: 100.0 - rl_kp: [20, 20, 20, # FL - 20, 20, 20, # FR - 20, 20, 20, # RL - 20, 20, 20] # RR - rl_kd: [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 - fixed_kp: [80, 80, 80, # FL - 80, 80, 80, # FR - 80, 80, 80, # RL - 80, 80, 80] # RR - fixed_kd: [3, 3, 3, # FL - 3, 3, 3, # FR - 3, 3, 3, # RL - 3, 3, 3] # RR - action_scale: 0.25 + clip_actions_lower: [-100, -100, -100, + -100, -100, -100, + -100, -100, -100, + -100, -100, -100] + clip_actions_upper: [100, 100, 100, + 100, 100, 100, + 100, 100, 100, + 100, 100, 100] + rl_kp: [20, 20, 20, + 20, 20, 20, + 20, 20, 20, + 20, 20, 20] + rl_kd: [0.5, 0.5, 0.5, + 0.5, 0.5, 0.5, + 0.5, 0.5, 0.5, + 0.5, 0.5, 0.5] + fixed_kp: [80, 80, 80, + 80, 80, 80, + 80, 80, 80, + 80, 80, 80] + fixed_kd: [3, 3, 3, + 3, 3, 3, + 3, 3, 3, + 3, 3, 3] hip_scale_reduction: 0.5 hip_scale_reduction_indices: [0, 3, 6, 9] num_of_dofs: 12 + action_scale: 0.25 lin_vel_scale: 2.0 ang_vel_scale: 0.25 dof_pos_scale: 1.0 dof_vel_scale: 0.05 commands_scale: [2.0, 2.0, 0.5] - torque_limits: [33.5, 33.5, 33.5, # FL - 33.5, 33.5, 33.5, # FR - 33.5, 33.5, 33.5, # RL - 33.5, 33.5, 33.5] # RR - 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 + torque_limits: [33.5, 33.5, 33.5, + 33.5, 33.5, 33.5, + 33.5, 33.5, 33.5, + 33.5, 33.5, 33.5] + default_dof_pos: [ 0.1000, 0.8000, -1.5000, + -0.1000, 0.8000, -1.5000, + 0.1000, 1.0000, -1.5000, + -0.1000, 1.0000, -1.5000] 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", diff --git a/src/rl_sar/library/rl_sdk/rl_sdk.cpp b/src/rl_sar/library/rl_sdk/rl_sdk.cpp index 2caaa5a..418b8e1 100644 --- a/src/rl_sar/library/rl_sdk/rl_sdk.cpp +++ b/src/rl_sar/library/rl_sdk/rl_sdk.cpp @@ -275,7 +275,8 @@ void RL::ReadYaml(std::string robot_name) this->params.model_name = config["model_name"].as(); 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.clip_actions_upper = torch::tensor(ReadVectorFromYaml(config["clip_actions_upper"])).view({1, -1}); + this->params.clip_actions_lower = torch::tensor(ReadVectorFromYaml(config["clip_actions_lower"])).view({1, -1}); this->params.action_scale = config["action_scale"].as(); this->params.hip_scale_reduction = config["hip_scale_reduction"].as(); this->params.hip_scale_reduction_indices = ReadVectorFromYaml(config["hip_scale_reduction_indices"]); diff --git a/src/rl_sar/library/rl_sdk/rl_sdk.hpp b/src/rl_sar/library/rl_sdk/rl_sdk.hpp index 69cd972..d90c4f3 100644 --- a/src/rl_sar/library/rl_sdk/rl_sdk.hpp +++ b/src/rl_sar/library/rl_sdk/rl_sdk.hpp @@ -74,7 +74,8 @@ struct ModelParams double dof_pos_scale; double dof_vel_scale; double clip_obs; - double clip_actions; + torch::Tensor clip_actions_upper; + torch::Tensor clip_actions_lower; torch::Tensor torque_limits; torch::Tensor rl_kd; torch::Tensor rl_kp; diff --git a/src/rl_sar/src/rl_real_a1.cpp b/src/rl_sar/src/rl_real_a1.cpp index 39e532e..53a4c71 100644 --- a/src/rl_sar/src/rl_real_a1.cpp +++ b/src/rl_sar/src/rl_real_a1.cpp @@ -56,6 +56,7 @@ RL_Real::RL_Real() : unitree_safe(UNITREE_LEGGED_SDK::LeggedType::A1), unitree_u RL_Real::~RL_Real() { + loop_keyboard->shutdown(); loop_udpSend->shutdown(); loop_udpRecv->shutdown(); loop_control->shutdown(); @@ -145,15 +146,17 @@ void RL_Real::RunModel() unitree_low_state.motorState[9].dq, unitree_low_state.motorState[10].dq, unitree_low_state.motorState[11].dq, unitree_low_state.motorState[6].dq, unitree_low_state.motorState[7].dq, unitree_low_state.motorState[8].dq}}); - torch::Tensor actions = this->Forward(); + torch::Tensor clamped_actions = this->Forward(); for (int i : hip_scale_reduction_indices) { - actions[0][i] *= this->params.hip_scale_reduction; + clamped_actions[0][i] *= this->params.hip_scale_reduction; } - output_torques = this->ComputeTorques(actions); - output_dof_pos = this->ComputePosition(actions); + this->obs.actions = clamped_actions; + + output_torques = this->ComputeTorques(clamped_actions); + output_dof_pos = this->ComputePosition(clamped_actions); #ifdef CSV_LOGGER torch::Tensor tau_est = torch::tensor({{unitree_low_state.motorState[3].tauEst, unitree_low_state.motorState[4].tauEst, unitree_low_state.motorState[5].tauEst, unitree_low_state.motorState[0].tauEst, unitree_low_state.motorState[1].tauEst, unitree_low_state.motorState[2].tauEst, @@ -187,8 +190,7 @@ torch::Tensor RL_Real::Forward() torch::Tensor action = this->model.forward({history_obs}).toTensor(); - this->obs.actions = action; - torch::Tensor clamped = torch::clamp(action, -this->params.clip_actions, this->params.clip_actions); + torch::Tensor clamped = torch::clamp(action, this->params.clip_actions_upper, this->params.clip_actions_lower); return clamped; } diff --git a/src/rl_sar/src/rl_sim.cpp b/src/rl_sar/src/rl_sim.cpp index 5250b3c..1c70c28 100644 --- a/src/rl_sar/src/rl_sim.cpp +++ b/src/rl_sar/src/rl_sim.cpp @@ -84,6 +84,7 @@ RL_Sim::RL_Sim() RL_Sim::~RL_Sim() { + loop_keyboard->shutdown(); loop_control->shutdown(); loop_rl->shutdown(); #ifdef PLOT @@ -195,7 +196,9 @@ void RL_Sim::RunModel() clamped_actions[0][i] *= this->params.hip_scale_reduction; } - output_torques = this->ComputeTorques(clamped_actions); + this->obs.actions = clamped_actions; + + // output_torques = this->ComputeTorques(clamped_actions); output_dof_pos = this->ComputePosition(clamped_actions); #ifdef CSV_LOGGER @@ -207,8 +210,8 @@ void RL_Sim::RunModel() torch::Tensor RL_Sim::ComputeObservation() { - torch::Tensor obs = torch::cat({// this->QuatRotateInverse(this->obs.base_quat, this->obs.lin_vel) * this->params.lin_vel_scale, - this->QuatRotateInverse(this->obs.base_quat, this->obs.ang_vel) * this->params.ang_vel_scale, + torch::Tensor obs = torch::cat({// this->obs.lin_vel * this->params.lin_vel_scale, + this->obs.ang_vel * this->params.ang_vel_scale, this->QuatRotateInverse(this->obs.base_quat, this->obs.gravity_vec), this->obs.commands * this->params.commands_scale, (this->obs.dof_pos - this->params.default_dof_pos) * this->params.dof_pos_scale, @@ -238,8 +241,7 @@ torch::Tensor RL_Sim::Forward() actions = this->model.forward({obs}).toTensor(); } - this->obs.actions = actions; - torch::Tensor clamped_actions = torch::clamp(actions, -this->params.clip_actions, this->params.clip_actions); + torch::Tensor clamped_actions = torch::clamp(actions, this->params.clip_actions_lower, this->params.clip_actions_upper); return clamped_actions; }