diff --git a/src/rl_sar/config.yaml b/src/rl_sar/config.yaml index 4ca2a7a..3300be1 100644 --- a/src/rl_sar/config.yaml +++ b/src/rl_sar/config.yaml @@ -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 diff --git a/src/rl_sar/library/rl_sdk/rl_sdk.cpp b/src/rl_sar/library/rl_sdk/rl_sdk.cpp index 1aff98f..816684c 100644 --- a/src/rl_sar/library/rl_sdk/rl_sdk.cpp +++ b/src/rl_sar/library/rl_sdk/rl_sdk.cpp @@ -35,7 +35,8 @@ void RL::ReadYaml(std::string robot_name) this->params.ang_vel_scale = config["ang_vel_scale"].as(); this->params.dof_pos_scale = config["dof_pos_scale"].as(); this->params.dof_vel_scale = config["dof_vel_scale"].as(); - this->params.commands_scale = torch::tensor(ReadVectorFromYaml(config["commands_scale"])).view({1, -1}); + // this->params.commands_scale = torch::tensor(ReadVectorFromYaml(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(); // this->params.stiffness = config["stiffness"].as(); // this->params.d_gains = torch::ones(12) * this->params.damping; diff --git a/src/rl_sar/models/a1/model_0522.pt b/src/rl_sar/models/a1/model_0522.pt new file mode 100644 index 0000000..18f80a8 Binary files /dev/null and b/src/rl_sar/models/a1/model_0522.pt differ diff --git a/src/rl_sar/src/rl_real_a1.cpp b/src/rl_sar/src/rl_real_a1.cpp index 35e7c1f..68c1d9a 100644 --- a/src/rl_sar/src/rl_real_a1.cpp +++ b/src/rl_sar/src/rl_real_a1.cpp @@ -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(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(plot_size, 0); } for(auto& vector : plot_target_joint_pos) { vector = std::vector(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(); 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()); diff --git a/src/rl_sar/src/rl_real_cyberdog.cpp b/src/rl_sar/src/rl_real_cyberdog.cpp index fc42e36..3291b0f 100644 --- a/src/rl_sar/src/rl_real_cyberdog.cpp +++ b/src/rl_sar/src/rl_real_cyberdog.cpp @@ -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(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(plot_size, 0); } for(auto& vector : plot_target_joint_pos) { vector = std::vector(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(); 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(); @@ -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()); diff --git a/src/rl_sar/src/rl_sim.cpp b/src/rl_sar/src/rl_sim.cpp index 61a9244..9c307aa 100644 --- a/src/rl_sar/src/rl_sim.cpp +++ b/src/rl_sar/src/rl_sim.cpp @@ -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(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(plot_size, 0); } for(auto& vector : plot_target_joint_pos) { vector = std::vector(plot_size, 0); } @@ -49,7 +49,7 @@ RL_Sim::RL_Sim() nh.param("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( 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(); @@ -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");