feat: add pd params

fan-ziqi · Apr 8, 2024 · 6df0191 · 6df0191
1 parent 2f7c3ef
commit 6df0191
Show file tree

Hide file tree

Showing 6 changed files with 62 additions and 40 deletions.
diff --git a/src/rl_sar/config.yaml b/src/rl_sar/config.yaml
@@ -2,44 +2,58 @@ a1:
   num_observations: 45
   clip_obs: 100.0
   clip_actions: 100.0
-  damping: 0.5
-  stiffness: 20.0
+  # damping: 0.5
+  # stiffness: 20.0
+  p_gains: [20, 20, 20, # FL
+            20, 20, 20, # FR
+            20, 20, 20, # RL
+            20, 20, 20] # RR
+  d_gains: [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
   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
+  torque_limits: [20.0, 55.0, 55.0, # FL
+                  20.0, 55.0, 55.0, # FR
+                  20.0, 55.0, 55.0, # RL
+                  20.0, 55.0, 55.0] # RR
   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
+                     0.1000, 1.0000, -1.5000, # RL
+                    -0.1000, 1.0000, -1.5000] # RR
 
 cyberdog1:
   num_observations: 45
   clip_obs: 100.0
   clip_actions: 100.0
-  damping: 0.5
-  stiffness: 20.0
+  # damping: 0.5
+  # stiffness: 20.0
+  p_gains: [30, 40, 50, # FL
+            30, 40, 50, # FR
+            30, 40, 50, # RL
+            30, 40, 50] # RR
+  d_gains: [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
   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
+  torque_limits: [24.0, 24.0, 24.0, # FL 
+                  24.0, 24.0, 24.0, # FR
+                  24.0, 24.0, 24.0, # RL
+                  24.0, 24.0, 24.0] # RR
   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
+                     0.1000, 1.0000, -1.5000, # RL
+                    -0.1000, 1.0000, -1.5000] # RR
diff --git a/src/rl_sar/include/rl_real_cyberdog.hpp b/src/rl_sar/include/rl_real_cyberdog.hpp
@@ -79,14 +79,6 @@ class RL_Real : public RL, public CustomInterface
 
     int dof_mapping[12] = {3, 4, 5, 0, 1, 2, 9, 10, 11, 6, 7, 8};
     int hip_scale_reduction_indices[4] = {0, 3, 6, 9};
-    float Kp[13] = {25, 40, 30, //FL
-                    25, 40, 30, //FR
-                    25, 40, 30, //RL
-                    25, 40, 30};//RR
-    float Kd[13] = {0.8, 1.5, 1.0, 
-                    0.8, 1.5, 1.0, 
-                    0.8, 1.5, 1.0, 
-                    0.8, 1.5, 1.0};
 
     std::chrono::high_resolution_clock::time_point start_time;
 

diff --git a/src/rl_sar/library/rl/rl.cpp b/src/rl_sar/library/rl/rl.cpp
@@ -16,10 +16,18 @@ void RL::ReadYaml(std::string robot_name)
     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.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.p_gains = torch::tensor({{config["p_gains"][0].as<float>(), config["p_gains"][1].as<float>(), config["p_gains"][2].as<float>(),    
+                                           config["p_gains"][3].as<float>(), config["p_gains"][4].as<float>(), config["p_gains"][5].as<float>(), 
+                                           config["p_gains"][6].as<float>(), config["p_gains"][7].as<float>(), config["p_gains"][8].as<float>(), 
+                                           config["p_gains"][9].as<float>(), config["p_gains"][10].as<float>(), config["p_gains"][11].as<float>()}});
+    this->params.d_gains = torch::tensor({{config["d_gains"][0].as<float>(), config["d_gains"][1].as<float>(), config["d_gains"][2].as<float>(),    
+                                           config["d_gains"][3].as<float>(), config["d_gains"][4].as<float>(), config["d_gains"][5].as<float>(), 
+                                           config["d_gains"][6].as<float>(), config["d_gains"][7].as<float>(), config["d_gains"][8].as<float>(), 
+                                           config["d_gains"][9].as<float>(), config["d_gains"][10].as<float>(), config["d_gains"][11].as<float>()}});
     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>();

