feat: add clip_actions_upper/lower

src/rl_sar/config.yaml

@@ -1,41 +1,53 @@
 a1:
+# order:
+# FL
+# FR
+# RL
+# RR
   model_name: "model_0522.pt"
   num_observations: 45
   clip_obs: 100.0
-  clip_actions: 100.0
-  rl_kp: [20, 20, 20, # FL
-          20, 20, 20, # FR
-          20, 20, 20, # RL
-          20, 20, 20] # RR
-  rl_kd: [0.5, 0.5, 0.5, # FL
-          0.5, 0.5, 0.5, # FR
-          0.5, 0.5, 0.5, # RL
-          0.5, 0.5, 0.5] # RR
-  fixed_kp: [80, 80, 80, # FL
-             80, 80, 80, # FR
-             80, 80, 80, # RL
-             80, 80, 80] # RR
-  fixed_kd: [3, 3, 3, # FL
-             3, 3, 3, # FR
-             3, 3, 3, # RL
-             3, 3, 3] # RR
-  action_scale: 0.25
+  clip_actions_lower: [-100, -100, -100, 
+                       -100, -100, -100, 
+                       -100, -100, -100, 
+                       -100, -100, -100] 
+  clip_actions_upper: [100, 100, 100, 
+                       100, 100, 100, 
+                       100, 100, 100, 
+                       100, 100, 100] 
+  rl_kp: [20, 20, 20, 
+          20, 20, 20, 
+          20, 20, 20, 
+          20, 20, 20] 
+  rl_kd: [0.5, 0.5, 0.5, 
+          0.5, 0.5, 0.5, 
+          0.5, 0.5, 0.5, 
+          0.5, 0.5, 0.5] 
+  fixed_kp: [80, 80, 80, 
+             80, 80, 80, 
+             80, 80, 80, 
+             80, 80, 80] 
+  fixed_kd: [3, 3, 3, 
+             3, 3, 3, 
+             3, 3, 3, 
+             3, 3, 3] 
   hip_scale_reduction: 0.5
   hip_scale_reduction_indices: [0, 3, 6, 9]
   num_of_dofs: 12
+  action_scale: 0.25
   lin_vel_scale: 2.0
   ang_vel_scale: 0.25
   dof_pos_scale: 1.0
   dof_vel_scale: 0.05
   commands_scale: [2.0, 2.0, 0.5]
-  torque_limits: [33.5, 33.5, 33.5, # FL
-                  33.5, 33.5, 33.5, # FR
-                  33.5, 33.5, 33.5, # RL
-                  33.5, 33.5, 33.5] # RR
-  default_dof_pos: [ 0.1000, 0.8000, -1.5000, # FL
-                    -0.1000, 0.8000, -1.5000, # FR
-                     0.1000, 1.0000, -1.5000, # RL
-                    -0.1000, 1.0000, -1.5000] # RR
+  torque_limits: [33.5, 33.5, 33.5, 
+                  33.5, 33.5, 33.5, 
+                  33.5, 33.5, 33.5, 
+                  33.5, 33.5, 33.5] 
+  default_dof_pos: [ 0.1000, 0.8000, -1.5000, 
+                    -0.1000, 0.8000, -1.5000, 
+                     0.1000, 1.0000, -1.5000, 
+                    -0.1000, 1.0000, -1.5000] 
   joint_names: ["FL_hip_joint", "FL_thigh_joint", "FL_calf_joint",
                 "FR_hip_joint", "FR_thigh_joint", "FR_calf_joint",
                 "RL_hip_joint", "RL_thigh_joint", "RL_calf_joint",

src/rl_sar/library/rl_sdk/rl_sdk.cpp

@@ -275,7 +275,8 @@ void RL::ReadYaml(std::string robot_name)
     this->params.model_name = config["model_name"].as<std::string>();
     this->params.num_observations = config["num_observations"].as<int>();
     this->params.clip_obs = config["clip_obs"].as<double>();
-    this->params.clip_actions = config["clip_actions"].as<double>();
+    this->params.clip_actions_upper = torch::tensor(ReadVectorFromYaml<double>(config["clip_actions_upper"])).view({1, -1});
+    this->params.clip_actions_lower = torch::tensor(ReadVectorFromYaml<double>(config["clip_actions_lower"])).view({1, -1});
     this->params.action_scale = config["action_scale"].as<double>();
     this->params.hip_scale_reduction = config["hip_scale_reduction"].as<double>();
     this->params.hip_scale_reduction_indices = ReadVectorFromYaml<int>(config["hip_scale_reduction_indices"]);

src/rl_sar/library/rl_sdk/rl_sdk.hpp

@@ -74,7 +74,8 @@ struct ModelParams
     double dof_pos_scale;
     double dof_vel_scale;
     double clip_obs;
-    double clip_actions;
+    torch::Tensor clip_actions_upper;
+    torch::Tensor clip_actions_lower;
     torch::Tensor torque_limits;
     torch::Tensor rl_kd;
     torch::Tensor rl_kp;

src/rl_sar/src/rl_real_a1.cpp

@@ -56,6 +56,7 @@ RL_Real::RL_Real() : unitree_safe(UNITREE_LEGGED_SDK::LeggedType::A1), unitree_u
 
 RL_Real::~RL_Real()
 {
+    loop_keyboard->shutdown();
     loop_udpSend->shutdown();
     loop_udpRecv->shutdown();
     loop_control->shutdown();
@@ -145,15 +146,17 @@ void RL_Real::RunModel()
                                             unitree_low_state.motorState[9].dq, unitree_low_state.motorState[10].dq, unitree_low_state.motorState[11].dq,
                                             unitree_low_state.motorState[6].dq, unitree_low_state.motorState[7].dq, unitree_low_state.motorState[8].dq}});
 
