From cbaef302965047980b6cb480505a0a620e3b7df5 Mon Sep 17 00:00:00 2001 From: fan-ziqi Date: Thu, 14 Mar 2024 13:11:01 +0800 Subject: [PATCH] fix: rename --- README.md | 8 ++-- README_CN.md | 8 ++-- src/{unitree_rl => rl_sar}/CMakeLists.txt | 22 +++++---- .../include/rl_real.hpp} | 35 ++------------ .../include/rl_sim.hpp} | 10 ++-- .../launch/start_env.launch | 2 +- .../observation_buffer/observation_buffer.cpp | 0 .../observation_buffer/observation_buffer.hpp | 0 .../model.cpp => rl_sar/library/rl/rl.cpp} | 44 ++++++++--------- .../model.hpp => rl_sar/library/rl/rl.hpp} | 14 +++--- .../include/unitree_legged_sdk/a1_const.h | 0 .../unitree_legged_sdk/aliengo_const.h | 0 .../include/unitree_legged_sdk/comm.h | 0 .../include/unitree_legged_sdk/lcm.h | 0 .../include/unitree_legged_sdk/lcm_server.h | 0 .../include/unitree_legged_sdk/loop.h | 0 .../include/unitree_legged_sdk/quadruped.h | 0 .../include/unitree_legged_sdk/safety.h | 0 .../include/unitree_legged_sdk/udp.h | 0 .../unitree_legged_sdk/unitree_joystick.h | 0 .../unitree_legged_sdk/unitree_legged_sdk.h | 0 .../lib/libunitree_legged_sdk_amd64.so | Bin .../lib/libunitree_legged_sdk_arm32.so | Bin .../lib/libunitree_legged_sdk_arm64.so | Bin src/{unitree_rl => rl_sar}/models/actor.pt | Bin src/{unitree_rl => rl_sar}/models/encoder.pt | Bin src/{unitree_rl => rl_sar}/models/vq_layer.pt | Bin src/{unitree_rl => rl_sar}/package.xml | 6 +-- .../src/rl_real.cpp} | 45 +++++++----------- .../unitree_rl.cpp => rl_sar/src/rl_sim.cpp} | 35 +++++++------- .../worlds/stairs.world | 0 31 files changed, 98 insertions(+), 131 deletions(-) rename src/{unitree_rl => rl_sar}/CMakeLists.txt (73%) rename src/{unitree_rl/include/unitree_rl_real.hpp => rl_sar/include/rl_real.hpp} (61%) rename src/{unitree_rl/include/unitree_rl.hpp => rl_sar/include/rl_sim.hpp} (91%) rename src/{unitree_rl => rl_sar}/launch/start_env.launch (97%) rename src/{unitree_rl => rl_sar}/library/observation_buffer/observation_buffer.cpp (100%) rename src/{unitree_rl => rl_sar}/library/observation_buffer/observation_buffer.hpp (100%) rename src/{unitree_rl/library/model/model.cpp => rl_sar/library/rl/rl.cpp} (84%) rename src/{unitree_rl/library/model/model.hpp => rl_sar/library/rl/rl.hpp} (89%) rename src/{unitree_rl => rl_sar}/library/unitree_legged_sdk_3.2/include/unitree_legged_sdk/a1_const.h (100%) rename src/{unitree_rl => rl_sar}/library/unitree_legged_sdk_3.2/include/unitree_legged_sdk/aliengo_const.h (100%) rename src/{unitree_rl => rl_sar}/library/unitree_legged_sdk_3.2/include/unitree_legged_sdk/comm.h (100%) rename src/{unitree_rl => rl_sar}/library/unitree_legged_sdk_3.2/include/unitree_legged_sdk/lcm.h (100%) rename src/{unitree_rl => rl_sar}/library/unitree_legged_sdk_3.2/include/unitree_legged_sdk/lcm_server.h (100%) rename src/{unitree_rl => rl_sar}/library/unitree_legged_sdk_3.2/include/unitree_legged_sdk/loop.h (100%) rename src/{unitree_rl => rl_sar}/library/unitree_legged_sdk_3.2/include/unitree_legged_sdk/quadruped.h (100%) rename src/{unitree_rl => rl_sar}/library/unitree_legged_sdk_3.2/include/unitree_legged_sdk/safety.h (100%) rename src/{unitree_rl => rl_sar}/library/unitree_legged_sdk_3.2/include/unitree_legged_sdk/udp.h (100%) rename src/{unitree_rl => rl_sar}/library/unitree_legged_sdk_3.2/include/unitree_legged_sdk/unitree_joystick.h (100%) rename src/{unitree_rl => rl_sar}/library/unitree_legged_sdk_3.2/include/unitree_legged_sdk/unitree_legged_sdk.h (100%) rename src/{unitree_rl => rl_sar}/library/unitree_legged_sdk_3.2/lib/libunitree_legged_sdk_amd64.so (100%) rename src/{unitree_rl => rl_sar}/library/unitree_legged_sdk_3.2/lib/libunitree_legged_sdk_arm32.so (100%) rename src/{unitree_rl => rl_sar}/library/unitree_legged_sdk_3.2/lib/libunitree_legged_sdk_arm64.so (100%) rename src/{unitree_rl => rl_sar}/models/actor.pt (100%) rename src/{unitree_rl => rl_sar}/models/encoder.pt (100%) rename src/{unitree_rl => rl_sar}/models/vq_layer.pt (100%) rename src/{unitree_rl => rl_sar}/package.xml (90%) rename src/{unitree_rl/src/unitree_rl_real.cpp => rl_sar/src/rl_real.cpp} (88%) rename src/{unitree_rl/src/unitree_rl.cpp => rl_sar/src/rl_sim.cpp} (84%) rename src/{unitree_rl => rl_sar}/worlds/stairs.world (100%) diff --git a/README.md b/README.md index 971535c..160783e 100644 --- a/README.md +++ b/README.md @@ -52,7 +52,7 @@ catkin build ## Running -Before running, copy the trained pt model file to `rl_sar/src/unitree_rl/models` +Before running, copy the trained pt model file to `rl_sar/src/rl_sar/models` ### Simulation @@ -60,14 +60,14 @@ Open a new terminal, launch the gazebo simulation environment ```bash source devel/setup.bash -roslaunch unitree_rl start_env.launch +roslaunch rl_sar start_env.launch ``` Open a new terminal, run the control program ```bash source devel/setup.bash -rosrun unitree_rl unitree_rl +rosrun rl_sar unitree_rl ``` Open a new terminal, run the keyboard control program @@ -82,7 +82,7 @@ Open a new terminal, run the control program ```bash source devel/setup.bash -rosrun unitree_rl unitree_rl_real +rosrun rl_sar unitree_rl_real ``` > Some code references: https://github.com/mertgungor/unitree_model_control \ No newline at end of file diff --git a/README_CN.md b/README_CN.md index 37a3b2b..893f71c 100644 --- a/README_CN.md +++ b/README_CN.md @@ -52,7 +52,7 @@ catkin build ## 运行 -运行前请将训练好的pt模型文件拷贝到`rl_sar/src/unitree_rl/models`中 +运行前请将训练好的pt模型文件拷贝到`rl_sar/src/rl_sar/models`中 ### 仿真 @@ -60,14 +60,14 @@ catkin build ```bash source devel/setup.bash -roslaunch unitree_rl start_env.launch +roslaunch rl_sar start_env.launch ``` 新建终端，启动控制程序 ```bash source devel/setup.bash -rosrun unitree_rl unitree_rl +rosrun rl_sar unitree_rl ``` 新建终端，键盘控制程序 @@ -82,7 +82,7 @@ rosrun teleop_twist_keyboard teleop_twist_keyboard.py ```bash source devel/setup.bash -rosrun unitree_rl unitree_rl_real +rosrun rl_sar unitree_rl_real ``` > 部分代码参考https://github.com/mertgungor/unitree_model_control diff --git a/src/unitree_rl/CMakeLists.txt b/src/rl_sar/CMakeLists.txt similarity index 73% rename from src/unitree_rl/CMakeLists.txt rename to src/rl_sar/CMakeLists.txt index 4b5161c..761f933 100644 --- a/src/unitree_rl/CMakeLists.txt +++ b/src/rl_sar/CMakeLists.txt @@ -1,8 +1,10 @@ cmake_minimum_required(VERSION 3.0.2) -project(unitree_rl) +project(rl_sar) set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${TORCH_CXX_FLAGS}") +add_definitions(-DCMAKE_CURRENT_SOURCE_DIR="${CMAKE_CURRENT_SOURCE_DIR}") + find_package(Torch REQUIRED) find_package(catkin REQUIRED COMPONENTS @@ -39,22 +41,22 @@ include_directories( set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${GAZEBO_CXX_FLAGS}") -add_library(model library/model/model.cpp library/model/model.hpp) -target_link_libraries(model "${TORCH_LIBRARIES}") -set_property(TARGET model PROPERTY CXX_STANDARD 14) +add_library(rl library/rl/rl.cpp library/rl/rl.hpp) +target_link_libraries(rl "${TORCH_LIBRARIES}") +set_property(TARGET rl PROPERTY CXX_STANDARD 14) add_library(observation_buffer library/observation_buffer/observation_buffer.cpp library/observation_buffer/observation_buffer.hpp) target_link_libraries(observation_buffer "${TORCH_LIBRARIES}") set_property(TARGET observation_buffer PROPERTY CXX_STANDARD 14) -add_executable(unitree_rl src/unitree_rl.cpp ) -target_link_libraries(unitree_rl +add_executable(rl_sim src/rl_sim.cpp ) +target_link_libraries(rl_sim ${catkin_LIBRARIES} ${EXTRA_LIBS} "${TORCH_LIBRARIES}" - model observation_buffer + rl observation_buffer ) -add_executable(unitree_rl_real src/unitree_rl_real.cpp ) -target_link_libraries(unitree_rl_real +add_executable(rl_real src/rl_real.cpp ) +target_link_libraries(rl_real ${catkin_LIBRARIES} ${EXTRA_LIBS} "${TORCH_LIBRARIES}" - model observation_buffer + rl observation_buffer ) \ No newline at end of file diff --git a/src/unitree_rl/include/unitree_rl_real.hpp b/src/rl_sar/include/rl_real.hpp similarity index 61% rename from src/unitree_rl/include/unitree_rl_real.hpp rename to src/rl_sar/include/rl_real.hpp index e4875ae..69a4c03 100644 --- a/src/unitree_rl/include/unitree_rl_real.hpp +++ b/src/rl_sar/include/rl_real.hpp @@ -1,11 +1,7 @@ -#ifndef UNITREE_RL -#define UNITREE_RL +#ifndef RL_REAL_HPP +#define RL_REAL_HPP -#include -#include -#include -#include -#include "../library/model/model.hpp" +#include "../library/rl/rl.hpp" #include "../library/observation_buffer/observation_buffer.hpp" #include #include "unitree_legged_msgs/LowState.h" @@ -16,13 +12,10 @@ using namespace UNITREE_LEGGED_SDK; -class Unitree_RL : public Model +class RL_Real : public RL { public: - Unitree_RL(); - void modelStatesCallback(const gazebo_msgs::ModelStates::ConstPtr &msg); - void jointStatesCallback(const sensor_msgs::JointState::ConstPtr &msg); - void cmdvelCallback(const geometry_msgs::Twist::ConstPtr &msg); + RL_Real(); void runModel(); torch::Tensor forward() override; torch::Tensor compute_observation() override; @@ -60,35 +53,17 @@ private: std::vector torque_command_topics; - ros::Subscriber model_state_subscriber_; - ros::Subscriber joint_state_subscriber_; - ros::Subscriber cmd_vel_subscriber_; - - std::map torque_publishers; std::vector torque_commands; - geometry_msgs::Twist vel; - geometry_msgs::Pose pose; - geometry_msgs::Twist cmd_vel; - std::vector joint_names; std::vector joint_positions; std::vector joint_velocities; - - - ros::Timer timer; - std::chrono::high_resolution_clock::time_point start_time; // other rl module torch::jit::script::Module encoder; torch::jit::script::Module vq; - - UNITREE_LEGGED_SDK::LowCmd SendLowLCM = {0}; - UNITREE_LEGGED_SDK::LowState RecvLowLCM = {0}; - unitree_legged_msgs::LowCmd SendLowROS; - unitree_legged_msgs::LowState RecvLowROS; }; #endif \ No newline at end of file diff --git a/src/unitree_rl/include/unitree_rl.hpp b/src/rl_sar/include/rl_sim.hpp similarity index 91% rename from src/unitree_rl/include/unitree_rl.hpp rename to src/rl_sar/include/rl_sim.hpp index 3a89efb..01197fd 100644 --- a/src/unitree_rl/include/unitree_rl.hpp +++ b/src/rl_sar/include/rl_sim.hpp @@ -1,18 +1,18 @@ -#ifndef UNITREE_RL -#define UNITREE_RL +#ifndef RL_SIM_HPP +#define RL_SIM_HPP #include #include #include #include -#include "../library/model/model.hpp" +#include "../library/rl/rl.hpp" #include "../library/observation_buffer/observation_buffer.hpp" #include "unitree_legged_msgs/MotorCmd.h" -class Unitree_RL : public Model +class RL_Sim : public RL { public: - Unitree_RL(); + RL_Sim(); void modelStatesCallback(const gazebo_msgs::ModelStates::ConstPtr &msg); void jointStatesCallback(const sensor_msgs::JointState::ConstPtr &msg); void cmdvelCallback(const geometry_msgs::Twist::ConstPtr &msg); diff --git a/src/unitree_rl/launch/start_env.launch b/src/rl_sar/launch/start_env.launch similarity index 97% rename from src/unitree_rl/launch/start_env.launch rename to src/rl_sar/launch/start_env.launch index 7405294..158ca39 100644 --- a/src/unitree_rl/launch/start_env.launch +++ b/src/rl_sar/launch/start_env.launch @@ -13,7 +13,7 @@ - + diff --git a/src/unitree_rl/library/observation_buffer/observation_buffer.cpp b/src/rl_sar/library/observation_buffer/observation_buffer.cpp similarity index 100% rename from src/unitree_rl/library/observation_buffer/observation_buffer.cpp rename to src/rl_sar/library/observation_buffer/observation_buffer.cpp diff --git a/src/unitree_rl/library/observation_buffer/observation_buffer.hpp b/src/rl_sar/library/observation_buffer/observation_buffer.hpp similarity index 100% rename from src/unitree_rl/library/observation_buffer/observation_buffer.hpp rename to src/rl_sar/library/observation_buffer/observation_buffer.hpp diff --git a/src/unitree_rl/library/model/model.cpp b/src/rl_sar/library/rl/rl.cpp similarity index 84% rename from src/unitree_rl/library/model/model.cpp rename to src/rl_sar/library/rl/rl.cpp index 51aac75..722e910 100644 --- a/src/unitree_rl/library/model/model.cpp +++ b/src/rl_sar/library/rl/rl.cpp @@ -1,6 +1,6 @@ -#include "model.hpp" +#include "rl.hpp" -torch::Tensor Model::quat_rotate_inverse(torch::Tensor q, torch::Tensor v) +torch::Tensor RL::quat_rotate_inverse(torch::Tensor q, torch::Tensor v) { c10::IntArrayRef shape = q.sizes(); torch::Tensor q_w = q.index({torch::indexing::Slice(), -1}); @@ -11,7 +11,19 @@ torch::Tensor Model::quat_rotate_inverse(torch::Tensor q, torch::Tensor v) return a - b + c; } -torch::Tensor Model::compute_torques(torch::Tensor actions) +void RL::init_observations() +{ + this->obs.lin_vel = torch::tensor({{0.0, 0.0, 0.0}}); + this->obs.ang_vel = torch::tensor({{0.0, 0.0, 0.0}}); + this->obs.gravity_vec = torch::tensor({{0.0, 0.0, -1.0}}); + this->obs.commands = torch::tensor({{0.0, 0.0, 0.0}}); + this->obs.base_quat = torch::tensor({{0.0, 0.0, 0.0, 1.0}}); + this->obs.dof_pos = this->params.default_dof_pos; + this->obs.dof_vel = torch::tensor({{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}}); + this->obs.actions = torch::tensor({{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}}); +} + +torch::Tensor RL::compute_torques(torch::Tensor actions) { torch::Tensor actions_scaled = actions * this->params.action_scale; int indices[] = {0, 3, 6, 9}; @@ -25,10 +37,9 @@ torch::Tensor Model::compute_torques(torch::Tensor actions) return clamped; } -torch::Tensor Model::compute_observation() +/* You may need to override this compute_observation() function +torch::Tensor RL::compute_observation() { - std::cout << "You may need to override this compute_observation() function" << std::endl; - torch::Tensor obs = torch::cat({(this->quat_rotate_inverse(this->base_quat, this->lin_vel)) * this->params.lin_vel_scale, (this->quat_rotate_inverse(this->obs.base_quat, this->obs.ang_vel)) * this->params.ang_vel_scale, this->quat_rotate_inverse(this->obs.base_quat, this->obs.gravity_vec), @@ -40,27 +51,15 @@ torch::Tensor Model::compute_observation() obs = torch::clamp(obs, -this->params.clip_obs, this->params.clip_obs); - // printf("observation size: %d, %d\n", obs.sizes()[0], obs.sizes()[1]); + printf("observation size: %d, %d\n", obs.sizes()[0], obs.sizes()[1]); return obs; } +*/ -void Model::init_observations() +/* You may need to override this forward() function +torch::Tensor RL::forward() { - this->obs.lin_vel = torch::tensor({{0.0, 0.0, 0.0}}); - this->obs.ang_vel = torch::tensor({{0.0, 0.0, 0.0}}); - this->obs.gravity_vec = torch::tensor({{0.0, 0.0, -1.0}}); - this->obs.commands = torch::tensor({{0.0, 0.0, 0.0}}); - this->obs.base_quat = torch::tensor({{0.0, 0.0, 0.0, 1.0}}); - this->obs.dof_pos = this->params.default_dof_pos; - this->obs.dof_vel = torch::tensor({{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}}); - this->obs.actions = torch::tensor({{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}}); -} - -torch::Tensor Model::forward() -{ - std::cout << "You may need to override this forward() function" << std::endl; - torch::Tensor obs = this->compute_observation(); torch::Tensor actor_input = torch::cat({obs}, 1); @@ -72,3 +71,4 @@ torch::Tensor Model::forward() return clamped; } +*/ diff --git a/src/unitree_rl/library/model/model.hpp b/src/rl_sar/library/rl/rl.hpp similarity index 89% rename from src/unitree_rl/library/model/model.hpp rename to src/rl_sar/library/rl/rl.hpp index 791e7c6..9bb57c1 100644 --- a/src/unitree_rl/library/model/model.hpp +++ b/src/rl_sar/library/rl/rl.hpp @@ -1,5 +1,5 @@ -#ifndef MODEL_HPP -#define MODEL_HPP +#ifndef RL_HPP +#define RL_HPP #include #include @@ -36,14 +36,14 @@ struct Observations { torch::Tensor actions; }; -class Model { +class RL { public: - Model(){}; + RL(){}; ModelParams params; Observations obs; - virtual torch::Tensor forward(); - virtual torch::Tensor compute_observation(); + virtual torch::Tensor forward() = 0; + virtual torch::Tensor compute_observation() = 0; torch::Tensor compute_torques(torch::Tensor actions); torch::Tensor quat_rotate_inverse(torch::Tensor q, torch::Tensor v); @@ -63,4 +63,4 @@ protected: torch::Tensor actions; }; -#endif // MODEL_HPP \ No newline at end of file +#endif // RL_HPP \ No newline at end of file diff --git a/src/unitree_rl/library/unitree_legged_sdk_3.2/include/unitree_legged_sdk/a1_const.h b/src/rl_sar/library/unitree_legged_sdk_3.2/include/unitree_legged_sdk/a1_const.h similarity index 100% rename from src/unitree_rl/library/unitree_legged_sdk_3.2/include/unitree_legged_sdk/a1_const.h rename to src/rl_sar/library/unitree_legged_sdk_3.2/include/unitree_legged_sdk/a1_const.h diff --git a/src/unitree_rl/library/unitree_legged_sdk_3.2/include/unitree_legged_sdk/aliengo_const.h b/src/rl_sar/library/unitree_legged_sdk_3.2/include/unitree_legged_sdk/aliengo_const.h similarity index 100% rename from src/unitree_rl/library/unitree_legged_sdk_3.2/include/unitree_legged_sdk/aliengo_const.h rename to src/rl_sar/library/unitree_legged_sdk_3.2/include/unitree_legged_sdk/aliengo_const.h diff --git a/src/unitree_rl/library/unitree_legged_sdk_3.2/include/unitree_legged_sdk/comm.h b/src/rl_sar/library/unitree_legged_sdk_3.2/include/unitree_legged_sdk/comm.h similarity index 100% rename from src/unitree_rl/library/unitree_legged_sdk_3.2/include/unitree_legged_sdk/comm.h rename to src/rl_sar/library/unitree_legged_sdk_3.2/include/unitree_legged_sdk/comm.h diff --git a/src/unitree_rl/library/unitree_legged_sdk_3.2/include/unitree_legged_sdk/lcm.h b/src/rl_sar/library/unitree_legged_sdk_3.2/include/unitree_legged_sdk/lcm.h similarity index 100% rename from src/unitree_rl/library/unitree_legged_sdk_3.2/include/unitree_legged_sdk/lcm.h rename to src/rl_sar/library/unitree_legged_sdk_3.2/include/unitree_legged_sdk/lcm.h diff --git a/src/unitree_rl/library/unitree_legged_sdk_3.2/include/unitree_legged_sdk/lcm_server.h b/src/rl_sar/library/unitree_legged_sdk_3.2/include/unitree_legged_sdk/lcm_server.h similarity index 100% rename from src/unitree_rl/library/unitree_legged_sdk_3.2/include/unitree_legged_sdk/lcm_server.h rename to src/rl_sar/library/unitree_legged_sdk_3.2/include/unitree_legged_sdk/lcm_server.h diff --git a/src/unitree_rl/library/unitree_legged_sdk_3.2/include/unitree_legged_sdk/loop.h b/src/rl_sar/library/unitree_legged_sdk_3.2/include/unitree_legged_sdk/loop.h similarity index 100% rename from src/unitree_rl/library/unitree_legged_sdk_3.2/include/unitree_legged_sdk/loop.h rename to src/rl_sar/library/unitree_legged_sdk_3.2/include/unitree_legged_sdk/loop.h diff --git a/src/unitree_rl/library/unitree_legged_sdk_3.2/include/unitree_legged_sdk/quadruped.h b/src/rl_sar/library/unitree_legged_sdk_3.2/include/unitree_legged_sdk/quadruped.h similarity index 100% rename from src/unitree_rl/library/unitree_legged_sdk_3.2/include/unitree_legged_sdk/quadruped.h rename to src/rl_sar/library/unitree_legged_sdk_3.2/include/unitree_legged_sdk/quadruped.h diff --git a/src/unitree_rl/library/unitree_legged_sdk_3.2/include/unitree_legged_sdk/safety.h b/src/rl_sar/library/unitree_legged_sdk_3.2/include/unitree_legged_sdk/safety.h similarity index 100% rename from src/unitree_rl/library/unitree_legged_sdk_3.2/include/unitree_legged_sdk/safety.h rename to src/rl_sar/library/unitree_legged_sdk_3.2/include/unitree_legged_sdk/safety.h diff --git a/src/unitree_rl/library/unitree_legged_sdk_3.2/include/unitree_legged_sdk/udp.h b/src/rl_sar/library/unitree_legged_sdk_3.2/include/unitree_legged_sdk/udp.h similarity index 100% rename from src/unitree_rl/library/unitree_legged_sdk_3.2/include/unitree_legged_sdk/udp.h rename to src/rl_sar/library/unitree_legged_sdk_3.2/include/unitree_legged_sdk/udp.h diff --git a/src/unitree_rl/library/unitree_legged_sdk_3.2/include/unitree_legged_sdk/unitree_joystick.h b/src/rl_sar/library/unitree_legged_sdk_3.2/include/unitree_legged_sdk/unitree_joystick.h similarity index 100% rename from src/unitree_rl/library/unitree_legged_sdk_3.2/include/unitree_legged_sdk/unitree_joystick.h rename to src/rl_sar/library/unitree_legged_sdk_3.2/include/unitree_legged_sdk/unitree_joystick.h diff --git a/src/unitree_rl/library/unitree_legged_sdk_3.2/include/unitree_legged_sdk/unitree_legged_sdk.h b/src/rl_sar/library/unitree_legged_sdk_3.2/include/unitree_legged_sdk/unitree_legged_sdk.h similarity index 100% rename from src/unitree_rl/library/unitree_legged_sdk_3.2/include/unitree_legged_sdk/unitree_legged_sdk.h rename to src/rl_sar/library/unitree_legged_sdk_3.2/include/unitree_legged_sdk/unitree_legged_sdk.h diff --git a/src/unitree_rl/library/unitree_legged_sdk_3.2/lib/libunitree_legged_sdk_amd64.so b/src/rl_sar/library/unitree_legged_sdk_3.2/lib/libunitree_legged_sdk_amd64.so similarity index 100% rename from src/unitree_rl/library/unitree_legged_sdk_3.2/lib/libunitree_legged_sdk_amd64.so rename to src/rl_sar/library/unitree_legged_sdk_3.2/lib/libunitree_legged_sdk_amd64.so diff --git a/src/unitree_rl/library/unitree_legged_sdk_3.2/lib/libunitree_legged_sdk_arm32.so b/src/rl_sar/library/unitree_legged_sdk_3.2/lib/libunitree_legged_sdk_arm32.so similarity index 100% rename from src/unitree_rl/library/unitree_legged_sdk_3.2/lib/libunitree_legged_sdk_arm32.so rename to src/rl_sar/library/unitree_legged_sdk_3.2/lib/libunitree_legged_sdk_arm32.so diff --git a/src/unitree_rl/library/unitree_legged_sdk_3.2/lib/libunitree_legged_sdk_arm64.so b/src/rl_sar/library/unitree_legged_sdk_3.2/lib/libunitree_legged_sdk_arm64.so similarity index 100% rename from src/unitree_rl/library/unitree_legged_sdk_3.2/lib/libunitree_legged_sdk_arm64.so rename to src/rl_sar/library/unitree_legged_sdk_3.2/lib/libunitree_legged_sdk_arm64.so diff --git a/src/unitree_rl/models/actor.pt b/src/rl_sar/models/actor.pt similarity index 100% rename from src/unitree_rl/models/actor.pt rename to src/rl_sar/models/actor.pt diff --git a/src/unitree_rl/models/encoder.pt b/src/rl_sar/models/encoder.pt similarity index 100% rename from src/unitree_rl/models/encoder.pt rename to src/rl_sar/models/encoder.pt diff --git a/src/unitree_rl/models/vq_layer.pt b/src/rl_sar/models/vq_layer.pt similarity index 100% rename from src/unitree_rl/models/vq_layer.pt rename to src/rl_sar/models/vq_layer.pt diff --git a/src/unitree_rl/package.xml b/src/rl_sar/package.xml similarity index 90% rename from src/unitree_rl/package.xml rename to src/rl_sar/package.xml index d5cbbad..3a738ae 100644 --- a/src/unitree_rl/package.xml +++ b/src/rl_sar/package.xml @@ -1,8 +1,8 @@ - unitree_rl - 0.0.0 - The unitree_rl package + rl_sar + 2.0.0 + The rl_sar package Ziqi Fan diff --git a/src/unitree_rl/src/unitree_rl_real.cpp b/src/rl_sar/src/rl_real.cpp similarity index 88% rename from src/unitree_rl/src/unitree_rl_real.cpp rename to src/rl_sar/src/rl_real.cpp index 397d1d7..2991989 100644 --- a/src/unitree_rl/src/unitree_rl_real.cpp +++ b/src/rl_sar/src/rl_real.cpp @@ -1,17 +1,16 @@ -#include "../include/unitree_rl_real.hpp" -#include +#include "../include/rl_real.hpp" -void Unitree_RL::UDPRecv() +void RL_Real::UDPRecv() { udp.Recv(); } -void Unitree_RL::UDPSend() +void RL_Real::UDPSend() { udp.Send(); } -void Unitree_RL::RobotControl() +void RL_Real::RobotControl() { motiontime++; udp.GetRecv(state); @@ -82,20 +81,17 @@ void Unitree_RL::RobotControl() udp.SetSend(cmd); } -Unitree_RL::Unitree_RL() : safe(LeggedType::A1), udp(LOWLEVEL) +RL_Real::RL_Real() : safe(LeggedType::A1), udp(LOWLEVEL) { udp.InitCmdData(cmd); start_time = std::chrono::high_resolution_clock::now(); - cmd_vel = geometry_msgs::Twist(); - torque_commands.resize(12); - std::string package_name = "unitree_rl"; - std::string actor_path = ros::package::getPath(package_name) + "/models/actor.pt"; - std::string encoder_path = ros::package::getPath(package_name) + "/models/encoder.pt"; - std::string vq_path = ros::package::getPath(package_name) + "/models/vq_layer.pt"; + std::string actor_path = std::string(CMAKE_CURRENT_SOURCE_DIR) + "/models/actor.pt"; + std::string encoder_path = std::string(CMAKE_CURRENT_SOURCE_DIR) + "/models/encoder.pt"; + std::string vq_path = std::string(CMAKE_CURRENT_SOURCE_DIR) + "/models/vq_layer.pt"; this->actor = torch::jit::load(actor_path); this->encoder = torch::jit::load(encoder_path); @@ -133,23 +129,18 @@ Unitree_RL::Unitree_RL() : safe(LeggedType::A1), udp(LOWLEVEL) this->history_obs_buf = ObservationBuffer(1, this->params.num_observations, 6); // InitEnvironment(); - loop_control = std::make_shared("control_loop", 0.02 , boost::bind(&Unitree_RL::RobotControl, this)); - loop_udpSend = std::make_shared("udp_send" , 0.002, 3, boost::bind(&Unitree_RL::UDPSend, this)); - loop_udpRecv = std::make_shared("udp_recv" , 0.002, 3, boost::bind(&Unitree_RL::UDPRecv, this)); - loop_rl = std::make_shared("rl_loop" , 0.02 , boost::bind(&Unitree_RL::runModel, this)); + loop_control = std::make_shared("control_loop", 0.02 , boost::bind(&RL_Real::RobotControl, this)); + loop_udpSend = std::make_shared("udp_send" , 0.002, 3, boost::bind(&RL_Real::UDPSend, this)); + loop_udpRecv = std::make_shared("udp_recv" , 0.002, 3, boost::bind(&RL_Real::UDPRecv, this)); + loop_rl = std::make_shared("rl_loop" , 0.02 , boost::bind(&RL_Real::runModel, this)); loop_udpSend->start(); loop_udpRecv->start(); loop_control->start(); } -void Unitree_RL::cmdvelCallback(const geometry_msgs::Twist::ConstPtr &msg) -{ - cmd_vel = *msg; -} - -// void Unitree_RL::runModel(const ros::TimerEvent &event) -void Unitree_RL::runModel() +// void RL_Real::runModel(const ros::TimerEvent &event) +void RL_Real::runModel() { if(init_done) { @@ -163,7 +154,7 @@ void Unitree_RL::runModel() // printf("%f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f\n", state.motorState[FL_0].dq, state.motorState[FL_1].dq, state.motorState[FL_2].dq, state.motorState[FR_0].dq, state.motorState[FR_1].dq, state.motorState[FR_2].dq, state.motorState[RL_0].dq, state.motorState[RL_1].dq, state.motorState[RL_2].dq, state.motorState[RR_0].dq, state.motorState[RR_1].dq, state.motorState[RR_2].dq); this->obs.ang_vel = torch::tensor({{state.imu.gyroscope[0], state.imu.gyroscope[1], state.imu.gyroscope[2]}}); - this->obs.commands = torch::tensor({{cmd_vel.linear.x, cmd_vel.linear.y, cmd_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({{state.imu.quaternion[1], state.imu.quaternion[2], state.imu.quaternion[3], state.imu.quaternion[0]}}); this->obs.dof_pos = torch::tensor({{state.motorState[FR_0].q, state.motorState[FR_1].q, state.motorState[FR_2].q, state.motorState[FL_0].q, state.motorState[FL_1].q, state.motorState[FL_2].q, @@ -180,7 +171,7 @@ void Unitree_RL::runModel() } -torch::Tensor Unitree_RL::compute_observation() +torch::Tensor RL_Real::compute_observation() { torch::Tensor ang_vel = this->quat_rotate_inverse(this->obs.base_quat, this->obs.ang_vel); // float ang_vel_temp = ang_vel[0][0].item(); @@ -204,7 +195,7 @@ torch::Tensor Unitree_RL::compute_observation() return obs; } -torch::Tensor Unitree_RL::forward() +torch::Tensor RL_Real::forward() { torch::Tensor obs = this->compute_observation(); @@ -229,7 +220,7 @@ torch::Tensor Unitree_RL::forward() int main(int argc, char **argv) { - Unitree_RL unitree_rl; + RL_Real rl_sar; while(1){ sleep(10); diff --git a/src/unitree_rl/src/unitree_rl.cpp b/src/rl_sar/src/rl_sim.cpp similarity index 84% rename from src/unitree_rl/src/unitree_rl.cpp rename to src/rl_sar/src/rl_sim.cpp index 2f34a44..b14d335 100644 --- a/src/unitree_rl/src/unitree_rl.cpp +++ b/src/rl_sar/src/rl_sim.cpp @@ -1,7 +1,7 @@ -#include "../include/unitree_rl.hpp" +#include "../include/rl_sim.hpp" #include -Unitree_RL::Unitree_RL() +RL_Sim::RL_Sim() { ros::NodeHandle nh; start_time = std::chrono::high_resolution_clock::now(); @@ -10,10 +10,9 @@ Unitree_RL::Unitree_RL() torque_commands.resize(12); - std::string package_name = "unitree_rl"; - std::string actor_path = ros::package::getPath(package_name) + "/models/actor.pt"; - std::string encoder_path = ros::package::getPath(package_name) + "/models/encoder.pt"; - std::string vq_path = ros::package::getPath(package_name) + "/models/vq_layer.pt"; + std::string actor_path = std::string(CMAKE_CURRENT_SOURCE_DIR) + "/models/actor.pt"; + std::string encoder_path = std::string(CMAKE_CURRENT_SOURCE_DIR) + "/models/encoder.pt"; + std::string vq_path = std::string(CMAKE_CURRENT_SOURCE_DIR) + "/models/vq_layer.pt"; this->actor = torch::jit::load(actor_path); this->encoder = torch::jit::load(encoder_path); @@ -51,9 +50,9 @@ Unitree_RL::Unitree_RL() this->history_obs_buf = ObservationBuffer(1, this->params.num_observations, 6); cmd_vel_subscriber_ = nh.subscribe( - "/cmd_vel", 10, &Unitree_RL::cmdvelCallback, this); + "/cmd_vel", 10, &RL_Sim::cmdvelCallback, this); - timer = nh.createTimer(ros::Duration(0.005), &Unitree_RL::runModel, this); + timer = nh.createTimer(ros::Duration(0.005), &RL_Sim::runModel, this); ros_namespace = "/a1_gazebo/"; @@ -71,31 +70,31 @@ Unitree_RL::Unitree_RL() } model_state_subscriber_ = nh.subscribe( - "/gazebo/model_states", 10, &Unitree_RL::modelStatesCallback, this); + "/gazebo/model_states", 10, &RL_Sim::modelStatesCallback, this); joint_state_subscriber_ = nh.subscribe( - "/a1_gazebo/joint_states", 10, &Unitree_RL::jointStatesCallback, this); + "/a1_gazebo/joint_states", 10, &RL_Sim::jointStatesCallback, this); } -void Unitree_RL::modelStatesCallback(const gazebo_msgs::ModelStates::ConstPtr &msg) +void RL_Sim::modelStatesCallback(const gazebo_msgs::ModelStates::ConstPtr &msg) { vel = msg->twist[2]; pose = msg->pose[2]; } -void Unitree_RL::cmdvelCallback(const geometry_msgs::Twist::ConstPtr &msg) +void RL_Sim::cmdvelCallback(const geometry_msgs::Twist::ConstPtr &msg) { cmd_vel = *msg; } -void Unitree_RL::jointStatesCallback(const sensor_msgs::JointState::ConstPtr &msg) +void RL_Sim::jointStatesCallback(const sensor_msgs::JointState::ConstPtr &msg) { joint_positions = msg->position; joint_velocities = msg->velocity; } -void Unitree_RL::runModel(const ros::TimerEvent &event) +void RL_Sim::runModel(const ros::TimerEvent &event) { auto duration = std::chrono::duration_cast(std::chrono::high_resolution_clock::now() - start_time).count(); // std::cout << "Execution time: " << duration << " microseconds" << std::endl; @@ -126,7 +125,7 @@ void Unitree_RL::runModel(const ros::TimerEvent &event) } } -torch::Tensor Unitree_RL::compute_observation() +torch::Tensor RL_Sim::compute_observation() { torch::Tensor obs = torch::cat({// (this->quat_rotate_inverse(this->base_quat, this->lin_vel)) * this->params.lin_vel_scale, (this->quat_rotate_inverse(this->obs.base_quat, this->obs.ang_vel)) * this->params.ang_vel_scale, @@ -140,7 +139,7 @@ torch::Tensor Unitree_RL::compute_observation() return obs; } -torch::Tensor Unitree_RL::forward() +torch::Tensor RL_Sim::forward() { torch::Tensor obs = this->compute_observation(); @@ -163,8 +162,8 @@ torch::Tensor Unitree_RL::forward() int main(int argc, char **argv) { - ros::init(argc, argv, "unitree_rl"); - Unitree_RL unitree_rl; + ros::init(argc, argv, "rl_sar"); + RL_Sim rl_sar; ros::spin(); return 0; } diff --git a/src/unitree_rl/worlds/stairs.world b/src/rl_sar/worlds/stairs.world similarity index 100% rename from src/unitree_rl/worlds/stairs.world rename to src/rl_sar/worlds/stairs.world