From 2c9cd21c000d63d8d5cef32f23e6d2f001204f5e Mon Sep 17 00:00:00 2001
From: Huang Zhenbiao <biao876990970@hotmail.com>
Date: Mon, 30 Sep 2024 15:10:08 +0800
Subject: [PATCH] achieved basic control on go2 and aliengo

---
 commands/joystick_input/README.md             |   6 +-
 commands/joystick_input/src/JoystickInput.cpp |  14 +-
 commands/keyboard_input/README.md             |   4 +-
 commands/keyboard_input/src/KeyboardInput.cpp |  19 +-
 .../ocs2_quadruped_controller/README.md       |   5 +
 .../control/TargetManager.h                   |   2 +
 .../src/control/TargetManager.cpp             |   8 +-
 .../src/FSM/StateFixedStand.cpp               |   4 +-
 .../aliengo_description/CMakeLists.txt        |   2 +-
 descriptions/aliengo_description/README.md    |  17 +-
 .../aliengo_description/config/ocs2/gait.info | 255 ++++++++
 .../config/ocs2/reference.info                |  46 ++
 .../aliengo_description/config/ocs2/task.info | 319 ++++++++++
 .../config/robot_control.yaml                 |  27 +-
 .../launch/ocs2_control.launch.py             | 122 ++++
 ...ware.launch.py => unitree_guide.launch.py} |   0
 .../aliengo_description/urdf/robot.urdf       | 584 +++++++-----------
 .../aliengo_description/xacro/leg.xacro       | 487 +++++++--------
 .../aliengo_description/xacro/robot.xacro     |   6 -
 .../xacro/ros2_control.xacro                  | 286 ++++-----
 .../config/ocs2/reference.info                |  16 +-
 descriptions/go1_description/urdf/robot.urdf  | 130 +---
 descriptions/go1_description/xacro/leg.xacro  | 484 +++++++--------
 descriptions/go2_description/README.md        |   2 +-
 .../config/ocs2/reference.info                |  18 +-
 ...ware.launch.py => unitree_guide.launch.py} |   0
 descriptions/go2_description/xacro/leg.xacro  |   4 -
 27 files changed, 1695 insertions(+), 1172 deletions(-)
 create mode 100644 descriptions/aliengo_description/config/ocs2/gait.info
 create mode 100644 descriptions/aliengo_description/config/ocs2/reference.info
 create mode 100644 descriptions/aliengo_description/config/ocs2/task.info
 create mode 100644 descriptions/aliengo_description/launch/ocs2_control.launch.py
 rename descriptions/aliengo_description/launch/{hardware.launch.py => unitree_guide.launch.py} (100%)
 rename descriptions/go2_description/launch/{hardware.launch.py => unitree_guide.launch.py} (100%)

diff --git a/commands/joystick_input/README.md b/commands/joystick_input/README.md
index e8a64de..f360fee 100644
--- a/commands/joystick_input/README.md
+++ b/commands/joystick_input/README.md
@@ -24,9 +24,9 @@ ros2 launch joystick_input joystick.launch.py
 * Passive Mode: LB + B
 * Fixed Stand: LB + A
     * Free Stand: LB + X
-    * Trot: start
-    * SwingTest: LT + A
-    * Balance: LT + X
+    * Trot: LB + Y
+    * SwingTest: LT + B
+    * Balance: LT + A
 
 ### 1.2 Control Input
 
