From f7e0e7253d81ff954f0797796ea117f0bf458091 Mon Sep 17 00:00:00 2001
From: fan-ziqi <fanziqi614@gmail.com>
Date: Tue, 19 Mar 2024 20:29:42 +0800
Subject: [PATCH] yeah it work

---
 .gitignore                 |  1 +
 src/rl_sar/src/rl_real.cpp | 11 ++++++-----
 2 files changed, 7 insertions(+), 5 deletions(-)

diff --git a/.gitignore b/.gitignore
index f2d570b..ea858ab 100644
--- a/.gitignore
+++ b/.gitignore
@@ -3,3 +3,4 @@ devel
 logs
 .catkin_tools
 .vscode
+old
\ No newline at end of file
diff --git a/src/rl_sar/src/rl_real.cpp b/src/rl_sar/src/rl_real.cpp
index 74d4a4a..64d2c12 100644
--- a/src/rl_sar/src/rl_real.cpp
+++ b/src/rl_sar/src/rl_real.cpp
@@ -1,6 +1,6 @@
 #include "../include/rl_real.hpp"
 
-#define PLOT
+// #define PLOT
 
 RL_Real rl_sar;
 
@@ -78,6 +78,7 @@ void RL_Real::RobotControl()
         for(int i = 0; i < 12; ++i)
         {
             cmd.motorCmd[i].mode = 0x0A;
+            // 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 = Kp[dof_mapping[i]];
@@ -165,10 +166,10 @@ RL_Real::RL_Real() : safe(LeggedType::A1), udp(LOWLEVEL)
                                                  20.0, 55.0, 55.0}});
 
     //                                              hip,    thigh,   calf
-    this->params.default_dof_pos = torch::tensor({{-0.1000, 0.8000, -1.5000,   // front right
-                                                    0.1000, 0.8000, -1.5000,   // front left
-                                                   -0.1000, 1.0000, -1.5000,   // rear  right
-                                                    0.1000, 1.0000, -1.5000}});// rear  left
+    this->params.default_dof_pos = torch::tensor({{ 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
 
     this->history_obs_buf = ObservationBuffer(1, this->params.num_observations, 6);