EmbodiedAI
/
rl_sar
mirror of https://github.com/fan-ziqi/rl_sar.git
3
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

feat: add pd params

This commit is contained in:
fan-ziqi 2024-04-08 10:41:39 +08:00
parent 2f7c3efc1b
commit 6df0191152
6 changed files with 62 additions and 40 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

50
src/rl_sar/config.yaml
View File

@ -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
0.1000, 1.0000, -1.5000, # RL
-0.1000, 1.0000, -1.5000] # RR

8
src/rl_sar/include/rl_real_cyberdog.hpp
View File

@ -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;

16
src/rl_sar/library/rl/rl.cpp
View File

@ -16,10 +16,18 @@ 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.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>();

8
src/rl_sar/src/rl_real_a1.cpp
View File

@ -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<double>();
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<double>();
cmd.motorCmd[i].Kd = params.d_gains[0][dof_mapping[i]].item<double>();
// cmd.motorCmd[i].tau = output_torques[0][dof_mapping[i]].item<double>();
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();
}

14
src/rl_sar/src/rl_real_cyberdog.cpp
View File

@ -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<double>();
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<double>();
cyberdogCmd.kd_des[i] = params.d_gains[0][dof_mapping[i]].item<double>();
// cyberdogCmd.tau_des[i] = output_torques[0][dof_mapping[i]].item<double>();
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();
}

6
src/rl_sar/src/rl_sim.cpp
View File

@ -84,8 +84,10 @@ void RL_Sim::RobotControl()
motor_commands[i].mode = 0x0A;
motor_commands[i].q = output_dof_pos[0][i].item<double>();
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<double>();
motor_commands[i].Kd = params.d_gains[0][i].item<double>();
// motor_commands[i].tau = output_torques[0][i].item<double>();
motor_commands[i].tau = 0;