diff --git a/src/rl_sar/src/rl_real_a1.cpp b/src/rl_sar/src/rl_real_a1.cpp
@@ -120,6 +120,7 @@ void RL_Real::RobotControl()
         {
             robot_state = STATE_RL_RUNNING;
             this->InitObservations();
+            this->InitOutputs();
             printf("\nstart rl loop\n");
             loop_rl->start();
         }
@@ -133,8 +134,10 @@ void RL_Real::RobotControl()
             // cmd.motorCmd[i].q = 0;
             cmd.motorCmd[i].q = output_dof_pos[0][dof_mapping[i]].item<double>();
             cmd.motorCmd[i].dq = 0;
-            cmd.motorCmd[i].Kp = params.stiffness;
-            cmd.motorCmd[i].Kd = params.damping;
+            // cmd.motorCmd[i].Kp = params.stiffness;
+            // cmd.motorCmd[i].Kd = params.damping;
+            cmd.motorCmd[i].Kp = params.p_gains[0][dof_mapping[i]].item<double>();
+            cmd.motorCmd[i].Kd = params.d_gains[0][dof_mapping[i]].item<double>();
             // cmd.motorCmd[i].tau = output_torques[0][dof_mapping[i]].item<double>();
             cmd.motorCmd[i].tau = 0;
         }
@@ -170,6 +173,7 @@ void RL_Real::RobotControl()
         {
             robot_state = STATE_WAITING;
             this->InitObservations();
+            this->InitOutputs();
             printf("\nstop rl loop\n");
             loop_rl->shutdown();
         }

diff --git a/src/rl_sar/src/rl_real_cyberdog.cpp b/src/rl_sar/src/rl_real_cyberdog.cpp
@@ -2,7 +2,7 @@
 
 #define ROBOT_NAME "cyberdog1"
 
-// #define PLOT
+#define PLOT
 
 RL_Real rl_sar;
 
@@ -15,7 +15,7 @@ RL_Real::RL_Real() : CustomInterface(500)
     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);
@@ -131,6 +131,7 @@ void RL_Real::RobotControl()
         {
             robot_state = STATE_RL_RUNNING;
             this->InitObservations();
+            this->InitOutputs();
             printf("\nstart rl loop\n");
             loop_rl->start();
         }
@@ -143,10 +144,10 @@ void RL_Real::RobotControl()
             // cyberdogCmd.q_des[i] = 0;
             cyberdogCmd.q_des[i] = output_dof_pos[0][dof_mapping[i]].item<double>();
             cyberdogCmd.qd_des[i] = 0;
-            cyberdogCmd.kp_des[i] = params.stiffness;
-            cyberdogCmd.kd_des[i] = params.damping;
-            // cyberdogCmd.kp_des[i] = Kp[dof_mapping[i]];
-            // cyberdogCmd.kd_des[i] = Kd[dof_mapping[i]];
+            // cyberdogCmd.kp_des[i] = params.stiffness;
+            // cyberdogCmd.kd_des[i] = params.damping;
+            cyberdogCmd.kp_des[i] = params.p_gains[0][dof_mapping[i]].item<double>();
+            cyberdogCmd.kd_des[i] = params.d_gains[0][dof_mapping[i]].item<double>();
             // cyberdogCmd.tau_des[i] = output_torques[0][dof_mapping[i]].item<double>();
             cyberdogCmd.tau_des[i] = 0;
         }
@@ -182,6 +183,7 @@ void RL_Real::RobotControl()
         {
             robot_state = STATE_WAITING;
             this->InitObservations();
+            this->InitOutputs();
             printf("\nstop rl loop\n");
             loop_rl->shutdown();
         }

diff --git a/src/rl_sar/src/rl_sim.cpp b/src/rl_sar/src/rl_sim.cpp
@@ -84,8 +84,10 @@ void RL_Sim::RobotControl()
         motor_commands[i].mode = 0x0A;
         motor_commands[i].q = output_dof_pos[0][i].item<double>();
         motor_commands[i].dq = 0;
-        motor_commands[i].Kp = params.stiffness;
-        motor_commands[i].Kd = params.damping;
+        // motor_commands[i].Kp = params.stiffness;
+        // motor_commands[i].Kd = params.damping;
+        motor_commands[i].Kp = params.p_gains[0][i].item<double>();
+        motor_commands[i].Kd = params.d_gains[0][i].item<double>();
         // motor_commands[i].tau = output_torques[0][i].item<double>();
         motor_commands[i].tau = 0;