diff --git a/src/rl_sar/config.yaml b/src/rl_sar/config.yaml index 4c4f1c3..2e21776 100644 --- a/src/rl_sar/config.yaml +++ b/src/rl_sar/config.yaml @@ -1,4 +1,5 @@ a1: + model_name: "model.pt" num_observations: 45 clip_obs: 100.0 clip_actions: 100.0 @@ -19,16 +20,17 @@ a1: ang_vel_scale: 0.25 dof_pos_scale: 1.0 dof_vel_scale: 0.05 - 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 + torque_limits: [33.5, 33.5, 33.5, # FL + 33.5, 33.5, 33.5, # FR + 33.5, 33.5, 33.5, # RL + 33.5, 33.5, 33.5] # RR default_dof_pos: [ 0.1000, 0.8000, -1.5000, # FL -0.1000, 0.8000, -1.5000, # FR 0.1000, 1.0000, -1.5000, # RL -0.1000, 1.0000, -1.5000] # RR cyberdog1: + model_name: "model.pt" num_observations: 45 clip_obs: 100.0 clip_actions: 100.0 diff --git a/src/rl_sar/include/rl_real.hpp b/src/rl_sar/include/rl_real_a1.hpp similarity index 94% rename from src/rl_sar/include/rl_real.hpp rename to src/rl_sar/include/rl_real_a1.hpp index c691843..55c0d4d 100644 --- a/src/rl_sar/include/rl_real.hpp +++ b/src/rl_sar/include/rl_real_a1.hpp @@ -71,10 +71,6 @@ private: int hip_scale_reduction_indices[4] = {0, 3, 6, 9}; std::chrono::high_resolution_clock::time_point start_time; - - // other rl module - torch::jit::script::Module encoder; - torch::jit::script::Module vq; }; #endif \ No newline at end of file diff --git a/src/rl_sar/include/rl_real_cyberdog.hpp b/src/rl_sar/include/rl_real_cyberdog.hpp index 630b195..44e2520 100644 --- a/src/rl_sar/include/rl_real_cyberdog.hpp +++ b/src/rl_sar/include/rl_real_cyberdog.hpp @@ -81,10 +81,6 @@ private: int hip_scale_reduction_indices[4] = {0, 3, 6, 9}; std::chrono::high_resolution_clock::time_point start_time; - - // other rl module - torch::jit::script::Module encoder; - torch::jit::script::Module vq; }; #endif \ No newline at end of file diff --git a/src/rl_sar/include/rl_sim.hpp b/src/rl_sar/include/rl_sim.hpp index 507808c..4dc21af 100644 --- a/src/rl_sar/include/rl_sim.hpp +++ b/src/rl_sar/include/rl_sim.hpp @@ -64,10 +64,6 @@ private: int hip_scale_reduction_indices[4] = {0, 3, 6, 9}; std::chrono::high_resolution_clock::time_point start_time; - - // other rl module - torch::jit::script::Module encoder; - torch::jit::script::Module vq; }; #endif \ No newline at end of file diff --git a/src/rl_sar/library/rl/rl.cpp b/src/rl_sar/library/rl/rl.cpp index 7aef355..9acb4cd 100644 --- a/src/rl_sar/library/rl/rl.cpp +++ b/src/rl_sar/library/rl/rl.cpp @@ -13,6 +13,7 @@ void RL::ReadYaml(std::string robot_name) return; } + this->params.model_name = config["model_name"].as(); this->params.num_observations = config["num_observations"].as(); this->params.clip_obs = config["clip_obs"].as(); this->params.clip_actions = config["clip_actions"].as(); diff --git a/src/rl_sar/library/rl/rl.hpp b/src/rl_sar/library/rl/rl.hpp index 923bbeb..e174bb2 100644 --- a/src/rl_sar/library/rl/rl.hpp +++ b/src/rl_sar/library/rl/rl.hpp @@ -13,6 +13,7 @@ namespace plt = matplotlibcpp; struct ModelParams { + std::string model_name; int num_observations; float damping; float stiffness; @@ -66,7 +67,7 @@ public: protected: // rl module - torch::jit::script::Module actor; + torch::jit::script::Module model; // observation buffer torch::Tensor lin_vel; torch::Tensor ang_vel; diff --git a/src/rl_sar/models/a1/actor.pt b/src/rl_sar/models/a1/actor.pt deleted file mode 100644 index 912342c..0000000 Binary files a/src/rl_sar/models/a1/actor.pt and /dev/null differ diff --git a/src/rl_sar/models/a1/encoder.pt b/src/rl_sar/models/a1/encoder.pt deleted file mode 100644 index 3a8edb6..0000000 Binary files a/src/rl_sar/models/a1/encoder.pt and /dev/null differ diff --git a/src/rl_sar/models/a1/model.pt b/src/rl_sar/models/a1/model.pt new file mode 100644 index 0000000..94351de Binary files /dev/null and b/src/rl_sar/models/a1/model.pt differ diff --git a/src/rl_sar/models/a1/vq_layer.pt b/src/rl_sar/models/a1/vq_layer.pt deleted file mode 100644 index 21bcc92..0000000 Binary files a/src/rl_sar/models/a1/vq_layer.pt and /dev/null differ diff --git a/src/rl_sar/models/cyberdog1/actor.pt b/src/rl_sar/models/cyberdog1/actor.pt deleted file mode 100644 index 15d9fb1..0000000 Binary files a/src/rl_sar/models/cyberdog1/actor.pt and /dev/null differ diff --git a/src/rl_sar/models/cyberdog1/encoder.pt b/src/rl_sar/models/cyberdog1/encoder.pt deleted file mode 100644 index 31ba4b7..0000000 Binary files a/src/rl_sar/models/cyberdog1/encoder.pt and /dev/null differ diff --git a/src/rl_sar/models/cyberdog1/vq_layer.pt b/src/rl_sar/models/cyberdog1/vq_layer.pt deleted file mode 100644 index 8a3ad3e..0000000 Binary files a/src/rl_sar/models/cyberdog1/vq_layer.pt and /dev/null differ diff --git a/src/rl_sar/src/rl_real_a1.cpp b/src/rl_sar/src/rl_real_a1.cpp index 0360d2f..10256a6 100644 --- a/src/rl_sar/src/rl_real_a1.cpp +++ b/src/rl_sar/src/rl_real_a1.cpp @@ -1,4 +1,4 @@ -#include "../include/rl_real.hpp" +#include "../include/rl_real_a1.hpp" #define ROBOT_NAME "a1" @@ -15,13 +15,9 @@ RL_Real::RL_Real() : safe(LeggedType::A1), udp(LOWLEVEL) start_time = std::chrono::high_resolution_clock::now(); - 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"; + std::string model_path = std::string(CMAKE_CURRENT_SOURCE_DIR) + "/models/" + ROBOT_NAME + "/" + this->params.model_name; + this->model = torch::jit::load(model_path); - this->actor = torch::jit::load(actor_path); - this->encoder = torch::jit::load(encoder_path); - this->vq = torch::jit::load(vq_path); this->InitObservations(); this->InitOutputs(); @@ -263,13 +259,7 @@ torch::Tensor RL_Real::Forward() history_obs_buf.insert(obs); history_obs = history_obs_buf.get_obs_vec({0, 1, 2, 3, 4, 5}); - torch::Tensor encoding = this->encoder.forward({history_obs}).toTensor(); - - torch::Tensor z = this->vq.forward({encoding}).toTensor(); - - torch::Tensor actor_input = torch::cat({obs, z}, 1); - - torch::Tensor action = this->actor.forward({actor_input}).toTensor(); + torch::Tensor action = this->model.forward({history_obs}).toTensor(); this->obs.actions = action; torch::Tensor clamped = torch::clamp(action, -this->params.clip_actions, this->params.clip_actions); diff --git a/src/rl_sar/src/rl_real_cyberdog.cpp b/src/rl_sar/src/rl_real_cyberdog.cpp index 148c147..1097e41 100644 --- a/src/rl_sar/src/rl_real_cyberdog.cpp +++ b/src/rl_sar/src/rl_real_cyberdog.cpp @@ -13,13 +13,9 @@ RL_Real::RL_Real() : CustomInterface(500) start_time = std::chrono::high_resolution_clock::now(); - 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"; + std::string model_path = std::string(CMAKE_CURRENT_SOURCE_DIR) + "/models/" + ROBOT_NAME + "/" + this->params.model_name; + this->model = torch::jit::load(model_path); - this->actor = torch::jit::load(actor_path); - this->encoder = torch::jit::load(encoder_path); - this->vq = torch::jit::load(vq_path); this->InitObservations(); this->InitOutputs(); @@ -310,13 +306,7 @@ torch::Tensor RL_Real::Forward() history_obs_buf.insert(obs); history_obs = history_obs_buf.get_obs_vec({0, 1, 2, 3, 4, 5}); - torch::Tensor encoding = this->encoder.forward({history_obs}).toTensor(); - - torch::Tensor z = this->vq.forward({encoding}).toTensor(); - - torch::Tensor actor_input = torch::cat({obs, z}, 1); - - torch::Tensor action = this->actor.forward({actor_input}).toTensor(); + torch::Tensor action = this->model.forward({history_obs}).toTensor(); this->obs.actions = action; torch::Tensor clamped = torch::clamp(action, -this->params.clip_actions, this->params.clip_actions); diff --git a/src/rl_sar/src/rl_sim.cpp b/src/rl_sar/src/rl_sim.cpp index 7f6414e..c84c7aa 100644 --- a/src/rl_sar/src/rl_sim.cpp +++ b/src/rl_sar/src/rl_sim.cpp @@ -16,13 +16,9 @@ RL_Sim::RL_Sim() motor_commands.resize(12); - 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"; + std::string model_path = std::string(CMAKE_CURRENT_SOURCE_DIR) + "/models/" + ROBOT_NAME + "/" + this->params.model_name; + this->model = torch::jit::load(model_path); - this->actor = torch::jit::load(actor_path); - this->encoder = torch::jit::load(encoder_path); - this->vq = torch::jit::load(vq_path); this->InitObservations(); this->InitOutputs(); @@ -194,13 +190,7 @@ torch::Tensor RL_Sim::Forward() history_obs_buf.insert(obs); history_obs = history_obs_buf.get_obs_vec({0, 1, 2, 3, 4, 5}); - torch::Tensor encoding = this->encoder.forward({history_obs}).toTensor(); - - torch::Tensor z = this->vq.forward({encoding}).toTensor(); - - torch::Tensor actor_input = torch::cat({obs, z}, 1); - - torch::Tensor action = this->actor.forward({actor_input}).toTensor(); + torch::Tensor action = this->model.forward({history_obs}).toTensor(); this->obs.actions = action; torch::Tensor clamped = torch::clamp(action, -this->params.clip_actions, this->params.clip_actions);