diff --git a/controllers/quadruped_ros2_control/config/quadruped_controller.yaml b/controllers/quadruped_ros2_control/config/quadruped_controller.yaml
deleted file mode 100644
index fc2f07a..0000000
--- a/controllers/quadruped_ros2_control/config/quadruped_controller.yaml
+++ /dev/null
@@ -1,24 +0,0 @@
-quadruped_controller:
-  ros__parameters:
-    type: quadruped_ros2_control/QuadrupedController
-    joints:
-      - RF_HAA
-      - LF_HAA
-      - RH_HAA
-      - LH_HAA
-      - RF_HFE
-      - LF_HFE
-      - RH_HFE
-      - LH_HFE
-      - RF_KFE
-      - LF_KFE
-      - RH_KFE
-      - LH_KFE
-
-    command_interfaces:
-      - effort
-
-    state_interfaces:
-      - effort
-      - position
-      - velocity
\ No newline at end of file
diff --git a/controllers/quadruped_ros2_control/include/quadruped_ros2_control/FSM/FSMState.h b/controllers/quadruped_ros2_control/include/quadruped_ros2_control/FSM/FSMState.h
deleted file mode 100644
index 4cab71e..0000000
--- a/controllers/quadruped_ros2_control/include/quadruped_ros2_control/FSM/FSMState.h
+++ /dev/null
@@ -1,30 +0,0 @@
-//
-// Created by tlab-uav on 24-9-6.
-//
-
-#ifndef FSMSTATE_H
-#define FSMSTATE_H
-
-#include <string>
-#include "quadruped_ros2_control/common/enumClass.h"
-
-class FSMState {
-public:
-    virtual ~FSMState() = default;
-
-    FSMState(FSMStateName stateName, std::string stateNameString);
-
-    virtual void enter();
-
-    virtual void run();
-
-    virtual void exit();
-
-    virtual FSMStateName checkChange() { return FSMStateName::INVALID; }
-
-    FSMStateName state_name;
-    std::string state_name_string;
-};
-
-
-#endif //FSMSTATE_H
diff --git a/controllers/quadruped_ros2_control/package.xml b/controllers/quadruped_ros2_control/package.xml
deleted file mode 100644
index b467ceb..0000000
--- a/controllers/quadruped_ros2_control/package.xml
+++ /dev/null
@@ -1,22 +0,0 @@
-<?xml version="1.0"?>
-<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
-<package format="3">
-  <name>quadruped_ros2_control</name>
-  <version>0.0.0</version>
-  <description>TODO: Package description</description>
-  <maintainer email="biao876990970@hotmail.com">tlab-uav</maintainer>
-  <license>Apache-2.0</license>
-
-  <buildtool_depend>ament_cmake</buildtool_depend>
-
-  <depend>backward_ros</depend>
-  <depend>controller_interface</depend>
-  <depend>pluginlib</depend>
-
-  <test_depend>ament_lint_auto</test_depend>
-  <test_depend>ament_lint_common</test_depend>
-
-  <export>
-    <build_type>ament_cmake</build_type>
-  </export>
-</package>
diff --git a/controllers/quadruped_ros2_control/quadruped_ros2_control.xml b/controllers/quadruped_ros2_control/quadruped_ros2_control.xml
deleted file mode 100644
index b44297f..0000000
--- a/controllers/quadruped_ros2_control/quadruped_ros2_control.xml
+++ /dev/null
@@ -1,8 +0,0 @@
-<library path="quadruped_ros2_control">
-    <class name="quadruped_ros2_control/QuadrupedController" type="quadruped_ros2_control::QuadrupedController"
-           base_class_type="controller_interface::ControllerInterface">
-        <description>
-            Basic Quadruped Controller based on Unitree Guide.
-        </description>
-    </class>
-</library>
diff --git a/controllers/quadruped_ros2_control/src/FSM/FSMState.cpp b/controllers/quadruped_ros2_control/src/FSM/FSMState.cpp
deleted file mode 100644
index 6432c49..0000000
--- a/controllers/quadruped_ros2_control/src/FSM/FSMState.cpp
+++ /dev/null
@@ -1,11 +0,0 @@
-//
-// Created by tlab-uav on 24-9-6.
-//
-
-#include <quadruped_ros2_control/FSM/FSMState.h>
-
-#include <utility>
-
-FSMState::FSMState(const FSMStateName stateName, std::string stateNameString)
-    : state_name(stateName), state_name_string(std::move(stateNameString)) {
-}
diff --git a/controllers/quadruped_ros2_control/src/FSM/StatePassive.cpp b/controllers/quadruped_ros2_control/src/FSM/StatePassive.cpp
deleted file mode 100644
index 90440f2..0000000
--- a/controllers/quadruped_ros2_control/src/FSM/StatePassive.cpp
+++ /dev/null
@@ -1,12 +0,0 @@
-//
-// Created by tlab-uav on 24-9-6.
-//
-
-#include <quadruped_ros2_control/FSM/StatePassive.h>
-
-StatePassive::StatePassive() : FSMState(FSMStateName::PASSIVE, "passive") {
-}
-
-void StatePassive::enter() {
-
-}
diff --git a/controllers/quadruped_ros2_control/CMakeLists.txt b/controllers/unitree_guide_controller/CMakeLists.txt
similarity index 90%
rename from controllers/quadruped_ros2_control/CMakeLists.txt
rename to controllers/unitree_guide_controller/CMakeLists.txt
index 2b81830..b58e3ca 100644
--- a/controllers/quadruped_ros2_control/CMakeLists.txt
+++ b/controllers/unitree_guide_controller/CMakeLists.txt
@@ -1,5 +1,5 @@
 cmake_minimum_required(VERSION 3.8)