diff --git a/commands/joystick_input/src/JoystickInput.cpp b/commands/joystick_input/src/JoystickInput.cpp
index 5a28926..1aa18f8 100644
--- a/commands/joystick_input/src/JoystickInput.cpp
+++ b/commands/joystick_input/src/JoystickInput.cpp
@@ -19,14 +19,18 @@ void JoystickInput::joy_callback(sensor_msgs::msg::Joy::SharedPtr msg) {
         inputs_.command = 2; // LB + A
     } else if (msg->buttons[2] && msg->buttons[4]) {
         inputs_.command = 3; // LB + X
-    } else if (msg->buttons[7]) {
-        inputs_.command = 4; // START
-    } else if (msg->axes[2] != 1 && msg->buttons[2]) {
-        inputs_.command = 10; // LT + X
+    } else if (msg->buttons[3] && msg->buttons[4]) {
+        inputs_.command = 4; // LB + Y
+    } else if (msg->axes[2] != 1 && msg->buttons[1]) {
+        inputs_.command = 5; // LT + B
     } else if (msg->axes[2] != 1 && msg->buttons[0]) {
-        inputs_.command = 9; // LT + A
+        inputs_.command = 6; // LT + A
+    } else if (msg->axes[2] != 1 && msg->buttons[2]) {
+        inputs_.command = 7; // LT + X
     } else if (msg->axes[2] != 1 && msg->buttons[3]) {
         inputs_.command = 8; // LT + Y
+    } else if (msg->buttons[7]) {
+        inputs_.command = 9; // START
     } else {
         inputs_.command = 0;
         inputs_.lx = -msg->axes[0];
diff --git a/commands/keyboard_input/README.md b/commands/keyboard_input/README.md
index 9a5a68d..05a1368 100644
--- a/commands/keyboard_input/README.md
+++ b/commands/keyboard_input/README.md
@@ -24,8 +24,8 @@ ros2 run keyboard_input keyboard_input
 * Fixed Stand: Keyboard 2
     * Free Stand: Keyboard 3
     * Trot: Keyboard 4
-    * SwingTest: Keyboard 9
-    * Balance: Keyboard 0
+    * SwingTest: Keyboard 5
+    * Balance: Keyboard 6
 ### 1.2 Control Input
 * WASD IJKL: Move robot
 * Space: Reset Speed Input
\ No newline at end of file
diff --git a/commands/keyboard_input/src/KeyboardInput.cpp b/commands/keyboard_input/src/KeyboardInput.cpp
index 601395d..5f51829 100644
--- a/commands/keyboard_input/src/KeyboardInput.cpp
+++ b/commands/keyboard_input/src/KeyboardInput.cpp
@@ -56,17 +56,26 @@ void KeyboardInput::check_command(const char key) {
             inputs_.command = 3; // L2_X
             break;
         case '4':
-            inputs_.command = 4; // START
+            inputs_.command = 4; // L2_Y
             break;
-        case '0':
-            inputs_.command = 10; // L1_X
+        case '5':
+            inputs_.command = 5; // L1_A
             break;
-        case '9':
-            inputs_.command = 9; // L1_A
+        case '6':
+            inputs_.command = 6; // L1_B
+            break;
+        case '7':
+            inputs_.command = 7; // L1_X
             break;
         case '8':
             inputs_.command = 8; // L1_Y
             break;
+        case '9':
+            inputs_.command = 9;
+            break;
+        case '0':
+            inputs_.command = 10;
+        break;
         case ' ':
             inputs_.lx = 0;
             inputs_.ly = 0;
diff --git a/controllers/ocs2_quadruped_controller/README.md b/controllers/ocs2_quadruped_controller/README.md
index e75189c..6589c6f 100644
--- a/controllers/ocs2_quadruped_controller/README.md
+++ b/controllers/ocs2_quadruped_controller/README.md
@@ -26,6 +26,11 @@ colcon build --packages-up-to ocs2_quadruped_controller
   source ~/ros2_ws/install/setup.bash
   ros2 launch go1_description ocs2_control.launch.py
   ```
+* Unitree Aliengo Robot
+  ```bash
+  source ~/ros2_ws/install/setup.bash
+  ros2 launch aliengo_description ocs2_control.launch.py
+  ```
 * Unitree Go2 Robot
   ```bash
   source ~/ros2_ws/install/setup.bash
diff --git a/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/control/TargetManager.h b/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/control/TargetManager.h
index 9de9ebb..65afa12 100644
--- a/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/control/TargetManager.h
+++ b/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/control/TargetManager.h
@@ -51,6 +51,8 @@ namespace ocs2::legged_robot {
         vector_t default_joint_state_{};
         scalar_t command_height_{};
         scalar_t time_to_target_{};
+        scalar_t target_displacement_velocity_;
+        scalar_t target_rotation_velocity_;
     };
 }
 
diff --git a/controllers/ocs2_quadruped_controller/src/control/TargetManager.cpp b/controllers/ocs2_quadruped_controller/src/control/TargetManager.cpp
index bdddf12..ad81bd1 100644
--- a/controllers/ocs2_quadruped_controller/src/control/TargetManager.cpp
+++ b/controllers/ocs2_quadruped_controller/src/control/TargetManager.cpp
@@ -20,14 +20,16 @@ namespace ocs2::legged_robot {
         loadData::loadCppDataType(reference_file, "comHeight", command_height_);
         loadData::loadEigenMatrix(reference_file, "defaultJointState", default_joint_state_);
         loadData::loadCppDataType(task_file, "mpc.timeHorizon", time_to_target_);
+        loadData::loadCppDataType(reference_file, "targetRotationVelocity", target_rotation_velocity_);
+        loadData::loadCppDataType(reference_file, "targetDisplacementVelocity", target_displacement_velocity_);
     }
 
     void TargetManager::update() {
         vector_t cmdGoal = vector_t::Zero(6);
-        cmdGoal[0] = ctrl_component_.control_inputs_.ly;
-        cmdGoal[1] = -ctrl_component_.control_inputs_.lx;
+        cmdGoal[0] = ctrl_component_.control_inputs_.ly * target_displacement_velocity_;
+        cmdGoal[1] = -ctrl_component_.control_inputs_.lx * target_displacement_velocity_;
         cmdGoal[2] = ctrl_component_.control_inputs_.ry;
-        cmdGoal[3] = -ctrl_component_.control_inputs_.rx;
+        cmdGoal[3] = -ctrl_component_.control_inputs_.rx * target_rotation_velocity_;
 
         const vector_t currentPose = ctrl_component_.observation_.state.segment<6>(6);
         const Eigen::Matrix<scalar_t, 3, 1> zyx = currentPose.tail(3);
diff --git a/controllers/unitree_guide_controller/src/FSM/StateFixedStand.cpp b/controllers/unitree_guide_controller/src/FSM/StateFixedStand.cpp
index f37f88c..d80910e 100644
--- a/controllers/unitree_guide_controller/src/FSM/StateFixedStand.cpp
+++ b/controllers/unitree_guide_controller/src/FSM/StateFixedStand.cpp
@@ -49,9 +49,9 @@ FSMStateName StateFixedStand::checkChange() {
             return FSMStateName::FREESTAND;
         case 4:
             return FSMStateName::TROTTING;
-        case 9:
+        case 5:
             return FSMStateName::SWINGTEST;
-        case 10:
+        case 6:
             return FSMStateName::BALANCETEST;
         default:
             return FSMStateName::FIXEDSTAND;
diff --git a/descriptions/aliengo_description/CMakeLists.txt b/descriptions/aliengo_description/CMakeLists.txt
index a0f70fe..74d443a 100644
--- a/descriptions/aliengo_description/CMakeLists.txt
+++ b/descriptions/aliengo_description/CMakeLists.txt
@@ -4,7 +4,7 @@ project(aliengo_description)
 find_package(ament_cmake REQUIRED)
 
 install(
-        DIRECTORY meshes xacro launch config
+        DIRECTORY meshes xacro launch config urdf
         DESTINATION share/${PROJECT_NAME}/
 )
 
diff --git a/descriptions/aliengo_description/README.md b/descriptions/aliengo_description/README.md
index b7a0c24..719a32a 100644
--- a/descriptions/aliengo_description/README.md
+++ b/descriptions/aliengo_description/README.md
@@ -18,8 +18,15 @@ source ~/ros2_ws/install/setup.bash
 ros2 launch aliengo_description visualize.launch.py
 ```
 
-## Launch Hardware Interface
-```bash
-source ~/ros2_ws/install/setup.bash
-ros2 launch aliengo_description hardware.launch.py
-```
+## Launch ROS2 Control
+* Unitree Guide Controller
+  ```bash
+  source ~/ros2_ws/install/setup.bash
+  ros2 launch aliengo_description unitree_guide.launch.py
+  ```
+* OCS2 Quadruped Controller
+  ```bash
+  source ~/ros2_ws/install/setup.bash
+  ros2 launch aliengo_description ocs2_control.launch.py
+  ```
+
diff --git a/descriptions/aliengo_description/config/ocs2/gait.info b/descriptions/aliengo_description/config/ocs2/gait.info
new file mode 100644
index 0000000..6ea163b
--- /dev/null
+++ b/descriptions/aliengo_description/config/ocs2/gait.info
@@ -0,0 +1,255 @@
+list
+{
+  [0] stance
+  [1] trot
+  [2] standing_trot
+  [3] flying_trot
+  [4] pace
+  [5] standing_pace
+  [6] dynamic_walk
+  [7] static_walk
+  [8] amble
+  [9] lindyhop
+  [10] skipping
+  [11] pawup
+}
+
+stance
+ {
+   modeSequence
+   {
+     [0]     STANCE
+   }
+   switchingTimes
+   {
+     [0]     0.0
+     [1]     0.5
+   }
+}
+
+trot
+ {
+   modeSequence
+   {
+     [0]     LF_RH
+     [1]     RF_LH
+   }
+   switchingTimes
+   {
+     [0]     0.0
+     [1]     0.3
+     [2]     0.6
+   }
+}
+
+standing_trot
+{
+  modeSequence
+  {
+    [0]     LF_RH
+    [1]     STANCE
+    [2]     RF_LH
+    [3]     STANCE
+  }
+  switchingTimes
+  {
+    [0]     0.00
+    [1]     0.25
+    [2]     0.3
+    [3]     0.55
+    [4]     0.6
+  }
+}
+
+flying_trot
+{
+  modeSequence
+  {
+    [0]     LF_RH
+    [1]     FLY
+    [2]     RF_LH
+    [3]     FLY
+  }
+  switchingTimes
+  {
+    [0]     0.00
+    [1]     0.15
+    [2]     0.2
+    [3]     0.35
+    [4]     0.4
+  }
+}
+
+pace
+{
+  modeSequence
+  {
+    [0]     LF_LH
+    [1]     FLY
+    [2]     RF_RH
+    [3]     FLY
+  }
+  switchingTimes
+  {
+    [0]     0.0
+    [1]     0.28
+    [2]     0.30
+    [3]     0.58
+    [4]     0.60
+  }
+}
+
+standing_pace
+{
+  modeSequence
+  {
+    [0]     LF_LH
+    [1]     STANCE
+    [2]     RF_RH
+    [3]     STANCE
+  }
+  switchingTimes
+  {
+    [0]     0.0
+    [1]     0.30
+    [2]     0.35
+    [3]     0.65
+    [4]     0.70
+  }
+}
+
+dynamic_walk
+{
+  modeSequence
+  {
+    [0]     LF_RF_RH
+    [1]     RF_RH
+    [2]     RF_LH_RH
+    [3]     LF_RF_LH
+    [4]     LF_LH
+    [5]     LF_LH_RH
+  }
+  switchingTimes
+  {
+    [0]     0.0
+    [1]     0.2
+    [2]     0.3
+    [3]     0.5
+    [4]     0.7
+    [5]     0.8
+    [6]     1.0
+  }
+}
+
+static_walk
+{
+  modeSequence
+  {
+    [0]     LF_RF_RH
+    [1]     RF_LH_RH
+    [2]     LF_RF_LH
+    [3]     LF_LH_RH
+  }
+  switchingTimes
+  {
+    [0]     0.0
+    [1]     0.3
+    [2]     0.6
+    [3]     0.9
+    [4]     1.2
+  }
+}
+
+amble
+{
+  modeSequence
+  {
+    [0]     RF_LH
+    [1]     LF_LH
+    [2]     LF_RH
+    [3]     RF_RH
+  }
+  switchingTimes
+  {
+    [0]     0.0
+    [1]     0.15
+    [2]     0.40
+    [3]     0.55
+    [4]     0.80
+  }
+}
+
+lindyhop
+{
+  modeSequence
+  {
+    [0]     LF_RH
+    [1]     STANCE
+    [2]     RF_LH
+    [3]     STANCE
+    [4]     LF_LH
+    [5]     RF_RH
+    [6]     LF_LH
+    [7]     STANCE
+    [8]     RF_RH
+    [9]     LF_LH
+    [10]     RF_RH
+    [11]     STANCE
+  }
+  switchingTimes
+  {
+    [0]     0.00 ; Step 1
+    [1]     0.35 ; Stance
+    [2]     0.45 ; Step 2
+    [3]     0.80 ; Stance
+    [4]     0.90  ; Tripple step
+    [5]     1.125 ;
+    [6]     1.35  ;
+    [7]     1.70  ; Stance
+    [8]     1.80  ; Tripple step
+    [9]     2.025 ;
+    [10]    2.25  ;
+    [11]    2.60  ; Stance
+    [12]    2.70  ;
+  }
+}
+
+skipping
+{
+  modeSequence
+  {
+    [0]     LF_RH
+    [1]     FLY
+    [2]     LF_RH
+    [3]     FLY
+    [4]     RF_LH
+    [5]     FLY
+    [6]     RF_LH
+    [7]     FLY
+  }
+  switchingTimes
+  {
+    [0]     0.00
+    [1]     0.27
+    [2]     0.30
+    [3]     0.57
+    [4]     0.60
+    [5]     0.87
+    [6]     0.90
+    [7]     1.17
+    [8]     1.20
+  }
+}
+
+pawup
+{
+   modeSequence
+   {
+     [0]     RF_LH_RH
+   }
+   switchingTimes
+   {
+     [0]     0.0
+     [1]     2.0
+   }
+}
diff --git a/descriptions/aliengo_description/config/ocs2/reference.info b/descriptions/aliengo_description/config/ocs2/reference.info
new file mode 100644
index 0000000..6a6be26
--- /dev/null
+++ b/descriptions/aliengo_description/config/ocs2/reference.info
@@ -0,0 +1,46 @@
+targetDisplacementVelocity          0.5;
+targetRotationVelocity              1.57;
+
+comHeight                           0.4
+
+defaultJointState
+{
+   (0,0)  -0.10   ; FL_hip_joint
+   (1,0)   0.62   ; FL_thigh_joint
+   (2,0)  -1.24   ; FL_calf_joint
+   (3,0)   0.10   ; FR_hip_joint
+   (4,0)   0.62   ; FR_thigh_joint
+   (5,0)  -1.24   ; FR_calf_joint
+   (6,0)  -0.10   ; RL_hip_joint
+   (7,0)   0.62   ; RL_thigh_joint
+   (8,0)  -1.24   ; RL_calf_joint
+   (9,0)   0.10   ; RR_hip_joint
+   (10,0)  0.62   ; RR_thigh_joint
+   (11,0) -1.24   ; RR_calf_joint
+}
+
+initialModeSchedule
+{
+  modeSequence
+  {
+    [0]  STANCE
+    [1]  STANCE
+  }
+  eventTimes
+  {
+    [0]  0.5
+  }
+}
+
+defaultModeSequenceTemplate
+{
+  modeSequence
+  {
+    [0]  STANCE
+  }
+  switchingTimes
+  {
+    [0]  0.0
+    [1]  1.0
+  }
+}
diff --git a/descriptions/aliengo_description/config/ocs2/task.info b/descriptions/aliengo_description/config/ocs2/task.info
new file mode 100644
index 0000000..4a67c83
--- /dev/null
+++ b/descriptions/aliengo_description/config/ocs2/task.info
@@ -0,0 +1,319 @@
+centroidalModelType             0      // 0: FullCentroidalDynamics, 1: Single Rigid Body Dynamics
+
+legged_robot_interface
+{
+  verbose                               false  // show the loaded parameters
+}
+
+model_settings
+{
+  positionErrorGain             0.0
+  phaseTransitionStanceTime     0.1
+
+  verboseCppAd                  true
+  recompileLibrariesCppAd       false
+  modelFolderCppAd              /tmp/ocs2_quadruped_controller/aliengo
+}
+
+swing_trajectory_config
+{
+  liftOffVelocity               0.2
+  touchDownVelocity            -0.4
+  swingHeight                   0.1
+  swingTimeScale                0.15
+}
+
+; DDP settings
+ddp
+{
+  algorithm                       SLQ
+
+  nThreads                        3
+  threadPriority                  50
+
+  maxNumIterations                1
+  minRelCost                      1e-1
+  constraintTolerance             5e-3
+
+  displayInfo                     false
+  displayShortSummary             false
+  checkNumericalStability         false
+  debugPrintRollout               false
+  debugCaching                    false
+
+  AbsTolODE                       1e-5
+  RelTolODE                       1e-3
+  maxNumStepsPerSecond            10000
+  timeStep                        0.015
+  backwardPassIntegratorType      ODE45
+
+  constraintPenaltyInitialValue   20.0
+  constraintPenaltyIncreaseRate   2.0
+
+  preComputeRiccatiTerms          true
+
+  useFeedbackPolicy               false
+
+  strategy                        LINE_SEARCH
+  lineSearch
+  {
+    minStepLength                 1e-2
+    maxStepLength                 1.0
+    hessianCorrectionStrategy     DIAGONAL_SHIFT
+    hessianCorrectionMultiple     1e-5
+  }
+}
+
+
+; Multiple_Shooting SQP settings
+sqp
+{
+  nThreads                              3
+  dt                                    0.015
+  sqpIteration                          1
+  deltaTol                              1e-4
+  g_max                                 1e-2
+  g_min                                 1e-6
+  inequalityConstraintMu                0.1
+  inequalityConstraintDelta             5.0
+  projectStateInputEqualityConstraints  true
+  printSolverStatistics                 true
+  printSolverStatus                     false
+  printLinesearch                       false
+  useFeedbackPolicy                     false
+  integratorType                        RK2
+  threadPriority                        50
+}
+
+; Multiple_Shooting IPM settings
+ipm
+{
+  nThreads                              3
+  dt                                    0.015
+  ipmIteration                          1
+  deltaTol                              1e-4
+  g_max                                 10.0
+  g_min                                 1e-6
+  computeLagrangeMultipliers            true
+  printSolverStatistics                 true
+  printSolverStatus                     false
+  printLinesearch                       false
+  useFeedbackPolicy                     false
+  integratorType                        RK2
+  threadPriority                        50
+
+  initialBarrierParameter               1e-4
+  targetBarrierParameter                1e-4
+  barrierLinearDecreaseFactor           0.2
+  barrierSuperlinearDecreasePower       1.5
+  barrierReductionCostTol               1e-3
+  barrierReductionConstraintTol         1e-3
+
+  fractionToBoundaryMargin              0.995
+  usePrimalStepSizeForDual              false
+
+  initialSlackLowerBound                1e-4
+  initialDualLowerBound                 1e-4
+  initialSlackMarginRate                1e-2
+  initialDualMarginRate                 1e-2
+}
+
+; Rollout settings
+rollout
+{
+  AbsTolODE                       1e-5
+  RelTolODE                       1e-3
+  timeStep                        0.015
+  integratorType                  ODE45
+  maxNumStepsPerSecond            10000
+  checkNumericalStability         false
+}
+
+mpc
+{
+  timeHorizon                     1.0  ; [s]
+  solutionTimeWindow              -1   ; maximum [s]
+  coldStart                       false
+
+  debugPrint                      false
+
+  mpcDesiredFrequency             100  ; [Hz]
+  mrtDesiredFrequency             1000 ; [Hz] Useless
+}
+
+initialState
+{
+   ;; Normalized Centroidal Momentum: [linear, angular] ;;
+   (0,0)  0.0     ; vcom_x
+   (1,0)  0.0     ; vcom_y
+   (2,0)  0.0     ; vcom_z
+   (3,0)  0.0     ; L_x / robotMass
+   (4,0)  0.0     ; L_y / robotMass
+   (5,0)  0.0     ; L_z / robotMass
+
+   ;; Base Pose: [position, orientation] ;;
+   (6,0)  0.0     ; p_base_x
+   (7,0)  0.0     ; p_base_y
+   (8,0)  0.3     ; p_base_z
+   (9,0)  0.0     ; theta_base_z
+   (10,0) 0.0     ; theta_base_y
+   (11,0) 0.0     ; theta_base_x
+
+   ;; Leg Joint Positions: [FL, RL, FR, RR] ;;
+   (12,0) -0.20   ; FL_hip_joint
+   (13,0)  0.62   ; FL_thigh_joint
+   (14,0) -1.24   ; FL_calf_joint
+   (15,0) -0.20   ; RL_hip_joint
+   (16,0)  0.62   ; RL_thigh_joint
+   (17,0) -1.24   ; RL_calf_joint
+   (18,0)  0.20   ; FR_hip_joint
+   (19,0)  0.62   ; FR_thigh_joint
+   (20,0) -1.24   ; FR_calf_joint
+   (21,0)  0.20   ; RR_hip_joint
+   (22,0)  0.62   ; RR_thigh_joint
+   (23,0) -1.24   ; RR_calf_joint
+}
+
+; standard state weight matrix
+Q
+{
+  scaling 1e+0
+
+  ;; Normalized Centroidal Momentum: [linear, angular] ;;
+  (0,0)   15.0     ; vcom_x
+  (1,1)   15.0     ; vcom_y
+  (2,2)   100.0    ; vcom_z
+  (3,3)   10.0     ; L_x / robotMass
+  (4,4)   30.0     ; L_y / robotMass
+  (5,5)   30.0     ; L_z / robotMass
+
+  ;; Base Pose: [position, orientation] ;;
+  (6,6)   1000.0   ; p_base_x
+  (7,7)   1000.0   ; p_base_y
+  (8,8)   1500.0   ; p_base_z
+  (9,9)   100.0    ; theta_base_z
+  (10,10) 300.0    ; theta_base_y
+  (11,11) 300.0    ; theta_base_x
+
+  ;; Leg Joint Positions: [FL, RL, FR, RR] ;;
+  (12,12) 5.0     ; FL_hip_joint
+  (13,13) 5.0     ; FL_thigh_joint
+  (14,14) 2.5     ; FL_calf_joint
+  (15,15) 5.0     ; RL_hip_joint
+  (16,16) 5.0     ; RL_thigh_joint
+  (17,17) 2.5     ; RL_calf_joint
+  (18,18) 5.0     ; FR_hip_joint
+  (19,19) 5.0     ; FR_thigh_joint
+  (20,20) 2.5     ; FR_calf_joint
+  (21,21) 5.0     ; RR_hip_joint
+  (22,22) 5.0     ; RR_thigh_joint
+  (23,23) 2.5     ; RR_calf_joint
+}
+
+; control weight matrix
+R
+{
+  scaling 1e-3
+
+  ;; Feet Contact Forces: [FL, FR, RL, RR] ;;
+  (0,0)   5.0       ; front_left_force
+  (1,1)   5.0       ; front_left_force
+  (2,2)   5.0       ; front_left_force
+  (3,3)   5.0       ; front_right_force
+  (4,4)   5.0       ; front_right_force
+  (5,5)   5.0       ; front_right_force
+  (6,6)   5.0       ; rear_left_force
+  (7,7)   5.0       ; rear_left_force
+  (8,8)   5.0       ; rear_left_force
+  (9,9)   5.0       ; rear_right_force
+  (10,10) 5.0       ; rear_right_force
+  (11,11) 5.0       ; rear_right_force
+
+  ;; foot velocity relative to base: [FL, RL, FR, RR] (uses the Jacobian at nominal configuration) ;;
+  (12,12) 5000.0    ; x
+  (13,13) 5000.0    ; y
+  (14,14) 5000.0    ; z
+  (15,15) 5000.0    ; x
+  (16,16) 5000.0    ; y
+  (17,17) 5000.0    ; z
+  (18,18) 5000.0    ; x
+  (19,19) 5000.0    ; y
+  (20,20) 5000.0    ; z
+  (21,21) 5000.0    ; x
+  (22,22) 5000.0    ; y
+  (23,23) 5000.0    ; z
+}
+
+frictionConeSoftConstraint
+{
+  frictionCoefficient    0.3
+
+  ; relaxed log barrier parameters
+  mu                     0.1
+  delta                  5.0
+}
+
+selfCollision
+{
+  ; Self Collision raw object pairs
+  collisionObjectPairs
+  {
+  }
+
+  ; Self Collision pairs
+  collisionLinkPairs
+  {
+    [0] "FL_calf, FR_calf"
+    [1] "RL_calf, RR_calf"
+    [2] "FL_calf, RL_calf"
+    [3] "FR_calf, RR_calf"
+    [4] "FL_foot, FR_foot"
+    [5] "RL_foot, RR_foot"
+    [6] "FL_foot, RL_foot"
+    [7] "FR_foot, RR_foot"
+  }
+
+  minimumDistance  0.05
+
+  ; relaxed log barrier parameters
+  mu      1e-2
+  delta   1e-3
+}
+
+; Whole body control
+torqueLimitsTask
+{
+   (0,0)  35.278     ; HAA
+   (1,0)  35.278     ; HFE
+   (2,0)  44.4       ; KFE
+}
+
+frictionConeTask
+{
+  frictionCoefficient    0.3
+}
+
+swingLegTask
+{
+    kp                   350
+    kd                   37
+}
+
+weight
+{
+    swingLeg        100
+    baseAccel       1
+    contactForce    0.01
+}
+
+; State Estimation
+kalmanFilter
+{
+    footRadius                  0.02
+    imuProcessNoisePosition     0.02
+    imuProcessNoiseVelocity     0.02
+    footProcessNoisePosition    0.002
+    footSensorNoisePosition     0.005
+    footSensorNoiseVelocity     0.1
+    footHeightSensorNoise       0.01
+}
diff --git a/descriptions/aliengo_description/config/robot_control.yaml b/descriptions/aliengo_description/config/robot_control.yaml
index c42be8f..ae2cfb6 100644
--- a/descriptions/aliengo_description/config/robot_control.yaml
+++ b/descriptions/aliengo_description/config/robot_control.yaml
@@ -74,19 +74,20 @@ unitree_guide_controller:
 ocs2_quadruped_controller:
   ros__parameters:
     update_rate: 500  # Hz
+
     joints:
-      - FR_hip_joint
-      - FR_thigh_joint
-      - FR_calf_joint
       - FL_hip_joint
       - FL_thigh_joint
       - FL_calf_joint
-      - RR_hip_joint
-      - RR_thigh_joint
-      - RR_calf_joint
+      - FR_hip_joint
+      - FR_thigh_joint
+      - FR_calf_joint
       - RL_hip_joint
       - RL_thigh_joint
       - RL_calf_joint
+      - RR_hip_joint
+      - RR_thigh_joint
+      - RR_calf_joint
 
     command_interfaces:
       - effort
@@ -101,10 +102,11 @@ ocs2_quadruped_controller:
       - velocity
 
     feet_names:
-      - FR_foot
       - FL_foot
-      - RR_foot
+      - FR_foot
       - RL_foot
+      - RR_foot
+
 
     imu_name: "imu_sensor"
     base_name: "base"
@@ -119,4 +121,11 @@ ocs2_quadruped_controller:
       - angular_velocity.z
       - linear_acceleration.x
       - linear_acceleration.y
-      - linear_acceleration.z
\ No newline at end of file
+      - linear_acceleration.z
+
+    foot_force_name: "foot_force"
+    foot_force_interfaces:
+      - FL
+      - RL
+      - FR
+      - RR
\ No newline at end of file
diff --git a/descriptions/aliengo_description/launch/ocs2_control.launch.py b/descriptions/aliengo_description/launch/ocs2_control.launch.py
new file mode 100644
index 0000000..93f52b3
--- /dev/null
+++ b/descriptions/aliengo_description/launch/ocs2_control.launch.py
@@ -0,0 +1,122 @@
+import os
+
+import xacro
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, OpaqueFunction, IncludeLaunchDescription, RegisterEventHandler
+from launch.event_handlers import OnProcessExit
+from launch.substitutions import PathJoinSubstitution
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+from sympy.physics.vector.printing import params
+
+package_description = "aliengo_description"
+
+
+def process_xacro(context):
+    robot_type_value = context.launch_configurations['robot_type']
+    pkg_path = os.path.join(get_package_share_directory(package_description))
+    xacro_file = os.path.join(pkg_path, 'xacro', 'robot.xacro')
+    robot_description_config = xacro.process_file(xacro_file, mappings={'robot_type': robot_type_value})
+    return (robot_description_config.toxml(), robot_type_value)
+
+
+def launch_setup(context, *args, **kwargs):
+    (robot_description, robot_type) = process_xacro(context)
+    robot_controllers = PathJoinSubstitution(
+        [
+            FindPackageShare(package_description),
+            "config",
+            "robot_control.yaml",
+        ]
+    )
+
+    robot_state_publisher = Node(
+        package='robot_state_publisher',
+        executable='robot_state_publisher',
+        name='robot_state_publisher',
+        parameters=[
+            {
+                'publish_frequency': 20.0,
+                'use_tf_static': True,
+                'robot_description': robot_description,
+                'ignore_timestamp': True
+            }
+        ],
+    )
+
+    controller_manager = Node(
+        package="controller_manager",
+        executable="ros2_control_node",
+        parameters=[robot_controllers,
+                    {
+                        'urdf_file': os.path.join(get_package_share_directory(package_description), 'urdf', 'robot.urdf'),
+                        'task_file': os.path.join(get_package_share_directory(package_description), 'config', 'ocs2',
+                                                  'task.info'),
+                        'reference_file': os.path.join(get_package_share_directory(package_description), 'config',
+                                                       'ocs2', 'reference.info'),
+                        'gait_file': os.path.join(get_package_share_directory(package_description), 'config',
+                                                       'ocs2', 'gait.info')
+                    }],
+        output="both",
+    )
+
+    joint_state_publisher = Node(
+        package="controller_manager",
+        executable="spawner",
+        arguments=["joint_state_broadcaster",
+                   "--controller-manager", "/controller_manager"],
+    )
+
+    imu_sensor_broadcaster = Node(
+        package="controller_manager",
+        executable="spawner",
+        arguments=["imu_sensor_broadcaster",
+                   "--controller-manager", "/controller_manager"],
+    )
+
+    ocs2_controller = Node(
+        package="controller_manager",
+        executable="spawner",
+        arguments=["ocs2_quadruped_controller", "--controller-manager", "/controller_manager"]
+    )
+
+    return [
+        robot_state_publisher,
+        controller_manager,
+        joint_state_publisher,
+        RegisterEventHandler(
+            event_handler=OnProcessExit(
+                target_action=joint_state_publisher,
+                on_exit=[imu_sensor_broadcaster],
+            )
+        ),
+        RegisterEventHandler(
+            event_handler=OnProcessExit(
+                target_action=imu_sensor_broadcaster,
+                on_exit=[ocs2_controller],
+            )
+        ),
+    ]
+
+
+def generate_launch_description():
+    robot_type_arg = DeclareLaunchArgument(
+        'robot_type',
+        default_value='aliengo',
+        description='Type of the robot'
+    )
+
+    rviz_config_file = os.path.join(get_package_share_directory(package_description), "config", "visualize_urdf.rviz")
+
+    return LaunchDescription([
+        robot_type_arg,
+        OpaqueFunction(function=launch_setup),
+        Node(
+            package='rviz2',
+            executable='rviz2',
+            name='rviz_ocs2',
+            output='screen',
+            arguments=["-d", rviz_config_file]
+        )
+    ])
diff --git a/descriptions/aliengo_description/launch/hardware.launch.py b/descriptions/aliengo_description/launch/unitree_guide.launch.py
similarity index 100%
rename from descriptions/aliengo_description/launch/hardware.launch.py
rename to descriptions/aliengo_description/launch/unitree_guide.launch.py
diff --git a/descriptions/aliengo_description/urdf/robot.urdf b/descriptions/aliengo_description/urdf/robot.urdf
index f45b7e2..ade0e7b 100644
--- a/descriptions/aliengo_description/urdf/robot.urdf
+++ b/descriptions/aliengo_description/urdf/robot.urdf
@@ -31,265 +31,152 @@
   <material name="white">
     <color rgba="1.0 1.0 1.0 1.0"/>
   </material>
-  <!-- ros_control plugin -->
-  <gazebo>
-    <plugin filename="libgazebo_ros_control.so" name="gazebo_ros_control">
-      <robotNamespace>/aliengo_gazebo</robotNamespace>
-      <robotSimType>gazebo_ros_control/DefaultRobotHWSim</robotSimType>
-    </plugin>
-  </gazebo>
-  <!-- Show the trajectory of trunk center. -->
-  <gazebo>
-    <plugin filename="libLinkPlot3DPlugin.so" name="3dplot">
-      <frequency>10</frequency>
-      <plot>
-        <link>base</link>
-        <pose>0 0 0 0 0 0</pose>
-        <material>Gazebo/Yellow</material>
-      </plot>
-    </plugin>
-  </gazebo>
-  <!-- Show the trajectory of foot. You can add another trajectory about another foot. -->
-  <!-- <gazebo>
-        <plugin name="3dplot" filename="libLinkPlot3DPlugin.so">
-            <frequency>100</frequency>
-            <plot>
-                <link>FL_foot</link>
-                <pose>0 0 0 0 0 0</pose>
-                <material>Gazebo/Green</material>
-            </plot>
-        </plugin>
-    </gazebo> -->
-  <gazebo>
-    <plugin filename="libgazebo_ros_force.so" name="gazebo_ros_force">
-      <bodyName>trunk</bodyName>
-      <topicName>/apply_force/trunk</topicName>
-    </plugin>
-  </gazebo>
-  <gazebo reference="imu_link">
-    <gravity>true</gravity>
-    <sensor name="imu_sensor" type="imu">
-      <always_on>true</always_on>
-      <update_rate>1000</update_rate>
-      <visualize>true</visualize>
-      <topic>__default_topic__</topic>
-      <plugin filename="libgazebo_ros_imu_sensor.so" name="imu_plugin">
-        <topicName>trunk_imu</topicName>
-        <bodyName>imu_link</bodyName>
-        <updateRateHZ>1000.0</updateRateHZ>
-        <gaussianNoise>0.0</gaussianNoise>
-        <xyzOffset>0 0 0</xyzOffset>
-        <rpyOffset>0 0 0</rpyOffset>
-        <frameName>imu_link</frameName>
-      </plugin>
-      <pose>0 0 0 0 0 0</pose>
+  <ros2_control name="UnitreeSystem" type="system">
+    <hardware>
+      <plugin>hardware_unitree_mujoco/HardwareUnitree</plugin>
+    </hardware>
+    <joint name="FR_hip_joint">
+      <command_interface name="position"/>
+      <command_interface name="velocity"/>
+      <command_interface name="effort"/>
+      <command_interface name="kp"/>
+      <command_interface name="kd"/>
+      <state_interface name="position"/>
+      <state_interface name="velocity"/>
+      <state_interface name="effort"/>
+    </joint>
+    <joint name="FR_thigh_joint">
+      <command_interface name="position"/>
+      <command_interface name="velocity"/>
+      <command_interface name="effort"/>
+      <command_interface name="kp"/>
+      <command_interface name="kd"/>
+      <state_interface name="position"/>
+      <state_interface name="velocity"/>
+      <state_interface name="effort"/>
+    </joint>
+    <joint name="FR_calf_joint">
+      <command_interface name="position"/>
+      <command_interface name="velocity"/>
+      <command_interface name="effort"/>
+      <command_interface name="kp"/>
+      <command_interface name="kd"/>
+      <state_interface name="position"/>
+      <state_interface name="velocity"/>
+      <state_interface name="effort"/>
+    </joint>
+    <joint name="FL_hip_joint">
+      <command_interface name="position"/>
+      <command_interface name="velocity"/>
+      <command_interface name="effort"/>
+      <command_interface name="kp"/>
+      <command_interface name="kd"/>
+      <state_interface name="position"/>
+      <state_interface name="velocity"/>
+      <state_interface name="effort"/>
+    </joint>
+    <joint name="FL_thigh_joint">
+      <command_interface name="position"/>
+      <command_interface name="velocity"/>
+      <command_interface name="effort"/>
+      <command_interface name="kp"/>
+      <command_interface name="kd"/>
+      <state_interface name="position"/>
+      <state_interface name="velocity"/>
+      <state_interface name="effort"/>
+    </joint>
+    <joint name="FL_calf_joint">
+      <command_interface name="position"/>
+      <command_interface name="velocity"/>
+      <command_interface name="effort"/>
+      <command_interface name="kp"/>
+      <command_interface name="kd"/>
+      <state_interface name="position"/>
+      <state_interface name="velocity"/>
+      <state_interface name="effort"/>
+    </joint>
+    <joint name="RR_hip_joint">
+      <command_interface name="position"/>
+      <command_interface name="velocity"/>
+      <command_interface name="effort"/>
+      <command_interface name="kp"/>
+      <command_interface name="kd"/>
+      <state_interface name="position"/>
+      <state_interface name="velocity"/>
+      <state_interface name="effort"/>
+    </joint>
+    <joint name="RR_thigh_joint">
+      <command_interface name="position"/>
+      <command_interface name="velocity"/>
+      <command_interface name="effort"/>
+      <command_interface name="kp"/>
+      <command_interface name="kd"/>
+      <state_interface name="position"/>
+      <state_interface name="velocity"/>
+      <state_interface name="effort"/>
+    </joint>
+    <joint name="RR_calf_joint">
+      <command_interface name="position"/>
+      <command_interface name="velocity"/>
+      <command_interface name="effort"/>
+      <command_interface name="kp"/>
+      <command_interface name="kd"/>
+      <state_interface name="position"/>
+      <state_interface name="velocity"/>
+      <state_interface name="effort"/>
+    </joint>
+    <joint name="RL_hip_joint">
+      <command_interface name="position"/>
+      <command_interface name="velocity"/>
+      <command_interface name="effort"/>
+      <command_interface name="kp"/>
+      <command_interface name="kd"/>
+      <state_interface name="position"/>
+      <state_interface name="velocity"/>
+      <state_interface name="effort"/>
+    </joint>
+    <joint name="RL_thigh_joint">
+      <command_interface name="position"/>
+      <command_interface name="velocity"/>
+      <command_interface name="effort"/>
+      <command_interface name="kp"/>
+      <command_interface name="kd"/>
+      <state_interface name="position"/>
+      <state_interface name="velocity"/>
+      <state_interface name="effort"/>
+    </joint>
+    <joint name="RL_calf_joint">
+      <command_interface name="position"/>
+      <command_interface name="velocity"/>
+      <command_interface name="effort"/>
+      <command_interface name="kp"/>
+      <command_interface name="kd"/>
+      <state_interface name="position"/>
+      <state_interface name="velocity"/>
+      <state_interface name="effort"/>
+    </joint>
+    <sensor name="imu_sensor">
+      <state_interface name="orientation.w"/>
+      <state_interface name="orientation.x"/>
+      <state_interface name="orientation.y"/>
+      <state_interface name="orientation.z"/>
+      <state_interface name="angular_velocity.x"/>
+      <state_interface name="angular_velocity.y"/>
+      <state_interface name="angular_velocity.z"/>
+      <state_interface name="linear_acceleration.x"/>
+      <state_interface name="linear_acceleration.y"/>
+      <state_interface name="linear_acceleration.z"/>
     </sensor>
-  </gazebo>
-  <!-- Foot contacts. -->
-  <gazebo reference="FR_calf">
-    <sensor name="FR_foot_contact" type="contact">
-      <update_rate>100</update_rate>
-      <plugin filename="libunitreeFootContactPlugin.so" name="contactPlugin"/>
-      <contact>
-        <collision>FR_calf_fixed_joint_lump__FR_foot_collision_1</collision>
-      </contact>
+    <sensor name="foot_force">
+      <state_interface name="FR"/>
+      <state_interface name="FL"/>
+      <state_interface name="RR"/>
+      <state_interface name="RL"/>
     </sensor>
-  </gazebo>
-  <gazebo reference="FL_calf">
-    <sensor name="FL_foot_contact" type="contact">
-      <update_rate>100</update_rate>
-      <plugin filename="libunitreeFootContactPlugin.so" name="contactPlugin"/>
-      <contact>
-        <collision>FL_calf_fixed_joint_lump__FL_foot_collision_1</collision>
-      </contact>
-    </sensor>
-  </gazebo>
-  <gazebo reference="RR_calf">
-    <sensor name="RR_foot_contact" type="contact">
-      <update_rate>100</update_rate>
-      <plugin filename="libunitreeFootContactPlugin.so" name="contactPlugin"/>
-      <contact>
-        <collision>RR_calf_fixed_joint_lump__RR_foot_collision_1</collision>
-      </contact>
-    </sensor>
-  </gazebo>
-  <gazebo reference="RL_calf">
-    <sensor name="RL_foot_contact" type="contact">
-      <update_rate>100</update_rate>
-      <plugin filename="libunitreeFootContactPlugin.so" name="contactPlugin"/>
-      <contact>
-        <collision>RL_calf_fixed_joint_lump__RL_foot_collision_1</collision>
-      </contact>
-    </sensor>
-  </gazebo>
-  <!-- Visualization of Foot contacts. -->
-  <gazebo reference="FR_foot">
-    <visual>
-      <plugin filename="libunitreeDrawForcePlugin.so" name="drawForcePlugin">
-        <topicName>FR_foot_contact</topicName>
-      </plugin>
-    </visual>
-  </gazebo>
-  <gazebo reference="FL_foot">
-    <visual>
-      <plugin filename="libunitreeDrawForcePlugin.so" name="drawForcePlugin">
-        <topicName>FL_foot_contact</topicName>
-      </plugin>
-    </visual>
-  </gazebo>
-  <gazebo reference="RR_foot">
-    <visual>
-      <plugin filename="libunitreeDrawForcePlugin.so" name="drawForcePlugin">
-        <topicName>RR_foot_contact</topicName>
-      </plugin>
-    </visual>
-  </gazebo>
-  <gazebo reference="RL_foot">
-    <visual>
-      <plugin filename="libunitreeDrawForcePlugin.so" name="drawForcePlugin">
-        <topicName>RL_foot_contact</topicName>
-      </plugin>
-    </visual>
-  </gazebo>
-  <gazebo reference="base">
-    <material>Gazebo/Green</material>
-    <turnGravityOff>false</turnGravityOff>
-  </gazebo>
-  <gazebo reference="trunk">
-    <mu1>0.2</mu1>
-    <mu2>0.2</mu2>
-    <kp value="1000000.0"/>
-    <kd value="1.0"/>
-  </gazebo>
-  <gazebo reference="stick_link">
-    <mu1>0.2</mu1>
-    <mu2>0.2</mu2>
-    <material>Gazebo/White</material>
-  </gazebo>
-  <gazebo reference="imu_link">
-    <mu1>0.2</mu1>
-    <mu2>0.2</mu2>
-    <material>Gazebo/Red</material>
-  </gazebo>
-  <!-- FL leg -->
-  <gazebo reference="FL_hip">
-    <mu1>0.2</mu1>
-    <mu2>0.2</mu2>
-    <material>Gazebo/DarkGrey</material>
-  </gazebo>
-  <gazebo reference="FL_thigh">
-    <mu1>0.2</mu1>
-    <mu2>0.2</mu2>
-    <self_collide>1</self_collide>
-    <material>Gazebo/DarkGrey</material>
-    <kp value="1000000.0"/>
-    <kd value="1.0"/>
-  </gazebo>
-  <gazebo reference="FL_calf">
-    <mu1>0.2</mu1>
-    <mu2>0.2</mu2>
-    <self_collide>1</self_collide>
-  </gazebo>
-  <gazebo reference="FL_foot">
-    <mu1>0.6</mu1>
-    <mu2>0.6</mu2>
-    <self_collide>1</self_collide>
-    <material>Gazebo/DarkGrey</material>
-    <kp value="1000000.0"/>
-    <kd value="1.0"/>
-  </gazebo>
-  <!-- FR leg -->
-  <gazebo reference="FR_hip">
-    <mu1>0.2</mu1>
-    <mu2>0.2</mu2>
-    <material>Gazebo/DarkGrey</material>
-  </gazebo>
-  <gazebo reference="FR_thigh">
-    <mu1>0.2</mu1>
-    <mu2>0.2</mu2>
-    <self_collide>1</self_collide>
-    <material>Gazebo/DarkGrey</material>
-    <kp value="1000000.0"/>
-    <kd value="1.0"/>
-  </gazebo>
-  <gazebo reference="FR_calf">
-    <mu1>0.2</mu1>
-    <mu2>0.2</mu2>
-    <self_collide>1</self_collide>
-  </gazebo>
-  <gazebo reference="FR_foot">
-    <mu1>0.6</mu1>
-    <mu2>0.6</mu2>
-    <self_collide>1</self_collide>
-    <material>Gazebo/DarkGrey</material>
-    <kp value="1000000.0"/>
-    <kd value="1.0"/>
-  </gazebo>
-  <!-- RL leg -->
-  <gazebo reference="RL_hip">
-    <mu1>0.2</mu1>
-    <mu2>0.2</mu2>
-    <material>Gazebo/DarkGrey</material>
-  </gazebo>
-  <gazebo reference="RL_thigh">
-    <mu1>0.2</mu1>
-    <mu2>0.2</mu2>
-    <self_collide>1</self_collide>
-    <material>Gazebo/DarkGrey</material>
-    <kp value="1000000.0"/>
-    <kd value="1.0"/>
-  </gazebo>
-  <gazebo reference="RL_calf">
-    <mu1>0.2</mu1>
-    <mu2>0.2</mu2>
-    <self_collide>1</self_collide>
-  </gazebo>
-  <gazebo reference="RL_foot">
-    <mu1>0.6</mu1>
-    <mu2>0.6</mu2>
-    <self_collide>1</self_collide>
-    <material>Gazebo/DarkGrey</material>
-    <kp value="1000000.0"/>
-    <kd value="1.0"/>
-  </gazebo>
-  <!-- RR leg -->
-  <gazebo reference="RR_hip">
-    <mu1>0.2</mu1>
-    <mu2>0.2</mu2>
-    <material>Gazebo/DarkGrey</material>
-  </gazebo>
-  <gazebo reference="RR_thigh">
-    <mu1>0.2</mu1>
-    <mu2>0.2</mu2>
-    <self_collide>1</self_collide>
-    <material>Gazebo/DarkGrey</material>
-    <kp value="1000000.0"/>
-    <kd value="1.0"/>
-  </gazebo>
-  <gazebo reference="RR_calf">
-    <mu1>0.2</mu1>
-    <mu2>0.2</mu2>
-    <self_collide>1</self_collide>
-  </gazebo>
-  <gazebo reference="RR_foot">
-    <mu1>0.6</mu1>
-    <mu2>0.6</mu2>
-    <self_collide>1</self_collide>
-    <material>Gazebo/DarkGrey</material>
-    <kp value="1000000.0"/>
-    <kd value="1.0"/>
-  </gazebo>
+  </ros2_control>
+  <!--    <xacro:include filename="$(find aliengo_description)/xacro/gazebo.xacro"/>-->
   <!-- Rotor related joint and link is only for demonstrate location. -->
   <!-- Actually, the rotor will rotate and the joint is not fixed. Reduction ratio should be considered. -->
-  <!-- Debug mode will hung up the robot, use "true" or "false" to switch it. -->
-  <!-- <xacro:if value="$(arg DEBUG)">
-        <link name="world"/>
-        <joint name="base_static_joint" type="fixed">
-            <origin rpy="0 0 0" xyz="0 0 0"/>
-            <parent link="world"/>
-            <child link="base"/>
-        </joint>
-    </xacro:if>  -->
   <link name="base">
     <visual>
       <origin rpy="0 0 0" xyz="0 0 0"/>
@@ -307,7 +194,7 @@
     <visual>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://aliengo_description/meshes/trunk.dae" scale="1 1 1"/>
+        <mesh filename="file:///home/tlab-uav/ros2_ws/install/aliengo_description/share/aliengo_description/meshes/trunk.dae" scale="1 1 1"/>
       </geometry>
       <material name="orange"/>
     </visual>
@@ -317,12 +204,6 @@
         <box size="0.647 0.15 0.112"/>
       </geometry>
     </collision>
-    <!-- <collision>
-            <origin rpy="0 0 0" xyz="0 0 0"/>
-            <geometry>
-                <mesh filename="package://aliengo_description/meshes/trunk.dae" scale="1 1 1"/>
-            </geometry>
-        </collision> -->
     <inertial>
       <origin rpy="0 0 0" xyz="0.008811 0.003839 0.000273"/>
       <mass value="11.644"/>
@@ -338,7 +219,7 @@
     <inertial>
       <mass value="0.001"/>
       <origin rpy="0 0 0" xyz="0 0 0"/>
-      <inertia ixx="0.0001" ixy="0" ixz="0" iyy="0.000001" iyz="0" izz="0.0001"/>
+      <inertia ixx="0.0001" ixy="0" ixz="0" iyy="0.0001" iyz="0" izz="0.0001"/>
     </inertial>
     <visual>
       <origin rpy="0 0 0" xyz="0 0 0"/>
@@ -371,7 +252,7 @@
     <visual>
       <origin rpy="3.141592653589793 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://aliengo_description/meshes/hip.dae" scale="1 1 1"/>
+        <mesh filename="file:///home/tlab-uav/ros2_ws/install/aliengo_description/share/aliengo_description/meshes/hip.dae" scale="1 1 1"/>
       </geometry>
       <material name="orange"/>
     </visual>
@@ -395,19 +276,19 @@
       </geometry>
       <material name="green"/>
     </visual>
-    <collision>
-      <origin rpy="0 1.5707963267948966 0" xyz="0 0 0"/>
-      <geometry>
-        <cylinder length="0.02" radius="0.035"/>
-      </geometry>
-    </collision>
+    <!-- <collision>
+          <origin rpy="0 ${PI/2.0} 0" xyz="0 0 0"/>
+          <geometry>
+              <cylinder length="0.02" radius="0.035"/>
+          </geometry>
+      </collision> -->
     <inertial>
       <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
       <mass value="0.146"/>
       <inertia ixx="0.000138702" ixy="0.0" ixz="0.0" iyy="8.3352e-05" iyz="0.0" izz="8.3352e-05"/>
     </inertial>
   </link>
-  <joint name="FR_thigh_joint" type="continuous">
+  <joint name="FR_thigh_joint" type="revolute">
     <origin rpy="0 0 0" xyz="0 -0.0868 0"/>
     <parent link="FR_hip"/>
     <child link="FR_thigh"/>
@@ -424,7 +305,7 @@
     <visual>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://aliengo_description/meshes/thigh_mirror.dae" scale="1 1 1"/>
+        <mesh filename="file:///home/tlab-uav/ros2_ws/install/aliengo_description/share/aliengo_description/meshes/thigh_mirror.dae" scale="1 1 1"/>
       </geometry>
       <material name="orange"/>
     </visual>
@@ -448,12 +329,12 @@
       </geometry>
       <material name="green"/>
     </visual>
-    <collision>
-      <origin rpy="1.5707963267948966 0 0" xyz="0 0 0"/>
-      <geometry>
-        <cylinder length="0.02" radius="0.035"/>
-      </geometry>
-    </collision>
+    <!-- <collision>
+          <origin rpy="${PI/2.0} 0 0" xyz="0 0 0"/>
+          <geometry>
+              <cylinder length="0.02" radius="0.035"/>
+          </geometry>
+      </collision> -->
     <inertial>
       <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
       <mass value="0.146"/>
@@ -477,7 +358,7 @@
     <visual>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://aliengo_description/meshes/calf.dae" scale="1 1 1"/>
+        <mesh filename="file:///home/tlab-uav/ros2_ws/install/aliengo_description/share/aliengo_description/meshes/calf.dae" scale="1 1 1"/>
       </geometry>
       <material name="orange"/>
     </visual>
@@ -501,12 +382,12 @@
       </geometry>
       <material name="green"/>
     </visual>
-    <collision>
-      <origin rpy="1.5707963267948966 0 0" xyz="0 0 0"/>
-      <geometry>
-        <cylinder length="0.02" radius="0.035"/>
-      </geometry>
-    </collision>
+    <!-- <collision>
+          <origin rpy="${PI/2.0} 0 0" xyz="0 0 0"/>
+          <geometry>
+              <cylinder length="0.02" radius="0.035"/>
+          </geometry>
+      </collision> -->
     <inertial>
       <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
       <mass value="0.132"/>
@@ -584,7 +465,7 @@
     <visual>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://aliengo_description/meshes/hip.dae" scale="1 1 1"/>
+        <mesh filename="file:///home/tlab-uav/ros2_ws/install/aliengo_description/share/aliengo_description/meshes/hip.dae" scale="1 1 1"/>
       </geometry>
       <material name="orange"/>
     </visual>
@@ -608,19 +489,19 @@
       </geometry>
       <material name="green"/>
     </visual>
-    <collision>
-      <origin rpy="0 1.5707963267948966 0" xyz="0 0 0"/>
-      <geometry>
-        <cylinder length="0.02" radius="0.035"/>
-      </geometry>
-    </collision>
+    <!-- <collision>
+          <origin rpy="0 ${PI/2.0} 0" xyz="0 0 0"/>
+          <geometry>
+              <cylinder length="0.02" radius="0.035"/>
+          </geometry>
+      </collision> -->
     <inertial>
       <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
       <mass value="0.146"/>
       <inertia ixx="0.000138702" ixy="0.0" ixz="0.0" iyy="8.3352e-05" iyz="0.0" izz="8.3352e-05"/>
     </inertial>
   </link>
-  <joint name="FL_thigh_joint" type="continuous">
+  <joint name="FL_thigh_joint" type="revolute">
     <origin rpy="0 0 0" xyz="0 0.0868 0"/>
     <parent link="FL_hip"/>
     <child link="FL_thigh"/>
@@ -637,7 +518,7 @@
     <visual>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://aliengo_description/meshes/thigh.dae" scale="1 1 1"/>
+        <mesh filename="file:///home/tlab-uav/ros2_ws/install/aliengo_description/share/aliengo_description/meshes/thigh.dae" scale="1 1 1"/>
       </geometry>
       <material name="orange"/>
     </visual>
@@ -661,12 +542,12 @@
       </geometry>
       <material name="green"/>
     </visual>
-    <collision>
-      <origin rpy="1.5707963267948966 0 0" xyz="0 0 0"/>
-      <geometry>
-        <cylinder length="0.02" radius="0.035"/>
-      </geometry>
-    </collision>
+    <!-- <collision>
+          <origin rpy="${PI/2.0} 0 0" xyz="0 0 0"/>
+          <geometry>
+              <cylinder length="0.02" radius="0.035"/>
+          </geometry>
+      </collision> -->
     <inertial>
       <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
       <mass value="0.146"/>
@@ -690,7 +571,7 @@
     <visual>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://aliengo_description/meshes/calf.dae" scale="1 1 1"/>
+        <mesh filename="file:///home/tlab-uav/ros2_ws/install/aliengo_description/share/aliengo_description/meshes/calf.dae" scale="1 1 1"/>
       </geometry>
       <material name="orange"/>
     </visual>
@@ -714,12 +595,12 @@
       </geometry>
       <material name="green"/>
     </visual>
-    <collision>
-      <origin rpy="1.5707963267948966 0 0" xyz="0 0 0"/>
-      <geometry>
-        <cylinder length="0.02" radius="0.035"/>
-      </geometry>
-    </collision>
+    <!-- <collision>
+          <origin rpy="${PI/2.0} 0 0" xyz="0 0 0"/>
+          <geometry>
+              <cylinder length="0.02" radius="0.035"/>
+          </geometry>
+      </collision> -->
     <inertial>
       <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
       <mass value="0.132"/>
@@ -797,7 +678,7 @@
     <visual>
       <origin rpy="3.141592653589793 3.141592653589793 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://aliengo_description/meshes/hip.dae" scale="1 1 1"/>
+        <mesh filename="file:///home/tlab-uav/ros2_ws/install/aliengo_description/share/aliengo_description/meshes/hip.dae" scale="1 1 1"/>
       </geometry>
       <material name="orange"/>
     </visual>
@@ -821,19 +702,19 @@
       </geometry>
       <material name="green"/>
     </visual>
-    <collision>
-      <origin rpy="0 1.5707963267948966 0" xyz="0 0 0"/>
-      <geometry>
-        <cylinder length="0.02" radius="0.035"/>
-      </geometry>
-    </collision>
+    <!-- <collision>
+          <origin rpy="0 ${PI/2.0} 0" xyz="0 0 0"/>
+          <geometry>
+              <cylinder length="0.02" radius="0.035"/>
+          </geometry>
+      </collision> -->
     <inertial>
       <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
       <mass value="0.146"/>
       <inertia ixx="0.000138702" ixy="0.0" ixz="0.0" iyy="8.3352e-05" iyz="0.0" izz="8.3352e-05"/>
     </inertial>
   </link>
-  <joint name="RR_thigh_joint" type="continuous">
+  <joint name="RR_thigh_joint" type="revolute">
     <origin rpy="0 0 0" xyz="0 -0.0868 0"/>
     <parent link="RR_hip"/>
     <child link="RR_thigh"/>
@@ -850,7 +731,7 @@
     <visual>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://aliengo_description/meshes/thigh_mirror.dae" scale="1 1 1"/>
+        <mesh filename="file:///home/tlab-uav/ros2_ws/install/aliengo_description/share/aliengo_description/meshes/thigh_mirror.dae" scale="1 1 1"/>
       </geometry>
       <material name="orange"/>
     </visual>
@@ -874,12 +755,12 @@
       </geometry>
       <material name="green"/>
     </visual>
-    <collision>
-      <origin rpy="1.5707963267948966 0 0" xyz="0 0 0"/>
-      <geometry>
-        <cylinder length="0.02" radius="0.035"/>
-      </geometry>
-    </collision>
+    <!-- <collision>
+          <origin rpy="${PI/2.0} 0 0" xyz="0 0 0"/>
+          <geometry>
+              <cylinder length="0.02" radius="0.035"/>
+          </geometry>
+      </collision> -->
     <inertial>
       <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
       <mass value="0.146"/>
@@ -903,7 +784,7 @@
     <visual>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://aliengo_description/meshes/calf.dae" scale="1 1 1"/>
+        <mesh filename="file:///home/tlab-uav/ros2_ws/install/aliengo_description/share/aliengo_description/meshes/calf.dae" scale="1 1 1"/>
       </geometry>
       <material name="orange"/>
     </visual>
@@ -927,12 +808,12 @@
       </geometry>
       <material name="green"/>
     </visual>
-    <collision>
-      <origin rpy="1.5707963267948966 0 0" xyz="0 0 0"/>
-      <geometry>
-        <cylinder length="0.02" radius="0.035"/>
-      </geometry>
-    </collision>
+    <!-- <collision>
+          <origin rpy="${PI/2.0} 0 0" xyz="0 0 0"/>
+          <geometry>
+              <cylinder length="0.02" radius="0.035"/>
+          </geometry>
+      </collision> -->
     <inertial>
       <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
       <mass value="0.132"/>
@@ -1010,7 +891,7 @@
     <visual>
       <origin rpy="0 3.141592653589793 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://aliengo_description/meshes/hip.dae" scale="1 1 1"/>
+        <mesh filename="file:///home/tlab-uav/ros2_ws/install/aliengo_description/share/aliengo_description/meshes/hip.dae" scale="1 1 1"/>
       </geometry>
       <material name="orange"/>
     </visual>
@@ -1034,19 +915,19 @@
       </geometry>
       <material name="green"/>
     </visual>
-    <collision>
-      <origin rpy="0 1.5707963267948966 0" xyz="0 0 0"/>
-      <geometry>
-        <cylinder length="0.02" radius="0.035"/>
-      </geometry>
-    </collision>
+    <!-- <collision>
+          <origin rpy="0 ${PI/2.0} 0" xyz="0 0 0"/>
+          <geometry>
+              <cylinder length="0.02" radius="0.035"/>
+          </geometry>
+      </collision> -->
     <inertial>
       <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
       <mass value="0.146"/>
       <inertia ixx="0.000138702" ixy="0.0" ixz="0.0" iyy="8.3352e-05" iyz="0.0" izz="8.3352e-05"/>
     </inertial>
   </link>
-  <joint name="RL_thigh_joint" type="continuous">
+  <joint name="RL_thigh_joint" type="revolute">
     <origin rpy="0 0 0" xyz="0 0.0868 0"/>
     <parent link="RL_hip"/>
     <child link="RL_thigh"/>
@@ -1063,7 +944,7 @@
     <visual>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://aliengo_description/meshes/thigh.dae" scale="1 1 1"/>
+        <mesh filename="file:///home/tlab-uav/ros2_ws/install/aliengo_description/share/aliengo_description/meshes/thigh.dae" scale="1 1 1"/>
       </geometry>
       <material name="orange"/>
     </visual>
@@ -1087,12 +968,12 @@
       </geometry>
       <material name="green"/>
     </visual>
-    <collision>
-      <origin rpy="1.5707963267948966 0 0" xyz="0 0 0"/>
-      <geometry>
-        <cylinder length="0.02" radius="0.035"/>
-      </geometry>
-    </collision>
+    <!-- <collision>
+          <origin rpy="${PI/2.0} 0 0" xyz="0 0 0"/>
+          <geometry>
+              <cylinder length="0.02" radius="0.035"/>
+          </geometry>
+      </collision> -->
     <inertial>
       <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
       <mass value="0.146"/>
@@ -1116,7 +997,7 @@
     <visual>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://aliengo_description/meshes/calf.dae" scale="1 1 1"/>
+        <mesh filename="file:///home/tlab-uav/ros2_ws/install/aliengo_description/share/aliengo_description/meshes/calf.dae" scale="1 1 1"/>
       </geometry>
       <material name="orange"/>
     </visual>
@@ -1140,12 +1021,12 @@
       </geometry>
       <material name="green"/>
     </visual>
-    <collision>
-      <origin rpy="1.5707963267948966 0 0" xyz="0 0 0"/>
-      <geometry>
-        <cylinder length="0.02" radius="0.035"/>
-      </geometry>
-    </collision>
+    <!-- <collision>
+          <origin rpy="${PI/2.0} 0 0" xyz="0 0 0"/>
+          <geometry>
+              <cylinder length="0.02" radius="0.035"/>
+          </geometry>
+      </collision> -->
     <inertial>
       <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
       <mass value="0.132"/>
@@ -1208,4 +1089,3 @@
   </transmission>
   <!-- <xacro:stairs stairs="15" xpos="0" ypos="2.0" zpos="0" /> -->
 </robot>
-
diff --git a/descriptions/aliengo_description/xacro/leg.xacro b/descriptions/aliengo_description/xacro/leg.xacro
index 65009e8..f6509ab 100644
--- a/descriptions/aliengo_description/xacro/leg.xacro
+++ b/descriptions/aliengo_description/xacro/leg.xacro
@@ -2,274 +2,253 @@
 
 <robot xmlns:xacro="http://www.ros.org/wiki/xacro">
 
-	<xacro:include filename="$(find aliengo_description)/xacro/transmission.xacro"/>
+  <xacro:include filename="$(find aliengo_description)/xacro/transmission.xacro"/>
 
-	<xacro:macro name="leg" params="name mirror mirror_dae front_hind front_hind_dae">
+  <xacro:macro name="leg" params="name mirror mirror_dae front_hind front_hind_dae">
 
-		<joint name="${name}_hip_joint" type="revolute">
-			<xacro:if value="${(mirror_dae == False) and (front_hind_dae == True)}">
-				<origin rpy="0 0 0" xyz="${leg_offset_x} ${-leg_offset_y} 0"/>
-			</xacro:if>
-			<xacro:if value="${(mirror_dae == True) and (front_hind_dae == True)}">
-				<origin rpy="0 0 0" xyz="${leg_offset_x} ${leg_offset_y} 0"/>
-			</xacro:if>
-			<xacro:if value="${(mirror_dae == False) and (front_hind_dae == False)}">
-				<origin rpy="0 0 0" xyz="${-leg_offset_x} ${-leg_offset_y} 0"/>
-			</xacro:if>
-			<xacro:if value="${(mirror_dae == True) and (front_hind_dae == False)}">
-				<origin rpy="0 0 0" xyz="${-leg_offset_x} ${leg_offset_y} 0"/>
-			</xacro:if>
-			<parent link="trunk"/>
-			<child link="${name}_hip"/>
-			<axis xyz="1 0 0"/>
-			<dynamics damping="${damping}" friction="${friction}"/>
-			<xacro:if value="${(mirror_dae == True)}">
-				<limit effort="${hip_torque_max}" velocity="${hip_velocity_max}" lower="${hip_position_min}"
-				       upper="${hip_position_max}"/>
-			</xacro:if>
-			<xacro:if value="${(mirror_dae == False)}">
-				<limit effort="${hip_torque_max}" velocity="${hip_velocity_max}" lower="${-hip_position_max}"
-				       upper="${-hip_position_min}"/>
-			</xacro:if>
-		</joint>
+    <joint name="${name}_hip_joint" type="revolute">
+      <xacro:if value="${(mirror_dae == False) and (front_hind_dae == True)}">
+        <origin rpy="0 0 0" xyz="${leg_offset_x} ${-leg_offset_y} 0"/>
+      </xacro:if>
+      <xacro:if value="${(mirror_dae == True) and (front_hind_dae == True)}">
+        <origin rpy="0 0 0" xyz="${leg_offset_x} ${leg_offset_y} 0"/>
+      </xacro:if>
+      <xacro:if value="${(mirror_dae == False) and (front_hind_dae == False)}">
+        <origin rpy="0 0 0" xyz="${-leg_offset_x} ${-leg_offset_y} 0"/>
+      </xacro:if>
+      <xacro:if value="${(mirror_dae == True) and (front_hind_dae == False)}">
+        <origin rpy="0 0 0" xyz="${-leg_offset_x} ${leg_offset_y} 0"/>
+      </xacro:if>
+      <parent link="trunk"/>
+      <child link="${name}_hip"/>
+      <axis xyz="1 0 0"/>
+      <dynamics damping="${damping}" friction="${friction}"/>
+      <xacro:if value="${(mirror_dae == True)}">
+        <limit effort="${hip_torque_max}" velocity="${hip_velocity_max}" lower="${hip_position_min}"
+               upper="${hip_position_max}"/>
+      </xacro:if>
+      <xacro:if value="${(mirror_dae == False)}">
+        <limit effort="${hip_torque_max}" velocity="${hip_velocity_max}" lower="${-hip_position_max}"
+               upper="${-hip_position_min}"/>
+      </xacro:if>
+    </joint>
 
-		<joint name="${name}_hip_rotor_joint" type="fixed">
-			<xacro:if value="${(mirror_dae == False) and (front_hind_dae == True)}">
-				<origin rpy="0 0 0" xyz="${hip_rotor_offset_x} ${-hip_rotor_offset_y} 0"/>
-			</xacro:if>
-			<xacro:if value="${(mirror_dae == True) and (front_hind_dae == True)}">
-				<origin rpy="0 0 0" xyz="${hip_rotor_offset_x} ${hip_rotor_offset_y} 0"/>
-			</xacro:if>
-			<xacro:if value="${(mirror_dae == False) and (front_hind_dae == False)}">
-				<origin rpy="0 0 0" xyz="${-hip_rotor_offset_x} ${-hip_rotor_offset_y} 0"/>
-			</xacro:if>
-			<xacro:if value="${(mirror_dae == True) and (front_hind_dae == False)}">
-				<origin rpy="0 0 0" xyz="${-hip_rotor_offset_x} ${hip_rotor_offset_y} 0"/>
-			</xacro:if>
-			<parent link="trunk"/>
-			<child link="${name}_hip_rotor"/>
-		</joint>
+    <joint name="${name}_hip_rotor_joint" type="fixed">
+      <xacro:if value="${(mirror_dae == False) and (front_hind_dae == True)}">
+        <origin rpy="0 0 0" xyz="${hip_rotor_offset_x} ${-hip_rotor_offset_y} 0"/>
+      </xacro:if>
+      <xacro:if value="${(mirror_dae == True) and (front_hind_dae == True)}">
+        <origin rpy="0 0 0" xyz="${hip_rotor_offset_x} ${hip_rotor_offset_y} 0"/>
+      </xacro:if>
+      <xacro:if value="${(mirror_dae == False) and (front_hind_dae == False)}">
+        <origin rpy="0 0 0" xyz="${-hip_rotor_offset_x} ${-hip_rotor_offset_y} 0"/>
+      </xacro:if>
+      <xacro:if value="${(mirror_dae == True) and (front_hind_dae == False)}">
+        <origin rpy="0 0 0" xyz="${-hip_rotor_offset_x} ${hip_rotor_offset_y} 0"/>
+      </xacro:if>
+      <parent link="trunk"/>
+      <child link="${name}_hip_rotor"/>
+    </joint>
 
-		<link name="${name}_hip">
-			<visual>
-				<xacro:if value="${(mirror_dae == True) and (front_hind_dae == True)}">
-					<origin rpy="0 0 0" xyz="0 0 0"/>
-				</xacro:if>
-				<xacro:if value="${(mirror_dae == False) and (front_hind_dae == True)}">
-					<origin rpy="${PI} 0 0" xyz="0 0 0"/>
-				</xacro:if>
-				<xacro:if value="${(mirror_dae == True) and (front_hind_dae == False)}">
-					<origin rpy="0 ${PI} 0" xyz="0 0 0"/>
-				</xacro:if>
-				<xacro:if value="${(mirror_dae == False) and (front_hind_dae == False)}">
-					<origin rpy="${PI} ${PI} 0" xyz="0 0 0"/>
-				</xacro:if>
-				<geometry>
-					<mesh filename="file://$(find aliengo_description)/meshes/hip.dae" scale="1 1 1"/>
-				</geometry>
-				<material name="orange"/>
-			</visual>
-			<collision>
-				<origin rpy="${PI/2.0} 0 0" xyz="0 ${hip_offset*mirror} 0"/>
-				<geometry>
-					<cylinder length="${hip_length}" radius="${hip_radius}"/>
-				</geometry>
-			</collision>
-			<inertial>
-				<origin rpy="0 0 0" xyz="${hip_com_x*front_hind} ${hip_com_y*mirror} ${hip_com_z}"/>
-				<mass value="${hip_mass}"/>
-				<inertia
-						ixx="${hip_ixx}" ixy="${hip_ixy*mirror*front_hind}" ixz="${hip_ixz*front_hind}"
-						iyy="${hip_iyy}" iyz="${hip_iyz*mirror}"
-						izz="${hip_izz}"/>
-			</inertial>
-		</link>
+    <link name="${name}_hip">
+      <visual>
+        <xacro:if value="${(mirror_dae == True) and (front_hind_dae == True)}">
+          <origin rpy="0 0 0" xyz="0 0 0"/>
+        </xacro:if>
+        <xacro:if value="${(mirror_dae == False) and (front_hind_dae == True)}">
+          <origin rpy="${PI} 0 0" xyz="0 0 0"/>
+        </xacro:if>
+        <xacro:if value="${(mirror_dae == True) and (front_hind_dae == False)}">
+          <origin rpy="0 ${PI} 0" xyz="0 0 0"/>
+        </xacro:if>
+        <xacro:if value="${(mirror_dae == False) and (front_hind_dae == False)}">
+          <origin rpy="${PI} ${PI} 0" xyz="0 0 0"/>
+        </xacro:if>
+        <geometry>
+          <mesh filename="file://$(find aliengo_description)/meshes/hip.dae" scale="1 1 1"/>
+        </geometry>
+        <material name="orange"/>
+      </visual>
+      <collision>
+        <origin rpy="${PI/2.0} 0 0" xyz="0 ${hip_offset*mirror} 0"/>
+        <geometry>
+          <cylinder length="${hip_length}" radius="${hip_radius}"/>
+        </geometry>
+      </collision>
+      <inertial>
+        <origin rpy="0 0 0" xyz="${hip_com_x*front_hind} ${hip_com_y*mirror} ${hip_com_z}"/>
+        <mass value="${hip_mass}"/>
+        <inertia
+            ixx="${hip_ixx}" ixy="${hip_ixy*mirror*front_hind}" ixz="${hip_ixz*front_hind}"
+            iyy="${hip_iyy}" iyz="${hip_iyz*mirror}"
+            izz="${hip_izz}"/>
+      </inertial>
+    </link>
 
-		<link name="${name}_hip_rotor">
-			<visual>
-				<origin rpy="0 ${PI/2.0} 0" xyz="0 0 0"/>
-				<geometry>
-					<cylinder length="0.02" radius="0.035"/>
-				</geometry>
-				<material name="green"/>
-			</visual>
-			<!-- <collision>
-					<origin rpy="0 ${PI/2.0} 0" xyz="0 0 0"/>
-					<geometry>
-							<cylinder length="0.02" radius="0.035"/>
-					</geometry>
-			</collision> -->
-			<inertial>
-				<origin rpy="0 0 0" xyz="${hip_rotor_com_x} ${hip_rotor_com_y} ${hip_rotor_com_z}"/>
-				<mass value="${hip_rotor_mass}"/>
-				<inertia
-						ixx="${hip_rotor_ixx}" ixy="${hip_rotor_ixy}" ixz="${hip_rotor_ixz}"
-						iyy="${hip_rotor_iyy}" iyz="${hip_rotor_iyz}"
-						izz="${hip_rotor_izz}"/>
-			</inertial>
-		</link>
+    <link name="${name}_hip_rotor">
+      <visual>
+        <origin rpy="0 ${PI/2.0} 0" xyz="0 0 0"/>
+        <geometry>
+          <cylinder length="0.02" radius="0.035"/>
+        </geometry>
+        <material name="green"/>
+      </visual>
+      <inertial>
+        <origin rpy="0 0 0" xyz="${hip_rotor_com_x} ${hip_rotor_com_y} ${hip_rotor_com_z}"/>
+        <mass value="${hip_rotor_mass}"/>
+        <inertia
+            ixx="${hip_rotor_ixx}" ixy="${hip_rotor_ixy}" ixz="${hip_rotor_ixz}"
+            iyy="${hip_rotor_iyy}" iyz="${hip_rotor_iyz}"
+            izz="${hip_rotor_izz}"/>
+      </inertial>
+    </link>
 
-		<joint name="${name}_thigh_joint" type="continuous">
-			<origin rpy="0 0 0" xyz="0 ${thigh_offset*mirror} 0"/>
-			<parent link="${name}_hip"/>
-			<child link="${name}_thigh"/>
-			<axis xyz="0 1 0"/>
-			<dynamics damping="${damping}" friction="${friction}"/>
-			<limit effort="${thigh_torque_max}" velocity="${thigh_velocity_max}"/>
-		</joint>
+    <joint name="${name}_thigh_joint" type="revolute">
+      <origin rpy="0 0 0" xyz="0 ${thigh_offset*mirror} 0"/>
+      <parent link="${name}_hip"/>
+      <child link="${name}_thigh"/>
+      <axis xyz="0 1 0"/>
+      <dynamics damping="${damping}" friction="${friction}"/>
+      <limit effort="${thigh_torque_max}" velocity="${thigh_velocity_max}"/>
+    </joint>
 
-		<joint name="${name}_thigh_rotor_joint" type="fixed">
-			<origin rpy="0 0 0" xyz="${thigh_rotor_offset_x} ${thigh_rotor_offset_y*mirror} 0"/>
-			<parent link="${name}_hip"/>
-			<child link="${name}_thigh_rotor"/>
-		</joint>
+    <joint name="${name}_thigh_rotor_joint" type="fixed">
+      <origin rpy="0 0 0" xyz="${thigh_rotor_offset_x} ${thigh_rotor_offset_y*mirror} 0"/>
+      <parent link="${name}_hip"/>
+      <child link="${name}_thigh_rotor"/>
+    </joint>
 
-		<link name="${name}_thigh">
-			<visual>
-				<origin rpy="0 0 0" xyz="0 0 0"/>
-				<geometry>
-					<xacro:if value="${mirror_dae == True}">
-						<mesh filename="file://$(find aliengo_description)/meshes/thigh.dae" scale="1 1 1"/>
-					</xacro:if>
-					<xacro:if value="${mirror_dae == False}">
-						<mesh filename="file://$(find aliengo_description)/meshes/thigh_mirror.dae" scale="1 1 1"/>
-					</xacro:if>
-				</geometry>
-				<material name="orange"/>
-			</visual>
-			<collision>
-				<origin rpy="0 ${PI/2.0} 0" xyz="0 0 ${-thigh_length/2.0}"/>
-				<geometry>
-					<box size="${thigh_length} ${thigh_width} ${thigh_height}"/>
-				</geometry>
-			</collision>
-			<inertial>
-				<origin rpy="0 0 0" xyz="${thigh_com_x} ${thigh_com_y*mirror} ${thigh_com_z}"/>
-				<mass value="${thigh_mass}"/>
-				<inertia
-						ixx="${thigh_ixx}" ixy="${thigh_ixy*mirror}" ixz="${thigh_ixz}"
-						iyy="${thigh_iyy}" iyz="${thigh_iyz*mirror}"
-						izz="${thigh_izz}"/>
-			</inertial>
-		</link>
+    <link name="${name}_thigh">
+      <visual>
+        <origin rpy="0 0 0" xyz="0 0 0"/>
+        <geometry>
+          <xacro:if value="${mirror_dae == True}">
+            <mesh filename="file://$(find aliengo_description)/meshes/thigh.dae" scale="1 1 1"/>
+          </xacro:if>
+          <xacro:if value="${mirror_dae == False}">
+            <mesh filename="file://$(find aliengo_description)/meshes/thigh_mirror.dae" scale="1 1 1"/>
+          </xacro:if>
+        </geometry>
+        <material name="orange"/>
+      </visual>
+      <collision>
+        <origin rpy="0 ${PI/2.0} 0" xyz="0 0 ${-thigh_length/2.0}"/>
+        <geometry>
+          <box size="${thigh_length} ${thigh_width} ${thigh_height}"/>
+        </geometry>
+      </collision>
+      <inertial>
+        <origin rpy="0 0 0" xyz="${thigh_com_x} ${thigh_com_y*mirror} ${thigh_com_z}"/>
+        <mass value="${thigh_mass}"/>
+        <inertia
+            ixx="${thigh_ixx}" ixy="${thigh_ixy*mirror}" ixz="${thigh_ixz}"
+            iyy="${thigh_iyy}" iyz="${thigh_iyz*mirror}"
+            izz="${thigh_izz}"/>
+      </inertial>
+    </link>
 
-		<link name="${name}_thigh_rotor">
-			<visual>
-				<origin rpy="${PI/2.0} 0 0" xyz="0 0 0"/>
-				<geometry>
-					<cylinder length="0.02" radius="0.035"/>
-				</geometry>
-				<material name="green"/>
-			</visual>
-			<!-- <collision>
-					<origin rpy="${PI/2.0} 0 0" xyz="0 0 0"/>
-					<geometry>
-							<cylinder length="0.02" radius="0.035"/>
-					</geometry>
-			</collision> -->
-			<inertial>
-				<origin rpy="0 0 0" xyz="${thigh_rotor_com_x} ${thigh_rotor_com_y} ${thigh_rotor_com_z}"/>
-				<mass value="${thigh_rotor_mass}"/>
-				<inertia
-						ixx="${thigh_rotor_ixx}" ixy="${thigh_rotor_ixy}" ixz="${thigh_rotor_ixz}"
-						iyy="${thigh_rotor_iyy}" iyz="${thigh_rotor_iyz}"
-						izz="${thigh_rotor_izz}"/>
-			</inertial>
-		</link>
+    <link name="${name}_thigh_rotor">
+      <visual>
+        <origin rpy="${PI/2.0} 0 0" xyz="0 0 0"/>
+        <geometry>
+          <cylinder length="0.02" radius="0.035"/>
+        </geometry>
+        <material name="green"/>
+      </visual>
+      <inertial>
+        <origin rpy="0 0 0" xyz="${thigh_rotor_com_x} ${thigh_rotor_com_y} ${thigh_rotor_com_z}"/>
+        <mass value="${thigh_rotor_mass}"/>
+        <inertia
+            ixx="${thigh_rotor_ixx}" ixy="${thigh_rotor_ixy}" ixz="${thigh_rotor_ixz}"
+            iyy="${thigh_rotor_iyy}" iyz="${thigh_rotor_iyz}"
+            izz="${thigh_rotor_izz}"/>
+      </inertial>
+    </link>
 
-		<joint name="${name}_calf_joint" type="revolute">
-			<origin rpy="0 0 0" xyz="0 0 ${-thigh_length}"/>
-			<parent link="${name}_thigh"/>
-			<child link="${name}_calf"/>
-			<axis xyz="0 1 0"/>
-			<dynamics damping="${damping}" friction="${friction}"/>
-			<limit effort="${calf_torque_max}" velocity="${calf_velocity_max}" lower="${calf_position_min}"
-			       upper="${calf_position_max}"/>
-		</joint>
+    <joint name="${name}_calf_joint" type="revolute">
+      <origin rpy="0 0 0" xyz="0 0 ${-thigh_length}"/>
+      <parent link="${name}_thigh"/>
+      <child link="${name}_calf"/>
+      <axis xyz="0 1 0"/>
+      <dynamics damping="${damping}" friction="${friction}"/>
+      <limit effort="${calf_torque_max}" velocity="${calf_velocity_max}" lower="${calf_position_min}"
+             upper="${calf_position_max}"/>
+    </joint>
 
-		<joint name="${name}_calf_rotor_joint" type="fixed">
-			<origin rpy="0 0 0" xyz="${calf_rotor_offset_x} ${calf_rotor_offset_y*mirror} 0"/>
-			<parent link="${name}_thigh"/>
-			<child link="${name}_calf_rotor"/>
-		</joint>
+    <joint name="${name}_calf_rotor_joint" type="fixed">
+      <origin rpy="0 0 0" xyz="${calf_rotor_offset_x} ${calf_rotor_offset_y*mirror} 0"/>
+      <parent link="${name}_thigh"/>
+      <child link="${name}_calf_rotor"/>
+    </joint>
 
-		<link name="${name}_calf">
-			<visual>
-				<origin rpy="0 0 0" xyz="0 0 0"/>
-				<geometry>
-					<mesh filename="file://$(find aliengo_description)/meshes/calf.dae" scale="1 1 1"/>
-				</geometry>
-				<material name="orange"/>
-			</visual>
-			<collision>
-				<origin rpy="0 ${PI/2.0} 0" xyz="0 0 ${-calf_length/2.0}"/>
-				<geometry>
-					<box size="${calf_length} ${calf_width} ${calf_height}"/>
-				</geometry>
-			</collision>
-			<inertial>
-				<origin rpy="0 0 0" xyz="${calf_com_x} ${calf_com_y} ${calf_com_z}"/>
-				<mass value="${calf_mass}"/>
-				<inertia
-						ixx="${calf_ixx}" ixy="${calf_ixy}" ixz="${calf_ixz}"
-						iyy="${calf_iyy}" iyz="${calf_iyz}"
-						izz="${calf_izz}"/>
-			</inertial>
-		</link>
+    <link name="${name}_calf">
+      <visual>
+        <origin rpy="0 0 0" xyz="0 0 0"/>
+        <geometry>
+          <mesh filename="file://$(find aliengo_description)/meshes/calf.dae" scale="1 1 1"/>
+        </geometry>
+      </visual>
+      <collision>
+        <origin rpy="0 ${PI/2.0} 0" xyz="0 0 ${-calf_length/2.0}"/>
+        <geometry>
+          <box size="${calf_length} ${calf_width} ${calf_height}"/>
+        </geometry>
+      </collision>
+      <inertial>
+        <origin rpy="0 0 0" xyz="${calf_com_x} ${calf_com_y} ${calf_com_z}"/>
+        <mass value="${calf_mass}"/>
+        <inertia
+            ixx="${calf_ixx}" ixy="${calf_ixy}" ixz="${calf_ixz}"
+            iyy="${calf_iyy}" iyz="${calf_iyz}"
+            izz="${calf_izz}"/>
+      </inertial>
+    </link>
 
-		<link name="${name}_calf_rotor">
-			<visual>
-				<origin rpy="${PI/2.0} 0 0" xyz="0 0 0"/>
-				<geometry>
-					<cylinder length="0.02" radius="0.035"/>
-				</geometry>
-				<material name="green"/>
-			</visual>
-			<!-- <collision>
-					<origin rpy="${PI/2.0} 0 0" xyz="0 0 0"/>
-					<geometry>
-							<cylinder length="0.02" radius="0.035"/>
-					</geometry>
-			</collision> -->
-			<inertial>
-				<origin rpy="0 0 0" xyz="${calf_rotor_com_x} ${calf_rotor_com_y} ${calf_rotor_com_z}"/>
-				<mass value="${calf_rotor_mass}"/>
-				<inertia
-						ixx="${calf_rotor_ixx}" ixy="${calf_rotor_ixy}" ixz="${calf_rotor_ixz}"
-						iyy="${calf_rotor_iyy}" iyz="${calf_rotor_iyz}"
-						izz="${calf_rotor_izz}"/>
-			</inertial>
-		</link>
+    <link name="${name}_calf_rotor">
+      <visual>
+        <origin rpy="${PI/2.0} 0 0" xyz="0 0 0"/>
+        <geometry>
+          <cylinder length="0.02" radius="0.035"/>
+        </geometry>
+      </visual>
+      <inertial>
+        <origin rpy="0 0 0" xyz="${calf_rotor_com_x} ${calf_rotor_com_y} ${calf_rotor_com_z}"/>
+        <mass value="${calf_rotor_mass}"/>
+        <inertia
+            ixx="${calf_rotor_ixx}" ixy="${calf_rotor_ixy}" ixz="${calf_rotor_ixz}"
+            iyy="${calf_rotor_iyy}" iyz="${calf_rotor_iyz}"
+            izz="${calf_rotor_izz}"/>
+      </inertial>
+    </link>
 
-		<joint name="${name}_foot_fixed" type="fixed">
-			<origin rpy="0 0 0" xyz="0 0 ${-(calf_length)}"/>
-			<parent link="${name}_calf"/>
-			<child link="${name}_foot"/>
-		</joint>
+    <joint name="${name}_foot_fixed" type="fixed">
+      <origin rpy="0 0 0" xyz="0 0 ${-(calf_length)}"/>
+      <parent link="${name}_calf"/>
+      <child link="${name}_foot"/>
+    </joint>
 
-		<link name="${name}_foot">
-			<visual>
-				<origin rpy="0 0 0" xyz="0 0 0"/>
-				<geometry>
-					<sphere radius="${foot_radius-0.01}"/>
-				</geometry>
-				<material name="green"/>
-			</visual>
-			<collision>
-				<origin rpy="0 0 0" xyz="0 0 0"/>
-				<geometry>
-					<sphere radius="${foot_radius}"/>
-				</geometry>
-			</collision>
-			<inertial>
-				<mass value="${foot_mass}"/>
-				<inertia
-						ixx="${(2*foot_mass)/5.0*(foot_radius*foot_radius)}" ixy="0.0" ixz="0.0"
-						iyy="${(2*foot_mass)/5.0*(foot_radius*foot_radius)}" iyz="0.0"
-						izz="${(2*foot_mass)/5.0*(foot_radius*foot_radius)}"/>
-			</inertial>
-		</link>
+    <link name="${name}_foot">
+      <visual>
+        <origin rpy="0 0 0" xyz="0 0 0"/>
+        <geometry>
+          <sphere radius="${foot_radius-0.01}"/>
+        </geometry>
+      </visual>
+      <collision>
+        <origin rpy="0 0 0" xyz="0 0 0"/>
+        <geometry>
+          <sphere radius="${foot_radius}"/>
+        </geometry>
+      </collision>
+      <inertial>
+        <mass value="${foot_mass}"/>
+        <inertia
+            ixx="${(2*foot_mass)/5.0*(foot_radius*foot_radius)}" ixy="0.0" ixz="0.0"
+            iyy="${(2*foot_mass)/5.0*(foot_radius*foot_radius)}" iyz="0.0"
+            izz="${(2*foot_mass)/5.0*(foot_radius*foot_radius)}"/>
+      </inertial>
+    </link>
 
-		<xacro:leg_transmission name="${name}"/>
-	</xacro:macro>
+    <xacro:leg_transmission name="${name}"/>
+  </xacro:macro>
 </robot>
diff --git a/descriptions/aliengo_description/xacro/robot.xacro b/descriptions/aliengo_description/xacro/robot.xacro
index 262feaf..cc2543a 100644
--- a/descriptions/aliengo_description/xacro/robot.xacro
+++ b/descriptions/aliengo_description/xacro/robot.xacro
@@ -52,12 +52,6 @@
 				<box size="${trunk_length} ${trunk_width} ${trunk_height}"/>
 			</geometry>
 		</collision>
-		<!-- <collision>
-				<origin rpy="0 0 0" xyz="0 0 0"/>
-				<geometry>
-						<mesh filename="package://aliengo_description/meshes/trunk.dae" scale="1 1 1"/>
-				</geometry>
-		</collision> -->
 		<inertial>
 			<origin rpy="0 0 0" xyz="${trunk_com_x} ${trunk_com_y} ${trunk_com_z}"/>
 			<mass value="${trunk_mass}"/>
diff --git a/descriptions/aliengo_description/xacro/ros2_control.xacro b/descriptions/aliengo_description/xacro/ros2_control.xacro
index 688e259..0f47197 100644
--- a/descriptions/aliengo_description/xacro/ros2_control.xacro
+++ b/descriptions/aliengo_description/xacro/ros2_control.xacro
@@ -1,176 +1,176 @@
 <?xml version="1.0"?>
 <robot xmlns:xacro="http://www.ros.org/wiki/xacro">
 
-	<ros2_control name="UnitreeSystem" type="system">
+  <ros2_control name="UnitreeSystem" type="system">
 
-		<hardware>
-			<plugin>hardware_unitree_mujoco/HardwareUnitree</plugin>
-		</hardware>
+    <hardware>
+      <plugin>hardware_unitree_mujoco/HardwareUnitree</plugin>
+    </hardware>
 
-		<joint name="FR_hip_joint">
-			<command_interface name="position"/>
-			<command_interface name="velocity"/>
-			<command_interface name="effort"/>
-			<command_interface name="kp"/>
-			<command_interface name="kd"/>
+    <joint name="FR_hip_joint">
+      <command_interface name="position"/>
+      <command_interface name="velocity"/>
+      <command_interface name="effort"/>
+      <command_interface name="kp"/>
+      <command_interface name="kd"/>
 
-			<state_interface name="position"/>
-			<state_interface name="velocity"/>
-			<state_interface name="effort"/>
-		</joint>
+      <state_interface name="position"/>
+      <state_interface name="velocity"/>
+      <state_interface name="effort"/>
+    </joint>
 
-		<joint name="FR_thigh_joint">
-			<command_interface name="position"/>
-			<command_interface name="velocity"/>
-			<command_interface name="effort"/>
-			<command_interface name="kp"/>
-			<command_interface name="kd"/>
+    <joint name="FR_thigh_joint">
+      <command_interface name="position"/>
+      <command_interface name="velocity"/>
+      <command_interface name="effort"/>
+      <command_interface name="kp"/>
+      <command_interface name="kd"/>
 
-			<state_interface name="position"/>
-			<state_interface name="velocity"/>
-			<state_interface name="effort"/>
-		</joint>
+      <state_interface name="position"/>
+      <state_interface name="velocity"/>
+      <state_interface name="effort"/>
+    </joint>
 
-		<joint name="FR_calf_joint">
-			<command_interface name="position"/>
-			<command_interface name="velocity"/>
-			<command_interface name="effort"/>
-			<command_interface name="kp"/>
-			<command_interface name="kd"/>
+    <joint name="FR_calf_joint">
+      <command_interface name="position"/>
+      <command_interface name="velocity"/>
+      <command_interface name="effort"/>
+      <command_interface name="kp"/>
+      <command_interface name="kd"/>
 
-			<state_interface name="position"/>
-			<state_interface name="velocity"/>
-			<state_interface name="effort"/>
-		</joint>
+      <state_interface name="position"/>
+      <state_interface name="velocity"/>
+      <state_interface name="effort"/>
+    </joint>
 
-		<joint name="FL_hip_joint">
-			<command_interface name="position"/>
-			<command_interface name="velocity"/>
-			<command_interface name="effort"/>
-			<command_interface name="kp"/>
-			<command_interface name="kd"/>
+    <joint name="FL_hip_joint">
+      <command_interface name="position"/>
+      <command_interface name="velocity"/>
+      <command_interface name="effort"/>
+      <command_interface name="kp"/>
+      <command_interface name="kd"/>
 
-			<state_interface name="position"/>
-			<state_interface name="velocity"/>
-			<state_interface name="effort"/>
-		</joint>
+      <state_interface name="position"/>
+      <state_interface name="velocity"/>
+      <state_interface name="effort"/>
+    </joint>
 
-		<joint name="FL_thigh_joint">
-			<command_interface name="position"/>
-			<command_interface name="velocity"/>
-			<command_interface name="effort"/>
-			<command_interface name="kp"/>
-			<command_interface name="kd"/>
+    <joint name="FL_thigh_joint">
+      <command_interface name="position"/>
+      <command_interface name="velocity"/>
+      <command_interface name="effort"/>
+      <command_interface name="kp"/>
+      <command_interface name="kd"/>
 
-			<state_interface name="position"/>
-			<state_interface name="velocity"/>
-			<state_interface name="effort"/>
-		</joint>
+      <state_interface name="position"/>
+      <state_interface name="velocity"/>
+      <state_interface name="effort"/>
+    </joint>
 
-		<joint name="FL_calf_joint">
-			<command_interface name="position"/>
-			<command_interface name="velocity"/>
-			<command_interface name="effort"/>
-			<command_interface name="kp"/>
-			<command_interface name="kd"/>
+    <joint name="FL_calf_joint">
+      <command_interface name="position"/>
+      <command_interface name="velocity"/>
+      <command_interface name="effort"/>
+      <command_interface name="kp"/>
+      <command_interface name="kd"/>
 
-			<state_interface name="position"/>
-			<state_interface name="velocity"/>
-			<state_interface name="effort"/>
-		</joint>
+      <state_interface name="position"/>
+      <state_interface name="velocity"/>
+      <state_interface name="effort"/>
+    </joint>
 
-		<joint name="RR_hip_joint">
-			<command_interface name="position"/>
-			<command_interface name="velocity"/>
-			<command_interface name="effort"/>
-			<command_interface name="kp"/>
-			<command_interface name="kd"/>
+    <joint name="RR_hip_joint">
+      <command_interface name="position"/>
+      <command_interface name="velocity"/>
+      <command_interface name="effort"/>
+      <command_interface name="kp"/>
+      <command_interface name="kd"/>
 
-			<state_interface name="position"/>
-			<state_interface name="velocity"/>
-			<state_interface name="effort"/>
-		</joint>
+      <state_interface name="position"/>
+      <state_interface name="velocity"/>
+      <state_interface name="effort"/>
+    </joint>
 
-		<joint name="RR_thigh_joint">
-			<command_interface name="position"/>
-			<command_interface name="velocity"/>
-			<command_interface name="effort"/>
-			<command_interface name="kp"/>
-			<command_interface name="kd"/>
+    <joint name="RR_thigh_joint">
+      <command_interface name="position"/>
+      <command_interface name="velocity"/>
+      <command_interface name="effort"/>
+      <command_interface name="kp"/>
+      <command_interface name="kd"/>
 
-			<state_interface name="position"/>
-			<state_interface name="velocity"/>
-			<state_interface name="effort"/>
-		</joint>
+      <state_interface name="position"/>
+      <state_interface name="velocity"/>
+      <state_interface name="effort"/>
+    </joint>
 
-		<joint name="RR_calf_joint">
-			<command_interface name="position"/>
-			<command_interface name="velocity"/>
-			<command_interface name="effort"/>
-			<command_interface name="kp"/>
-			<command_interface name="kd"/>
+    <joint name="RR_calf_joint">
+      <command_interface name="position"/>
+      <command_interface name="velocity"/>
+      <command_interface name="effort"/>
+      <command_interface name="kp"/>
+      <command_interface name="kd"/>
 
-			<state_interface name="position"/>
-			<state_interface name="velocity"/>
-			<state_interface name="effort"/>
-		</joint>
+      <state_interface name="position"/>
+      <state_interface name="velocity"/>
+      <state_interface name="effort"/>
+    </joint>
 
-		<joint name="RL_hip_joint">
-			<command_interface name="position"/>
-			<command_interface name="velocity"/>
-			<command_interface name="effort"/>
-			<command_interface name="kp"/>
-			<command_interface name="kd"/>
+    <joint name="RL_hip_joint">
+      <command_interface name="position"/>
+      <command_interface name="velocity"/>
+      <command_interface name="effort"/>
+      <command_interface name="kp"/>
+      <command_interface name="kd"/>
 
-			<state_interface name="position"/>
-			<state_interface name="velocity"/>
-			<state_interface name="effort"/>
-		</joint>
+      <state_interface name="position"/>
+      <state_interface name="velocity"/>
+      <state_interface name="effort"/>
+    </joint>
 
-		<joint name="RL_thigh_joint">
-			<command_interface name="position"/>
-			<command_interface name="velocity"/>
-			<command_interface name="effort"/>
-			<command_interface name="kp"/>
-			<command_interface name="kd"/>
+    <joint name="RL_thigh_joint">
+      <command_interface name="position"/>
+      <command_interface name="velocity"/>
+      <command_interface name="effort"/>
+      <command_interface name="kp"/>
+      <command_interface name="kd"/>
 
-			<state_interface name="position"/>
-			<state_interface name="velocity"/>
-			<state_interface name="effort"/>
-		</joint>
+      <state_interface name="position"/>
+      <state_interface name="velocity"/>
+      <state_interface name="effort"/>
+    </joint>
 
-		<joint name="RL_calf_joint">
-			<command_interface name="position"/>
-			<command_interface name="velocity"/>
-			<command_interface name="effort"/>
-			<command_interface name="kp"/>
-			<command_interface name="kd"/>
+    <joint name="RL_calf_joint">
+      <command_interface name="position"/>
+      <command_interface name="velocity"/>
+      <command_interface name="effort"/>
+      <command_interface name="kp"/>
+      <command_interface name="kd"/>
 
-			<state_interface name="position"/>
-			<state_interface name="velocity"/>
-			<state_interface name="effort"/>
-		</joint>
+      <state_interface name="position"/>
+      <state_interface name="velocity"/>
+      <state_interface name="effort"/>
+    </joint>
 
-		<sensor name="imu_sensor">
-			<state_interface name="orientation.w"/>
-			<state_interface name="orientation.x"/>
-			<state_interface name="orientation.y"/>
-			<state_interface name="orientation.z"/>
-			<state_interface name="angular_velocity.x"/>
-			<state_interface name="angular_velocity.y"/>
-			<state_interface name="angular_velocity.z"/>
-			<state_interface name="linear_acceleration.x"/>
-			<state_interface name="linear_acceleration.y"/>
-			<state_interface name="linear_acceleration.z"/>
-		</sensor>
+    <sensor name="imu_sensor">
+      <state_interface name="orientation.w"/>
+      <state_interface name="orientation.x"/>
+      <state_interface name="orientation.y"/>
+      <state_interface name="orientation.z"/>
+      <state_interface name="angular_velocity.x"/>
+      <state_interface name="angular_velocity.y"/>
+      <state_interface name="angular_velocity.z"/>
+      <state_interface name="linear_acceleration.x"/>
+      <state_interface name="linear_acceleration.y"/>
+      <state_interface name="linear_acceleration.z"/>
+    </sensor>
 
-		<sensor name="foot_force">
-			<state_interface name="FR"/>
-			<state_interface name="FL"/>
-			<state_interface name="RR"/>
-			<state_interface name="RL"/>
-		</sensor>
+    <sensor name="foot_force">
+      <state_interface name="FR"/>
+      <state_interface name="FL"/>
+      <state_interface name="RR"/>
+      <state_interface name="RL"/>
+    </sensor>
 
-	</ros2_control>
+  </ros2_control>
 
 </robot>
diff --git a/descriptions/go1_description/config/ocs2/reference.info b/descriptions/go1_description/config/ocs2/reference.info
index b36ce8e..36aa1eb 100644
--- a/descriptions/go1_description/config/ocs2/reference.info
+++ b/descriptions/go1_description/config/ocs2/reference.info
@@ -5,16 +5,16 @@ comHeight                           0.3
 
 defaultJointState
 {
-   (0,0)  -0.20   ; FL_hip_joint
+   (0,0)  -0.10   ; FL_hip_joint
    (1,0)   0.72   ; FL_thigh_joint
    (2,0)  -1.44   ; FL_calf_joint
-   (3,0)  -0.20   ; RL_hip_joint
-   (4,0)   0.72   ; RL_thigh_joint
-   (5,0)  -1.44   ; RL_calf_joint
-   (6,0)   0.20   ; FR_hip_joint
-   (7,0)   0.72   ; FR_thigh_joint
-   (8,0)  -1.44   ; FR_calf_joint
-   (9,0)   0.20   ; RR_hip_joint
+   (3,0)   0.10   ; FR_hip_joint
+   (4,0)   0.72   ; FR_thigh_joint
+   (5,0)  -1.44   ; FR_calf_joint
+   (6,0)  -0.10   ; RL_hip_joint
+   (7,0)   0.72   ; RL_thigh_joint
+   (8,0)  -1.44   ; RL_calf_joint
+   (9,0)   0.10   ; RR_hip_joint
    (10,0)  0.72   ; RR_thigh_joint
    (11,0) -1.44   ; RR_calf_joint
 }
diff --git a/descriptions/go1_description/urdf/robot.urdf b/descriptions/go1_description/urdf/robot.urdf
index 6a1e2c1..ae2ca1e 100644
--- a/descriptions/go1_description/urdf/robot.urdf
+++ b/descriptions/go1_description/urdf/robot.urdf
@@ -194,7 +194,7 @@
     <visual>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="file:///home/biao/ros2_ws/install/go1_description/share/go1_description/meshes/trunk.dae" scale="1 1 1"/>
+        <mesh filename="file:///home/tlab-uav/ros2_ws/install/go1_description/share/go1_description/meshes/trunk.dae" scale="1 1 1"/>
       </geometry>
       <!-- <material name="orange"/> -->
     </visual>
@@ -252,7 +252,7 @@
     <visual>
       <origin rpy="3.141592653589793 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="file:///home/biao/ros2_ws/install/go1_description/share/go1_description/meshes/hip.dae" scale="1 1 1"/>
+        <mesh filename="file:///home/tlab-uav/ros2_ws/install/go1_description/share/go1_description/meshes/hip.dae" scale="1 1 1"/>
       </geometry>
       <!-- <material name="orange"/> -->
     </visual>
@@ -276,12 +276,6 @@
       </geometry>
       <material name="green"/>
     </visual>
-    <!-- <collision>
-                <origin rpy="0 ${PI/2.0} 0" xyz="0 0 0"/>
-                <geometry>
-                    <cylinder length="0.02" radius="0.035"/>
-                </geometry>
-            </collision> -->
     <inertial>
       <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
       <mass value="0.089"/>
@@ -305,9 +299,8 @@
     <visual>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="file:///home/biao/ros2_ws/install/go1_description/share/go1_description/meshes/thigh_mirror.dae" scale="1 1 1"/>
+        <mesh filename="file:///home/tlab-uav/ros2_ws/install/go1_description/share/go1_description/meshes/thigh_mirror.dae" scale="1 1 1"/>
       </geometry>
-      <!-- <material name="orange"/> -->
     </visual>
     <collision>
       <origin rpy="0 1.5707963267948966 0" xyz="0 0 -0.1065"/>
@@ -329,12 +322,6 @@
       </geometry>
       <material name="green"/>
     </visual>
-    <!-- <collision>
-                <origin rpy="${PI/2.0} 0 0" xyz="0 0 0"/>
-                <geometry>
-                    <cylinder length="0.02" radius="0.035"/>
-                </geometry>
-            </collision> -->
     <inertial>
       <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
       <mass value="0.089"/>
@@ -358,9 +345,8 @@
     <visual>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="file:///home/biao/ros2_ws/install/go1_description/share/go1_description/meshes/calf.dae" scale="1 1 1"/>
+        <mesh filename="file:///home/tlab-uav/ros2_ws/install/go1_description/share/go1_description/meshes/calf.dae" scale="1 1 1"/>
       </geometry>
-      <!-- <material name="orange"/> -->
     </visual>
     <collision>
       <origin rpy="0 1.5707963267948966 0" xyz="0 0 -0.1065"/>
@@ -380,14 +366,7 @@
       <geometry>
         <cylinder length="0.02" radius="0.035"/>
       </geometry>
-      <material name="green"/>
     </visual>
-    <!-- <collision>
-                <origin rpy="${PI/2.0} 0 0" xyz="0 0 0"/>
-                <geometry>
-                    <cylinder length="0.02" radius="0.035"/>
-                </geometry>
-            </collision> -->
     <inertial>
       <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
       <mass value="0.089"/>
@@ -405,7 +384,6 @@
       <geometry>
         <sphere radius="0.01"/>
       </geometry>
-      <!-- <material name="orange"/> -->
     </visual>
     <collision>
       <origin rpy="0 0 0" xyz="0 0 0"/>
@@ -465,7 +443,7 @@
     <visual>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="file:///home/biao/ros2_ws/install/go1_description/share/go1_description/meshes/hip.dae" scale="1 1 1"/>
+        <mesh filename="file:///home/tlab-uav/ros2_ws/install/go1_description/share/go1_description/meshes/hip.dae" scale="1 1 1"/>
       </geometry>
       <!-- <material name="orange"/> -->
     </visual>
@@ -489,12 +467,6 @@
       </geometry>
       <material name="green"/>
     </visual>
-    <!-- <collision>
-                <origin rpy="0 ${PI/2.0} 0" xyz="0 0 0"/>
-                <geometry>
-                    <cylinder length="0.02" radius="0.035"/>
-                </geometry>
-            </collision> -->
     <inertial>
       <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
       <mass value="0.089"/>
@@ -518,9 +490,8 @@
     <visual>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="file:///home/biao/ros2_ws/install/go1_description/share/go1_description/meshes/thigh.dae" scale="1 1 1"/>
+        <mesh filename="file:///home/tlab-uav/ros2_ws/install/go1_description/share/go1_description/meshes/thigh.dae" scale="1 1 1"/>
       </geometry>
-      <!-- <material name="orange"/> -->
     </visual>
     <collision>
       <origin rpy="0 1.5707963267948966 0" xyz="0 0 -0.1065"/>
@@ -542,12 +513,6 @@
       </geometry>
       <material name="green"/>
     </visual>
-    <!-- <collision>
-                <origin rpy="${PI/2.0} 0 0" xyz="0 0 0"/>
-                <geometry>
-                    <cylinder length="0.02" radius="0.035"/>
-                </geometry>
-            </collision> -->
     <inertial>
       <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
       <mass value="0.089"/>
@@ -571,9 +536,8 @@
     <visual>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="file:///home/biao/ros2_ws/install/go1_description/share/go1_description/meshes/calf.dae" scale="1 1 1"/>
+        <mesh filename="file:///home/tlab-uav/ros2_ws/install/go1_description/share/go1_description/meshes/calf.dae" scale="1 1 1"/>
       </geometry>
-      <!-- <material name="orange"/> -->
     </visual>
     <collision>
       <origin rpy="0 1.5707963267948966 0" xyz="0 0 -0.1065"/>
@@ -593,14 +557,7 @@
       <geometry>
         <cylinder length="0.02" radius="0.035"/>
       </geometry>
-      <material name="green"/>
     </visual>
-    <!-- <collision>
-                <origin rpy="${PI/2.0} 0 0" xyz="0 0 0"/>
-                <geometry>
-                    <cylinder length="0.02" radius="0.035"/>
-                </geometry>
-            </collision> -->
     <inertial>
       <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
       <mass value="0.089"/>
@@ -618,7 +575,6 @@
       <geometry>
         <sphere radius="0.01"/>
       </geometry>
-      <!-- <material name="orange"/> -->
     </visual>
     <collision>
       <origin rpy="0 0 0" xyz="0 0 0"/>
@@ -678,7 +634,7 @@
     <visual>
       <origin rpy="3.141592653589793 3.141592653589793 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="file:///home/biao/ros2_ws/install/go1_description/share/go1_description/meshes/hip.dae" scale="1 1 1"/>
+        <mesh filename="file:///home/tlab-uav/ros2_ws/install/go1_description/share/go1_description/meshes/hip.dae" scale="1 1 1"/>
       </geometry>
       <!-- <material name="orange"/> -->
     </visual>
@@ -702,12 +658,6 @@
       </geometry>
       <material name="green"/>
     </visual>
-    <!-- <collision>
-                <origin rpy="0 ${PI/2.0} 0" xyz="0 0 0"/>
-                <geometry>
-                    <cylinder length="0.02" radius="0.035"/>
-                </geometry>
-            </collision> -->
     <inertial>
       <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
       <mass value="0.089"/>
@@ -731,9 +681,8 @@
     <visual>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="file:///home/biao/ros2_ws/install/go1_description/share/go1_description/meshes/thigh_mirror.dae" scale="1 1 1"/>
+        <mesh filename="file:///home/tlab-uav/ros2_ws/install/go1_description/share/go1_description/meshes/thigh_mirror.dae" scale="1 1 1"/>
       </geometry>
-      <!-- <material name="orange"/> -->
     </visual>
     <collision>
       <origin rpy="0 1.5707963267948966 0" xyz="0 0 -0.1065"/>
@@ -755,12 +704,6 @@
       </geometry>
       <material name="green"/>
     </visual>
-    <!-- <collision>
-                <origin rpy="${PI/2.0} 0 0" xyz="0 0 0"/>
-                <geometry>
-                    <cylinder length="0.02" radius="0.035"/>
-                </geometry>
-            </collision> -->
     <inertial>
       <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
       <mass value="0.089"/>
@@ -784,9 +727,8 @@
     <visual>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="file:///home/biao/ros2_ws/install/go1_description/share/go1_description/meshes/calf.dae" scale="1 1 1"/>
+        <mesh filename="file:///home/tlab-uav/ros2_ws/install/go1_description/share/go1_description/meshes/calf.dae" scale="1 1 1"/>
       </geometry>
-      <!-- <material name="orange"/> -->
     </visual>
     <collision>
       <origin rpy="0 1.5707963267948966 0" xyz="0 0 -0.1065"/>
@@ -806,14 +748,7 @@
       <geometry>
         <cylinder length="0.02" radius="0.035"/>
       </geometry>
-      <material name="green"/>
     </visual>
-    <!-- <collision>
-                <origin rpy="${PI/2.0} 0 0" xyz="0 0 0"/>
-                <geometry>
-                    <cylinder length="0.02" radius="0.035"/>
-                </geometry>
-            </collision> -->
     <inertial>
       <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
       <mass value="0.089"/>
@@ -831,7 +766,6 @@
       <geometry>
         <sphere radius="0.01"/>
       </geometry>
-      <!-- <material name="orange"/> -->
     </visual>
     <collision>
       <origin rpy="0 0 0" xyz="0 0 0"/>
@@ -891,7 +825,7 @@
     <visual>
       <origin rpy="0 3.141592653589793 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="file:///home/biao/ros2_ws/install/go1_description/share/go1_description/meshes/hip.dae" scale="1 1 1"/>
+        <mesh filename="file:///home/tlab-uav/ros2_ws/install/go1_description/share/go1_description/meshes/hip.dae" scale="1 1 1"/>
       </geometry>
       <!-- <material name="orange"/> -->
     </visual>
@@ -915,12 +849,6 @@
       </geometry>
       <material name="green"/>
     </visual>
-    <!-- <collision>
-                <origin rpy="0 ${PI/2.0} 0" xyz="0 0 0"/>
-                <geometry>
-                    <cylinder length="0.02" radius="0.035"/>
-                </geometry>
-            </collision> -->
     <inertial>
       <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
       <mass value="0.089"/>
@@ -944,9 +872,8 @@
     <visual>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="file:///home/biao/ros2_ws/install/go1_description/share/go1_description/meshes/thigh.dae" scale="1 1 1"/>
+        <mesh filename="file:///home/tlab-uav/ros2_ws/install/go1_description/share/go1_description/meshes/thigh.dae" scale="1 1 1"/>
       </geometry>
-      <!-- <material name="orange"/> -->
     </visual>
     <collision>
       <origin rpy="0 1.5707963267948966 0" xyz="0 0 -0.1065"/>
@@ -968,12 +895,6 @@
       </geometry>
       <material name="green"/>
     </visual>
-    <!-- <collision>
-                <origin rpy="${PI/2.0} 0 0" xyz="0 0 0"/>
-                <geometry>
-                    <cylinder length="0.02" radius="0.035"/>
-                </geometry>
-            </collision> -->
     <inertial>
       <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
       <mass value="0.089"/>
@@ -997,9 +918,8 @@
     <visual>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="file:///home/biao/ros2_ws/install/go1_description/share/go1_description/meshes/calf.dae" scale="1 1 1"/>
+        <mesh filename="file:///home/tlab-uav/ros2_ws/install/go1_description/share/go1_description/meshes/calf.dae" scale="1 1 1"/>
       </geometry>
-      <!-- <material name="orange"/> -->
     </visual>
     <collision>
       <origin rpy="0 1.5707963267948966 0" xyz="0 0 -0.1065"/>
@@ -1019,14 +939,7 @@
       <geometry>
         <cylinder length="0.02" radius="0.035"/>
       </geometry>
-      <material name="green"/>
     </visual>
-    <!-- <collision>
-                <origin rpy="${PI/2.0} 0 0" xyz="0 0 0"/>
-                <geometry>
-                    <cylinder length="0.02" radius="0.035"/>
-                </geometry>
-            </collision> -->
     <inertial>
       <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
       <mass value="0.089"/>
@@ -1044,7 +957,6 @@
       <geometry>
         <sphere radius="0.01"/>
       </geometry>
-      <!-- <material name="orange"/> -->
     </visual>
     <collision>
       <origin rpy="0 0 0" xyz="0 0 0"/>
@@ -1102,7 +1014,7 @@
     <visual>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="file:///home/biao/ros2_ws/install/go1_description/share/go1_description/meshes/depthCamera.dae" scale="1 1 1"/>
+        <mesh filename="file:///home/tlab-uav/ros2_ws/install/go1_description/share/go1_description/meshes/depthCamera.dae" scale="1 1 1"/>
       </geometry>
     </visual>
     <inertial>
@@ -1176,7 +1088,7 @@
     <visual>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="file:///home/biao/ros2_ws/install/go1_description/share/go1_description/meshes/depthCamera.dae" scale="1 1 1"/>
+        <mesh filename="file:///home/tlab-uav/ros2_ws/install/go1_description/share/go1_description/meshes/depthCamera.dae" scale="1 1 1"/>
       </geometry>
     </visual>
     <inertial>
@@ -1250,7 +1162,7 @@
     <visual>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="file:///home/biao/ros2_ws/install/go1_description/share/go1_description/meshes/depthCamera.dae" scale="1 1 1"/>
+        <mesh filename="file:///home/tlab-uav/ros2_ws/install/go1_description/share/go1_description/meshes/depthCamera.dae" scale="1 1 1"/>
       </geometry>
     </visual>
     <inertial>
@@ -1324,7 +1236,7 @@
     <visual>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="file:///home/biao/ros2_ws/install/go1_description/share/go1_description/meshes/depthCamera.dae" scale="1 1 1"/>
+        <mesh filename="file:///home/tlab-uav/ros2_ws/install/go1_description/share/go1_description/meshes/depthCamera.dae" scale="1 1 1"/>
       </geometry>
     </visual>
     <inertial>
@@ -1398,7 +1310,7 @@
     <visual>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="file:///home/biao/ros2_ws/install/go1_description/share/go1_description/meshes/depthCamera.dae" scale="1 1 1"/>
+        <mesh filename="file:///home/tlab-uav/ros2_ws/install/go1_description/share/go1_description/meshes/depthCamera.dae" scale="1 1 1"/>
       </geometry>
     </visual>
     <inertial>
@@ -1486,7 +1398,7 @@
     <visual>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="file:///home/biao/ros2_ws/install/go1_description/share/go1_description/meshes/ultraSound.dae" scale="1 1 1"/>
+        <mesh filename="file:///home/tlab-uav/ros2_ws/install/go1_description/share/go1_description/meshes/ultraSound.dae" scale="1 1 1"/>
       </geometry>
       <material name="black"/>
     </visual>
@@ -1511,7 +1423,7 @@
     <visual>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="file:///home/biao/ros2_ws/install/go1_description/share/go1_description/meshes/ultraSound.dae" scale="1 1 1"/>
+        <mesh filename="file:///home/tlab-uav/ros2_ws/install/go1_description/share/go1_description/meshes/ultraSound.dae" scale="1 1 1"/>
       </geometry>
       <material name="black"/>
     </visual>
@@ -1536,7 +1448,7 @@
     <visual>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="file:///home/biao/ros2_ws/install/go1_description/share/go1_description/meshes/ultraSound.dae" scale="1 1 1"/>
+        <mesh filename="file:///home/tlab-uav/ros2_ws/install/go1_description/share/go1_description/meshes/ultraSound.dae" scale="1 1 1"/>
       </geometry>
       <material name="black"/>
     </visual>
diff --git a/descriptions/go1_description/xacro/leg.xacro b/descriptions/go1_description/xacro/leg.xacro
index ae6b49a..3470542 100644
--- a/descriptions/go1_description/xacro/leg.xacro
+++ b/descriptions/go1_description/xacro/leg.xacro
@@ -2,271 +2,253 @@
 
 <robot xmlns:xacro="http://www.ros.org/wiki/xacro">
 
-    <xacro:include filename="$(find go1_description)/xacro/transmission.xacro"/>
+  <xacro:include filename="$(find go1_description)/xacro/transmission.xacro"/>
 
-    <xacro:macro name="leg" params="name mirror mirror_dae front_hind front_hind_dae">
+  <xacro:macro name="leg" params="name mirror mirror_dae front_hind front_hind_dae">
 
-        <joint name="${name}_hip_joint" type="revolute">
-            <xacro:if value="${(mirror_dae == False) and (front_hind_dae == True)}">
-                <origin rpy="0 0 0" xyz="${leg_offset_x} ${-leg_offset_y} 0"/>
-            </xacro:if>
-            <xacro:if value="${(mirror_dae == True) and (front_hind_dae == True)}">
-                <origin rpy="0 0 0" xyz="${leg_offset_x} ${leg_offset_y} 0"/>
-            </xacro:if>
-            <xacro:if value="${(mirror_dae == False) and (front_hind_dae == False)}">
-                <origin rpy="0 0 0" xyz="${-leg_offset_x} ${-leg_offset_y} 0"/>
-            </xacro:if>
-            <xacro:if value="${(mirror_dae == True) and (front_hind_dae == False)}">
-                <origin rpy="0 0 0" xyz="${-leg_offset_x} ${leg_offset_y} 0"/>
-            </xacro:if>
-            <parent link="trunk"/>
-            <child link="${name}_hip"/>
-            <axis xyz="1 0 0"/>
-            <dynamics damping="${damping}" friction="${friction}"/>
-            <xacro:if value="${(mirror_dae == True)}">
-                <limit effort="${hip_torque_max}" velocity="${hip_velocity_max}" lower="${hip_position_min}" upper="${hip_position_max}"/>
-            </xacro:if>
-            <xacro:if value="${(mirror_dae == False)}">
-                <limit effort="${hip_torque_max}" velocity="${hip_velocity_max}" lower="${-hip_position_max}" upper="${-hip_position_min}"/>
-            </xacro:if>            
-        </joint>
+    <joint name="${name}_hip_joint" type="revolute">
+      <xacro:if value="${(mirror_dae == False) and (front_hind_dae == True)}">
+        <origin rpy="0 0 0" xyz="${leg_offset_x} ${-leg_offset_y} 0"/>
+      </xacro:if>
+      <xacro:if value="${(mirror_dae == True) and (front_hind_dae == True)}">
+        <origin rpy="0 0 0" xyz="${leg_offset_x} ${leg_offset_y} 0"/>
+      </xacro:if>
+      <xacro:if value="${(mirror_dae == False) and (front_hind_dae == False)}">
+        <origin rpy="0 0 0" xyz="${-leg_offset_x} ${-leg_offset_y} 0"/>
+      </xacro:if>
+      <xacro:if value="${(mirror_dae == True) and (front_hind_dae == False)}">
+        <origin rpy="0 0 0" xyz="${-leg_offset_x} ${leg_offset_y} 0"/>
+      </xacro:if>
+      <parent link="trunk"/>
+      <child link="${name}_hip"/>
+      <axis xyz="1 0 0"/>
+      <dynamics damping="${damping}" friction="${friction}"/>
+      <xacro:if value="${(mirror_dae == True)}">
+        <limit effort="${hip_torque_max}" velocity="${hip_velocity_max}" lower="${hip_position_min}"
+               upper="${hip_position_max}"/>
+      </xacro:if>
+      <xacro:if value="${(mirror_dae == False)}">
+        <limit effort="${hip_torque_max}" velocity="${hip_velocity_max}" lower="${-hip_position_max}"
+               upper="${-hip_position_min}"/>
+      </xacro:if>
+    </joint>
 
-        <joint name="${name}_hip_rotor_joint" type="fixed">
-            <xacro:if value="${(mirror_dae == False) and (front_hind_dae == True)}">
-                <origin rpy="0 0 0" xyz="${hip_rotor_offset_x} ${-hip_rotor_offset_y} 0"/>
-            </xacro:if>
-            <xacro:if value="${(mirror_dae == True) and (front_hind_dae == True)}">
-                <origin rpy="0 0 0" xyz="${hip_rotor_offset_x} ${hip_rotor_offset_y} 0"/>
-            </xacro:if>
-            <xacro:if value="${(mirror_dae == False) and (front_hind_dae == False)}">
-                <origin rpy="0 0 0" xyz="${-hip_rotor_offset_x} ${-hip_rotor_offset_y} 0"/>
-            </xacro:if>
-            <xacro:if value="${(mirror_dae == True) and (front_hind_dae == False)}">
-                <origin rpy="0 0 0" xyz="${-hip_rotor_offset_x} ${hip_rotor_offset_y} 0"/>
-            </xacro:if>
-            <parent link="trunk"/>
-            <child link="${name}_hip_rotor"/>         
-        </joint>
+    <joint name="${name}_hip_rotor_joint" type="fixed">
+      <xacro:if value="${(mirror_dae == False) and (front_hind_dae == True)}">
+        <origin rpy="0 0 0" xyz="${hip_rotor_offset_x} ${-hip_rotor_offset_y} 0"/>
+      </xacro:if>
+      <xacro:if value="${(mirror_dae == True) and (front_hind_dae == True)}">
+        <origin rpy="0 0 0" xyz="${hip_rotor_offset_x} ${hip_rotor_offset_y} 0"/>
+      </xacro:if>
+      <xacro:if value="${(mirror_dae == False) and (front_hind_dae == False)}">
+        <origin rpy="0 0 0" xyz="${-hip_rotor_offset_x} ${-hip_rotor_offset_y} 0"/>
+      </xacro:if>
+      <xacro:if value="${(mirror_dae == True) and (front_hind_dae == False)}">
+        <origin rpy="0 0 0" xyz="${-hip_rotor_offset_x} ${hip_rotor_offset_y} 0"/>
+      </xacro:if>
+      <parent link="trunk"/>
+      <child link="${name}_hip_rotor"/>
+    </joint>
 
-        <link name="${name}_hip">
-            <visual>
-                <xacro:if value="${(mirror_dae == True) and (front_hind_dae == True)}">
-                    <origin rpy="0 0 0" xyz="0 0 0"/>
-                </xacro:if>
-                <xacro:if value="${(mirror_dae == False) and (front_hind_dae == True)}">
-                    <origin rpy="${PI} 0 0" xyz="0 0 0"/>
-                </xacro:if>
-                <xacro:if value="${(mirror_dae == True) and (front_hind_dae == False)}">
-                    <origin rpy="0 ${PI} 0" xyz="0 0 0"/>
-                </xacro:if>
-                <xacro:if value="${(mirror_dae == False) and (front_hind_dae == False)}">
-                    <origin rpy="${PI} ${PI} 0" xyz="0 0 0"/>
-                </xacro:if>
-                <geometry>
-                    <mesh filename="file://$(find go1_description)/meshes/hip.dae" scale="1 1 1"/>
-                </geometry>
-                <!-- <material name="orange"/> -->
-            </visual>
-            <collision>
-                <origin rpy="${PI/2.0} 0 0" xyz="0 ${hip_offset*mirror} 0"/>
-                <geometry>
-                    <cylinder length="${hip_length}" radius="${hip_radius}"/>
-                </geometry>
-            </collision>
-            <inertial>
-                <origin rpy="0 0 0" xyz="${hip_com_x*front_hind} ${hip_com_y*mirror} ${hip_com_z}"/>
-                <mass value="${hip_mass}"/>
-                <inertia
-                    ixx="${hip_ixx}" ixy="${hip_ixy*mirror*front_hind}" ixz="${hip_ixz*front_hind}"
-                    iyy="${hip_iyy}" iyz="${hip_iyz*mirror}"
-                    izz="${hip_izz}"/>       
-            </inertial>
-        </link>
+    <link name="${name}_hip">
+      <visual>
+        <xacro:if value="${(mirror_dae == True) and (front_hind_dae == True)}">
+          <origin rpy="0 0 0" xyz="0 0 0"/>
+        </xacro:if>
+        <xacro:if value="${(mirror_dae == False) and (front_hind_dae == True)}">
+          <origin rpy="${PI} 0 0" xyz="0 0 0"/>
+        </xacro:if>
+        <xacro:if value="${(mirror_dae == True) and (front_hind_dae == False)}">
+          <origin rpy="0 ${PI} 0" xyz="0 0 0"/>
+        </xacro:if>
+        <xacro:if value="${(mirror_dae == False) and (front_hind_dae == False)}">
+          <origin rpy="${PI} ${PI} 0" xyz="0 0 0"/>
+        </xacro:if>
+        <geometry>
+          <mesh filename="file://$(find go1_description)/meshes/hip.dae" scale="1 1 1"/>
+        </geometry>
+        <!-- <material name="orange"/> -->
+      </visual>
+      <collision>
+        <origin rpy="${PI/2.0} 0 0" xyz="0 ${hip_offset*mirror} 0"/>
+        <geometry>
+          <cylinder length="${hip_length}" radius="${hip_radius}"/>
+        </geometry>
+      </collision>
+      <inertial>
+        <origin rpy="0 0 0" xyz="${hip_com_x*front_hind} ${hip_com_y*mirror} ${hip_com_z}"/>
+        <mass value="${hip_mass}"/>
+        <inertia
+            ixx="${hip_ixx}" ixy="${hip_ixy*mirror*front_hind}" ixz="${hip_ixz*front_hind}"
+            iyy="${hip_iyy}" iyz="${hip_iyz*mirror}"
+            izz="${hip_izz}"/>
+      </inertial>
+    </link>
 
-        <link name="${name}_hip_rotor">
-            <visual>
-                <origin rpy="0 ${PI/2.0} 0" xyz="0 0 0"/>
-                <geometry>
-                    <cylinder length="0.02" radius="0.035"/>
-                </geometry>
-                <material name="green"/>
-            </visual>
-            <!-- <collision>
-                <origin rpy="0 ${PI/2.0} 0" xyz="0 0 0"/>
-                <geometry>
-                    <cylinder length="0.02" radius="0.035"/>
-                </geometry>
-            </collision> -->
-            <inertial>
-                <origin rpy="0 0 0" xyz="${hip_rotor_com_x} ${hip_rotor_com_y} ${hip_rotor_com_z}"/>
-                <mass value="${hip_rotor_mass}"/>
-                <inertia
-                    ixx="${hip_rotor_ixx}" ixy="${hip_rotor_ixy}" ixz="${hip_rotor_ixz}"
-                    iyy="${hip_rotor_iyy}" iyz="${hip_rotor_iyz}"
-                    izz="${hip_rotor_izz}"/>              
-            </inertial>
-        </link>
+    <link name="${name}_hip_rotor">
+      <visual>
+        <origin rpy="0 ${PI/2.0} 0" xyz="0 0 0"/>
+        <geometry>
+          <cylinder length="0.02" radius="0.035"/>
+        </geometry>
+        <material name="green"/>
+      </visual>
+      <inertial>
+        <origin rpy="0 0 0" xyz="${hip_rotor_com_x} ${hip_rotor_com_y} ${hip_rotor_com_z}"/>
+        <mass value="${hip_rotor_mass}"/>
+        <inertia
+            ixx="${hip_rotor_ixx}" ixy="${hip_rotor_ixy}" ixz="${hip_rotor_ixz}"
+            iyy="${hip_rotor_iyy}" iyz="${hip_rotor_iyz}"
+            izz="${hip_rotor_izz}"/>
+      </inertial>
+    </link>
 
-        <joint name="${name}_thigh_joint" type="revolute">
-            <origin rpy="0 0 0" xyz="0 ${thigh_offset*mirror} 0"/>
-            <parent link="${name}_hip"/>
-            <child link="${name}_thigh"/>
-            <axis xyz="0 1 0"/>
-            <dynamics damping="${damping}" friction="${friction}"/>
-            <limit effort="${thigh_torque_max}" velocity="${thigh_velocity_max}" lower="${thigh_position_min}" upper="${thigh_position_max}"/>
-        </joint>
+    <joint name="${name}_thigh_joint" type="revolute">
+      <origin rpy="0 0 0" xyz="0 ${thigh_offset*mirror} 0"/>
+      <parent link="${name}_hip"/>
+      <child link="${name}_thigh"/>
+      <axis xyz="0 1 0"/>
+      <dynamics damping="${damping}" friction="${friction}"/>
+      <limit effort="${thigh_torque_max}" velocity="${thigh_velocity_max}" lower="${thigh_position_min}"
+             upper="${thigh_position_max}"/>
+    </joint>
 
-        <joint name="${name}_thigh_rotor_joint" type="fixed">
-            <origin rpy="0 0 0" xyz="${thigh_rotor_offset_x} ${thigh_rotor_offset_y*mirror} 0"/>
-            <parent link="${name}_hip"/>
-            <child link="${name}_thigh_rotor"/>         
-        </joint>
+    <joint name="${name}_thigh_rotor_joint" type="fixed">
+      <origin rpy="0 0 0" xyz="${thigh_rotor_offset_x} ${thigh_rotor_offset_y*mirror} 0"/>
+      <parent link="${name}_hip"/>
+      <child link="${name}_thigh_rotor"/>
+    </joint>
 
-        <link name="${name}_thigh">
-            <visual>
-                <origin rpy="0 0 0" xyz="0 0 0"/>
-                <geometry>
-                    <xacro:if value="${mirror_dae == True}">
-                        <mesh filename="file://$(find go1_description)/meshes/thigh.dae" scale="1 1 1"/>
-                    </xacro:if>
-                    <xacro:if value="${mirror_dae == False}">
-                        <mesh filename="file://$(find go1_description)/meshes/thigh_mirror.dae" scale="1 1 1"/>
-                    </xacro:if>
-                </geometry>
-                <!-- <material name="orange"/> -->
-            </visual>
-            <collision>
-                <origin rpy="0 ${PI/2.0} 0" xyz="0 0 ${-thigh_length/2.0}"/>
-                <geometry>
-                    <box size="${thigh_length} ${thigh_width} ${thigh_height}"/>
-                </geometry>      
-            </collision>
-            <inertial>
-                <origin rpy="0 0 0" xyz="${thigh_com_x} ${thigh_com_y*mirror} ${thigh_com_z}"/>
-                <mass value="${thigh_mass}"/>
-                <inertia
-                    ixx="${thigh_ixx}" ixy="${thigh_ixy*mirror}" ixz="${thigh_ixz}"
-                    iyy="${thigh_iyy}" iyz="${thigh_iyz*mirror}"
-                    izz="${thigh_izz}"/>       
-            </inertial>
-        </link>
+    <link name="${name}_thigh">
+      <visual>
+        <origin rpy="0 0 0" xyz="0 0 0"/>
+        <geometry>
+          <xacro:if value="${mirror_dae == True}">
+            <mesh filename="file://$(find go1_description)/meshes/thigh.dae" scale="1 1 1"/>
+          </xacro:if>
+          <xacro:if value="${mirror_dae == False}">
+            <mesh filename="file://$(find go1_description)/meshes/thigh_mirror.dae" scale="1 1 1"/>
+          </xacro:if>
+        </geometry>
+      </visual>
+      <collision>
+        <origin rpy="0 ${PI/2.0} 0" xyz="0 0 ${-thigh_length/2.0}"/>
+        <geometry>
+          <box size="${thigh_length} ${thigh_width} ${thigh_height}"/>
+        </geometry>
+      </collision>
+      <inertial>
+        <origin rpy="0 0 0" xyz="${thigh_com_x} ${thigh_com_y*mirror} ${thigh_com_z}"/>
+        <mass value="${thigh_mass}"/>
+        <inertia
+            ixx="${thigh_ixx}" ixy="${thigh_ixy*mirror}" ixz="${thigh_ixz}"
+            iyy="${thigh_iyy}" iyz="${thigh_iyz*mirror}"
+            izz="${thigh_izz}"/>
+      </inertial>
+    </link>
 
-        <link name="${name}_thigh_rotor">
-            <visual>
-                <origin rpy="${PI/2.0} 0 0" xyz="0 0 0"/>
-                <geometry>
-                    <cylinder length="0.02" radius="0.035"/>
-                </geometry>
-                <material name="green"/>
-            </visual>
-            <!-- <collision>
-                <origin rpy="${PI/2.0} 0 0" xyz="0 0 0"/>
-                <geometry>
-                    <cylinder length="0.02" radius="0.035"/>
-                </geometry>
-            </collision> -->
-            <inertial>
-                <origin rpy="0 0 0" xyz="${thigh_rotor_com_x} ${thigh_rotor_com_y} ${thigh_rotor_com_z}"/>
-                <mass value="${thigh_rotor_mass}"/>
-                <inertia
-                    ixx="${thigh_rotor_ixx}" ixy="${thigh_rotor_ixy}" ixz="${thigh_rotor_ixz}"
-                    iyy="${thigh_rotor_iyy}" iyz="${thigh_rotor_iyz}"
-                    izz="${thigh_rotor_izz}"/>       
-            </inertial>
-        </link>
+    <link name="${name}_thigh_rotor">
+      <visual>
+        <origin rpy="${PI/2.0} 0 0" xyz="0 0 0"/>
+        <geometry>
+          <cylinder length="0.02" radius="0.035"/>
+        </geometry>
+        <material name="green"/>
+      </visual>
+      <inertial>
+        <origin rpy="0 0 0" xyz="${thigh_rotor_com_x} ${thigh_rotor_com_y} ${thigh_rotor_com_z}"/>
+        <mass value="${thigh_rotor_mass}"/>
+        <inertia
+            ixx="${thigh_rotor_ixx}" ixy="${thigh_rotor_ixy}" ixz="${thigh_rotor_ixz}"
+            iyy="${thigh_rotor_iyy}" iyz="${thigh_rotor_iyz}"
+            izz="${thigh_rotor_izz}"/>
+      </inertial>
+    </link>
 
-        <joint name="${name}_calf_joint" type="revolute">
-            <origin rpy="0 0 0" xyz="0 0 ${-thigh_length}"/>
-            <parent link="${name}_thigh"/>
-            <child link="${name}_calf"/>
-            <axis xyz="0 1 0"/>
-            <dynamics damping="${damping}" friction="${friction}"/>
-            <limit effort="${calf_torque_max}" velocity="${calf_velocity_max}" lower="${calf_position_min}" upper="${calf_position_max}"/>
-        </joint>
+    <joint name="${name}_calf_joint" type="revolute">
+      <origin rpy="0 0 0" xyz="0 0 ${-thigh_length}"/>
+      <parent link="${name}_thigh"/>
+      <child link="${name}_calf"/>
+      <axis xyz="0 1 0"/>
+      <dynamics damping="${damping}" friction="${friction}"/>
+      <limit effort="${calf_torque_max}" velocity="${calf_velocity_max}" lower="${calf_position_min}"
+             upper="${calf_position_max}"/>
+    </joint>
 
-        <joint name="${name}_calf_rotor_joint" type="fixed">
-            <origin rpy="0 0 0" xyz="${calf_rotor_offset_x} ${calf_rotor_offset_y*mirror} 0"/>
-            <parent link="${name}_thigh"/>
-            <child link="${name}_calf_rotor"/>         
-        </joint>
+    <joint name="${name}_calf_rotor_joint" type="fixed">
+      <origin rpy="0 0 0" xyz="${calf_rotor_offset_x} ${calf_rotor_offset_y*mirror} 0"/>
+      <parent link="${name}_thigh"/>
+      <child link="${name}_calf_rotor"/>
+    </joint>
 
-        <link name="${name}_calf">
-            <visual>
-                <origin rpy="0 0 0" xyz="0 0 0"/>
-                <geometry>
-                    <mesh filename="file://$(find go1_description)/meshes/calf.dae" scale="1 1 1"/>
-                </geometry>
-                <!-- <material name="orange"/> -->
-            </visual>
-            <collision>
-                <origin rpy="0 ${PI/2.0} 0" xyz="0 0 ${-calf_length/2.0}"/>
-                <geometry>
-                    <box size="${calf_length} ${calf_width} ${calf_height}"/>
-                </geometry>      
-            </collision>
-            <inertial>
-                <origin rpy="0 0 0" xyz="${calf_com_x} ${calf_com_y} ${calf_com_z}"/>
-                <mass value="${calf_mass}"/>
-                <inertia
-                    ixx="${calf_ixx}" ixy="${calf_ixy}" ixz="${calf_ixz}"
-                    iyy="${calf_iyy}" iyz="${calf_iyz}"
-                    izz="${calf_izz}"/>       
-            </inertial>
-        </link>
+    <link name="${name}_calf">
+      <visual>
+        <origin rpy="0 0 0" xyz="0 0 0"/>
+        <geometry>
+          <mesh filename="file://$(find go1_description)/meshes/calf.dae" scale="1 1 1"/>
+        </geometry>
+      </visual>
+      <collision>
+        <origin rpy="0 ${PI/2.0} 0" xyz="0 0 ${-calf_length/2.0}"/>
+        <geometry>
+          <box size="${calf_length} ${calf_width} ${calf_height}"/>
+        </geometry>
+      </collision>
+      <inertial>
+        <origin rpy="0 0 0" xyz="${calf_com_x} ${calf_com_y} ${calf_com_z}"/>
+        <mass value="${calf_mass}"/>
+        <inertia
+            ixx="${calf_ixx}" ixy="${calf_ixy}" ixz="${calf_ixz}"
+            iyy="${calf_iyy}" iyz="${calf_iyz}"
+            izz="${calf_izz}"/>
+      </inertial>
+    </link>
 
-        <link name="${name}_calf_rotor">
-            <visual>
-                <origin rpy="${PI/2.0} 0 0" xyz="0 0 0"/>
-                <geometry>
-                    <cylinder length="0.02" radius="0.035"/>
-                </geometry>
-                <material name="green"/>
-            </visual>
-            <!-- <collision>
-                <origin rpy="${PI/2.0} 0 0" xyz="0 0 0"/>
-                <geometry>
-                    <cylinder length="0.02" radius="0.035"/>
-                </geometry>
-            </collision> -->
-            <inertial>
-                <origin rpy="0 0 0" xyz="${calf_rotor_com_x} ${calf_rotor_com_y} ${calf_rotor_com_z}"/>
-                <mass value="${calf_rotor_mass}"/>
-                <inertia
-                    ixx="${calf_rotor_ixx}" ixy="${calf_rotor_ixy}" ixz="${calf_rotor_ixz}"
-                    iyy="${calf_rotor_iyy}" iyz="${calf_rotor_iyz}"
-                    izz="${calf_rotor_izz}"/>     
-            </inertial>
-        </link>
+    <link name="${name}_calf_rotor">
+      <visual>
+        <origin rpy="${PI/2.0} 0 0" xyz="0 0 0"/>
+        <geometry>
+          <cylinder length="0.02" radius="0.035"/>
+        </geometry>
+      </visual>
+      <inertial>
+        <origin rpy="0 0 0" xyz="${calf_rotor_com_x} ${calf_rotor_com_y} ${calf_rotor_com_z}"/>
+        <mass value="${calf_rotor_mass}"/>
+        <inertia
+            ixx="${calf_rotor_ixx}" ixy="${calf_rotor_ixy}" ixz="${calf_rotor_ixz}"
+            iyy="${calf_rotor_iyy}" iyz="${calf_rotor_iyz}"
+            izz="${calf_rotor_izz}"/>
+      </inertial>
+    </link>
 
-        <joint name="${name}_foot_fixed" type="fixed">
-            <origin rpy="0 0 0" xyz="0 0 ${-(calf_length)}"/>
-            <parent link="${name}_calf"/>
-            <child link="${name}_foot"/>
-        </joint>
+    <joint name="${name}_foot_fixed" type="fixed">
+      <origin rpy="0 0 0" xyz="0 0 ${-(calf_length)}"/>
+      <parent link="${name}_calf"/>
+      <child link="${name}_foot"/>
+    </joint>
 
-        <link name="${name}_foot">
-            <visual>
-                <origin rpy="0 0 0" xyz="0 0 0"/>
-                <geometry>
-                    <sphere radius="${foot_radius-0.01}"/>
-                </geometry>
-                <!-- <material name="orange"/> -->
-            </visual>
-            <collision>
-                <origin rpy="0 0 0" xyz="0 0 0"/>
-                <geometry>
-                    <sphere radius="${foot_radius}"/>
-                </geometry>
-            </collision>
-            <inertial>
-                <mass value="${foot_mass}"/>
-                <inertia
-                    ixx="${(2*foot_mass)/5.0*(foot_radius*foot_radius)}" ixy="0.0" ixz="0.0"
-                    iyy="${(2*foot_mass)/5.0*(foot_radius*foot_radius)}" iyz="0.0"
-                    izz="${(2*foot_mass)/5.0*(foot_radius*foot_radius)}"/>             
-            </inertial>
-        </link>
+    <link name="${name}_foot">
+      <visual>
+        <origin rpy="0 0 0" xyz="0 0 0"/>
+        <geometry>
+          <sphere radius="${foot_radius-0.01}"/>
+        </geometry>
+      </visual>
+      <collision>
+        <origin rpy="0 0 0" xyz="0 0 0"/>
+        <geometry>
+          <sphere radius="${foot_radius}"/>
+        </geometry>
+      </collision>
+      <inertial>
+        <mass value="${foot_mass}"/>
+        <inertia
+            ixx="${(2*foot_mass)/5.0*(foot_radius*foot_radius)}" ixy="0.0" ixz="0.0"
+            iyy="${(2*foot_mass)/5.0*(foot_radius*foot_radius)}" iyz="0.0"
+            izz="${(2*foot_mass)/5.0*(foot_radius*foot_radius)}"/>
+      </inertial>
+    </link>
 
-        <xacro:leg_transmission name="${name}"/>
-    </xacro:macro>
+    <xacro:leg_transmission name="${name}"/>
+  </xacro:macro>
 </robot>
diff --git a/descriptions/go2_description/README.md b/descriptions/go2_description/README.md
index f14ce12..3d8b103 100644
--- a/descriptions/go2_description/README.md
+++ b/descriptions/go2_description/README.md
@@ -24,7 +24,7 @@ ros2 launch go2_description visualize.launch.py
 * Unitree Guide Controller
   ```bash
   source ~/ros2_ws/install/setup.bash
-  ros2 launch go2_description hardware.launch.py
+  ros2 launch go2_description unitree_guide.launch.py
   ```
 * OCS2 Quadruped Controller
   ```bash
diff --git a/descriptions/go2_description/config/ocs2/reference.info b/descriptions/go2_description/config/ocs2/reference.info
index b36ce8e..82f5088 100644
--- a/descriptions/go2_description/config/ocs2/reference.info
+++ b/descriptions/go2_description/config/ocs2/reference.info
@@ -1,20 +1,20 @@
 targetDisplacementVelocity          0.5;
 targetRotationVelocity              1.57;
 
-comHeight                           0.3
+comHeight                           0.29
 
 defaultJointState
 {
-   (0,0)  -0.20   ; FL_hip_joint
+   (0,0)  -0.10   ; FL_hip_joint
    (1,0)   0.72   ; FL_thigh_joint
    (2,0)  -1.44   ; FL_calf_joint
-   (3,0)  -0.20   ; RL_hip_joint
-   (4,0)   0.72   ; RL_thigh_joint
-   (5,0)  -1.44   ; RL_calf_joint
-   (6,0)   0.20   ; FR_hip_joint
-   (7,0)   0.72   ; FR_thigh_joint
-   (8,0)  -1.44   ; FR_calf_joint
-   (9,0)   0.20   ; RR_hip_joint
+   (3,0)   0.10   ; FR_hip_joint
+   (4,0)   0.72   ; FR_thigh_joint
+   (5,0)  -1.44   ; FR_calf_joint
+   (6,0)  -0.10   ; RL_hip_joint
+   (7,0)   0.72   ; RL_thigh_joint
+   (8,0)  -1.44   ; RL_calf_joint
+   (9,0)   0.10   ; RR_hip_joint
    (10,0)  0.72   ; RR_thigh_joint
    (11,0) -1.44   ; RR_calf_joint
 }
diff --git a/descriptions/go2_description/launch/hardware.launch.py b/descriptions/go2_description/launch/unitree_guide.launch.py
similarity index 100%
rename from descriptions/go2_description/launch/hardware.launch.py
rename to descriptions/go2_description/launch/unitree_guide.launch.py
diff --git a/descriptions/go2_description/xacro/leg.xacro b/descriptions/go2_description/xacro/leg.xacro
index 63cc853..4b78614 100755
--- a/descriptions/go2_description/xacro/leg.xacro
+++ b/descriptions/go2_description/xacro/leg.xacro
@@ -51,7 +51,6 @@
                 <geometry>
                     <mesh filename="file://$(find go2_description)/meshes/hip.dae" scale="1 1 1"/>
                 </geometry>
-                <!-- <material name="orange"/> -->
             </visual>
             <collision>
                 <origin rpy="${PI/2.0} 0 0" xyz="0 ${hip_offset*mirror} 0"/>
@@ -91,7 +90,6 @@
                         <mesh filename="file://$(find go2_description)/meshes/thigh_mirror.dae" scale="1 1 1"/>
                     </xacro:if>
                 </geometry>
-                <!-- <material name="orange"/> -->
             </visual>
             <collision>
                 <origin rpy="0 ${PI/2.0} 0" xyz="0 0 ${-thigh_length/2.0}"/>
@@ -127,7 +125,6 @@
                 <geometry>
                     <mesh filename="file://$(find go2_description)/meshes/calf.dae" scale="1 1 1"/>
                 </geometry>
-                <!-- <material name="orange"/> -->
             </visual>
             <collision>
                 <origin rpy="0 ${PI/2.0} 0" xyz="0 0 ${-calf_length/2.0}"/>
@@ -158,7 +155,6 @@
                 <geometry>
                     <sphere radius="${foot_radius-0.01}"/>
                 </geometry>
-                <!-- <material name="orange"/> -->
             </visual>
             <collision>
                 <origin rpy="0 0 0" xyz="0 0 0"/>