From 13dbb895bfe98776d1b23253ed972edb8e2b6b5e Mon Sep 17 00:00:00 2001 From: fan-ziqi Date: Thu, 23 May 2024 15:52:32 +0800 Subject: [PATCH] fix: make the code more universal --- src/rl_sar/config.yaml | 3 +++ src/rl_sar/include/rl_sim.hpp | 2 -- src/rl_sar/library/rl_sdk/rl_sdk.cpp | 1 + src/rl_sar/library/rl_sdk/rl_sdk.hpp | 1 + src/rl_sar/src/rl_sim.cpp | 23 +++++++---------------- 5 files changed, 12 insertions(+), 18 deletions(-) diff --git a/src/rl_sar/config.yaml b/src/rl_sar/config.yaml index 3300be1..f6fe237 100644 --- a/src/rl_sar/config.yaml +++ b/src/rl_sar/config.yaml @@ -15,6 +15,7 @@ a1: 0.5, 0.5, 0.5] # RR action_scale: 0.25 hip_scale_reduction: 0.5 + hip_scale_reduction_indices: [0, 3, 6, 9] num_of_dofs: 12 lin_vel_scale: 2.0 ang_vel_scale: 0.25 @@ -51,6 +52,7 @@ cyberdog1: 0.5, 0.5, 0.5] # RR action_scale: 0.25 hip_scale_reduction: 0.5 + hip_scale_reduction_indices: [0, 3, 6, 9] num_of_dofs: 12 lin_vel_scale: 2.0 ang_vel_scale: 0.25 @@ -86,6 +88,7 @@ lite3_wheel: 0.5, 0.5, 0.5, 0.5] # RR action_scale: 0.25 hip_scale_reduction: 1.0 + hip_scale_reduction_indices: [0, 3, 6, 9] num_of_dofs: 16 lin_vel_scale: 2.0 ang_vel_scale: 0.25 diff --git a/src/rl_sar/include/rl_sim.hpp b/src/rl_sar/include/rl_sim.hpp index f0161ad..fe0faf8 100644 --- a/src/rl_sar/include/rl_sim.hpp +++ b/src/rl_sar/include/rl_sim.hpp @@ -61,8 +61,6 @@ private: std::vector joint_velocities; std::vector joint_efforts; - int hip_scale_reduction_indices[4] = {0, 3, 6, 9}; - std::chrono::high_resolution_clock::time_point start_time; void MapData(const std::vector& source_data, std::vector& target_data); diff --git a/src/rl_sar/library/rl_sdk/rl_sdk.cpp b/src/rl_sar/library/rl_sdk/rl_sdk.cpp index 816684c..1e2bcba 100644 --- a/src/rl_sar/library/rl_sdk/rl_sdk.cpp +++ b/src/rl_sar/library/rl_sdk/rl_sdk.cpp @@ -30,6 +30,7 @@ void RL::ReadYaml(std::string robot_name) this->params.clip_actions = config["clip_actions"].as(); 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"]); this->params.num_of_dofs = config["num_of_dofs"].as(); this->params.lin_vel_scale = config["lin_vel_scale"].as(); this->params.ang_vel_scale = config["ang_vel_scale"].as(); diff --git a/src/rl_sar/library/rl_sdk/rl_sdk.hpp b/src/rl_sar/library/rl_sdk/rl_sdk.hpp index 2df07db..a0326d8 100644 --- a/src/rl_sar/library/rl_sdk/rl_sdk.hpp +++ b/src/rl_sar/library/rl_sdk/rl_sdk.hpp @@ -19,6 +19,7 @@ struct ModelParams float stiffness; float action_scale; float hip_scale_reduction; + std::vector hip_scale_reduction_indices; int num_of_dofs; float lin_vel_scale; float ang_vel_scale; diff --git a/src/rl_sar/src/rl_sim.cpp b/src/rl_sar/src/rl_sim.cpp index 9c307aa..286bcef 100644 --- a/src/rl_sar/src/rl_sim.cpp +++ b/src/rl_sar/src/rl_sim.cpp @@ -35,9 +35,9 @@ RL_Sim::RL_Sim() this->history_obs_buf = ObservationBuffer(1, this->params.num_observations, 6); - joint_positions = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; - joint_velocities = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; - 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}; + joint_positions = std::vector(params.num_of_dofs, 0.0); + joint_velocities = std::vector(params.num_of_dofs, 0.0); + joint_efforts = std::vector(params.num_of_dofs, 0.0); plot_t = std::vector(plot_size, 0); plot_real_joint_pos.resize(params.num_of_dofs); @@ -138,18 +138,12 @@ void RL_Sim::RunModel() this->obs.ang_vel = torch::tensor({{vel.angular.x, vel.angular.y, vel.angular.z}}); this->obs.commands = torch::tensor({{cmd_vel.linear.x, cmd_vel.linear.y, cmd_vel.angular.z}}); this->obs.base_quat = torch::tensor({{pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w}}); - this->obs.dof_pos = torch::tensor({{joint_positions[0], joint_positions[1], joint_positions[2], - joint_positions[3], joint_positions[4], joint_positions[5], - joint_positions[6], joint_positions[7], joint_positions[8], - joint_positions[9], joint_positions[10], joint_positions[11]}}); - this->obs.dof_vel = torch::tensor({{joint_velocities[0], joint_velocities[1], joint_velocities[2], - joint_velocities[3], joint_velocities[4], joint_velocities[5], - joint_velocities[6], joint_velocities[7], joint_velocities[8], - joint_velocities[9], joint_velocities[10], joint_velocities[11]}}); + this->obs.dof_pos = torch::tensor(joint_positions).unsqueeze(0); + this->obs.dof_vel = torch::tensor(joint_velocities).unsqueeze(0); torch::Tensor actions = this->Forward(); - for (int i : hip_scale_reduction_indices) + for (int i : this->params.hip_scale_reduction_indices) { actions[0][i] *= this->params.hip_scale_reduction; } @@ -158,10 +152,7 @@ void RL_Sim::RunModel() output_dof_pos = this->ComputePosition(actions); #ifdef CSV_LOGGER - 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]}}); + torch::Tensor tau_est = torch::tensor(joint_efforts).unsqueeze(0); CSVLogger(output_torques, tau_est, this->obs.dof_pos, output_dof_pos, this->obs.dof_vel); #endif }