feat: update plot funtion

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
 )

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 <unitree_legged_msgs/LowCmd.h>
 #include "unitree_legged_msgs/LowState.h"
 #include <unitree_legged_msgs/MotorCmd.h>
@@ -59,7 +60,8 @@ public:
 
     int robot_state = STATE_WAITING;
 
-    std::vector<double> plot_t;
+    const int plot_size = 100;
+    std::vector<int> plot_t;
     std::vector<std::vector<double>> plot_real_joint_pos, plot_target_joint_pos;
     void Plot();
 private:

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 <boost/bind.hpp>
 #include <CustomInterface.h>
@@ -65,7 +65,8 @@ public:
 
     int robot_state = STATE_WAITING;
 
-    std::vector<double> plot_t;
+    const int plot_size = 100;
+    std::vector<int> plot_t;
     std::vector<std::vector<double>> plot_real_joint_pos, plot_target_joint_pos;
     void Plot();
 

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 <ros/ros.h>
 #include <gazebo_msgs/ModelStates.h>
 #include <sensor_msgs/JointState.h>
@@ -37,7 +37,8 @@ public:
     std::shared_ptr<LoopFunc> loop_rl;
     std::shared_ptr<LoopFunc> loop_plot;
 
-    std::vector<double> plot_t;
+    const int plot_size = 100;
+    std::vector<int> plot_t;
     std::vector<std::vector<double>> plot_real_joint_pos, plot_target_joint_pos;
     void Plot();
 private:

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)
 {

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 <torch/script.h>
 #include <iostream>
@@ -83,4 +83,4 @@ protected:
     torch::Tensor output_dof_pos;
 };
 
-#endif // RL_HPP
+#endif // RL_SDK_HPP

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<int>(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<double>(plot_size, 0); }
+    for(auto& vector : plot_target_joint_pos) { vector = std::vector<double>(plot_size, 0); }
 
     loop_control = std::make_shared<LoopFunc>("loop_control", 0.002,    boost::bind(&RL_Real::RobotControl, this));
     loop_udpSend = std::make_shared<LoopFunc>("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);

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<int>(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<double>(plot_size, 0); }
+    for(auto& vector : plot_target_joint_pos) { vector = std::vector<double>(plot_size, 0); }
 
     loop_control = std::make_shared<LoopFunc>("loop_control", 0.002,    boost::bind(&RL_Real::RobotControl, this));
     loop_rl      = std::make_shared<LoopFunc>("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);

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<int>(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<double>(plot_size, 0); }
+    for(auto& vector : plot_target_joint_pos) { vector = std::vector<double>(plot_size, 0); }
 
     cmd_vel_subscriber_ = nh.subscribe<geometry_msgs::Twist>("/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);