From be79e7f64ad4e5dee4bdcfead3d25074fbb3e218 Mon Sep 17 00:00:00 2001
From: fan-ziqi <fanziqi614@gmail.com>
Date: Wed, 29 May 2024 12:27:09 +0800
Subject: [PATCH] fix: make up/down static

---
 src/rl_sar/library/rl_sdk/rl_sdk.cpp | 17 +++++++++++------
 src/rl_sar/library/rl_sdk/rl_sdk.hpp |  5 -----
 src/rl_sar/src/rl_real_a1.cpp        |  2 --
 src/rl_sar/src/rl_sim.cpp            |  2 --
 4 files changed, 11 insertions(+), 15 deletions(-)

diff --git a/src/rl_sar/library/rl_sdk/rl_sdk.cpp b/src/rl_sar/library/rl_sdk/rl_sdk.cpp
index e3e7530..215610a 100644
--- a/src/rl_sar/library/rl_sdk/rl_sdk.cpp
+++ b/src/rl_sar/library/rl_sdk/rl_sdk.cpp
@@ -82,6 +82,11 @@ torch::Tensor RL::QuatRotateInverse(torch::Tensor q, torch::Tensor v)
 
 void RL::StateController(const RobotState<double> *state, RobotCommand<double> *command)
 {
+    static RobotState<double> start_state;
+    static RobotState<double> now_state;
+    static float getup_percent = 0.0;
+    static float getdown_percent = 0.0;
+
     // waiting
     if(running_state == STATE_WAITING)
     {
@@ -95,8 +100,8 @@ void RL::StateController(const RobotState<double> *state, RobotCommand<double> *
             getup_percent = 0.0;
             for(int i = 0; i < params.num_of_dofs; ++i)
             {
-                now_pos[i] = state->motor_state.q[i];
-                start_pos[i] = now_pos[i];
+                now_state.motor_state.q[i] = state->motor_state.q[i];
+                start_state.motor_state.q[i] = now_state.motor_state.q[i];
             }
             running_state = STATE_POS_GETUP;
         }
@@ -110,7 +115,7 @@ void RL::StateController(const RobotState<double> *state, RobotCommand<double> *
             getup_percent = getup_percent > 1.0 ? 1.0 : getup_percent;
             for(int i = 0; i < params.num_of_dofs; ++i)
             {
-                command->motor_command.q[i] = (1 - getup_percent) * now_pos[i] + getup_percent * params.default_dof_pos[0][i].item<double>();
+                command->motor_command.q[i] = (1 - getup_percent) * now_state.motor_state.q[i] + getup_percent * params.default_dof_pos[0][i].item<double>();
                 command->motor_command.dq[i] = 0;
                 command->motor_command.kp[i] = params.fixed_kp[0][i].item<double>();
                 command->motor_command.kd[i] = params.fixed_kd[0][i].item<double>();
@@ -130,7 +135,7 @@ void RL::StateController(const RobotState<double> *state, RobotCommand<double> *
             getdown_percent = 0.0;
             for(int i = 0; i < params.num_of_dofs; ++i)
             {
-                now_pos[i] = state->motor_state.q[i];
+                now_state.motor_state.q[i] = state->motor_state.q[i];
             }
             running_state = STATE_POS_GETDOWN;
         }
@@ -163,7 +168,7 @@ void RL::StateController(const RobotState<double> *state, RobotCommand<double> *
             getdown_percent = 0.0;
             for(int i = 0; i < params.num_of_dofs; ++i)
             {
-                now_pos[i] = state->motor_state.q[i];
+                now_state.motor_state.q[i] = state->motor_state.q[i];
             }
             running_state = STATE_POS_GETDOWN;
         }
@@ -177,7 +182,7 @@ void RL::StateController(const RobotState<double> *state, RobotCommand<double> *
             getdown_percent = getdown_percent > 1.0 ? 1.0 : getdown_percent;
             for(int i = 0; i < params.num_of_dofs; ++i)
             {
-                command->motor_command.q[i] = (1 - getdown_percent) * now_pos[i] + getdown_percent * start_pos[i];
+                command->motor_command.q[i] = (1 - getdown_percent) * now_state.motor_state.q[i] + getdown_percent * start_state.motor_state.q[i];
                 command->motor_command.dq[i] = 0;
                 command->motor_command.kp[i] = params.fixed_kp[0][i].item<double>();
                 command->motor_command.kd[i] = params.fixed_kd[0][i].item<double>();
diff --git a/src/rl_sar/library/rl_sdk/rl_sdk.hpp b/src/rl_sar/library/rl_sdk/rl_sdk.hpp
index 0c95bb4..72e3776 100644
--- a/src/rl_sar/library/rl_sdk/rl_sdk.hpp
+++ b/src/rl_sar/library/rl_sdk/rl_sdk.hpp
@@ -157,11 +157,6 @@ protected:
     // output buffer
     torch::Tensor output_torques;
     torch::Tensor output_dof_pos;
-    // getup getdown buffer
-    float getup_percent = 0.0;
-    float getdown_percent = 0.0;
-    std::vector<double> start_pos;
-    std::vector<double> now_pos;
 };
 
 #endif
\ 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 20ffdf8..5b3d8fe 100644
--- a/src/rl_sar/src/rl_real_a1.cpp
+++ b/src/rl_sar/src/rl_real_a1.cpp
@@ -18,8 +18,6 @@ RL_Real::RL_Real() : unitree_safe(UNITREE_LEGGED_SDK::LeggedType::A1), unitree_u
 
     // init
     torch::autograd::GradMode::set_enabled(false);
-    start_pos.resize(params.num_of_dofs);
-    now_pos.resize(params.num_of_dofs);
     this->InitObservations();
     this->InitOutputs();
     this->InitControl();
diff --git a/src/rl_sar/src/rl_sim.cpp b/src/rl_sar/src/rl_sim.cpp
index be3225d..821199c 100644
--- a/src/rl_sar/src/rl_sim.cpp
+++ b/src/rl_sar/src/rl_sim.cpp
@@ -33,8 +33,6 @@ RL_Sim::RL_Sim()
     // init
     torch::autograd::GradMode::set_enabled(false);
     motor_commands.resize(params.num_of_dofs);
-    start_pos.resize(params.num_of_dofs);
-    now_pos.resize(params.num_of_dofs);
     this->InitObservations();
     this->InitOutputs();
     this->InitControl();