fix rl controller problem at gazebo

README.md

@@ -85,7 +85,7 @@ Video on Real Unitree Go2 Robot:
 
 * Install Gazebo
   ```bash
-  sudo apt-get install ros-jazzy-ros-gz ros-jazzy-gz-ros2-control
+  sudo apt-get install ros-jazzy-ros-gz
   ```
 
 * Compile Gazebo Playground

controllers/rl_quadruped_controller/README.md

@@ -15,17 +15,19 @@ Tested environment:
 
 ### 2.1 Installing libtorch
 
-```bash
-cd ~/CLionProjects/
-wget https://download.pytorch.org/libtorch/cpu/libtorch-cxx11-abi-shared-with-deps-2.5.0%2Bcpu.zip
-unzip unzip libtorch-cxx11-abi-shared-with-deps-2.5.0+cpu.zip
-```
+> You can also choose `libtorch` with cuda. Just remember to download for c++ 11 ABI version. The position to place `libtorch` is also not fixed, just need to config the `.bashrc`.
 
 ```bash
 cd ~/CLionProjects/
+wget https://download.pytorch.org/libtorch/cpu/libtorch-cxx11-abi-shared-with-deps-2.5.0%2Bcpu.zip
+unzip libtorch-cxx11-abi-shared-with-deps-2.5.0+cpu.zip
+```
+
+```bash
+cd ~
 rm -rf libtorch-cxx11-abi-shared-with-deps-2.5.0+cpu.zip
-echo 'export Torch_DIR=~/CLionProjects/libtorch' >> ~/.bashrc
-echo 'export LD_LIBRARY_PATH=$LD_LIBRARY_PATH:~/CLionProjects/libtorch/lib' >> ~/.bashrc
+echo 'export Torch_DIR=~/libtorch' >> ~/.bashrc
+echo 'export LD_LIBRARY_PATH=$LD_LIBRARY_PATH:~/libtorch/lib' >> ~/.bashrc
 ```
 
 ### 2.2 Build Controller

controllers/rl_quadruped_controller/src/FSM/StateRL.cpp

@@ -139,7 +139,9 @@ void StateRL::enter()
     control_.yaw = 0.0;
 
     // history
-    history_obs_buf_->clear();
+    if (!params_.observations_history.empty()) {
+        history_obs_buf_->clear();
+    }
 
     running_ = true;
 }
@@ -161,7 +163,7 @@ void StateRL::exit()
 
 FSMStateName StateRL::checkChange()
 {
-    if (!estimator_->safety())
+    if (enable_estimator_ and !estimator_->safety())
     {
         return FSMStateName::PASSIVE;
     }

controllers/rl_quadruped_controller/src/robot/QuadrupedRobot.cpp

@@ -114,15 +114,15 @@ void QuadrupedRobot::update() {
     if (mass_ == 0) return;
     for (int i = 0; i < 4; i++) {
         KDL::JntArray pos_array(3);
-        pos_array(0) = ctrl_interfaces_.joint_position_state_interface_[i * 3].get().get_value();
-        pos_array(1) = ctrl_interfaces_.joint_position_state_interface_[i * 3 + 1].get().get_value();
-        pos_array(2) = ctrl_interfaces_.joint_position_state_interface_[i * 3 + 2].get().get_value();
+        pos_array(0) = ctrl_interfaces_.joint_position_state_interface_[i * 3].get().get_optional().value();
+        pos_array(1) = ctrl_interfaces_.joint_position_state_interface_[i * 3 + 1].get().get_optional().value();
+        pos_array(2) = ctrl_interfaces_.joint_position_state_interface_[i * 3 + 2].get().get_optional().value();
         current_joint_pos_[i] = pos_array;
 
         KDL::JntArray vel_array(3);
-        vel_array(0) = ctrl_interfaces_.joint_velocity_state_interface_[i * 3].get().get_value();
-        vel_array(1) = ctrl_interfaces_.joint_velocity_state_interface_[i * 3 + 1].get().get_value();
-        vel_array(2) = ctrl_interfaces_.joint_velocity_state_interface_[i * 3 + 2].get().get_value();
+        vel_array(0) = ctrl_interfaces_.joint_velocity_state_interface_[i * 3].get().get_optional().value();
+        vel_array(1) = ctrl_interfaces_.joint_velocity_state_interface_[i * 3 + 1].get().get_optional().value();
+        vel_array(2) = ctrl_interfaces_.joint_velocity_state_interface_[i * 3 + 2].get().get_optional().value();
         current_joint_vel_[i] = vel_array;
     }
 }

controllers/unitree_guide_controller/src/control/Estimator.cpp

@@ -182,19 +182,19 @@ void Estimator::update() {
     }
 
     Quat quat;
-    quat << ctrl_interfaces_.imu_state_interface_[0].get().get_value(),
-            ctrl_interfaces_.imu_state_interface_[1].get().get_value(),
-            ctrl_interfaces_.imu_state_interface_[2].get().get_value(),
-            ctrl_interfaces_.imu_state_interface_[3].get().get_value();
+    quat << ctrl_interfaces_.imu_state_interface_[0].get().get_optional().value(),
+            ctrl_interfaces_.imu_state_interface_[1].get().get_optional().value(),
+            ctrl_interfaces_.imu_state_interface_[2].get().get_optional().value(),
+            ctrl_interfaces_.imu_state_interface_[3].get().get_optional().value();
     rotation_ = quatToRotMat(quat);
 
-    gyro_ << ctrl_interfaces_.imu_state_interface_[4].get().get_value(),
-            ctrl_interfaces_.imu_state_interface_[5].get().get_value(),
-            ctrl_interfaces_.imu_state_interface_[6].get().get_value();
+    gyro_ << ctrl_interfaces_.imu_state_interface_[4].get().get_optional().value(),
+            ctrl_interfaces_.imu_state_interface_[5].get().get_optional().value(),
+            ctrl_interfaces_.imu_state_interface_[6].get().get_optional().value();
 
-    acceleration_ << ctrl_interfaces_.imu_state_interface_[7].get().get_value(),
-            ctrl_interfaces_.imu_state_interface_[8].get().get_value(),
-            ctrl_interfaces_.imu_state_interface_[9].get().get_value();
+    acceleration_ << ctrl_interfaces_.imu_state_interface_[7].get().get_optional().value(),
+            ctrl_interfaces_.imu_state_interface_[8].get().get_optional().value(),
+            ctrl_interfaces_.imu_state_interface_[9].get().get_optional().value();
 
     u_ = rotation_ * acceleration_ + g_;
     x_hat_ = A * x_hat_ + B * u_;

descriptions/unitree/go2_description/config/gazebo.yaml

@@ -164,6 +164,20 @@ rl_quadruped_controller:
       - effort
       - position
       - velocity
+    
+    down_pos:
+      - 0.01
+      - 1.27
+      - -2.8
+      - -0.01
+      - 1.27
+      - -2.8
+      - 0.3
+      - 1.31
+      - -2.8
+      - -0.3
+      - 1.31
+      - -2.8
 
     imu_name: "imu_sensor"
     imu_interfaces:

descriptions/unitree/go2_description/config/legged_gym/config.yaml

@@ -2,7 +2,7 @@ model_name: "policy.pt"
 framework: "isaacgym"
 rows: 4
 cols: 3
-decimation: 1
+decimation: 4
 num_observations: 45
 observations: [ "commands", "ang_vel", "gravity_vec",  "dof_pos", "dof_vel", "actions" ]
 #observations_history: [6, 5, 4, 3, 2, 1, 0]