-project(quadruped_ros2_control)
+project(unitree_guide_controller)
 
 if (CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
     add_compile_options(-Wall -Wextra -Wpedantic)
@@ -23,9 +23,9 @@ foreach (Dependency IN ITEMS ${CONTROLLER_INCLUDE_DEPENDS})
 endforeach ()
 
 add_library(${PROJECT_NAME} SHARED
-        src/QuadrupedController.cpp
-        src/FSM/FSMState.cpp
+        src/UnitreeGuideController.cpp
         src/FSM/StatePassive.cpp
+        src/FSM/StateFixedStand.cpp
 )
 target_include_directories(${PROJECT_NAME}
         PUBLIC
@@ -36,7 +36,7 @@ ament_target_dependencies(
         ${CONTROLLER_INCLUDE_DEPENDS}
 )
 
-pluginlib_export_plugin_description_file(controller_interface quadruped_ros2_control.xml)
+pluginlib_export_plugin_description_file(controller_interface unitree_guide_controller.xml)
 
 install(
         DIRECTORY include/
diff --git a/controllers/quadruped_ros2_control/LICENSE b/controllers/unitree_guide_controller/LICENSE
similarity index 100%
rename from controllers/quadruped_ros2_control/LICENSE
rename to controllers/unitree_guide_controller/LICENSE
diff --git a/controllers/unitree_guide_controller/config/controller.yaml b/controllers/unitree_guide_controller/config/controller.yaml
new file mode 100644
index 0000000..e21fb2a
--- /dev/null
+++ b/controllers/unitree_guide_controller/config/controller.yaml
@@ -0,0 +1,28 @@
+unitree_guide_controller:
+  ros__parameters:
+    type: unitree_guide_controller/UnitreeGuideController
+    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
+      - RL_hip_joint
+      - RL_thigh_joint
+      - RL_calf_joint
+
+    command_interfaces:
+      - effort
+      - position
+      - velocity
+      - kp
+      - kd
+
+    state_interfaces:
+      - effort
+      - position
+      - velocity
\ No newline at end of file
diff --git a/controllers/unitree_guide_controller/include/unitree_guide_controller/FSM/FSMState.h b/controllers/unitree_guide_controller/include/unitree_guide_controller/FSM/FSMState.h
new file mode 100644
index 0000000..aab64e3
--- /dev/null
+++ b/controllers/unitree_guide_controller/include/unitree_guide_controller/FSM/FSMState.h
@@ -0,0 +1,38 @@
+//
+// Created by tlab-uav on 24-9-6.
+//
+
+#ifndef FSMSTATE_H
+#define FSMSTATE_H
+
+#include <string>
+#include <utility>
+#include <unitree_guide_controller/common/enumClass.h>
+#include <unitree_guide_controller/common/interface.h>
+
+class FSMState {
+public:
+    virtual ~FSMState() = default;
+
+    FSMState(const FSMStateName &stateName, std::string stateNameString, CtrlComponent ctrlComp)
+        : state_name(stateName),
+          state_name_string(std::move(stateNameString)),
+          ctrlComp_(std::move(ctrlComp)) {
+    }
+
+    virtual void enter() = 0;
+
+    virtual void run() = 0;
+
+    virtual void exit() = 0;
+
+    virtual FSMStateName checkChange() { return FSMStateName::INVALID; }
+
+    FSMStateName state_name;
+    std::string state_name_string;
+
+protected:
+    CtrlComponent ctrlComp_;
+};
+
+#endif //FSMSTATE_H
diff --git a/controllers/unitree_guide_controller/include/unitree_guide_controller/FSM/StateFixedStand.h b/controllers/unitree_guide_controller/include/unitree_guide_controller/FSM/StateFixedStand.h
new file mode 100644
index 0000000..8e7b06d
--- /dev/null
+++ b/controllers/unitree_guide_controller/include/unitree_guide_controller/FSM/StateFixedStand.h
@@ -0,0 +1,26 @@
+//
+// Created by biao on 24-9-10.
+//
+
+#include "FSMState.h"
+
+#ifndef STATEFIXEDSTAND_H
+#define STATEFIXEDSTAND_H
+
+
+
+class StateFixedStand : public FSMState {
+    explicit StateFixedStand(CtrlComponent ctrlComp);
+
+    void enter() override;
+
+    void run() override;
+
+    void exit() override;
+
+    FSMStateName checkChange() override;
+};
+
+
+
+#endif //STATEFIXEDSTAND_H
diff --git a/controllers/quadruped_ros2_control/include/quadruped_ros2_control/FSM/StatePassive.h b/controllers/unitree_guide_controller/include/unitree_guide_controller/FSM/StatePassive.h
similarity index 86%
rename from controllers/quadruped_ros2_control/include/quadruped_ros2_control/FSM/StatePassive.h
rename to controllers/unitree_guide_controller/include/unitree_guide_controller/FSM/StatePassive.h
index 8a4bc65..136bacc 100644
--- a/controllers/quadruped_ros2_control/include/quadruped_ros2_control/FSM/StatePassive.h
+++ b/controllers/unitree_guide_controller/include/unitree_guide_controller/FSM/StatePassive.h
@@ -9,7 +9,7 @@
 
 class StatePassive final : public FSMState {
 public:
-    explicit StatePassive();
+    explicit StatePassive(CtrlComponent ctrlComp);
 
     void enter() override;
 
diff --git a/controllers/quadruped_ros2_control/include/quadruped_ros2_control/QuadrupedController.h b/controllers/unitree_guide_controller/include/unitree_guide_controller/UnitreeGuideController.h
similarity index 62%
rename from controllers/quadruped_ros2_control/include/quadruped_ros2_control/QuadrupedController.h
rename to controllers/unitree_guide_controller/include/unitree_guide_controller/UnitreeGuideController.h
index 62fb214..9b0b9ff 100644
--- a/controllers/quadruped_ros2_control/include/quadruped_ros2_control/QuadrupedController.h
+++ b/controllers/unitree_guide_controller/include/unitree_guide_controller/UnitreeGuideController.h
@@ -5,14 +5,25 @@
 #ifndef QUADRUPEDCONTROLLER_H
 #define QUADRUPEDCONTROLLER_H
 
-#include "controller_interface/controller_interface.hpp"
+#include <controller_interface/controller_interface.hpp>
 #include <std_msgs/msg/string.hpp>
+#include <unitree_guide_controller/FSM/FSMState.h>
+#include <unitree_guide_controller/common/enumClass.h>
 
-namespace quadruped_ros2_control {
-    class QuadrupedController final : public controller_interface::ControllerInterface {
+#include "FSM/StateFixedStand.h"
+#include "FSM/StatePassive.h"
+
+namespace unitree_guide_controller {
+    struct FSMStateList {
+        std::shared_ptr<FSMState> invalid;
+        std::shared_ptr<StatePassive> passive;
+        std::shared_ptr<StateFixedStand> fixedStand;
+    };
+
+    class UnitreeGuideController final : public controller_interface::ControllerInterface {
     public:
         CONTROLLER_INTERFACE_PUBLIC
-        QuadrupedController() = default;
+        UnitreeGuideController() = default;
 
         CONTROLLER_INTERFACE_PUBLIC
         controller_interface::InterfaceConfiguration command_interface_configuration() const override;
@@ -51,6 +62,8 @@ namespace quadruped_ros2_control {
         controller_interface::CallbackReturn on_shutdown(
             const rclcpp_lifecycle::State &previous_state) override;
 
+        CtrlComponent ctrl_comp_;
+
     protected:
         std::vector<std::string> joint_names_;
         std::vector<std::string> command_interface_types_;
@@ -58,29 +71,29 @@ namespace quadruped_ros2_control {
 
         rclcpp::Subscription<std_msgs::msg::String>::SharedPtr state_command_subscriber_;
 
-        std::vector<std::reference_wrapper<hardware_interface::LoanedCommandInterface> >
-        joint_effort_command_interface_;
-        std::vector<std::reference_wrapper<hardware_interface::LoanedStateInterface> >
-        joint_effort_state_interface_;
-        std::vector<std::reference_wrapper<hardware_interface::LoanedStateInterface> >
-        joint_position_state_interface_;
-        std::vector<std::reference_wrapper<hardware_interface::LoanedStateInterface> >
-        joint_velocity_state_interface_;
-
         std::unordered_map<
             std::string, std::vector<std::reference_wrapper<hardware_interface::LoanedCommandInterface> > *>
         command_interface_map_ = {
-            {"effort", &joint_effort_command_interface_}
+            {"effort", &ctrl_comp_.joint_effort_command_interface_},
+            {"position", &ctrl_comp_.joint_position_command_interface_},
+            {"velocity", &ctrl_comp_.joint_velocity_command_interface_},
+            {"kp", &ctrl_comp_.joint_kp_command_interface_},
+            {"kd", &ctrl_comp_.joint_kd_command_interface_}
         };
 
-        bool stand_ = false;
+        FSMMode mode_ = FSMMode::NORMAL;
+        std::string state_name_;
+        FSMStateName next_state_name_ = FSMStateName::INVALID;
+        FSMStateList state_list_;
+        std::shared_ptr<FSMState> current_state_;
+        std::shared_ptr<FSMState> next_state_;
 
         std::unordered_map<
             std::string, std::vector<std::reference_wrapper<hardware_interface::LoanedStateInterface> > *>
         state_interface_map_ = {
-            {"position", &joint_position_state_interface_},
-            {"effort", &joint_effort_state_interface_},
-            {"velocity", &joint_velocity_state_interface_}
+            {"position", &ctrl_comp_.joint_position_state_interface_},
+            {"effort", &ctrl_comp_.joint_effort_state_interface_},
+            {"velocity", &ctrl_comp_.joint_velocity_state_interface_}
         };
     };
 }
diff --git a/controllers/quadruped_ros2_control/include/quadruped_ros2_control/common/enumClass.h b/controllers/unitree_guide_controller/include/unitree_guide_controller/common/enumClass.h
similarity index 83%
rename from controllers/quadruped_ros2_control/include/quadruped_ros2_control/common/enumClass.h
rename to controllers/unitree_guide_controller/include/unitree_guide_controller/common/enumClass.h
index 5652230..7d74777 100644
--- a/controllers/quadruped_ros2_control/include/quadruped_ros2_control/common/enumClass.h
+++ b/controllers/unitree_guide_controller/include/unitree_guide_controller/common/enumClass.h
@@ -15,7 +15,11 @@ enum class FSMStateName {
 
     SWINGTEST,
     BALANCETEST,
-  };
+};
 
+enum class FSMMode {
+    NORMAL,
+    CHANGE
+};
 
 #endif //ENUMCLASS_H
diff --git a/controllers/unitree_guide_controller/include/unitree_guide_controller/common/interface.h b/controllers/unitree_guide_controller/include/unitree_guide_controller/common/interface.h
new file mode 100644
index 0000000..9142ca8
--- /dev/null
+++ b/controllers/unitree_guide_controller/include/unitree_guide_controller/common/interface.h
@@ -0,0 +1,46 @@
+//
+// Created by biao on 24-9-10.
+//
+
+#ifndef INTERFACE_H
+#define INTERFACE_H
+
+#include <vector>
+#include <hardware_interface/loaned_command_interface.hpp>
+#include <hardware_interface/loaned_state_interface.hpp>
+
+struct CtrlComponent {
+    std::vector<std::reference_wrapper<hardware_interface::LoanedCommandInterface> >
+    joint_effort_command_interface_;
+    std::vector<std::reference_wrapper<hardware_interface::LoanedCommandInterface> >
+    joint_position_command_interface_;
+    std::vector<std::reference_wrapper<hardware_interface::LoanedCommandInterface> >
+    joint_velocity_command_interface_;
+    std::vector<std::reference_wrapper<hardware_interface::LoanedCommandInterface> >
+    joint_kp_command_interface_;
+    std::vector<std::reference_wrapper<hardware_interface::LoanedCommandInterface> >
+    joint_kd_command_interface_;
+
+
+    std::vector<std::reference_wrapper<hardware_interface::LoanedStateInterface> >
+    joint_effort_state_interface_;
+    std::vector<std::reference_wrapper<hardware_interface::LoanedStateInterface> >
+    joint_position_state_interface_;
+    std::vector<std::reference_wrapper<hardware_interface::LoanedStateInterface> >
+    joint_velocity_state_interface_;
+
+public:
+    void clear() {
+        joint_effort_command_interface_.clear();
+        joint_position_command_interface_.clear();
+        joint_velocity_command_interface_.clear();
+        joint_kd_command_interface_.clear();
+        joint_kp_command_interface_.clear();
+
+        joint_effort_state_interface_.clear();
+        joint_position_state_interface_.clear();
+        joint_velocity_state_interface_.clear();
+    }
+};
+
+#endif //INTERFACE_H
diff --git a/controllers/quadruped_ros2_control/launch/controller.launch.py b/controllers/unitree_guide_controller/launch/controller.launch.py
similarity index 86%
rename from controllers/quadruped_ros2_control/launch/controller.launch.py
rename to controllers/unitree_guide_controller/launch/controller.launch.py
index 4ca1361..8382500 100644
--- a/controllers/quadruped_ros2_control/launch/controller.launch.py
+++ b/controllers/unitree_guide_controller/launch/controller.launch.py
@@ -24,16 +24,16 @@ def generate_launch_description():
 
     robot_controllers = PathJoinSubstitution(
         [
-            FindPackageShare("quadruped_ros2_control"),
+            FindPackageShare("unitree_guide_controller"),
             "config",
-            "quadruped_controller.yaml",
+            "controller.yaml",
         ]
     )
 
     robot_controller_spawner = Node(
         package="controller_manager",
         executable="spawner",
-        arguments=["quadruped_controller", "--param-file", robot_controllers],
+        arguments=["unitree_guide_controller", "--param-file", robot_controllers],
     )
 
     nodes = [
diff --git a/controllers/unitree_guide_controller/package.xml b/controllers/unitree_guide_controller/package.xml
new file mode 100644
index 0000000..bcb8b1e
--- /dev/null
+++ b/controllers/unitree_guide_controller/package.xml
@@ -0,0 +1,22 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+    <name>unitree_guide_controller</name>
+    <version>0.0.0</version>
+    <description>A Ros2 Control Controller based on Unitree Guide</description>
+    <maintainer email="biao876990970@hotmail.com">Huang Zhenbiao</maintainer>
+    <license>Apache-2.0</license>
+
+    <buildtool_depend>ament_cmake</buildtool_depend>
+
+    <depend>backward_ros</depend>
+    <depend>controller_interface</depend>
+    <depend>pluginlib</depend>
+
+    <test_depend>ament_lint_auto</test_depend>
+    <test_depend>ament_lint_common</test_depend>
+
+    <export>
+        <build_type>ament_cmake</build_type>
+    </export>
+</package>
diff --git a/controllers/unitree_guide_controller/readme.md b/controllers/unitree_guide_controller/readme.md
new file mode 100644
index 0000000..df95a92
--- /dev/null
+++ b/controllers/unitree_guide_controller/readme.md
@@ -0,0 +1,13 @@
+# Unitree Guide Controller
+
+This is a ros2-control controller based on unitree guide.
+
+```bash
+cd ~/ros2_ws
+colcon build --packages-up-to unitree_guide_controller
+```
+
+```bash
+source ~/ros2_ws/install/setup.bash
+ros2 launch unitree_guide_controller controller.launch.py
+```
\ No newline at end of file
diff --git a/controllers/unitree_guide_controller/src/FSM/StateFixedStand.cpp b/controllers/unitree_guide_controller/src/FSM/StateFixedStand.cpp
new file mode 100644
index 0000000..36a3481
--- /dev/null
+++ b/controllers/unitree_guide_controller/src/FSM/StateFixedStand.cpp
@@ -0,0 +1,22 @@
+//
+// Created by biao on 24-9-10.
+//
+
+#include <unitree_guide_controller/FSM/StateFixedStand.h>
+
+StateFixedStand::StateFixedStand(CtrlComponent ctrlComp): FSMState(
+    FSMStateName::FIXEDSTAND, "fixed stand", std::move(ctrlComp)) {
+}
+
+void StateFixedStand::enter() {
+}
+
+void StateFixedStand::run() {
+}
+
+void StateFixedStand::exit() {
+}
+
+FSMStateName StateFixedStand::checkChange() {
+    return FSMStateName::FIXEDSTAND;
+}
diff --git a/controllers/unitree_guide_controller/src/FSM/StatePassive.cpp b/controllers/unitree_guide_controller/src/FSM/StatePassive.cpp
new file mode 100644
index 0000000..ec8f12c
--- /dev/null
+++ b/controllers/unitree_guide_controller/src/FSM/StatePassive.cpp
@@ -0,0 +1,45 @@
+//
+// Created by tlab-uav on 24-9-6.
+//
+
+#include <iostream>
+#include <ostream>
+#include <unitree_guide_controller/FSM/StatePassive.h>
+
+#include <utility>
+
+StatePassive::StatePassive(CtrlComponent ctrlComp) : FSMState(
+    FSMStateName::PASSIVE, "passive", std::move(ctrlComp)) {
+}
+
+void StatePassive::enter() {
+    for (auto i: ctrlComp_.joint_effort_command_interface_) {
+        i.get().set_value(0);
+    }
+    for (auto i: ctrlComp_.joint_position_command_interface_) {
+        i.get().set_value(0);
+    }
+    for (auto i: ctrlComp_.joint_velocity_command_interface_) {
+        i.get().set_value(0);
+    }
+    for (auto i: ctrlComp_.joint_kp_command_interface_) {
+        i.get().set_value(0);
+    }
+    for (auto i: ctrlComp_.joint_kd_command_interface_) {
+        i.get().set_value(3.5);
+    }
+    std::cout<<"passive"<<std::endl;
+}
+
+void StatePassive::run() {
+}
+
+void StatePassive::exit() {
+}
+
+FSMStateName StatePassive::checkChange() {
+    // if (controller_->state_name_ == "fixed_stand") {
+    //     return FSMStateName::FIXEDSTAND;
+    // }
+    return FSMStateName::PASSIVE;
+}
diff --git a/controllers/quadruped_ros2_control/src/QuadrupedController.cpp b/controllers/unitree_guide_controller/src/UnitreeGuideController.cpp
similarity index 63%
rename from controllers/quadruped_ros2_control/src/QuadrupedController.cpp
rename to controllers/unitree_guide_controller/src/UnitreeGuideController.cpp
index 75da76b..ffad0ee 100644
--- a/controllers/quadruped_ros2_control/src/QuadrupedController.cpp
+++ b/controllers/unitree_guide_controller/src/UnitreeGuideController.cpp
@@ -2,12 +2,13 @@
 // Created by tlab-uav on 24-9-6.
 //
 
-#include <quadruped_ros2_control/QuadrupedController.h>
+#include <unitree_guide_controller/UnitreeGuideController.h>
+#include <unitree_guide_controller/FSM/StatePassive.h>
 
-namespace quadruped_ros2_control {
+namespace unitree_guide_controller {
     using config_type = controller_interface::interface_configuration_type;
 
-    controller_interface::InterfaceConfiguration QuadrupedController::command_interface_configuration() const {
+    controller_interface::InterfaceConfiguration UnitreeGuideController::command_interface_configuration() const {
         controller_interface::InterfaceConfiguration conf = {config_type::INDIVIDUAL, {}};
 
         conf.names.reserve(joint_names_.size() * command_interface_types_.size());
@@ -20,7 +21,7 @@ namespace quadruped_ros2_control {
         return conf;
     }
 
-    controller_interface::InterfaceConfiguration QuadrupedController::state_interface_configuration() const {
+    controller_interface::InterfaceConfiguration UnitreeGuideController::state_interface_configuration() const {
         controller_interface::InterfaceConfiguration conf = {config_type::INDIVIDUAL, {}};
 
         conf.names.reserve(joint_names_.size() * state_interface_types_.size());
@@ -33,21 +34,16 @@ namespace quadruped_ros2_control {
         return conf;
     }
 
-    controller_interface::return_type QuadrupedController::
+    controller_interface::return_type UnitreeGuideController::
     update(const rclcpp::Time &time, const rclcpp::Duration &period) {
-        if (stand_) {
-            for (auto i: joint_effort_command_interface_) {
-                i.get().set_value(30);
-            }
-        } else {
-            for (auto i: joint_effort_command_interface_) {
-                i.get().set_value(0);
-            }
+        if (mode_ == FSMMode::NORMAL) {
+            current_state_->run();
         }
+
         return controller_interface::return_type::OK;
     }
 
-    controller_interface::CallbackReturn QuadrupedController::on_init() {
+    controller_interface::CallbackReturn UnitreeGuideController::on_init() {
         try {
             joint_names_ = auto_declare<std::vector<std::string> >("joints", joint_names_);
             command_interface_types_ =
@@ -64,29 +60,21 @@ namespace quadruped_ros2_control {
         return CallbackReturn::SUCCESS;
     }
 
-    controller_interface::CallbackReturn QuadrupedController::on_configure(
+    controller_interface::CallbackReturn UnitreeGuideController::on_configure(
         const rclcpp_lifecycle::State &previous_state) {
-        stand_ = false;
         state_command_subscriber_ = get_node()->create_subscription<std_msgs::msg::String>(
             "state_command", 10, [this](const std_msgs::msg::String::SharedPtr msg) {
                 // Handle message
-                RCLCPP_INFO(get_node()->get_logger(), "Received command: %s", msg->data.c_str());
-                if (msg.get()->data == "stand") {
-                    stand_ = true;
-                } else {
-                    stand_ = false;
-                }
+                RCLCPP_INFO(get_node()->get_logger(), "Switch State: %s", msg->data.c_str());
+                state_name_ = msg->data;
             });
         return CallbackReturn::SUCCESS;
     }
 
     controller_interface::CallbackReturn
-    QuadrupedController::on_activate(const rclcpp_lifecycle::State &previous_state) {
+    UnitreeGuideController::on_activate(const rclcpp_lifecycle::State &previous_state) {
         // clear out vectors in case of restart
-        joint_effort_command_interface_.clear();
-        joint_effort_state_interface_.clear();
-        joint_position_state_interface_.clear();
-        joint_velocity_state_interface_.clear();
+        ctrl_comp_.clear();
 
         // assign command interfaces
         for (auto &interface: command_interfaces_) {
@@ -98,29 +86,40 @@ namespace quadruped_ros2_control {
             state_interface_map_[interface.get_interface_name()]->push_back(interface);
         }
 
+        state_list_.passive = std::make_shared<StatePassive>(ctrl_comp_);
+        state_list_.fixedStand = std::make_shared<StateFixedStand>(ctrl_comp_);
+
+        // Initialize FSM
+        current_state_ = state_list_.passive;
+        current_state_->enter();
+        next_state_ = current_state_;
+        next_state_name_ = current_state_->state_name;
+        mode_ = FSMMode::NORMAL;
+
         return CallbackReturn::SUCCESS;
     }
 
-    controller_interface::CallbackReturn QuadrupedController::on_deactivate(
+    controller_interface::CallbackReturn UnitreeGuideController::on_deactivate(
         const rclcpp_lifecycle::State &previous_state) {
         release_interfaces();
         return CallbackReturn::SUCCESS;
     }
 
     controller_interface::CallbackReturn
-    QuadrupedController::on_cleanup(const rclcpp_lifecycle::State &previous_state) {
-        return CallbackReturn::SUCCESS;
-    }
-
-    controller_interface::CallbackReturn QuadrupedController::on_error(const rclcpp_lifecycle::State &previous_state) {
+    UnitreeGuideController::on_cleanup(const rclcpp_lifecycle::State &previous_state) {
         return CallbackReturn::SUCCESS;
     }
 
     controller_interface::CallbackReturn
-    QuadrupedController::on_shutdown(const rclcpp_lifecycle::State &previous_state) {
+    UnitreeGuideController::on_error(const rclcpp_lifecycle::State &previous_state) {
+        return CallbackReturn::SUCCESS;
+    }
+
+    controller_interface::CallbackReturn
+    UnitreeGuideController::on_shutdown(const rclcpp_lifecycle::State &previous_state) {
         return CallbackReturn::SUCCESS;
     }
 }
 
 #include "pluginlib/class_list_macros.hpp"
-PLUGINLIB_EXPORT_CLASS(quadruped_ros2_control::QuadrupedController, controller_interface::ControllerInterface);
+PLUGINLIB_EXPORT_CLASS(unitree_guide_controller::UnitreeGuideController, controller_interface::ControllerInterface);
diff --git a/controllers/unitree_guide_controller/unitree_guide_controller.xml b/controllers/unitree_guide_controller/unitree_guide_controller.xml
new file mode 100644
index 0000000..5abb569
--- /dev/null
+++ b/controllers/unitree_guide_controller/unitree_guide_controller.xml
@@ -0,0 +1,9 @@
+<library path="unitree_guide_controller">
+    <class name="unitree_guide_controller/UnitreeGuideController"
+           type="unitree_guide_controller::UnitreeGuideController"
+           base_class_type="controller_interface::ControllerInterface">
+        <description>
+            Quadruped Controller based on Unitree Guide.
+        </description>
+    </class>
+</library>
diff --git a/descriptions/go2_description/README.md b/descriptions/go2_description/README.md
index 5abbf86..b471c8b 100644
--- a/descriptions/go2_description/README.md
+++ b/descriptions/go2_description/README.md
@@ -15,6 +15,12 @@ source ~/ros2_ws/install/setup.bash
 ros2 launch go2_description visualize.launch.py
 ```
 
+## Launch Hardware Interface
+```bash
+source ~/ros2_ws/install/setup.bash
+ros2 launch go2_description hardware.launch.py
+```
+
 ## When used for isaac gym or other similiar engine 
 
 Collision parameters in urdf can be amended to better train the robot:
diff --git a/descriptions/go2_description/config/robot_control.yaml b/descriptions/go2_description/config/robot_control.yaml
index 714bb17..1fb66e9 100644
--- a/descriptions/go2_description/config/robot_control.yaml
+++ b/descriptions/go2_description/config/robot_control.yaml
@@ -1,7 +1,7 @@
 # Controller Manager configuration
 controller_manager:
   ros__parameters:
-    update_rate: 1000  # Hz
+    update_rate: 50  # Hz
     use_sim_time: true  # If running in simulation
 
     # Define the available controllers
diff --git a/descriptions/go2_description/config/visualize_urdf.rviz b/descriptions/go2_description/config/visualize_urdf.rviz
index be8c6bd..454f381 100644
--- a/descriptions/go2_description/config/visualize_urdf.rviz
+++ b/descriptions/go2_description/config/visualize_urdf.rviz
@@ -1,6 +1,6 @@
 Panels:
   - Class: rviz_common/Displays
-    Help Height: 78
+    Help Height: 138
     Name: Displays
     Property Tree Widget:
       Expanded:
@@ -8,7 +8,7 @@ Panels:
         - /Status1
         - /RobotModel1
       Splitter Ratio: 0.5
-    Tree Height: 542
+    Tree Height: 832
   - Class: rviz_common/Selection
     Name: Selection
   - Class: rviz_common/Tool Properties
@@ -230,18 +230,18 @@ Visualization Manager:
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 0.42039796710014343
+      Pitch: 0.5053979158401489
       Target Frame: <Fixed Frame>
       Value: Orbit (rviz)
-      Yaw: 0.6653964519500732
+      Yaw: 0.8203961253166199
     Saved: ~
 Window Geometry:
   Displays:
-    collapsed: false
-  Height: 846
-  Hide Left Dock: false
-  Hide Right Dock: false
-  QMainWindow State: 000000ff00000000fd000000040000000000000156000002acfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005d00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003f000002ac000000cc00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002acfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003f000002ac000000a900fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b00000026f00fffffffb0000000800540069006d006501000000000000045000000000000000000000023f000002ac00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+    collapsed: true
+  Height: 1194
+  Hide Left Dock: true
+  Hide Right Dock: true
+  QMainWindow State: 000000ff00000000fd0000000400000000000001f40000043cfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b000000b200fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007300000000700000043c0000018600fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000015d0000043cfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a0056006900650077007300000000700000043c0000013800fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000006180000005efc0100000002fb0000000800540069006d00650100000000000006180000047a00fffffffb0000000800540069006d0065010000000000000450000000000000000000000618000003a000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Selection:
     collapsed: false
   Time:
@@ -249,7 +249,7 @@ Window Geometry:
   Tool Properties:
     collapsed: false
   Views:
-    collapsed: false
-  Width: 1200
-  X: 523
-  Y: 102
+    collapsed: true
+  Width: 1560
+  X: 1327
+  Y: 195
diff --git a/hardwares/hardware_unitree_mujoco/src/HardwareUnitree.cpp b/hardwares/hardware_unitree_mujoco/src/HardwareUnitree.cpp
index bdf78f4..04cdf02 100644
--- a/hardwares/hardware_unitree_mujoco/src/HardwareUnitree.cpp
+++ b/hardwares/hardware_unitree_mujoco/src/HardwareUnitree.cpp
@@ -96,6 +96,7 @@ std::vector<hardware_interface::CommandInterface> HardwareUnitree::export_comman
         command_interfaces.emplace_back(joint_name, "effort", &joint_effort_command_[ind]);
         command_interfaces.emplace_back(joint_name, "kp", &joint_kp_command_[ind]);
         command_interfaces.emplace_back(joint_name, "kd", &joint_kd_command_[ind]);
+        ind++;
     }
     return command_interfaces;
 }