From cdece56ec9098898e1c27a7814f749133649fb0a Mon Sep 17 00:00:00 2001 From: fan-ziqi Date: Thu, 2 May 2024 12:17:15 +0800 Subject: [PATCH] feat: update plot funtion --- src/rl_sar/CMakeLists.txt | 20 ++++++++++--------- src/rl_sar/include/rl_real_a1.hpp | 8 +++++--- src/rl_sar/include/rl_real_cyberdog.hpp | 7 ++++--- src/rl_sar/include/rl_sim.hpp | 7 ++++--- .../library/{rl/rl.cpp => rl_sdk/rl_sdk.cpp} | 2 +- .../library/{rl/rl.hpp => rl_sdk/rl_sdk.hpp} | 6 +++--- src/rl_sar/src/rl_real_a1.cpp | 8 +++++++- src/rl_sar/src/rl_real_cyberdog.cpp | 8 +++++++- src/rl_sar/src/rl_sim.cpp | 9 ++++++++- 9 files changed, 50 insertions(+), 25 deletions(-) rename src/rl_sar/library/{rl/rl.cpp => rl_sdk/rl_sdk.cpp} (99%) rename src/rl_sar/library/{rl/rl.hpp => rl_sdk/rl_sdk.hpp} (97%) diff --git a/src/rl_sar/CMakeLists.txt b/src/rl_sar/CMakeLists.txt index ee38d39..d7dd6ff 100644 --- a/src/rl_sar/CMakeLists.txt +++ b/src/rl_sar/CMakeLists.txt @@ -49,37 +49,39 @@ include_directories( ${unitree_legged_sdk_INCLUDE_DIRS} ../unitree_controller/include library/matplotlibcpp + library/observation_buffer + library/rl_sdk ) -add_library(rl library/rl/rl.cpp library/rl/rl.hpp) -target_link_libraries(rl "${TORCH_LIBRARIES}" Python3::Python Python3::Module) -set_property(TARGET rl PROPERTY CXX_STANDARD 14) +add_library(rl_sdk library/rl_sdk/rl_sdk.cpp) +target_link_libraries(rl_sdk "${TORCH_LIBRARIES}" Python3::Python Python3::Module) +set_property(TARGET rl_sdk PROPERTY CXX_STANDARD 14) find_package(Python3 COMPONENTS NumPy) if(Python3_NumPy_FOUND) - target_link_libraries(rl Python3::NumPy) + target_link_libraries(rl_sdk Python3::NumPy) else() - target_compile_definitions(rl WITHOUT_NUMPY) + target_compile_definitions(rl_sdk WITHOUT_NUMPY) endif() -add_library(observation_buffer library/observation_buffer/observation_buffer.cpp library/observation_buffer/observation_buffer.hpp) +add_library(observation_buffer library/observation_buffer/observation_buffer.cpp) target_link_libraries(observation_buffer "${TORCH_LIBRARIES}") 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 yaml-cpp + rl_sdk observation_buffer yaml-cpp ) 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 yaml-cpp + rl_sdk observation_buffer yaml-cpp ) 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 yaml-cpp + rl_sdk observation_buffer cyberdog_motor_sdk yaml-cpp ) \ No newline at end of file diff --git a/src/rl_sar/include/rl_real_a1.hpp b/src/rl_sar/include/rl_real_a1.hpp index 55c0d4d..f7bbb81 100644 --- a/src/rl_sar/include/rl_real_a1.hpp +++ b/src/rl_sar/include/rl_real_a1.hpp @@ -1,8 +1,9 @@ #ifndef RL_REAL_HPP #define RL_REAL_HPP -#include "../library/rl/rl.hpp" -#include "../library/observation_buffer/observation_buffer.hpp" +// #include "../library/rl_sdk/rl_sdk.hpp" +#include "rl_sdk.hpp" +#include "observation_buffer.hpp" #include #include "unitree_legged_msgs/LowState.h" #include @@ -59,7 +60,8 @@ public: int robot_state = STATE_WAITING; - std::vector plot_t; + const int plot_size = 100; + std::vector plot_t; std::vector> plot_real_joint_pos, plot_target_joint_pos; void Plot(); private: diff --git a/src/rl_sar/include/rl_real_cyberdog.hpp b/src/rl_sar/include/rl_real_cyberdog.hpp index 44e2520..64cb30c 100644 --- a/src/rl_sar/include/rl_real_cyberdog.hpp +++ b/src/rl_sar/include/rl_real_cyberdog.hpp @@ -1,8 +1,8 @@ #ifndef RL_REAL_HPP #define RL_REAL_HPP -#include "../library/rl/rl.hpp" -#include "../library/observation_buffer/observation_buffer.hpp" +#include "rl_sdk.hpp" +#include "observation_buffer.hpp" #include "unitree_legged_sdk/loop.h" #include #include @@ -65,7 +65,8 @@ public: int robot_state = STATE_WAITING; - std::vector plot_t; + const int plot_size = 100; + std::vector plot_t; std::vector> plot_real_joint_pos, plot_target_joint_pos; void Plot(); diff --git a/src/rl_sar/include/rl_sim.hpp b/src/rl_sar/include/rl_sim.hpp index 7d1be50..f0161ad 100644 --- a/src/rl_sar/include/rl_sim.hpp +++ b/src/rl_sar/include/rl_sim.hpp @@ -1,8 +1,8 @@ #ifndef RL_SIM_HPP #define RL_SIM_HPP -#include "../library/rl/rl.hpp" -#include "../library/observation_buffer/observation_buffer.hpp" +#include "rl_sdk.hpp" +#include "observation_buffer.hpp" #include #include #include @@ -37,7 +37,8 @@ public: std::shared_ptr loop_rl; std::shared_ptr loop_plot; - std::vector plot_t; + const int plot_size = 100; + std::vector plot_t; std::vector> plot_real_joint_pos, plot_target_joint_pos; void Plot(); private: diff --git a/src/rl_sar/library/rl/rl.cpp b/src/rl_sar/library/rl_sdk/rl_sdk.cpp similarity index 99% rename from src/rl_sar/library/rl/rl.cpp rename to src/rl_sar/library/rl_sdk/rl_sdk.cpp index fd931ea..ce77a4f 100644 --- a/src/rl_sar/library/rl/rl.cpp +++ b/src/rl_sar/library/rl_sdk/rl_sdk.cpp @@ -1,4 +1,4 @@ -#include "rl.hpp" +#include "rl_sdk.hpp" torch::Tensor ReadTensorFromYaml(const YAML::Node& node) { diff --git a/src/rl_sar/library/rl/rl.hpp b/src/rl_sar/library/rl_sdk/rl_sdk.hpp similarity index 97% rename from src/rl_sar/library/rl/rl.hpp rename to src/rl_sar/library/rl_sdk/rl_sdk.hpp index b68a484..2df07db 100644 --- a/src/rl_sar/library/rl/rl.hpp +++ b/src/rl_sar/library/rl_sdk/rl_sdk.hpp @@ -1,5 +1,5 @@ -#ifndef RL_HPP -#define RL_HPP +#ifndef RL_SDK_HPP +#define RL_SDK_HPP #include #include @@ -83,4 +83,4 @@ protected: torch::Tensor output_dof_pos; }; -#endif // RL_HPP \ No newline at end of file +#endif // RL_SDK_HPP \ No newline at end of file diff --git a/src/rl_sar/src/rl_real_a1.cpp b/src/rl_sar/src/rl_real_a1.cpp index 02129be..cc0445e 100644 --- a/src/rl_sar/src/rl_real_a1.cpp +++ b/src/rl_sar/src/rl_real_a1.cpp @@ -25,8 +25,11 @@ 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); + 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); } loop_control = std::make_shared("loop_control", 0.002, boost::bind(&RL_Real::RobotControl, this)); loop_udpSend = std::make_shared("loop_udpSend", 0.002, 3, boost::bind(&RL_Real::UDPSend, this)); @@ -271,17 +274,20 @@ torch::Tensor RL_Real::Forward() void RL_Real::Plot() { + plot_t.erase(plot_t.begin()); plot_t.push_back(motiontime); plt::cla(); plt::clf(); for(int i = 0; i < 12; ++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(state.motorState[i].q); plot_target_joint_pos[i].push_back(cmd.motorCmd[i].q); plt::subplot(4, 3, i + 1); plt::named_plot("_real_joint_pos", plot_t, plot_real_joint_pos[i], "r"); plt::named_plot("_target_joint_pos", plot_t, plot_target_joint_pos[i], "b"); - plt::xlim(motiontime - 10000, motiontime); + plt::xlim(plot_t.front(), plot_t.back()); } // plt::legend(); plt::pause(0.0001); diff --git a/src/rl_sar/src/rl_real_cyberdog.cpp b/src/rl_sar/src/rl_real_cyberdog.cpp index 65f7e76..fc42e36 100644 --- a/src/rl_sar/src/rl_real_cyberdog.cpp +++ b/src/rl_sar/src/rl_real_cyberdog.cpp @@ -23,8 +23,11 @@ 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); + 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); } loop_control = std::make_shared("loop_control", 0.002, boost::bind(&RL_Real::RobotControl, this)); loop_rl = std::make_shared("loop_rl" , 0.02 , boost::bind(&RL_Real::RunModel, this)); @@ -318,17 +321,20 @@ torch::Tensor RL_Real::Forward() void RL_Real::Plot() { + plot_t.erase(plot_t.begin()); plot_t.push_back(motiontime); plt::cla(); plt::clf(); for(int i = 0; i < 12; ++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(cyberdogData.q[i]); plot_target_joint_pos[i].push_back(cyberdogCmd.q_des[i]); plt::subplot(4, 3, i+1); plt::named_plot("_real_joint_pos", plot_t, plot_real_joint_pos[i], "r"); plt::named_plot("_target_joint_pos", plot_t, plot_target_joint_pos[i], "b"); - plt::xlim(motiontime-10000, motiontime); + plt::xlim(plot_t.front(), plot_t.back()); } // plt::legend(); plt::pause(0.0001); diff --git a/src/rl_sar/src/rl_sim.cpp b/src/rl_sar/src/rl_sim.cpp index 58d094c..61a9244 100644 --- a/src/rl_sar/src/rl_sim.cpp +++ b/src/rl_sar/src/rl_sim.cpp @@ -38,8 +38,12 @@ RL_Sim::RL_Sim() 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}; + + plot_t = std::vector(plot_size, 0); plot_real_joint_pos.resize(12); plot_target_joint_pos.resize(12); + 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); } cmd_vel_subscriber_ = nh.subscribe("/cmd_vel", 10, &RL_Sim::CmdvelCallback, this); @@ -194,17 +198,20 @@ 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) { + 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_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"); plt::named_plot("_target_joint_pos", plot_t, plot_target_joint_pos[i], "b"); - plt::xlim(motiontime-10000, motiontime); + plt::xlim(plot_t.front(), plot_t.back()); } // plt::legend(); plt::pause(0.0001);