From da99daa31c04fd9a6a980d84db841c0ad56d7bde Mon Sep 17 00:00:00 2001
From: Huang Zhenbiao <biao876990970@hotmail.com>
Date: Fri, 20 Sep 2024 22:10:43 +0800
Subject: [PATCH] use swing all rather than stance all

---
 .../src/FSM/StateBalanceTest.cpp              |  1 +
 .../src/FSM/StateTrotting.cpp                 |  2 +-
 .../src/gait/WaveGenerator.cpp                |  4 +-
 .../quadruped_gazebo/urdf/common/imu.xacro    | 38 +++++++++----------
 4 files changed, 23 insertions(+), 22 deletions(-)

diff --git a/controllers/unitree_guide_controller/src/FSM/StateBalanceTest.cpp b/controllers/unitree_guide_controller/src/FSM/StateBalanceTest.cpp
index 64a907c..7e6538f 100644
--- a/controllers/unitree_guide_controller/src/FSM/StateBalanceTest.cpp
+++ b/controllers/unitree_guide_controller/src/FSM/StateBalanceTest.cpp
@@ -52,6 +52,7 @@ void StateBalanceTest::run() {
 }
 
 void StateBalanceTest::exit() {
+    wave_generator_.status_ = WaveStatus::SWING_ALL;
 }
 
 FSMStateName StateBalanceTest::checkChange() {
diff --git a/controllers/unitree_guide_controller/src/FSM/StateTrotting.cpp b/controllers/unitree_guide_controller/src/FSM/StateTrotting.cpp
index 50c666b..af646d7 100644
--- a/controllers/unitree_guide_controller/src/FSM/StateTrotting.cpp
+++ b/controllers/unitree_guide_controller/src/FSM/StateTrotting.cpp
@@ -64,7 +64,7 @@ void StateTrotting::run() {
 }
 
 void StateTrotting::exit() {
-    wave_generator_.status_ = WaveStatus::STANCE_ALL;
+    wave_generator_.status_ = WaveStatus::SWING_ALL;
 }
 
 FSMStateName StateTrotting::checkChange() {
diff --git a/controllers/unitree_guide_controller/src/gait/WaveGenerator.cpp b/controllers/unitree_guide_controller/src/gait/WaveGenerator.cpp
index ac69bd2..9c7423e 100644
--- a/controllers/unitree_guide_controller/src/gait/WaveGenerator.cpp
+++ b/controllers/unitree_guide_controller/src/gait/WaveGenerator.cpp
@@ -11,8 +11,8 @@
 WaveGenerator::WaveGenerator() {
     phase_past_ << 0.5, 0.5, 0.5, 0.5;
     contact_past_.setZero();
-    status_past_ = WaveStatus::STANCE_ALL;
-    status_ = WaveStatus::STANCE_ALL;
+    status_past_ = WaveStatus::SWING_ALL;
+    status_ = WaveStatus::SWING_ALL;
 }
 
 void WaveGenerator::init(const double period, const double st_ratio, const Vec4 &bias) {
diff --git a/descriptions/quadruped_gazebo/urdf/common/imu.xacro b/descriptions/quadruped_gazebo/urdf/common/imu.xacro
index 24d8979..52dd7ea 100644
--- a/descriptions/quadruped_gazebo/urdf/common/imu.xacro
+++ b/descriptions/quadruped_gazebo/urdf/common/imu.xacro
@@ -28,7 +28,7 @@
         <gazebo reference="${imu_name}">
             <sensor name="imu_sensor" type="imu">
                 <always_on>1</always_on>
-                <update_rate>500</update_rate>
+                <update_rate>1000</update_rate>
                 <visualize>true</visualize>
                 <topic>imu</topic>
                 <imu>
@@ -60,28 +60,28 @@
                     </angular_velocity>
                     <linear_acceleration>
                         <x>
-                            <noise type="gaussian">
-                                <mean>0.0</mean>
-                                <stddev>1.7e-2</stddev>
-                                <bias_mean>0.1</bias_mean>
-                                <bias_stddev>0.001</bias_stddev>
-                            </noise>
+<!--                            <noise type="gaussian">-->
+<!--                                <mean>0.0</mean>-->
+<!--                                <stddev>1.7e-2</stddev>-->
+<!--                                <bias_mean>0.1</bias_mean>-->
+<!--                                <bias_stddev>0.001</bias_stddev>-->
+<!--                            </noise>-->
                         </x>
                         <y>
-                            <noise type="gaussian">
-                                <mean>0.0</mean>
-                                <stddev>1.7e-2</stddev>
-                                <bias_mean>0.1</bias_mean>
-                                <bias_stddev>0.001</bias_stddev>
-                            </noise>
+<!--                            <noise type="gaussian">-->
+<!--                                <mean>0.0</mean>-->
+<!--                                <stddev>1.7e-2</stddev>-->
+<!--                                <bias_mean>0.1</bias_mean>-->
+<!--                                <bias_stddev>0.001</bias_stddev>-->
+<!--                            </noise>-->
                         </y>
                         <z>
-                            <noise type="gaussian">
-                                <mean>0.0</mean>
-                                <stddev>1.7e-2</stddev>
-                                <bias_mean>0.1</bias_mean>
-                                <bias_stddev>0.001</bias_stddev>
-                            </noise>
+<!--                            <noise type="gaussian">-->
+<!--                                <mean>0.0</mean>-->
+<!--                                <stddev>1.7e-2</stddev>-->
+<!--                                <bias_mean>0.1</bias_mean>-->
+<!--                                <bias_stddev>0.001</bias_stddev>-->
+<!--                            </noise>-->
                         </z>
                     </linear_acceleration>
                 </imu>