Skip to content

Commit

Permalink
feat: use num_of_dofs instead 12
Browse files Browse the repository at this point in the history
  • Loading branch information
@fan-ziqi
fan-ziqi committed May 23, 2024
1 parent a37fa1c commit 5a4d8d5
Show file tree
Hide file tree
Showing 6 changed files with 41 additions and 41 deletions.
8 changes: 4 additions & 4 deletions src/rl_sar/config.yaml
View file
Original file line number Diff line number Diff line change
@@ -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
Expand All @@ -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
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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
Expand Down
3 changes: 2 additions & 1 deletion src/rl_sar/library/rl_sdk/rl_sdk.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down
Binary file added src/rl_sar/models/a1/model_0522.pt
View file
Binary file not shown.
28 changes: 14 additions & 14 deletions src/rl_sar/src/rl_real_a1.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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); }

Expand Down Expand Up @@ -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];
Expand All @@ -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;
}
Expand All @@ -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;
}
Expand All @@ -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;
Expand All @@ -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;
}
Expand All @@ -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;
}
Expand All @@ -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);
}

Expand Down Expand Up @@ -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());
Expand Down
20 changes: 10 additions & 10 deletions src/rl_sar/src/rl_real_cyberdog.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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); }

Expand Down Expand Up @@ -81,15 +81,15 @@ 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];
}
if(keyboard.robot_state == STATE_POS_GETUP)
{
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];
Expand All @@ -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;
Expand All @@ -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];
}
Expand All @@ -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>();
Expand All @@ -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];
}
Expand All @@ -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;
Expand Down Expand Up @@ -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());
Expand Down
23 changes: 11 additions & 12 deletions src/rl_sar/src/rl_sim.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand All @@ -40,16 +40,16 @@ 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); }

cmd_vel_subscriber_ = nh.subscribe<geometry_msgs::Twist>("/cmd_vel", 10, &RL_Sim::CmdvelCallback, this);

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);
Expand Down Expand Up @@ -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>();
Expand Down Expand Up @@ -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
}
Expand Down Expand Up @@ -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");
Expand Down

0 comments on commit 5a4d8d5

Please sign in to comment.