-        torch::Tensor actions = this->Forward();
+        torch::Tensor clamped_actions = this->Forward();
 
         for (int i : hip_scale_reduction_indices)
         {
-            actions[0][i] *= this->params.hip_scale_reduction;
+            clamped_actions[0][i] *= this->params.hip_scale_reduction;
         }
 
-        output_torques = this->ComputeTorques(actions);
-        output_dof_pos = this->ComputePosition(actions);
+        this->obs.actions = clamped_actions;
+
+        output_torques = this->ComputeTorques(clamped_actions);
+        output_dof_pos = this->ComputePosition(clamped_actions);
 #ifdef CSV_LOGGER
         torch::Tensor tau_est = torch::tensor({{unitree_low_state.motorState[3].tauEst, unitree_low_state.motorState[4].tauEst, unitree_low_state.motorState[5].tauEst,
                                                 unitree_low_state.motorState[0].tauEst, unitree_low_state.motorState[1].tauEst, unitree_low_state.motorState[2].tauEst,
@@ -187,8 +190,7 @@ torch::Tensor RL_Real::Forward()
 
     torch::Tensor action = this->model.forward({history_obs}).toTensor();
 
-    this->obs.actions = action;
-    torch::Tensor clamped = torch::clamp(action, -this->params.clip_actions, this->params.clip_actions);
+    torch::Tensor clamped = torch::clamp(action, this->params.clip_actions_upper, this->params.clip_actions_lower);
 
     return clamped;
 }

src/rl_sar/src/rl_sim.cpp

@@ -84,6 +84,7 @@ RL_Sim::RL_Sim()
 
 RL_Sim::~RL_Sim()
 {
+    loop_keyboard->shutdown();
     loop_control->shutdown();
     loop_rl->shutdown();
 #ifdef PLOT
@@ -195,7 +196,9 @@ void RL_Sim::RunModel()
             clamped_actions[0][i] *= this->params.hip_scale_reduction;
         }
 
-        output_torques = this->ComputeTorques(clamped_actions);
+        this->obs.actions = clamped_actions;
+
+        // output_torques = this->ComputeTorques(clamped_actions);
         output_dof_pos = this->ComputePosition(clamped_actions);
 
 #ifdef CSV_LOGGER
@@ -207,8 +210,8 @@ void RL_Sim::RunModel()
 
 torch::Tensor RL_Sim::ComputeObservation()
 {
-    torch::Tensor obs = torch::cat({// this->QuatRotateInverse(this->obs.base_quat, this->obs.lin_vel) * this->params.lin_vel_scale,
-                                    this->QuatRotateInverse(this->obs.base_quat, this->obs.ang_vel) * this->params.ang_vel_scale,
+    torch::Tensor obs = torch::cat({// this->obs.lin_vel * this->params.lin_vel_scale,
+                                    this->obs.ang_vel * this->params.ang_vel_scale,
                                     this->QuatRotateInverse(this->obs.base_quat, this->obs.gravity_vec),
                                     this->obs.commands * this->params.commands_scale,
                                     (this->obs.dof_pos - this->params.default_dof_pos) * this->params.dof_pos_scale,
@@ -238,8 +241,7 @@ torch::Tensor RL_Sim::Forward()
         actions = this->model.forward({obs}).toTensor();
     }  
 
-    this->obs.actions = actions;
-    torch::Tensor clamped_actions = torch::clamp(actions, -this->params.clip_actions, this->params.clip_actions);
+    torch::Tensor clamped_actions = torch::clamp(actions, this->params.clip_actions_lower, this->params.clip_actions_upper);
 
     return clamped_actions;
 }