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: use num_of_dofs instead 12

This commit is contained in:
fan-ziqi 2024-05-23 15:15:34 +08:00
parent a37fa1c742
commit 5a4d8d5442
6 changed files with 41 additions and 41 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
src/rl_sar/config.yaml
View File

@ -1,5 +1,5 @@
a1:
model_name: "model_0501.pt"
model_name: "model_0522.pt"
num_observations: 45
clip_obs: 100.0
clip_actions: 100.0
@ -20,7 +20,7 @@ a1:
ang_vel_scale: 0.25
dof_pos_scale: 1.0
dof_vel_scale: 0.05
commands_scale: [1.0, 1.0, 1.0]
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
@ -56,7 +56,7 @@ cyberdog1:
ang_vel_scale: 0.25
dof_pos_scale: 1.0
dof_vel_scale: 0.05
commands_scale: [1.0, 1.0, 1.0]
commands_scale: [2.0, 2.0, 0.25]
torque_limits: [24.0, 24.0, 24.0, # FL
24.0, 24.0, 24.0, # FR
24.0, 24.0, 24.0, # RL
@ -91,7 +91,7 @@ lite3_wheel:
ang_vel_scale: 0.25
dof_pos_scale: 1.0
dof_vel_scale: 0.05
commands_scale: [1.0, 1.0, 1.0]
commands_scale: [2.0, 2.0, 0.25]
torque_limits: [21.6, 21.6, 32.4, 1.8, # FL
21.6, 21.6, 32.4, 1.8, # FR
21.6, 21.6, 32.4, 1.8, # RL

3
src/rl_sar/library/rl_sdk/rl_sdk.cpp
View File

@ -35,7 +35,8 @@ 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(ReadVectorFromYaml<float>(config["commands_scale"])).view({1, -1});
// this->params.commands_scale = torch::tensor(ReadVectorFromYaml<float>(config["commands_scale"])).view({1, -1});
this->params.commands_scale = torch::tensor({this->params.lin_vel_scale, this->params.lin_vel_scale, this->params.ang_vel_scale});
// this->params.damping = config["damping"].as<float>();
// this->params.stiffness = config["stiffness"].as<float>();
// this->params.d_gains = torch::ones(12) * this->params.damping;

BIN
src/rl_sar/models/a1/model_0522.pt Normal file
View File

Binary file not shown.

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

@ -26,8 +26,8 @@ RL_Real::RL_Real() : safe(LeggedType::A1), udp(LOWLEVEL)
this->history_obs_buf = ObservationBuffer(1, this->params.num_observations, 6);
plot_t = std::vector<int>(plot_size, 0);
plot_real_joint_pos.resize(12);
plot_target_joint_pos.resize(12);
plot_real_joint_pos.resize(params.num_of_dofs);
plot_target_joint_pos.resize(params.num_of_dofs);
for(auto& vector : plot_real_joint_pos) { vector = std::vector<double>(plot_size, 0); }
for(auto& vector : plot_target_joint_pos) { vector = std::vector<double>(plot_size, 0); }
@ -72,14 +72,14 @@ void RL_Real::RobotControl()
// waiting
if(robot_state == STATE_WAITING)
{
for(int i = 0; i < 12; ++i)
for(int i = 0; i < params.num_of_dofs; ++i)
{
cmd.motorCmd[i].q = state.motorState[i].q;
}
if((int)_keyData.btn.components.R2 == 1)
{
getup_percent = 0.0;
for(int i = 0; i < 12; ++i)
for(int i = 0; i < params.num_of_dofs; ++i)
{
now_pos[i] = state.motorState[i].q;
start_pos[i] = now_pos[i];
@ -94,12 +94,12 @@ void RL_Real::RobotControl()
{
getup_percent += 1 / 1000.0;
getup_percent = getup_percent > 1 ? 1 : getup_percent;
for(int i = 0; i < 12; ++i)
for(int i = 0; i < params.num_of_dofs; ++i)
{
cmd.motorCmd[i].mode = 0x0A;
cmd.motorCmd[i].q = (1 - getup_percent) * now_pos[i] + getup_percent * params.default_dof_pos[0][dof_mapping[i]].item<double>();
cmd.motorCmd[i].dq = 0;
cmd.motorCmd[i].Kp = 50;
cmd.motorCmd[i].Kp = 80;
cmd.motorCmd[i].Kd = 3;
cmd.motorCmd[i].tau = 0;
}
@ -112,7 +112,7 @@ void RL_Real::RobotControl()
else if((int)_keyData.btn.components.L2 == 1)
{
getdown_percent = 0.0;
for(int i = 0; i < 12; ++i)
for(int i = 0; i < params.num_of_dofs; ++i)
{
now_pos[i] = state.motorState[i].q;
}
@ -134,7 +134,7 @@ void RL_Real::RobotControl()
// rl loop
else if(robot_state == STATE_RL_RUNNING)
{
for(int i = 0; i < 12; ++i)
for(int i = 0; i < params.num_of_dofs; ++i)
{
cmd.motorCmd[i].mode = 0x0A;
// cmd.motorCmd[i].q = 0;
@ -150,7 +150,7 @@ void RL_Real::RobotControl()
if((int)_keyData.btn.components.L2 == 1)
{
getdown_percent = 0.0;
for(int i = 0; i < 12; ++i)
for(int i = 0; i < params.num_of_dofs; ++i)
{
now_pos[i] = state.motorState[i].q;
}
@ -164,12 +164,12 @@ void RL_Real::RobotControl()
{
getdown_percent += 1 / 1000.0;
getdown_percent = getdown_percent > 1 ? 1 : getdown_percent;
for(int i = 0; i < 12; ++i)
for(int i = 0; i < params.num_of_dofs; ++i)
{
cmd.motorCmd[i].mode = 0x0A;
cmd.motorCmd[i].q = (1 - getdown_percent) * now_pos[i] + getdown_percent * start_pos[i];
cmd.motorCmd[i].dq = 0;
cmd.motorCmd[i].Kp = 50;
cmd.motorCmd[i].Kp = 80;
cmd.motorCmd[i].Kd = 3;
cmd.motorCmd[i].tau = 0;
}
@ -185,8 +185,8 @@ void RL_Real::RobotControl()
}
}
safe.PowerProtect(cmd, state, 7);
safe.PositionProtect(cmd, state);
safe.PowerProtect(cmd, state, 8);
// safe.PositionProtect(cmd, state);
udp.SetSend(cmd);
}
@ -279,7 +279,7 @@ void RL_Real::Plot()
plot_t.push_back(motiontime);
plt::cla();
plt::clf();
for(int i = 0; i < 12; ++i)
for(int i = 0; i < params.num_of_dofs; ++i)
{
plot_real_joint_pos[i].erase(plot_real_joint_pos[i].begin());
plot_target_joint_pos[i].erase(plot_target_joint_pos[i].begin());

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

@ -24,8 +24,8 @@ RL_Real::RL_Real() : CustomInterface(500)
this->history_obs_buf = ObservationBuffer(1, this->params.num_observations, 6);
plot_t = std::vector<int>(plot_size, 0);
plot_real_joint_pos.resize(12);
plot_target_joint_pos.resize(12);
plot_real_joint_pos.resize(params.num_of_dofs);
plot_target_joint_pos.resize(params.num_of_dofs);
for(auto& vector : plot_real_joint_pos) { vector = std::vector<double>(plot_size, 0); }
for(auto& vector : plot_target_joint_pos) { vector = std::vector<double>(plot_size, 0); }
@ -81,7 +81,7 @@ void RL_Real::RobotControl()
// waiting
if(robot_state == STATE_WAITING)
{
for(int i = 0; i < 12; ++i)
for(int i = 0; i < params.num_of_dofs; ++i)
{
cyberdogCmd.q_des[i] = cyberdogData.q[i];
}
@ -89,7 +89,7 @@ void RL_Real::RobotControl()
{
keyboard.robot_state = STATE_WAITING;
getup_percent = 0.0;
for(int i = 0; i < 12; ++i)
for(int i = 0; i < params.num_of_dofs; ++i)
{
now_pos[i] = cyberdogData.q[i];
start_pos[i] = now_pos[i];
@ -104,7 +104,7 @@ void RL_Real::RobotControl()
{
getup_percent += 1 / 1000.0;
getup_percent = getup_percent > 1 ? 1 : getup_percent;
for(int i = 0; i < 12; ++i)
for(int i = 0; i < params.num_of_dofs; ++i)
{
cyberdogCmd.q_des[i] = (1 - getup_percent) * now_pos[i] + getup_percent * params.default_dof_pos[0][dof_mapping[i]].item<double>();
cyberdogCmd.qd_des[i] = 0;
@ -123,7 +123,7 @@ void RL_Real::RobotControl()
{
keyboard.robot_state = STATE_WAITING;
getdown_percent = 0.0;
for(int i = 0; i < 12; ++i)
for(int i = 0; i < params.num_of_dofs; ++i)
{
now_pos[i] = cyberdogData.q[i];
}
@ -145,7 +145,7 @@ void RL_Real::RobotControl()
// rl loop
else if(robot_state == STATE_RL_RUNNING)
{
for(int i = 0; i < 12; ++i)
for(int i = 0; i < params.num_of_dofs; ++i)
{
// cyberdogCmd.q_des[i] = 0;
cyberdogCmd.q_des[i] = output_dof_pos[0][dof_mapping[i]].item<double>();
@ -161,7 +161,7 @@ void RL_Real::RobotControl()
{
keyboard.robot_state = STATE_WAITING;
getdown_percent = 0.0;
for(int i = 0; i < 12; ++i)
for(int i = 0; i < params.num_of_dofs; ++i)
{
now_pos[i] = cyberdogData.q[i];
}
@ -175,7 +175,7 @@ void RL_Real::RobotControl()
{
getdown_percent += 1 / 1000.0;
getdown_percent = getdown_percent > 1 ? 1 : getdown_percent;
for(int i = 0; i < 12; ++i)
for(int i = 0; i < params.num_of_dofs; ++i)
{
cyberdogCmd.q_des[i] = (1 - getdown_percent) * now_pos[i] + getdown_percent * start_pos[i];
cyberdogCmd.qd_des[i] = 0;
@ -325,7 +325,7 @@ void RL_Real::Plot()
plot_t.push_back(motiontime);
plt::cla();
plt::clf();
for(int i = 0; i < 12; ++i)
for(int i = 0; i < params.num_of_dofs; ++i)
{
plot_real_joint_pos[i].erase(plot_real_joint_pos[i].begin());
plot_target_joint_pos[i].erase(plot_target_joint_pos[i].begin());

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

@ -25,7 +25,7 @@ RL_Sim::RL_Sim()
cmd_vel = geometry_msgs::Twist();
motor_commands.resize(12);
motor_commands.resize(params.num_of_dofs);
std::string model_path = std::string(CMAKE_CURRENT_SOURCE_DIR) + "/models/" + ROBOT_NAME + "/" + this->params.model_name;
this->model = torch::jit::load(model_path);
@ -40,8 +40,8 @@ RL_Sim::RL_Sim()
joint_efforts = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
plot_t = std::vector<int>(plot_size, 0);
plot_real_joint_pos.resize(12);
plot_target_joint_pos.resize(12);
plot_real_joint_pos.resize(params.num_of_dofs);
plot_target_joint_pos.resize(params.num_of_dofs);
for(auto& vector : plot_real_joint_pos) { vector = std::vector<double>(plot_size, 0); }
for(auto& vector : plot_target_joint_pos) { vector = std::vector<double>(plot_size, 0); }
@ -49,7 +49,7 @@ RL_Sim::RL_Sim()
nh.param<std::string>("ros_namespace", ros_namespace, "");
for (int i = 0; i < 12; ++i)
for (int i = 0; i < params.num_of_dofs; ++i)
{
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);
@ -89,7 +89,7 @@ RL_Sim::~RL_Sim()
void RL_Sim::RobotControl()
{
motiontime++;
for (int i = 0; i < 12; ++i)
for (int i = 0; i < params.num_of_dofs; ++i)
{
motor_commands[i].mode = 0x0A;
motor_commands[i].q = output_dof_pos[0][i].item<double>();
@ -158,10 +158,10 @@ void RL_Sim::RunModel()
output_dof_pos = this->ComputePosition(actions);
#ifdef CSV_LOGGER
torch::Tensor tau_est = torch::tensor({{joint_efforts[1], joint_efforts[2], joint_efforts[0],
joint_efforts[4], joint_efforts[5], joint_efforts[3],
joint_efforts[7], joint_efforts[8], joint_efforts[6],
joint_efforts[10], joint_efforts[11], joint_efforts[9]}});
torch::Tensor tau_est = torch::tensor({{joint_efforts[0], joint_efforts[1], joint_efforts[2],
joint_efforts[3], joint_efforts[4], joint_efforts[5],
joint_efforts[6], joint_efforts[7], joint_efforts[8],
joint_efforts[9], joint_efforts[10], joint_efforts[11]}});
CSVLogger(output_torques, tau_est, this->obs.dof_pos, output_dof_pos, this->obs.dof_vel);
#endif
}
@ -197,16 +197,15 @@ torch::Tensor RL_Sim::Forward()
void RL_Sim::Plot()
{
int dof_mapping[13] = {1, 2, 0, 4, 5, 3, 7, 8, 6, 10, 11, 9};
plot_t.erase(plot_t.begin());
plot_t.push_back(motiontime);
plt::cla();
plt::clf();
for(int i = 0; i < 12; ++i)
for(int i = 0; i < params.num_of_dofs; ++i)
{
plot_real_joint_pos[i].erase(plot_real_joint_pos[i].begin());
plot_target_joint_pos[i].erase(plot_target_joint_pos[i].begin());
plot_real_joint_pos[i].push_back(joint_positions[dof_mapping[i]]);
plot_real_joint_pos[i].push_back(joint_positions[i]);
plot_target_joint_pos[i].push_back(motor_commands[i].q);
plt::subplot(4, 3, i+1);
plt::named_plot("_real_joint_pos", plot_t, plot_real_joint_pos[i], "r");