EmbodiedAI
/
rl_sar
mirror of https://github.com/fan-ziqi/rl_sar.git
3
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

feat: yaml-cpp

This commit is contained in:
fan-ziqi 2024-04-05 21:37:25 +08:00
parent f6fe3071d6
commit a0f7b07c4c
15 changed files with 151 additions and 110 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
.gitignore vendored
View File

@ -3,5 +3,3 @@ devel
logs
.catkin_tools
.vscode
old
*.pt

14
README_CN.md
View File

@ -34,6 +34,20 @@ echo 'export Torch_DIR=/path/to/your/torchlib' >> ~/.bashrc
sudo apt install ros-noetic-teleop-twist-keyboard
```
## 依赖
安装yaml-cpp
```bash
git clone https://github.com/jbeder/yaml-cpp.git
cd yaml-cpp && mkdir build && cd build
cmake -DYAML_BUILD_SHARED_LIBS=on .. && make
sudo make install
sudo ldconfig
```
头文件在/usr/local/include，库文件在/usr/local/lib
## 编译
自定义代码中的以下两个函数，以适配不同的模型：

19
src/rl_sar/CMakeLists.txt
View File

@ -27,6 +27,9 @@ find_package(catkin REQUIRED COMPONENTS
find_package(Python3 COMPONENTS Interpreter Development REQUIRED)
find_package(yaml-cpp REQUIRED)
include_directories(${YAML_CPP_INCLUDE_DIR})
catkin_package(
CATKIN_DEPENDS
unitree_legged_msgs
@ -36,6 +39,9 @@ include_directories(library/unitree_legged_sdk_3.2/include)
link_directories(library/unitree_legged_sdk_3.2/lib)
set(EXTRA_LIBS -pthread libunitree_legged_sdk_amd64.so lcm)
add_subdirectory(library/cyberdog_motor_sdk)
include_directories(library/cyberdog_motor_sdk/include)
include_directories(
include
${catkin_INCLUDE_DIRS}
@ -61,19 +67,18 @@ set_property(TARGET observation_buffer PROPERTY CXX_STANDARD 14)
add_executable(rl_sim src/rl_sim.cpp )
target_link_libraries(rl_sim
${catkin_LIBRARIES} ${EXTRA_LIBS} "${TORCH_LIBRARIES}"
rl observation_buffer
rl observation_buffer yaml-cpp
)
add_executable(rl_real src/rl_real.cpp )
target_link_libraries(rl_real
add_executable(rl_real_a1 src/rl_real_a1.cpp )
target_link_libraries(rl_real_a1
${catkin_LIBRARIES} ${EXTRA_LIBS} "${TORCH_LIBRARIES}"
rl observation_buffer
rl observation_buffer yaml-cpp
)
add_subdirectory(library/cyberdog_motor_sdk)
include_directories(library/cyberdog_motor_sdk/include)
add_executable(rl_real_cyberdog src/rl_real_cyberdog.cpp )
target_link_libraries(rl_real_cyberdog
${catkin_LIBRARIES} ${EXTRA_LIBS} "${TORCH_LIBRARIES}"
rl observation_buffer cyberdog_motor_sdk
rl observation_buffer cyberdog_motor_sdk yaml-cpp
)

45
src/rl_sar/config.yaml Normal file
View File

@ -0,0 +1,45 @@
a1:
num_observations: 45
clip_obs: 100.0
clip_actions: 100.0
damping: 0.5
stiffness: 20.0
action_scale: 0.25
hip_scale_reduction: 0.5
num_of_dofs: 12
lin_vel_scale: 2.0
ang_vel_scale: 0.25
dof_pos_scale: 1.0
dof_vel_scale: 0.05
torque_limits: [20.0, 55.0, 55.0,
20.0, 55.0, 55.0,
20.0, 55.0, 55.0,
20.0, 55.0, 55.0]
# hip, thigh, calf
default_dof_pos: [ 0.1000, 0.8000, -1.5000, # FL
-0.1000, 0.8000, -1.5000, # FR
0.1000, 1.0000, -1.5000, # RR
-0.1000, 1.0000, -1.5000] # RL
cyberdog1:
num_observations: 45
clip_obs: 100.0
clip_actions: 100.0
damping: 0.5
stiffness: 20.0
action_scale: 0.25
hip_scale_reduction: 0.5
num_of_dofs: 12
lin_vel_scale: 2.0
ang_vel_scale: 0.25
dof_pos_scale: 1.0
dof_vel_scale: 0.05
torque_limits: [24.0, 24.0, 24.0,
24.0, 24.0, 24.0,
24.0, 24.0, 24.0,
24.0, 24.0, 24.0]
# hip, thigh, calf
default_dof_pos: [ 0.1000, 0.8000, -1.5000, # FL
-0.1000, 0.8000, -1.5000, # FR
0.1000, 1.0000, -1.5000, # RR
-0.1000, 1.0000, -1.5000] # RL

46
src/rl_sar/library/rl/rl.cpp
View File

@ -1,5 +1,45 @@
#include "rl.hpp"
void RL::ReadYaml(std::string robot_name)
{
YAML::Node config;
try
{
config = YAML::LoadFile(CONFIG_PATH)[robot_name];
} catch(YAML::BadFile &e)
{
std::cout << "The file '" << CONFIG_PATH << "' does not exist" << std::endl;
return;
}
this->params.num_observations = config["num_observations"].as<int>();
this->params.clip_obs = config["clip_obs"].as<float>();
this->params.clip_actions = config["clip_actions"].as<float>();
this->params.damping = config["damping"].as<float>();
this->params.stiffness = config["stiffness"].as<float>();
this->params.d_gains = torch::ones(12) * this->params.damping;
this->params.p_gains = torch::ones(12) * this->params.stiffness;
this->params.action_scale = config["action_scale"].as<float>();
this->params.hip_scale_reduction = config["hip_scale_reduction"].as<float>();
this->params.num_of_dofs = config["num_of_dofs"].as<int>();
this->params.lin_vel_scale = config["lin_vel_scale"].as<float>();
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({this->params.lin_vel_scale, this->params.lin_vel_scale, this->params.ang_vel_scale});
this->params.torque_limits = torch::tensor({{config["torque_limits"][0].as<float>(), config["torque_limits"][1].as<float>(), config["torque_limits"][2].as<float>(),
config["torque_limits"][3].as<float>(), config["torque_limits"][4].as<float>(), config["torque_limits"][5].as<float>(),
config["torque_limits"][6].as<float>(), config["torque_limits"][7].as<float>(), config["torque_limits"][8].as<float>(),
config["torque_limits"][9].as<float>(), config["torque_limits"][10].as<float>(), config["torque_limits"][11].as<float>()}});
this->params.default_dof_pos = torch::tensor({{config["default_dof_pos"][0].as<float>(), config["default_dof_pos"][1].as<float>(), config["default_dof_pos"][2].as<float>(),
config["default_dof_pos"][3].as<float>(), config["default_dof_pos"][4].as<float>(), config["default_dof_pos"][5].as<float>(),
config["default_dof_pos"][6].as<float>(), config["default_dof_pos"][7].as<float>(), config["default_dof_pos"][8].as<float>(),
config["default_dof_pos"][9].as<float>(), config["default_dof_pos"][10].as<float>(), config["default_dof_pos"][11].as<float>()}});
}
torch::Tensor RL::QuatRotateInverse(torch::Tensor q, torch::Tensor v)
{
c10::IntArrayRef shape = q.sizes();
@ -23,6 +63,12 @@ void RL::InitObservations()
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}});
}
void RL::InitOutputs()
{
output_torques = 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}});
output_dof_pos = params.default_dof_pos;
}
torch::Tensor RL::ComputeTorques(torch::Tensor actions)
{
torch::Tensor actions_scaled = actions * this->params.action_scale;

7
src/rl_sar/library/rl/rl.hpp
View File

@ -8,6 +8,9 @@
#include "matplotlibcpp.h"
namespace plt = matplotlibcpp;
#include <yaml-cpp/yaml.h>
#define CONFIG_PATH CMAKE_CURRENT_SOURCE_DIR "/config.yaml"
struct ModelParams
{
int num_observations;
@ -15,7 +18,7 @@ struct ModelParams
float stiffness;
float action_scale;
float hip_scale_reduction;
float num_of_dofs;
int num_of_dofs;
float lin_vel_scale;
float ang_vel_scale;
float dof_pos_scale;
@ -55,6 +58,8 @@ public:
torch::Tensor ComputePosition(torch::Tensor actions);
torch::Tensor QuatRotateInverse(torch::Tensor q, torch::Tensor v);
void InitObservations();
void InitOutputs();
void ReadYaml(std::string robot_name);
protected:
// rl module

BIN
src/rl_sar/models/a1/actor.pt Normal file
View File

Binary file not shown.

BIN
src/rl_sar/models/a1/encoder.pt Normal file
View File

Binary file not shown.

BIN
src/rl_sar/models/a1/vq_layer.pt Normal file
View File

Binary file not shown.

0
src/rl_sar/models/actor.pt → src/rl_sar/models/cyberdog1/actor.pt
View File

0
src/rl_sar/models/encoder.pt → src/rl_sar/models/cyberdog1/encoder.pt
View File

0
src/rl_sar/models/vq_layer.pt → src/rl_sar/models/cyberdog1/vq_layer.pt
View File

40
src/rl_sar/src/rl_real.cpp → src/rl_sar/src/rl_real_a1.cpp
View File

@ -1,55 +1,31 @@
#include "../include/rl_real.hpp"
#define ROBOT_NAME "a1"
// #define PLOT
RL_Real rl_sar;
RL_Real::RL_Real() : safe(LeggedType::A1), udp(LOWLEVEL)
{
ReadYaml(ROBOT_NAME);
udp.InitCmdData(cmd);
start_time = std::chrono::high_resolution_clock::now();
std::string actor_path = std::string(CMAKE_CURRENT_SOURCE_DIR) + "/models/base/actor.pt";
std::string encoder_path = std::string(CMAKE_CURRENT_SOURCE_DIR) + "/models/base/encoder.pt";
std::string vq_path = std::string(CMAKE_CURRENT_SOURCE_DIR) + "/models/base/vq_layer.pt";
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";
this->actor = torch::jit::load(actor_path);
this->encoder = torch::jit::load(encoder_path);
this->vq = torch::jit::load(vq_path);
this->InitObservations();
this->params.num_observations = 45;
this->params.clip_obs = 100.0;
this->params.clip_actions = 100.0;
this->params.damping = 0.5;
this->params.stiffness = 20;
this->params.d_gains = torch::ones(12) * this->params.damping;
this->params.p_gains = torch::ones(12) * this->params.stiffness;
this->params.action_scale = 0.25;
this->params.hip_scale_reduction = 0.5;
this->params.num_of_dofs = 12;
this->params.lin_vel_scale = 2.0;
this->params.ang_vel_scale = 0.25;
this->params.dof_pos_scale = 1.0;
this->params.dof_vel_scale = 0.05;
this->params.commands_scale = torch::tensor({this->params.lin_vel_scale, this->params.lin_vel_scale, this->params.ang_vel_scale});
this->params.torque_limits = torch::tensor({{20.0, 55.0, 55.0,
20.0, 55.0, 55.0,
20.0, 55.0, 55.0,
20.0, 55.0, 55.0}});
// hip, thigh, calf
this->params.default_dof_pos = torch::tensor({{ 0.1000, 0.8000, -1.5000, // FL
-0.1000, 0.8000, -1.5000, // FR
0.1000, 1.0000, -1.5000, // RR
-0.1000, 1.0000, -1.5000}});// RL
this->InitOutputs();
this->history_obs_buf = ObservationBuffer(1, this->params.num_observations, 6);
output_torques = 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}});
output_dof_pos = params.default_dof_pos;
plot_real_joint_pos.resize(12);
plot_target_joint_pos.resize(12);

42
src/rl_sar/src/rl_real_cyberdog.cpp
View File

@ -1,53 +1,29 @@
#include "../include/rl_real_cyberdog.hpp"
#define PLOT
#define ROBOT_NAME "cyberdog1"
// #define PLOT
RL_Real rl_sar;
RL_Real::RL_Real() : CustomInterface(500)
{
ReadYaml(ROBOT_NAME);
start_time = std::chrono::high_resolution_clock::now();
std::string actor_path = std::string(CMAKE_CURRENT_SOURCE_DIR) + "/models/cyberdog/actor.pt";
std::string encoder_path = std::string(CMAKE_CURRENT_SOURCE_DIR) + "/models/cyberdog/encoder.pt";
std::string vq_path = std::string(CMAKE_CURRENT_SOURCE_DIR) + "/models/cyberdog/vq_layer.pt";
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";
this->actor = torch::jit::load(actor_path);
this->encoder = torch::jit::load(encoder_path);
this->vq = torch::jit::load(vq_path);
this->InitObservations();
this->params.num_observations = 45;
this->params.clip_obs = 100.0;
this->params.clip_actions = 100.0;
this->params.damping = 0.5;
this->params.stiffness = 20;
this->params.d_gains = torch::ones(12) * this->params.damping;
this->params.p_gains = torch::ones(12) * this->params.stiffness;
this->params.action_scale = 0.25;
this->params.hip_scale_reduction = 0.5;
this->params.num_of_dofs = 12;
this->params.lin_vel_scale = 2.0;
this->params.ang_vel_scale = 0.25;
this->params.dof_pos_scale = 1.0;
this->params.dof_vel_scale = 0.05;
this->params.commands_scale = torch::tensor({this->params.lin_vel_scale, this->params.lin_vel_scale, this->params.ang_vel_scale});
this->params.torque_limits = torch::tensor({{24.0, 24.0, 24.0,
24.0, 24.0, 24.0,
24.0, 24.0, 24.0,
24.0, 24.0, 24.0}});
// hip, thigh, calf
this->params.default_dof_pos = torch::tensor({{ 0.1000, 0.8000, -1.5000, // FL
-0.1000, 0.8000, -1.5000, // FR
0.1000, 1.0000, -1.5000, // RR
-0.1000, 1.0000, -1.5000}});// RL
this->InitOutputs();
this->history_obs_buf = ObservationBuffer(1, this->params.num_observations, 6);
output_torques = 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}});
output_dof_pos = params.default_dof_pos;
plot_real_joint_pos.resize(12);
plot_target_joint_pos.resize(12);

40
src/rl_sar/src/rl_sim.cpp
View File

@ -1,9 +1,13 @@
#include "../include/rl_sim.hpp"
#define ROBOT_NAME "cyberdog1"
// #define PLOT
RL_Sim::RL_Sim()
{
ReadYaml(ROBOT_NAME);
ros::NodeHandle nh;
start_time = std::chrono::high_resolution_clock::now();
@ -11,46 +15,18 @@ RL_Sim::RL_Sim()
motor_commands.resize(12);
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";
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";
this->actor = torch::jit::load(actor_path);
this->encoder = torch::jit::load(encoder_path);
this->vq = torch::jit::load(vq_path);
this->InitObservations();
this->params.num_observations = 45;
this->params.clip_obs = 100.0;
this->params.clip_actions = 100.0;
this->params.damping = 0.5;
this->params.stiffness = 20;
this->params.d_gains = torch::ones(12) * this->params.damping;
this->params.p_gains = torch::ones(12) * this->params.stiffness;
this->params.action_scale = 0.25;
this->params.hip_scale_reduction = 0.5;
this->params.num_of_dofs = 12;
this->params.lin_vel_scale = 2.0;
this->params.ang_vel_scale = 0.25;
this->params.dof_pos_scale = 1.0;
this->params.dof_vel_scale = 0.05;
this->params.commands_scale = torch::tensor({this->params.lin_vel_scale, this->params.lin_vel_scale, this->params.ang_vel_scale});
this->params.torque_limits = torch::tensor({{20.0, 55.0, 55.0,
20.0, 55.0, 55.0,
20.0, 55.0, 55.0,
20.0, 55.0, 55.0}});
// hip, thigh, calf
this->params.default_dof_pos = torch::tensor({{ 0.1000, 0.8000, -1.5000, // FL
-0.1000, 0.8000, -1.5000, // FR
0.1000, 1.0000, -1.5000, // RR
-0.1000, 1.0000, -1.5000}});// RL
this->InitOutputs();
this->history_obs_buf = ObservationBuffer(1, this->params.num_observations, 6);
output_torques = 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}});
output_dof_pos = params.default_dof_pos;
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};
plot_real_joint_pos.resize(12);