take FSM out as separate library

controllers/ocs2_quadruped_controller/launch/gazebo.launch.py

@@ -128,9 +128,6 @@ def generate_launch_description():
         executable="parameter_bridge",
         arguments=[
             "/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock",
-            # "/odom@nav_msgs/msg/Odometry@gz.msgs.Odometry",
-            # "/odom_with_covariance@nav_msgs/msg/Odometry@gz.msgs.OdometryWithCovariance",
-            # "/tf@tf2_msgs/msg/TFMessage@gz.msgs.Pose_V"
         ],
         output="screen"
     )

controllers/unitree_guide_controller/CMakeLists.txt

@@ -15,6 +15,7 @@ set(CONTROLLER_INCLUDE_DEPENDS
         realtime_tools
         std_msgs
         control_input_msgs
+        controller_common
         kdl_parser
 )
 
@@ -28,8 +29,6 @@ add_library(${PROJECT_NAME} SHARED
 
         src/UnitreeGuideController.cpp
 
-        src/FSM/StatePassive.cpp
-        src/FSM/StateFixedDown.cpp
         src/FSM/StateFixedStand.cpp
         src/FSM/StateSwingTest.cpp
         src/FSM/StateFreeStand.cpp

controllers/unitree_guide_controller/include/unitree_guide_controller/FSM/StateBalanceTest.h

@@ -4,12 +4,22 @@
 
 #ifndef STATEBALANCETEST_H
 #define STATEBALANCETEST_H
-#include "FSMState.h"
 
+#include <unitree_guide_controller/common/mathTypes.h>
+
+#include "controller_common/FSM/FSMState.h"
+
+
+class WaveGenerator;
+class BalanceCtrl;
+class QuadrupedRobot;
+class Estimator;
+struct CtrlComponent;
 
 class StateBalanceTest final : public FSMState {
 public:
-    explicit StateBalanceTest(CtrlComponent &ctrlComp);
+    explicit StateBalanceTest(CtrlInterfaces &ctrl_interfaces,
+                              CtrlComponent &ctrl_component);
 
     void enter() override;
 

controllers/unitree_guide_controller/include/unitree_guide_controller/FSM/StateFixedStand.h

@@ -5,35 +5,16 @@
 #ifndef STATEFIXEDSTAND_H
 #define STATEFIXEDSTAND_H
 
-#include <rclcpp/time.hpp>
+#include <controller_common/FSM/BaseFixedStand.h>
 
-#include "FSMState.h"
+class StateFixedStand final : public BaseFixedStand {
-
-class StateFixedStand final : public FSMState {
 public:
-    explicit StateFixedStand(CtrlComponent &ctrlComp,
+    explicit StateFixedStand(CtrlInterfaces &ctrl_interfaces,
                              const std::vector<double> &target_pos,
                              double kp,
                              double kd);
 
-    void enter() override;
-
-    void run() override;
-
-    void exit() override;
-
     FSMStateName checkChange() override;
-
-private:
-    double target_pos_[12] = {};
-    double start_pos_[12] = {};
-    rclcpp::Time start_time_;
-
-    double kp_, kd_;
-
-    double duration_ = 600; // steps
-    double percent_ = 0; //%
-    double phase = 0.0;
 };
 
 

controllers/unitree_guide_controller/include/unitree_guide_controller/FSM/StateFreeStand.h

@@ -4,12 +4,16 @@
 
 #ifndef STATEFREESTAND_H
 #define STATEFREESTAND_H
-#include "FSMState.h"
+#include <unitree_guide_controller/robot/QuadrupedRobot.h>
 
+#include "controller_common/FSM/FSMState.h"
+
+struct CtrlComponent;
 
 class StateFreeStand final : public FSMState {
 public:
-    explicit StateFreeStand(CtrlComponent &ctrl_component);
+    explicit StateFreeStand(CtrlInterfaces &ctrl_interfaces,
+                            CtrlComponent &ctrl_component);
 
     void enter() override;
 
@@ -21,6 +25,7 @@ public:
 
 private:
     std::shared_ptr<QuadrupedRobot> &robot_model_;
+
     void calc_body_target(float row, float pitch, float yaw, float height);
 
     float row_max_, row_min_;
@@ -35,5 +40,4 @@ private:
     std::vector<KDL::Frame> init_foot_pos_; // 4 feet position in fr-foot frame
 };
 
-
 #endif //STATEFREESTAND_H

controllers/unitree_guide_controller/include/unitree_guide_controller/FSM/StateSwingTest.h

@@ -5,12 +5,17 @@
 
 #ifndef STATESWINGTEST_H
 #define STATESWINGTEST_H
-#include "FSMState.h"
+#include <unitree_guide_controller/robot/QuadrupedRobot.h>
 
+#include "controller_common/FSM/FSMState.h"
+
+
+struct CtrlComponent;
 
 class StateSwingTest final : public FSMState {
 public:
-    explicit StateSwingTest(CtrlComponent &ctrl_component);
+    explicit StateSwingTest(CtrlInterfaces &ctrl_interfaces,
+                            CtrlComponent &ctrl_component);
 
     void enter() override;
 

controllers/unitree_guide_controller/include/unitree_guide_controller/FSM/StateTrotting.h

@@ -4,14 +4,14 @@
 
 #ifndef STATETROTTING_H
 #define STATETROTTING_H
+#include <unitree_guide_controller/control/BalanceCtrl.h>
 #include <unitree_guide_controller/gait/GaitGenerator.h>
-
+#include "controller_common/FSM/FSMState.h"
-#include "FSMState.h"
-
 
 class StateTrotting final : public FSMState {
 public:
-    explicit StateTrotting(CtrlComponent &ctrlComp);
+    explicit StateTrotting(CtrlInterfaces &ctrl_interfaces,
+                           CtrlComponent &ctrl_component);
 
     void enter() override;
 

controllers/unitree_guide_controller/include/unitree_guide_controller/UnitreeGuideController.h

@@ -7,14 +7,15 @@
 
 #include <controller_interface/controller_interface.hpp>
 #include <std_msgs/msg/string.hpp>
-#include <unitree_guide_controller/FSM/FSMState.h>
+#include <controller_common/FSM/FSMState.h>
-#include <unitree_guide_controller/common/enumClass.h>
+#include <controller_common/FSM/StatePassive.h>
+#include <controller_common/FSM/StateFixedDown.h>
+#include <controller_common/common/enumClass.h>
 
+#include "control/CtrlComponent.h"
 #include "FSM/StateBalanceTest.h"
-#include "FSM/StateFixedDown.h"
 #include "FSM/StateFixedStand.h"
 #include "FSM/StateFreeStand.h"
-#include "FSM/StatePassive.h"
 #include "FSM/StateSwingTest.h"
 #include "FSM/StateTrotting.h"
 
@@ -62,7 +63,8 @@ namespace unitree_guide_controller {
         controller_interface::CallbackReturn on_shutdown(
             const rclcpp_lifecycle::State &previous_state) override;
 
-        CtrlComponent ctrl_comp_;
+        CtrlComponent ctrl_component_;
+        CtrlInterfaces ctrl_interfaces_;
 
     protected:
         std::vector<std::string> joint_names_;
@@ -99,11 +101,11 @@ namespace unitree_guide_controller {
         std::unordered_map<
             std::string, std::vector<std::reference_wrapper<hardware_interface::LoanedCommandInterface> > *>
         command_interface_map_ = {
-            {"effort", &ctrl_comp_.joint_torque_command_interface_},
+            {"effort", &ctrl_interfaces_.joint_torque_command_interface_},
-            {"position", &ctrl_comp_.joint_position_command_interface_},
+            {"position", &ctrl_interfaces_.joint_position_command_interface_},
-            {"velocity", &ctrl_comp_.joint_velocity_command_interface_},
+            {"velocity", &ctrl_interfaces_.joint_velocity_command_interface_},
-            {"kp", &ctrl_comp_.joint_kp_command_interface_},
+            {"kp", &ctrl_interfaces_.joint_kp_command_interface_},
-            {"kd", &ctrl_comp_.joint_kd_command_interface_}
+            {"kd", &ctrl_interfaces_.joint_kd_command_interface_}
         };
 
         FSMMode mode_ = FSMMode::NORMAL;
@@ -121,9 +123,9 @@ namespace unitree_guide_controller {
         std::unordered_map<
             std::string, std::vector<std::reference_wrapper<hardware_interface::LoanedStateInterface> > *>
         state_interface_map_ = {
-            {"position", &ctrl_comp_.joint_position_state_interface_},
+            {"position", &ctrl_interfaces_.joint_position_state_interface_},
-            {"effort", &ctrl_comp_.joint_effort_state_interface_},
+            {"effort", &ctrl_interfaces_.joint_effort_state_interface_},
-            {"velocity", &ctrl_comp_.joint_velocity_state_interface_}
+            {"velocity", &ctrl_interfaces_.joint_velocity_state_interface_}
         };
     };
 }

controllers/unitree_guide_controller/include/unitree_guide_controller/control/CtrlComponent.h

@@ -1,67 +1,20 @@
 //
-// Created by biao on 24-9-10.
+// Created by tlab-uav on 25-2-27.
 //
 
-#ifndef INTERFACE_H
+#ifndef CTRLCOMPONENT_H
-#define INTERFACE_H
+#define CTRLCOMPONENT_H
-
-#include <vector>
-#include <hardware_interface/loaned_command_interface.hpp>
-#include <hardware_interface/loaned_state_interface.hpp>
-#include <control_input_msgs/msg/inputs.hpp>
 #include <unitree_guide_controller/gait/WaveGenerator.h>
-#include <unitree_guide_controller/robot/QuadrupedRobot.h>
 
 #include "BalanceCtrl.h"
 #include "Estimator.h"
 
 struct CtrlComponent {
-    std::vector<std::reference_wrapper<hardware_interface::LoanedCommandInterface> >
-    joint_torque_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_;
-
-    std::vector<std::reference_wrapper<hardware_interface::LoanedStateInterface> >
-    imu_state_interface_;
-
-    control_input_msgs::msg::Inputs control_inputs_;
-    int frequency_{};
-
     std::shared_ptr<QuadrupedRobot> robot_model_;
     std::shared_ptr<Estimator> estimator_;
-
     std::shared_ptr<BalanceCtrl> balance_ctrl_;
     std::shared_ptr<WaveGenerator> wave_generator_;
 
     CtrlComponent() = default;
-
-    void clear() {
-        joint_torque_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();
-
-        imu_state_interface_.clear();
-    }
 };
-
+#endif //CTRLCOMPONENT_H
-#endif //INTERFACE_H

controllers/unitree_guide_controller/include/unitree_guide_controller/control/Estimator.h

@@ -8,16 +8,16 @@
 #include <kdl/frames.hpp>
 #include <unitree_guide_controller/common/mathTypes.h>
 #include <unitree_guide_controller/robot/QuadrupedRobot.h>
-
 #include "LowPassFilter.h"
 
+struct CtrlInterfaces;
 class WaveGenerator;
 class QuadrupedRobot;
 struct CtrlComponent;
 
 class Estimator {
 public:
-    explicit Estimator(CtrlComponent &ctrl_component);
+    explicit Estimator(CtrlInterfaces &ctrl_interfaces, CtrlComponent &ctrl_component);
 
     ~Estimator() = default;
 
@@ -105,7 +105,7 @@ public:
     void update();
 
 private:
-    CtrlComponent &ctrl_component_;
+    CtrlInterfaces &ctrl_interfaces_;
     std::shared_ptr<QuadrupedRobot> &robot_model_;
     std::shared_ptr<WaveGenerator> &wave_generator_;
 

controllers/unitree_guide_controller/include/unitree_guide_controller/gait/WaveGenerator.h

@@ -6,7 +6,7 @@
 #ifndef WAVEGENERATOR_H
 #define WAVEGENERATOR_H
 #include <chrono>
-#include <unitree_guide_controller/common/enumClass.h>
+#include <controller_common/common/enumClass.h>
 #include <unitree_guide_controller/common/mathTypes.h>
 
 inline long long getSystemTime() {

controllers/unitree_guide_controller/include/unitree_guide_controller/robot/QuadrupedRobot.h

@@ -12,11 +12,11 @@
 #include "RobotLeg.h"
 
 
-struct CtrlComponent;
+struct CtrlInterfaces;
 
 class QuadrupedRobot {
 public:
-    explicit QuadrupedRobot(CtrlComponent &ctrl_component, const std::string &robot_description,
+    explicit QuadrupedRobot(CtrlInterfaces &ctrl_interfaces, const std::string &robot_description,
                             const std::vector<std::string> &feet_names, const std::string &base_name);
 
     ~QuadrupedRobot() = default;
@@ -91,7 +91,7 @@ public:
     void update();
 
 private:
-    CtrlComponent &ctrl_component_;
+    CtrlInterfaces &ctrl_interfaces_;
     std::vector<std::shared_ptr<RobotLeg> > robot_legs_;
 
     KDL::Chain fr_chain_;

controllers/unitree_guide_controller/package.xml

@@ -11,6 +11,7 @@
 
     <depend>backward_ros</depend>
     <depend>controller_interface</depend>
+    <depend>controller_common</depend>
     <depend>pluginlib</depend>
     <depend>control_input_msgs</depend>
     <depend>kdl_parser</depend>

controllers/unitree_guide_controller/src/FSM/StateBalanceTest.cpp

@@ -4,14 +4,21 @@
 
 #include "unitree_guide_controller/FSM/StateBalanceTest.h"
 
-#include "unitree_guide_controller/common/mathTools.h"
+#include <unitree_guide_controller/UnitreeGuideController.h>
 
-StateBalanceTest::StateBalanceTest(CtrlComponent &ctrlComp) : FSMState(FSMStateName::BALANCETEST, "balance test",
+#include "unitree_guide_controller/common/mathTools.h"
-                                                                       ctrlComp),
+#include "unitree_guide_controller/gait/WaveGenerator.h"
-                                                              estimator_(ctrlComp.estimator_),
+
-                                                              robot_model_(ctrlComp.robot_model_),
+StateBalanceTest::StateBalanceTest(CtrlInterfaces &ctrl_interfaces,
-                                                              balance_ctrl_(ctrlComp.balance_ctrl_),
+                                   CtrlComponent &ctrl_component)
-                                                              wave_generator_(ctrl_comp_.wave_generator_) {
+    : FSMState(FSMStateName::BALANCETEST,
+               "balance test",
+               ctrl_interfaces),
+      estimator_(ctrl_component.estimator_),
+      robot_model_(ctrl_component.robot_model_),
+      balance_ctrl_(ctrl_component.balance_ctrl_),
+      wave_generator_(
+          ctrl_component.wave_generator_) {
     _xMax = 0.05;
     _xMin = -_xMax;
     _yMax = 0.05;
@@ -36,16 +43,16 @@ void StateBalanceTest::enter() {
 }
 
 void StateBalanceTest::run() {
-    pcd_(0) = pcd_init_(0) + invNormalize(ctrl_comp_.control_inputs_.ly, _xMin, _xMax);
+    pcd_(0) = pcd_init_(0) + invNormalize(ctrl_interfaces_.control_inputs_.ly, _xMin, _xMax);
-    pcd_(1) = pcd_init_(1) - invNormalize(ctrl_comp_.control_inputs_.lx, _yMin, _yMax);
+    pcd_(1) = pcd_init_(1) - invNormalize(ctrl_interfaces_.control_inputs_.lx, _yMin, _yMax);
-    pcd_(2) = pcd_init_(2) + invNormalize(ctrl_comp_.control_inputs_.ry, _zMin, _zMax);
+    pcd_(2) = pcd_init_(2) + invNormalize(ctrl_interfaces_.control_inputs_.ry, _zMin, _zMax);
 
-    const float yaw = - invNormalize(ctrl_comp_.control_inputs_.rx, _yawMin, _yawMax);
+    const float yaw = -invNormalize(ctrl_interfaces_.control_inputs_.rx, _yawMin, _yawMax);
     Rd_ = rotz(yaw) * init_rotation_;
 
     for (int i = 0; i < 12; i++) {
-        ctrl_comp_.joint_kp_command_interface_[i].get().set_value(0.8);
+        std::ignore = ctrl_interfaces_.joint_kp_command_interface_[i].get().set_value(0.8);
-        ctrl_comp_.joint_kd_command_interface_[i].get().set_value(0.8);
+        std::ignore = ctrl_interfaces_.joint_kd_command_interface_[i].get().set_value(0.8);
     }
 
     calcTorque();
@@ -56,7 +63,7 @@ void StateBalanceTest::exit() {
 }
 
 FSMStateName StateBalanceTest::checkChange() {
-    switch (ctrl_comp_.control_inputs_.command) {
+    switch (ctrl_interfaces_.control_inputs_.command) {
         case 1:
             return FSMStateName::FIXEDDOWN;
         case 2:
@@ -90,8 +97,9 @@ void StateBalanceTest::calcTorque() {
     for (int i = 0; i < 4; i++) {
         KDL::JntArray torque = robot_model_->getTorque(force_feet_body.col(i), i);
         for (int j = 0; j < 3; j++) {
-            ctrl_comp_.joint_torque_command_interface_[i * 3 + j].get().set_value(torque(j));
+            std::ignore = ctrl_interfaces_.joint_torque_command_interface_[i * 3 + j].get().set_value(torque(j));
-            ctrl_comp_.joint_position_command_interface_[i * 3 + j].get().set_value(current_joints[i](j));
+            std::ignore = ctrl_interfaces_.joint_position_command_interface_[i * 3 + j].get().set_value(
+                current_joints[i](j));
         }
     }
 }

controllers/unitree_guide_controller/src/FSM/StateFixedDown.cpp

@@ -1,60 +0,0 @@
-//
-// Created by tlab-uav on 24-9-11.
-//
-
-#include "unitree_guide_controller/FSM/StateFixedDown.h"
-
-#include <cmath>
-
-StateFixedDown::StateFixedDown(CtrlComponent &ctrlComp,
-                               const std::vector<double> &target_pos,
-                               const double kp,
-                               const double kd)
-    : FSMState(FSMStateName::FIXEDDOWN, "fixed down", ctrlComp),
-      kp_(kp), kd_(kd) {
-    duration_ = ctrl_comp_.frequency_ * 1.2;
-    for (int i = 0; i < 12; i++) {
-        target_pos_[i] = target_pos[i];
-    }
-}
-
-void StateFixedDown::enter() {
-    for (int i = 0; i < 12; i++) {
-        start_pos_[i] = ctrl_comp_.joint_position_state_interface_[i].get().get_value();
-    }
-    ctrl_comp_.control_inputs_.command = 0;
-    for (int i = 0; i < 12; i++) {
-        ctrl_comp_.joint_position_command_interface_[i].get().set_value(start_pos_[i]);
-        ctrl_comp_.joint_velocity_command_interface_[i].get().set_value(0);
-        ctrl_comp_.joint_torque_command_interface_[i].get().set_value(0);
-        ctrl_comp_.joint_kp_command_interface_[i].get().set_value(kp_);
-        ctrl_comp_.joint_kd_command_interface_[i].get().set_value(kd_);
-    }
-}
-
-void StateFixedDown::run() {
-    percent_ += 1 / duration_;
-    phase = std::tanh(percent_);
-    for (int i = 0; i < 12; i++) {
-        ctrl_comp_.joint_position_command_interface_[i].get().set_value(
-            phase * target_pos_[i] + (1 - phase) * start_pos_[i]);
-    }
-}
-
-void StateFixedDown::exit() {
-    percent_ = 0;
-}
-
-FSMStateName StateFixedDown::checkChange() {
-    if (percent_ < 1.5) {
-        return FSMStateName::FIXEDDOWN;
-    }
-    switch (ctrl_comp_.control_inputs_.command) {
-        case 1:
-            return FSMStateName::PASSIVE;
-        case 2:
-            return FSMStateName::FIXEDSTAND;
-        default:
-            return FSMStateName::FIXEDDOWN;
-    }
-}

controllers/unitree_guide_controller/src/FSM/StateFixedStand.cpp

@@ -4,51 +4,17 @@
 
 #include "unitree_guide_controller/FSM/StateFixedStand.h"
 
-#include <cmath>
+StateFixedStand::StateFixedStand(CtrlInterfaces &ctrl_interfaces, const std::vector<double> &target_pos,
-
-StateFixedStand::StateFixedStand(CtrlComponent &ctrlComp, const std::vector<double> &target_pos,
                                  const double kp,
                                  const double kd)
-    : FSMState(FSMStateName::FIXEDSTAND, "fixed stand", ctrlComp),
+    : BaseFixedStand(ctrl_interfaces, target_pos, kp, kd) {
-      kp_(kp), kd_(kd) {
-    duration_ = ctrl_comp_.frequency_ * 1.2;
-    for (int i = 0; i < 12; i++) {
-        target_pos_[i] = target_pos[i];
-    }
-}
-
-void StateFixedStand::enter() {
-    for (int i = 0; i < 12; i++) {
-        start_pos_[i] = ctrl_comp_.joint_position_state_interface_[i].get().get_value();
-    }
-    for (int i = 0; i < 12; i++) {
-        ctrl_comp_.joint_position_command_interface_[i].get().set_value(start_pos_[i]);
-        ctrl_comp_.joint_velocity_command_interface_[i].get().set_value(0);
-        ctrl_comp_.joint_torque_command_interface_[i].get().set_value(0);
-        ctrl_comp_.joint_kp_command_interface_[i].get().set_value(kp_);
-        ctrl_comp_.joint_kd_command_interface_[i].get().set_value(kd_);
-    }
-    ctrl_comp_.control_inputs_.command = 0;
-}
-
-void StateFixedStand::run() {
-    percent_ += 1 / duration_;
-    phase = std::tanh(percent_);
-    for (int i = 0; i < 12; i++) {
-        ctrl_comp_.joint_position_command_interface_[i].get().set_value(
-            phase * target_pos_[i] + (1 - phase) * start_pos_[i]);
-    }
-}
-
-void StateFixedStand::exit() {
-    percent_ = 0;
 }
 
 FSMStateName StateFixedStand::checkChange() {
     if (percent_ < 1.5) {
         return FSMStateName::FIXEDSTAND;
     }
-    switch (ctrl_comp_.control_inputs_.command) {
+    switch (ctrl_interfaces_.control_inputs_.command) {
         case 1:
             return FSMStateName::PASSIVE;
         case 2:

controllers/unitree_guide_controller/src/FSM/StateFreeStand.cpp

@@ -3,10 +3,16 @@
 //
 
 #include "unitree_guide_controller/FSM/StateFreeStand.h"
+
+#include <unitree_guide_controller/UnitreeGuideController.h>
+
 #include "unitree_guide_controller/common/mathTools.h"
 
-StateFreeStand::StateFreeStand(CtrlComponent &ctrl_component) : FSMState(FSMStateName::FREESTAND, "free stand",
+StateFreeStand::StateFreeStand(CtrlInterfaces &ctrl_interfaces,
-                                                                         ctrl_component),
+                               CtrlComponent &ctrl_component)
+    : FSMState(
+          FSMStateName::FREESTAND, "free stand",
+          ctrl_interfaces),
       robot_model_(ctrl_component.robot_model_) {
     row_max_ = 20 * M_PI / 180;
     row_min_ = -row_max_;
@@ -20,8 +26,8 @@ StateFreeStand::StateFreeStand(CtrlComponent &ctrl_component) : FSMState(FSMStat
 
 void StateFreeStand::enter() {
     for (int i = 0; i < 12; i++) {
-        ctrl_comp_.joint_kp_command_interface_[i].get().set_value(100);
+        std::ignore = ctrl_interfaces_.joint_kp_command_interface_[i].get().set_value(100);
-        ctrl_comp_.joint_kd_command_interface_[i].get().set_value(5);
+        std::ignore = ctrl_interfaces_.joint_kd_command_interface_[i].get().set_value(5);
     }
 
     init_joint_pos_ = robot_model_->current_joint_pos_;
@@ -33,21 +39,21 @@ void StateFreeStand::enter() {
         foot_pos.p -= fr_init_pos_.p;
         foot_pos.M = KDL::Rotation::RPY(0, 0, 0);
     }
-    ctrl_comp_.control_inputs_.command = 0;
+    ctrl_interfaces_.control_inputs_.command = 0;
 }
 
 void StateFreeStand::run() {
-    calc_body_target(invNormalize(ctrl_comp_.control_inputs_.lx, row_min_, row_max_),
+    calc_body_target(invNormalize(ctrl_interfaces_.control_inputs_.lx, row_min_, row_max_),
-                     invNormalize(ctrl_comp_.control_inputs_.ly, pitch_min_, pitch_max_),
+                     invNormalize(ctrl_interfaces_.control_inputs_.ly, pitch_min_, pitch_max_),
-                     invNormalize(ctrl_comp_.control_inputs_.rx, yaw_min_, yaw_max_),
+                     invNormalize(ctrl_interfaces_.control_inputs_.rx, yaw_min_, yaw_max_),
-                     invNormalize(ctrl_comp_.control_inputs_.ry, height_min_, height_max_));
+                     invNormalize(ctrl_interfaces_.control_inputs_.ry, height_min_, height_max_));
 }
 
 void StateFreeStand::exit() {
 }
 
 FSMStateName StateFreeStand::checkChange() {
-    switch (ctrl_comp_.control_inputs_.command) {
+    switch (ctrl_interfaces_.control_inputs_.command) {
         case 1:
             return FSMStateName::PASSIVE;
         case 2:
@@ -72,8 +78,11 @@ void StateFreeStand::calc_body_target(const float row, const float pitch,
     target_joint_pos_ = robot_model_->getQ(goal_pos);
 
     for (int i = 0; i < 4; i++) {
-        ctrl_comp_.joint_position_command_interface_[i * 3].get().set_value(target_joint_pos_[i](0));
+        std::ignore = ctrl_interfaces_.joint_position_command_interface_[i * 3].get().set_value(
-        ctrl_comp_.joint_position_command_interface_[i * 3 + 1].get().set_value(target_joint_pos_[i](1));
+            target_joint_pos_[i](0));
-        ctrl_comp_.joint_position_command_interface_[i * 3 + 2].get().set_value(target_joint_pos_[i](2));
+        std::ignore = ctrl_interfaces_.joint_position_command_interface_[i * 3 + 1].get().set_value(
+            target_joint_pos_[i](1));
+        std::ignore = ctrl_interfaces_.joint_position_command_interface_[i * 3 + 2].get().set_value(
+            target_joint_pos_[i](2));
     }
 }

controllers/unitree_guide_controller/src/FSM/StatePassive.cpp

@@ -1,44 +0,0 @@
-//
-// Created by tlab-uav on 24-9-6.
-//
-
-#include "unitree_guide_controller/FSM/StatePassive.h"
-
-#include <iostream>
-#include <utility>
-
-StatePassive::StatePassive(CtrlComponent &ctrlComp) : FSMState(
-    FSMStateName::PASSIVE, "passive", ctrlComp) {
-}
-
-void StatePassive::enter() {
-    for (auto i: ctrl_comp_.joint_torque_command_interface_) {
-        i.get().set_value(0);
-    }
-    for (auto i: ctrl_comp_.joint_position_command_interface_) {
-        i.get().set_value(0);
-    }
-    for (auto i: ctrl_comp_.joint_velocity_command_interface_) {
-        i.get().set_value(0);
-    }
-    for (auto i: ctrl_comp_.joint_kp_command_interface_) {
-        i.get().set_value(0);
-    }
-    for (auto i: ctrl_comp_.joint_kd_command_interface_) {
-        i.get().set_value(1);
-    }
-    ctrl_comp_.control_inputs_.command = 0;
-}
-
-void StatePassive::run() {
-}
-
-void StatePassive::exit() {
-}
-
-FSMStateName StatePassive::checkChange() {
-    if (ctrl_comp_.control_inputs_.command == 2) {
-        return FSMStateName::FIXEDDOWN;
-    }
-    return FSMStateName::PASSIVE;
-}

controllers/unitree_guide_controller/src/FSM/StateSwingTest.cpp

@@ -4,11 +4,16 @@
 
 #include <iostream>
 #include "unitree_guide_controller/FSM/StateSwingTest.h"
+
+#include <unitree_guide_controller/control/CtrlComponent.h>
+
 #include "unitree_guide_controller/common/mathTools.h"
 
-StateSwingTest::StateSwingTest(CtrlComponent &ctrl_component): FSMState(
+StateSwingTest::StateSwingTest(CtrlInterfaces &ctrl_interfaces,
+                            CtrlComponent &ctrl_component)
+    : FSMState(
           FSMStateName::SWINGTEST, "swing test",
-                                                                   ctrl_component),
+          ctrl_interfaces),
       robot_model_(ctrl_component.robot_model_) {
     _xMin = -0.15;
     _xMax = 0.10;
@@ -20,12 +25,12 @@ StateSwingTest::StateSwingTest(CtrlComponent &ctrl_component): FSMState(
 
 void StateSwingTest::enter() {
     for (int i = 0; i < 3; i++) {
-        ctrl_comp_.joint_kp_command_interface_[i].get().set_value(3);
+        std::ignore = ctrl_interfaces_.joint_kp_command_interface_[i].get().set_value(3);
-        ctrl_comp_.joint_kd_command_interface_[i].get().set_value(2);
+        std::ignore = ctrl_interfaces_.joint_kd_command_interface_[i].get().set_value(2);
     }
     for (int i = 3; i < 12; i++) {
-        ctrl_comp_.joint_kp_command_interface_[i].get().set_value(180);
+        std::ignore = ctrl_interfaces_.joint_kp_command_interface_[i].get().set_value(180);
-        ctrl_comp_.joint_kd_command_interface_[i].get().set_value(5);
+        std::ignore = ctrl_interfaces_.joint_kd_command_interface_[i].get().set_value(5);
     }
 
     Kp = KDL::Vector(20, 20, 50);
@@ -40,25 +45,25 @@ void StateSwingTest::enter() {
 }
 
 void StateSwingTest::run() {
-    if (ctrl_comp_.control_inputs_.ly > 0) {
+    if (ctrl_interfaces_.control_inputs_.ly > 0) {
-        fr_goal_pos_.p.x(invNormalize(ctrl_comp_.control_inputs_.ly, fr_init_pos_.p.x(),
+        fr_goal_pos_.p.x(invNormalize(ctrl_interfaces_.control_inputs_.ly, fr_init_pos_.p.x(),
                                       fr_init_pos_.p.x() + _xMax, 0, 1));
     } else {
-        fr_goal_pos_.p.x(invNormalize(ctrl_comp_.control_inputs_.ly, fr_init_pos_.p.x() + _xMin,
+        fr_goal_pos_.p.x(invNormalize(ctrl_interfaces_.control_inputs_.ly, fr_init_pos_.p.x() + _xMin,
                                       fr_init_pos_.p.x(), -1, 0));
     }
-    if (ctrl_comp_.control_inputs_.lx > 0) {
+    if (ctrl_interfaces_.control_inputs_.lx > 0) {
-        fr_goal_pos_.p.y(invNormalize(ctrl_comp_.control_inputs_.lx, fr_init_pos_.p.y(),
+        fr_goal_pos_.p.y(invNormalize(ctrl_interfaces_.control_inputs_.lx, fr_init_pos_.p.y(),
                                       fr_init_pos_.p.y() + _yMax, 0, 1));
     } else {
-        fr_goal_pos_.p.y(invNormalize(ctrl_comp_.control_inputs_.lx, fr_init_pos_.p.y() + _yMin,
+        fr_goal_pos_.p.y(invNormalize(ctrl_interfaces_.control_inputs_.lx, fr_init_pos_.p.y() + _yMin,
                                       fr_init_pos_.p.y(), -1, 0));
     }
-    if (ctrl_comp_.control_inputs_.ry > 0) {
+    if (ctrl_interfaces_.control_inputs_.ry > 0) {
-        fr_goal_pos_.p.z(invNormalize(ctrl_comp_.control_inputs_.ry, fr_init_pos_.p.z(),
+        fr_goal_pos_.p.z(invNormalize(ctrl_interfaces_.control_inputs_.ry, fr_init_pos_.p.z(),
                                       fr_init_pos_.p.z() + _zMax, 0, 1));
     } else {
-        fr_goal_pos_.p.z(invNormalize(ctrl_comp_.control_inputs_.ry, fr_init_pos_.p.z() + _zMin,
+        fr_goal_pos_.p.z(invNormalize(ctrl_interfaces_.control_inputs_.ry, fr_init_pos_.p.z() + _zMin,
                                       fr_init_pos_.p.z(), -1, 0));
     }
 
@@ -70,7 +75,7 @@ void StateSwingTest::exit() {
 }
 
 FSMStateName StateSwingTest::checkChange() {
-    switch (ctrl_comp_.control_inputs_.command) {
+    switch (ctrl_interfaces_.control_inputs_.command) {
         case 1:
             return FSMStateName::PASSIVE;
         case 2:
@@ -84,9 +89,9 @@ void StateSwingTest::positionCtrl() {
     target_foot_pos_[0] = fr_goal_pos_;
     target_joint_pos_ = robot_model_->getQ(target_foot_pos_);
     for (int i = 0; i < 4; i++) {
-        ctrl_comp_.joint_position_command_interface_[i * 3].get().set_value(target_joint_pos_[i](0));
+        std::ignore = ctrl_interfaces_.joint_position_command_interface_[i * 3].get().set_value(target_joint_pos_[i](0));
-        ctrl_comp_.joint_position_command_interface_[i * 3 + 1].get().set_value(target_joint_pos_[i](1));
+        std::ignore = ctrl_interfaces_.joint_position_command_interface_[i * 3 + 1].get().set_value(target_joint_pos_[i](1));
-        ctrl_comp_.joint_position_command_interface_[i * 3 + 2].get().set_value(target_joint_pos_[i](2));
+        std::ignore = ctrl_interfaces_.joint_position_command_interface_[i * 3 + 2].get().set_value(target_joint_pos_[i](2));
     }
 }
 
@@ -101,6 +106,6 @@ void StateSwingTest::torqueCtrl() const {
     KDL::JntArray torque0 = robot_model_->getTorque(force0, 0);
 
     for (int i = 0; i < 3; i++) {
-        ctrl_comp_.joint_torque_command_interface_[i].get().set_value(torque0(i));
+        std::ignore = ctrl_interfaces_.joint_torque_command_interface_[i].get().set_value(torque0(i));
     }
 }

controllers/unitree_guide_controller/src/FSM/StateTrotting.cpp

@@ -5,13 +5,18 @@
 #include "unitree_guide_controller/FSM/StateTrotting.h"
 
 #include <unitree_guide_controller/common/mathTools.h>
+#include <unitree_guide_controller/control/CtrlComponent.h>
+#include <unitree_guide_controller/control/Estimator.h>
+#include <unitree_guide_controller/gait/WaveGenerator.h>
 
-StateTrotting::StateTrotting(CtrlComponent &ctrlComp) : FSMState(FSMStateName::TROTTING, "trotting", ctrlComp),
+StateTrotting::StateTrotting(CtrlInterfaces &ctrl_interfaces,
-                                                        estimator_(ctrlComp.estimator_),
+                            CtrlComponent &ctrl_component) : FSMState(FSMStateName::TROTTING, "trotting",
-                                                        robot_model_(ctrlComp.robot_model_),
+                                                                               ctrl_interfaces),
-                                                        balance_ctrl_(ctrlComp.balance_ctrl_),
+                                                                           estimator_(ctrl_component.estimator_),
-                                                        wave_generator_(ctrl_comp_.wave_generator_),
+                                                                           robot_model_(ctrl_component.robot_model_),
-                                                        gait_generator_(ctrlComp) {
+                                                                           balance_ctrl_(ctrl_component.balance_ctrl_),
+                                                                           wave_generator_(ctrl_component.wave_generator_),
+                                                                           gait_generator_(ctrl_component) {
     gait_height_ = 0.08;
     Kpp = Vec3(70, 70, 70).asDiagonal();
     Kdp = Vec3(10, 10, 10).asDiagonal();
@@ -23,7 +28,7 @@ StateTrotting::StateTrotting(CtrlComponent &ctrlComp) : FSMState(FSMStateName::T
     v_x_limit_ << -0.4, 0.4;
     v_y_limit_ << -0.3, 0.3;
     w_yaw_limit_ << -0.5, 0.5;
-    dt_ = 1.0 / ctrl_comp_.frequency_;
+    dt_ = 1.0 / ctrl_interfaces_.frequency_;
 }
 
 void StateTrotting::enter() {
@@ -34,7 +39,7 @@ void StateTrotting::enter() {
     Rd = rotz(yaw_cmd_);
     w_cmd_global_.setZero();
 
-    ctrl_comp_.control_inputs_.command = 0;
+    ctrl_interfaces_.control_inputs_.command = 0;
     gait_generator_.restart();
 }
 
@@ -68,7 +73,7 @@ void StateTrotting::exit() {
 }
 
 FSMStateName StateTrotting::checkChange() {
-    switch (ctrl_comp_.control_inputs_.command) {
+    switch (ctrl_interfaces_.control_inputs_.command) {
         case 1:
             return FSMStateName::PASSIVE;
         case 2:
@@ -80,12 +85,12 @@ FSMStateName StateTrotting::checkChange() {
 
 void StateTrotting::getUserCmd() {
     /* Movement */
-    v_cmd_body_(0) = invNormalize(ctrl_comp_.control_inputs_.ly, v_x_limit_(0), v_x_limit_(1));
+    v_cmd_body_(0) = invNormalize(ctrl_interfaces_.control_inputs_.ly, v_x_limit_(0), v_x_limit_(1));
-    v_cmd_body_(1) = -invNormalize(ctrl_comp_.control_inputs_.lx, v_y_limit_(0), v_y_limit_(1));
+    v_cmd_body_(1) = -invNormalize(ctrl_interfaces_.control_inputs_.lx, v_y_limit_(0), v_y_limit_(1));
     v_cmd_body_(2) = 0;
 
     /* Turning */
-    d_yaw_cmd_ = -invNormalize(ctrl_comp_.control_inputs_.rx, w_yaw_limit_(0), w_yaw_limit_(1));
+    d_yaw_cmd_ = -invNormalize(ctrl_interfaces_.control_inputs_.rx, w_yaw_limit_(0), w_yaw_limit_(1));
     d_yaw_cmd_ = 0.9 * d_yaw_cmd_past_ + (1 - 0.9) * d_yaw_cmd_;
     d_yaw_cmd_past_ = d_yaw_cmd_;
 }
@@ -150,7 +155,7 @@ void StateTrotting::calcTau() {
     for (int i = 0; i < 4; i++) {
         KDL::JntArray torque = robot_model_->getTorque(force_feet_body_.col(i), i);
         for (int j = 0; j < 3; j++) {
-            ctrl_comp_.joint_torque_command_interface_[i * 3 + j].get().set_value(torque(j));
+            std::ignore = ctrl_interfaces_.joint_torque_command_interface_[i * 3 + j].get().set_value(torque(j));
         }
     }
 }
@@ -167,8 +172,8 @@ void StateTrotting::calcQQd() {
     Vec12 q_goal = robot_model_->getQ(pos_feet_target);
     Vec12 qd_goal = robot_model_->getQd(pos_feet_body, vel_feet_target);
     for (int i = 0; i < 12; i++) {
-        ctrl_comp_.joint_position_command_interface_[i].get().set_value(q_goal(i));
+        std::ignore = ctrl_interfaces_.joint_position_command_interface_[i].get().set_value(q_goal(i));
-        ctrl_comp_.joint_velocity_command_interface_[i].get().set_value(qd_goal(i));
+        std::ignore = ctrl_interfaces_.joint_velocity_command_interface_[i].get().set_value(qd_goal(i));
     }
 }
 
@@ -177,14 +182,14 @@ void StateTrotting::calcGain() const {
         if (wave_generator_->contact_(i) == 0) {
             // swing gain
             for (int j = 0; j < 3; j++) {
-                ctrl_comp_.joint_kp_command_interface_[i * 3 + j].get().set_value(3);
+                std::ignore = ctrl_interfaces_.joint_kp_command_interface_[i * 3 + j].get().set_value(3);
-                ctrl_comp_.joint_kd_command_interface_[i * 3 + j].get().set_value(2);
+                std::ignore = ctrl_interfaces_.joint_kd_command_interface_[i * 3 + j].get().set_value(2);
             }
         } else {
             // stable gain
             for (int j = 0; j < 3; j++) {
-                ctrl_comp_.joint_kp_command_interface_[i * 3 + j].get().set_value(0.8);
+                std::ignore = ctrl_interfaces_.joint_kp_command_interface_[i * 3 + j].get().set_value(0.8);
-                ctrl_comp_.joint_kd_command_interface_[i * 3 + j].get().set_value(0.8);
+                std::ignore = ctrl_interfaces_.joint_kd_command_interface_[i * 3 + j].get().set_value(0.8);
             }
         }
     }

controllers/unitree_guide_controller/src/UnitreeGuideController.cpp

@@ -3,7 +3,8 @@
 //
 
 #include "unitree_guide_controller/UnitreeGuideController.h"
-#include "unitree_guide_controller/FSM/StatePassive.h"
+
+#include <unitree_guide_controller/gait/WaveGenerator.h>
 #include "unitree_guide_controller/robot/QuadrupedRobot.h"
 
 namespace unitree_guide_controller {
@@ -53,13 +54,13 @@ namespace unitree_guide_controller {
         // update_frequency_ = 1.0 / time_diff.count();
         // RCLCPP_INFO(get_node()->get_logger(), "Update frequency: %f Hz", update_frequency_);
 
-        if (ctrl_comp_.robot_model_ == nullptr) {
+        if (ctrl_component_.robot_model_ == nullptr) {
             return controller_interface::return_type::OK;
         }
 
-        ctrl_comp_.robot_model_->update();
+        ctrl_component_.robot_model_->update();
-        ctrl_comp_.wave_generator_->update();
+        ctrl_component_.wave_generator_->update();
-        ctrl_comp_.estimator_->update();
+        ctrl_component_.estimator_->update();
 
         if (mode_ == FSMMode::NORMAL) {
             current_state_->run();
@@ -103,10 +104,10 @@ namespace unitree_guide_controller {
             stand_kp_ = auto_declare<double>("stand_kp", stand_kp_);
             stand_kd_ = auto_declare<double>("stand_kd", stand_kd_);
 
-            get_node()->get_parameter("update_rate", ctrl_comp_.frequency_);
+            get_node()->get_parameter("update_rate", ctrl_interfaces_.frequency_);
-            RCLCPP_INFO(get_node()->get_logger(), "Controller Manager Update Rate: %d Hz", ctrl_comp_.frequency_);
+            RCLCPP_INFO(get_node()->get_logger(), "Controller Manager Update Rate: %d Hz", ctrl_interfaces_.frequency_);
 
-            ctrl_comp_.estimator_ = std::make_shared<Estimator>(ctrl_comp_);
+            ctrl_component_.estimator_ = std::make_shared<Estimator>(ctrl_interfaces_, ctrl_component_);
         } catch (const std::exception &e) {
             fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what());
             return controller_interface::CallbackReturn::ERROR;
@@ -120,22 +121,22 @@ namespace unitree_guide_controller {
         control_input_subscription_ = get_node()->create_subscription<control_input_msgs::msg::Inputs>(
             "/control_input", 10, [this](const control_input_msgs::msg::Inputs::SharedPtr msg) {
                 // Handle message
-                ctrl_comp_.control_inputs_.command = msg->command;
+                ctrl_interfaces_.control_inputs_.command = msg->command;
-                ctrl_comp_.control_inputs_.lx = msg->lx;
+                ctrl_interfaces_.control_inputs_.lx = msg->lx;
-                ctrl_comp_.control_inputs_.ly = msg->ly;
+                ctrl_interfaces_.control_inputs_.ly = msg->ly;
-                ctrl_comp_.control_inputs_.rx = msg->rx;
+                ctrl_interfaces_.control_inputs_.rx = msg->rx;
-                ctrl_comp_.control_inputs_.ry = msg->ry;
+                ctrl_interfaces_.control_inputs_.ry = msg->ry;
             });
 
         robot_description_subscription_ = get_node()->create_subscription<std_msgs::msg::String>(
             "/robot_description", rclcpp::QoS(rclcpp::KeepLast(1)).transient_local(),
             [this](const std_msgs::msg::String::SharedPtr msg) {
-                ctrl_comp_.robot_model_ = std::make_shared<QuadrupedRobot>(
+                ctrl_component_.robot_model_ = std::make_shared<QuadrupedRobot>(
-                    ctrl_comp_, msg->data, feet_names_, base_name_);
+                    ctrl_interfaces_, msg->data, feet_names_, base_name_);
-                ctrl_comp_.balance_ctrl_ = std::make_shared<BalanceCtrl>(ctrl_comp_.robot_model_);
+                ctrl_component_.balance_ctrl_ = std::make_shared<BalanceCtrl>(ctrl_component_.robot_model_);
             });
 
-        ctrl_comp_.wave_generator_ = std::make_shared<WaveGenerator>(0.45, 0.5, Vec4(0, 0.5, 0.5, 0));
+        ctrl_component_.wave_generator_ = std::make_shared<WaveGenerator>(0.45, 0.5, Vec4(0, 0.5, 0.5, 0));
 
         return CallbackReturn::SUCCESS;
     }
@@ -143,7 +144,7 @@ namespace unitree_guide_controller {
     controller_interface::CallbackReturn
     UnitreeGuideController::on_activate(const rclcpp_lifecycle::State & /*previous_state*/) {
         // clear out vectors in case of restart
-        ctrl_comp_.clear();
+        ctrl_interfaces_.clear();
 
         // assign command interfaces
         for (auto &interface: command_interfaces_) {
@@ -158,20 +159,20 @@ namespace unitree_guide_controller {
         // assign state interfaces
         for (auto &interface: state_interfaces_) {
             if (interface.get_prefix_name() == imu_name_) {
-                ctrl_comp_.imu_state_interface_.emplace_back(interface);
+                ctrl_interfaces_.imu_state_interface_.emplace_back(interface);
             } else {
                 state_interface_map_[interface.get_interface_name()]->push_back(interface);
             }
         }
 
         // Create FSM List
-        state_list_.passive = std::make_shared<StatePassive>(ctrl_comp_);
+        state_list_.passive = std::make_shared<StatePassive>(ctrl_interfaces_);
-        state_list_.fixedDown = std::make_shared<StateFixedDown>(ctrl_comp_, down_pos_, stand_kp_, stand_kd_);
+        state_list_.fixedDown = std::make_shared<StateFixedDown>(ctrl_interfaces_, down_pos_, stand_kp_, stand_kd_);
-        state_list_.fixedStand = std::make_shared<StateFixedStand>(ctrl_comp_, stand_pos_, stand_kp_, stand_kd_);
+        state_list_.fixedStand = std::make_shared<StateFixedStand>(ctrl_interfaces_, stand_pos_, stand_kp_, stand_kd_);
-        state_list_.swingTest = std::make_shared<StateSwingTest>(ctrl_comp_);
+        state_list_.swingTest = std::make_shared<StateSwingTest>(ctrl_interfaces_, ctrl_component_);
-        state_list_.freeStand = std::make_shared<StateFreeStand>(ctrl_comp_);
+        state_list_.freeStand = std::make_shared<StateFreeStand>(ctrl_interfaces_, ctrl_component_);
-        state_list_.balanceTest = std::make_shared<StateBalanceTest>(ctrl_comp_);
+        state_list_.balanceTest = std::make_shared<StateBalanceTest>(ctrl_interfaces_, ctrl_component_);
-        state_list_.trotting = std::make_shared<StateTrotting>(ctrl_comp_);
+        state_list_.trotting = std::make_shared<StateTrotting>(ctrl_interfaces_, ctrl_component_);
 
         // Initialize FSM
         current_state_ = state_list_.passive;

controllers/unitree_guide_controller/src/control/Estimator.cpp

@@ -5,14 +5,15 @@
 #include "unitree_guide_controller/control/Estimator.h"
 
 #include <unitree_guide_controller/common/mathTools.h>
+#include <unitree_guide_controller/control/CtrlComponent.h>
 
-#include "unitree_guide_controller/control/CtrlComponent.h"
+#include "controller_common/CtrlInterfaces.h"
 
-Estimator::Estimator(CtrlComponent &ctrl_component) : ctrl_component_(ctrl_component),
+Estimator::Estimator(CtrlInterfaces &ctrl_interfaces, CtrlComponent &ctrl_component) : ctrl_interfaces_(ctrl_interfaces),
                                                       robot_model_(ctrl_component.robot_model_),
                                                       wave_generator_(ctrl_component.wave_generator_) {
     g_ << 0, 0, -9.81;
-    dt_ = 1.0 / ctrl_component_.frequency_;
+    dt_ = 1.0 / ctrl_interfaces.frequency_;
 
     std::cout << "dt: " << dt_ << std::endl;
     large_variance_ = 100;
@@ -181,19 +182,19 @@ void Estimator::update() {
     }
 
     Quat quat;
-    quat << ctrl_component_.imu_state_interface_[0].get().get_value(),
+    quat << ctrl_interfaces_.imu_state_interface_[0].get().get_value(),
-            ctrl_component_.imu_state_interface_[1].get().get_value(),
+            ctrl_interfaces_.imu_state_interface_[1].get().get_value(),
-            ctrl_component_.imu_state_interface_[2].get().get_value(),
+            ctrl_interfaces_.imu_state_interface_[2].get().get_value(),
-            ctrl_component_.imu_state_interface_[3].get().get_value();
+            ctrl_interfaces_.imu_state_interface_[3].get().get_value();
     rotation_ = quatToRotMat(quat);
 
-    gyro_ << ctrl_component_.imu_state_interface_[4].get().get_value(),
+    gyro_ << ctrl_interfaces_.imu_state_interface_[4].get().get_value(),
-            ctrl_component_.imu_state_interface_[5].get().get_value(),
+            ctrl_interfaces_.imu_state_interface_[5].get().get_value(),
-            ctrl_component_.imu_state_interface_[6].get().get_value();
+            ctrl_interfaces_.imu_state_interface_[6].get().get_value();
 
-    acceleration_ << ctrl_component_.imu_state_interface_[7].get().get_value(),
+    acceleration_ << ctrl_interfaces_.imu_state_interface_[7].get().get_value(),
-            ctrl_component_.imu_state_interface_[8].get().get_value(),
+            ctrl_interfaces_.imu_state_interface_[8].get().get_value(),
-            ctrl_component_.imu_state_interface_[9].get().get_value();
+            ctrl_interfaces_.imu_state_interface_[9].get().get_value();
 
     u_ = rotation_ * acceleration_ + g_;
     x_hat_ = A * x_hat_ + B * u_;

controllers/unitree_guide_controller/src/gait/FeetEndCalc.cpp

@@ -4,9 +4,11 @@
 
 #include "unitree_guide_controller/gait/FeetEndCalc.h"
 
-#include "unitree_guide_controller/control/CtrlComponent.h"
+#include <unitree_guide_controller/control/CtrlComponent.h>
+#include <unitree_guide_controller/control/Estimator.h>
 
-FeetEndCalc::FeetEndCalc(CtrlComponent &ctrl_component) : ctrl_component_(ctrl_component),
+FeetEndCalc::FeetEndCalc(CtrlComponent &ctrl_component)
+    : ctrl_component_(ctrl_component),
       robot_model_(ctrl_component.robot_model_),
       estimator_(ctrl_component.estimator_) {
     k_x_ = 0.005;

controllers/unitree_guide_controller/src/gait/GaitGenerator.cpp

@@ -5,10 +5,12 @@
 #include "unitree_guide_controller/gait/GaitGenerator.h"
 
 #include <utility>
+#include <unitree_guide_controller/control/CtrlComponent.h>
+#include <unitree_guide_controller/control/Estimator.h>
+#include <unitree_guide_controller/gait/WaveGenerator.h>
 
-#include "unitree_guide_controller/control/CtrlComponent.h"
+GaitGenerator::GaitGenerator(CtrlComponent &ctrl_component)
-
+    : wave_generator_(ctrl_component.wave_generator_),
-GaitGenerator::GaitGenerator(CtrlComponent &ctrl_component) : wave_generator_(ctrl_component.wave_generator_),
       estimator_(ctrl_component.estimator_),
       feet_end_calc_(ctrl_component) {
     first_run_ = true;

controllers/unitree_guide_controller/src/gait/WaveGenerator.cpp

@@ -2,13 +2,11 @@
 // Created by biao on 24-9-18.
 //
 
-#include <utility>
-
 #include "unitree_guide_controller/gait/WaveGenerator.h"
 
 #include <iostream>
 
-WaveGenerator::WaveGenerator(double period, double st_ratio, const Vec4 &bias) {
+WaveGenerator::WaveGenerator(const double period, const double st_ratio, const Vec4 &bias) {
 
     phase_past_ << 0.5, 0.5, 0.5, 0.5;
     contact_past_.setZero();

controllers/unitree_guide_controller/src/robot/QuadrupedRobot.cpp

@@ -3,12 +3,12 @@
 //
 
 #include <iostream>
-#include "unitree_guide_controller/control/CtrlComponent.h"
+#include "controller_common/CtrlInterfaces.h"
 #include "unitree_guide_controller/robot/QuadrupedRobot.h"
 
-QuadrupedRobot::QuadrupedRobot(CtrlComponent &ctrl_component, const std::string &robot_description,
+QuadrupedRobot::QuadrupedRobot(CtrlInterfaces &ctrl_interfaces, const std::string &robot_description,
                                const std::vector<std::string> &feet_names,
-                               const std::string &base_name) : ctrl_component_(ctrl_component) {
+                               const std::string &base_name) : ctrl_interfaces_(ctrl_interfaces) {
     KDL::Tree robot_tree;
     kdl_parser::treeFromString(robot_description, robot_tree);
 
@@ -114,15 +114,15 @@ void QuadrupedRobot::update() {
     if (mass_ == 0) return;
     for (int i = 0; i < 4; i++) {
         KDL::JntArray pos_array(3);
-        pos_array(0) = ctrl_component_.joint_position_state_interface_[i * 3].get().get_value();
+        pos_array(0) = ctrl_interfaces_.joint_position_state_interface_[i * 3].get().get_value();
-        pos_array(1) = ctrl_component_.joint_position_state_interface_[i * 3 + 1].get().get_value();
+        pos_array(1) = ctrl_interfaces_.joint_position_state_interface_[i * 3 + 1].get().get_value();
-        pos_array(2) = ctrl_component_.joint_position_state_interface_[i * 3 + 2].get().get_value();
+        pos_array(2) = ctrl_interfaces_.joint_position_state_interface_[i * 3 + 2].get().get_value();
         current_joint_pos_[i] = pos_array;
 
         KDL::JntArray vel_array(3);
-        vel_array(0) = ctrl_component_.joint_velocity_state_interface_[i * 3].get().get_value();
+        vel_array(0) = ctrl_interfaces_.joint_velocity_state_interface_[i * 3].get().get_value();
-        vel_array(1) = ctrl_component_.joint_velocity_state_interface_[i * 3 + 1].get().get_value();
+        vel_array(1) = ctrl_interfaces_.joint_velocity_state_interface_[i * 3 + 1].get().get_value();
-        vel_array(2) = ctrl_component_.joint_velocity_state_interface_[i * 3 + 2].get().get_value();
+        vel_array(2) = ctrl_interfaces_.joint_velocity_state_interface_[i * 3 + 2].get().get_value();
         current_joint_vel_[i] = vel_array;
     }
 }

descriptions/unitree/go2_description/config/gazebo.yaml

@@ -77,10 +77,10 @@ ocs2_quadruped_controller:
 
     foot_force_name: "foot_force"
     foot_force_interfaces:
-      - FL_ft_sensor
+      - FL_foot_force
-      - RL_ft_sensor
+      - RL_foot_force
-      - FR_ft_sensor
+      - FR_foot_force
-      - RR_ft_sensor
+      - RR_foot_force
 
 unitree_guide_controller:
   ros__parameters:

descriptions/unitree/go2_description/xacro/ft_sensor.xacro

@@ -1,22 +0,0 @@
-<?xml version="1.0"?>
-
-<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
-    <xacro:macro name="ft_sensor" params="name">
-        <gazebo reference="${name}_foot_fixed">
-            <!-- Enable feedback for this joint -->
-            <provideFeedback>true</provideFeedback>
-            <!-- Prevent ros2_control from lumping this fixed joint with others -->
-            <disableFixedJointLumping>true</disableFixedJointLumping>
-            <sensor name="${name}_ft_sensor" type="force_torque">
-                <always_on>1</always_on>
-                <update_rate>500</update_rate>
-                <visualize>true</visualize>
-                <topic>${name}_ft</topic>
-                <force_torque>
-                    <frame>child</frame>
-                    <!--                <measure_direction>child_to_parent</measure_direction>-->
-                </force_torque>
-            </sensor>
-        </gazebo>
-    </xacro:macro>
-</robot>

descriptions/unitree/go2_description/xacro/gazebo.xacro

@@ -1,6 +1,6 @@
 <?xml version="1.0"?>
 <robot xmlns:xacro="http://www.ros.org/wiki/xacro">
-    <xacro:include filename="$(find go2_description)/xacro/ft_sensor.xacro"/>
+  <xacro:include filename="$(find gz_quadruped_hardware)/xacro/foot_force_sensor.xacro"/>
   <ros2_control name="GazeboSystem" type="system">
     <hardware>
       <plugin>gz_quadruped_hardware/GazeboSimSystem</plugin>
@@ -153,10 +153,10 @@
     </sensor>
 
     <sensor name="foot_force">
-            <state_interface name="FR_ft_sensor"/>
+      <state_interface name="FR_foot_force"/>
-            <state_interface name="FL_ft_sensor"/>
+      <state_interface name="FL_foot_force"/>
-            <state_interface name="RR_ft_sensor"/>
+      <state_interface name="RR_foot_force"/>
-            <state_interface name="RL_ft_sensor"/>
+      <state_interface name="RL_foot_force"/>
     </sensor>
 
   </ros2_control>
@@ -254,10 +254,10 @@
     <disableFixedJointLumping>true</disableFixedJointLumping>
   </gazebo>
 
-    <xacro:ft_sensor name="FL"/>
+  <xacro:foot_force_sensor name="FL"/>
-    <xacro:ft_sensor name="RL"/>
+  <xacro:foot_force_sensor name="RL"/>
-    <xacro:ft_sensor name="FR"/>
+  <xacro:foot_force_sensor name="FR"/>
-    <xacro:ft_sensor name="RR"/>
+  <xacro:foot_force_sensor name="RR"/>
 
   <xacro:arg name="EXTERNAL_SENSORS" default="false"/>
   <xacro:if value="$(arg EXTERNAL_SENSORS)">

hardwares/gz_quadruped_hardware/CMakeLists.txt

@@ -78,6 +78,11 @@ install(DIRECTORY
         DESTINATION include
 )
 
+install(
+        DIRECTORY xacro
+        DESTINATION share/${PROJECT_NAME}/
+)
+
 # Testing and linting
 if (BUILD_TESTING)
     find_package(ament_lint_auto REQUIRED)

hardwares/gz_quadruped_hardware/xacro/foot_force_sensor.xacro

@@ -0,0 +1,21 @@
+<?xml version="1.0"?>
+
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+  <xacro:macro name="foot_force_sensor" params="name">
+    <gazebo reference="${name}_foot_fixed">
+      <!-- Enable feedback for this joint -->
+      <provideFeedback>true</provideFeedback>
+      <!-- Prevent ros2_control from lumping this fixed joint with others -->
+      <disableFixedJointLumping>true</disableFixedJointLumping>
+      <sensor name="${name}_foot_force" type="force_torque">
+        <always_on>1</always_on>
+        <update_rate>500</update_rate>
+        <visualize>true</visualize>
+        <topic>${name}_foot_force</topic>
+        <force_torque>
+          <frame>child</frame>
+        </force_torque>
+      </sensor>
+    </gazebo>
+  </xacro:macro>
+</robot>

libraries/controller_common/CMakeLists.txt

@@ -0,0 +1,58 @@
+cmake_minimum_required(VERSION 3.8)
+project(controller_common)
+set(CMAKE_EXPORT_COMPILE_COMMANDS ON)
+
+if (CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+    add_compile_options(-Wall -Wextra -Wpedantic)
+endif ()
+
+set(CMAKE_BUILD_TYPE Release)
+set(CMAKE_EXPORT_COMPILE_COMMANDS ON)
+
+set(dependencies
+        hardware_interface
+        control_input_msgs
+        rclcpp
+)
+
+find_package(ament_cmake REQUIRED)
+foreach (Dependency IN ITEMS ${dependencies})
+    find_package(${Dependency} REQUIRED)
+endforeach ()
+
+add_library(${PROJECT_NAME} SHARED
+        src/FSM/StatePassive.cpp
+        src/FSM/BaseFixedStand.cpp
+        src/FSM/StateFixedDown.cpp
+)
+
+target_include_directories(${PROJECT_NAME}
+        PUBLIC
+        "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
+        "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
+        PRIVATE
+        src)
+
+ament_target_dependencies(
+        ${PROJECT_NAME} PUBLIC
+        ${dependencies}
+)
+
+
+install(
+        DIRECTORY include/
+        DESTINATION include/${PROJECT_NAME}
+)
+
+install(
+        TARGETS ${PROJECT_NAME}
+        EXPORT export_${PROJECT_NAME}
+        ARCHIVE DESTINATION lib
+        LIBRARY DESTINATION lib
+        RUNTIME DESTINATION bin
+)
+
+ament_export_dependencies(${CONTROLLER_INCLUDE_DEPENDS})
+ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET)
+
+ament_package()

libraries/controller_common/include/controller_common/CtrlInterfaces.h

@@ -0,0 +1,56 @@
+//
+// 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>
+#include <control_input_msgs/msg/inputs.hpp>
+
+struct CtrlInterfaces {
+    std::vector<std::reference_wrapper<hardware_interface::LoanedCommandInterface> >
+    joint_torque_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_;
+
+    std::vector<std::reference_wrapper<hardware_interface::LoanedStateInterface> >
+    imu_state_interface_;
+
+    control_input_msgs::msg::Inputs control_inputs_;
+    int frequency_{};
+
+    CtrlInterfaces() = default;
+
+    void clear() {
+        joint_torque_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();
+
+        imu_state_interface_.clear();
+    }
+};
+
+#endif //INTERFACE_H

libraries/controller_common/include/controller_common/FSM/BaseFixedStand.h

@@ -0,0 +1,40 @@
+//
+// Created by biao on 24-9-10.
+//
+
+#ifndef BASEFIXEDSTAND_H
+#define BASEFIXEDSTAND_H
+
+#include <rclcpp/time.hpp>
+
+#include "FSMState.h"
+
+class BaseFixedStand : public FSMState {
+public:
+    BaseFixedStand(CtrlInterfaces &ctrl_interfaces,
+                   const std::vector<double> &target_pos,
+                   double kp,
+                   double kd);
+
+    void enter() override;
+
+    void run() override;
+
+    void exit() override;
+
+    FSMStateName checkChange() override;
+
+protected:
+    double target_pos_[12] = {};
+    double start_pos_[12] = {};
+    rclcpp::Time start_time_;
+
+    double kp_, kd_;
+
+    double duration_ = 600; // steps
+    double percent_ = 0; //%
+    double phase = 0.0;
+};
+
+
+#endif //BASEFIXEDSTAND_H

libraries/controller_common/include/controller_common/FSM/FSMState.h

@@ -7,17 +7,17 @@
 
 #include <string>
 #include <utility>
-#include <unitree_guide_controller/common/enumClass.h>
+#include <controller_common/common/enumClass.h>
-#include <unitree_guide_controller/control/CtrlComponent.h>
+#include <controller_common/CtrlInterfaces.h>
 
 class FSMState {
 public:
     virtual ~FSMState() = default;
 
-    FSMState(const FSMStateName &state_name, std::string state_name_string, CtrlComponent &ctrl_comp)
+    FSMState(const FSMStateName &state_name, std::string state_name_string, CtrlInterfaces &ctrl_interfaces)
         : state_name(state_name),
           state_name_string(std::move(state_name_string)),
-          ctrl_comp_(ctrl_comp) {
+          ctrl_interfaces_(ctrl_interfaces) {
     }
 
     virtual void enter() = 0;
@@ -32,7 +32,7 @@ public:
     std::string state_name_string;
 
 protected:
-    CtrlComponent &ctrl_comp_;
+    CtrlInterfaces &ctrl_interfaces_;
 };
 
 #endif //FSMSTATE_H

libraries/controller_common/include/controller_common/FSM/StateFixedDown.h

@@ -11,7 +11,7 @@
 
 class StateFixedDown final : public FSMState {
 public:
-    explicit StateFixedDown(CtrlComponent &ctrlComp,
+    explicit StateFixedDown(CtrlInterfaces &ctrl_interfaces,
                             const std::vector<double> &target_pos,
                             double kp,
                             double kd

libraries/controller_common/include/controller_common/FSM/StatePassive.h

@@ -9,7 +9,7 @@
 
 class StatePassive final : public FSMState {
 public:
-    explicit StatePassive(CtrlComponent &ctrlComp);
+    explicit StatePassive(CtrlInterfaces &ctrl_interfaces);
 
     void enter() override;
 

libraries/controller_common/include/controller_common/common/enumClass.h

@@ -16,6 +16,9 @@ enum class FSMStateName {
 
     SWINGTEST,
     BALANCETEST,
+
+    OCS2,
+    RL
 };
 
 enum class FSMMode {

libraries/controller_common/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>controller_common</name>
+    <version>0.0.0</version>
+    <description>Common libraries used by the controller</description>
+    <maintainer email="biao876990970@hotmail.com">Huang Zhenbiao</maintainer>
+    <license>Apache-2.0</license>
+
+    <buildtool_depend>ament_cmake</buildtool_depend>
+
+    <depend>hardware_interface</depend>
+    <depend>control_input_msgs</depend>
+    <depend>rclcpp</depend>
+
+    <test_depend>ament_lint_auto</test_depend>
+    <test_depend>ament_lint_common</test_depend>
+
+    <export>
+        <build_type>ament_cmake</build_type>
+    </export>
+</package>

libraries/controller_common/src/FSM/BaseFixedStand.cpp

@@ -0,0 +1,59 @@
+//
+// Created by biao on 24-9-10.
+//
+
+#include "controller_common/FSM/BaseFixedStand.h"
+
+#include <cmath>
+
+BaseFixedStand::BaseFixedStand(CtrlInterfaces &ctrl_interfaces, const std::vector<double> &target_pos,
+                                 const double kp,
+                                 const double kd)
+    : FSMState(FSMStateName::FIXEDSTAND, "fixed stand", ctrl_interfaces),
+      kp_(kp), kd_(kd) {
+    duration_ = ctrl_interfaces_.frequency_ * 1.2;
+    for (int i = 0; i < 12; i++) {
+        target_pos_[i] = target_pos[i];
+    }
+}
+
+void BaseFixedStand::enter() {
+    for (int i = 0; i < 12; i++) {
+        start_pos_[i] = ctrl_interfaces_.joint_position_state_interface_[i].get().get_value();
+    }
+    for (int i = 0; i < 12; i++) {
+        std::ignore = ctrl_interfaces_.joint_position_command_interface_[i].get().set_value(start_pos_[i]);
+        std::ignore = ctrl_interfaces_.joint_velocity_command_interface_[i].get().set_value(0);
+        std::ignore = ctrl_interfaces_.joint_torque_command_interface_[i].get().set_value(0);
+        std::ignore = ctrl_interfaces_.joint_kp_command_interface_[i].get().set_value(kp_);
+        std::ignore = ctrl_interfaces_.joint_kd_command_interface_[i].get().set_value(kd_);
+    }
+    ctrl_interfaces_.control_inputs_.command = 0;
+}
+
+void BaseFixedStand::run() {
+    percent_ += 1 / duration_;
+    phase = std::tanh(percent_);
+    for (int i = 0; i < 12; i++) {
+        std::ignore = ctrl_interfaces_.joint_position_command_interface_[i].get().set_value(
+            phase * target_pos_[i] + (1 - phase) * start_pos_[i]);
+    }
+}
+
+void BaseFixedStand::exit() {
+    percent_ = 0;
+}
+
+FSMStateName BaseFixedStand::checkChange() {
+    if (percent_ < 1.5) {
+        return FSMStateName::FIXEDSTAND;
+    }
+    switch (ctrl_interfaces_.control_inputs_.command) {
+        case 1:
+            return FSMStateName::PASSIVE;
+        case 2:
+            return FSMStateName::FIXEDDOWN;
+        default:
+            return FSMStateName::FIXEDSTAND;
+    }
+}

libraries/controller_common/src/FSM/StateFixedDown.cpp

@@ -0,0 +1,60 @@
+//
+// Created by tlab-uav on 24-9-11.
+//
+
+#include "controller_common/FSM/StateFixedDown.h"
+
+#include <cmath>
+
+StateFixedDown::StateFixedDown(CtrlInterfaces &ctrl_interfaces,
+                               const std::vector<double> &target_pos,
+                               const double kp,
+                               const double kd)
+    : FSMState(FSMStateName::FIXEDDOWN, "fixed down", ctrl_interfaces),
+      kp_(kp), kd_(kd) {
+    duration_ = ctrl_interfaces_.frequency_ * 1.2;
+    for (int i = 0; i < 12; i++) {
+        target_pos_[i] = target_pos[i];
+    }
+}
+
+void StateFixedDown::enter() {
+    for (int i = 0; i < 12; i++) {
+        start_pos_[i] = ctrl_interfaces_.joint_position_state_interface_[i].get().get_value();
+    }
+    ctrl_interfaces_.control_inputs_.command = 0;
+    for (int i = 0; i < 12; i++) {
+        std::ignore = ctrl_interfaces_.joint_position_command_interface_[i].get().set_value(start_pos_[i]);
+        std::ignore = ctrl_interfaces_.joint_velocity_command_interface_[i].get().set_value(0);
+        std::ignore = ctrl_interfaces_.joint_torque_command_interface_[i].get().set_value(0);
+        std::ignore = ctrl_interfaces_.joint_kp_command_interface_[i].get().set_value(kp_);
+        std::ignore = ctrl_interfaces_.joint_kd_command_interface_[i].get().set_value(kd_);
+    }
+}
+
+void StateFixedDown::run() {
+    percent_ += 1 / duration_;
+    phase = std::tanh(percent_);
+    for (int i = 0; i < 12; i++) {
+        std::ignore = ctrl_interfaces_.joint_position_command_interface_[i].get().set_value(
+            phase * target_pos_[i] + (1 - phase) * start_pos_[i]);
+    }
+}
+
+void StateFixedDown::exit() {
+    percent_ = 0;
+}
+
+FSMStateName StateFixedDown::checkChange() {
+    if (percent_ < 1.5) {
+        return FSMStateName::FIXEDDOWN;
+    }
+    switch (ctrl_interfaces_.control_inputs_.command) {
+        case 1:
+            return FSMStateName::PASSIVE;
+        case 2:
+            return FSMStateName::FIXEDSTAND;
+        default:
+            return FSMStateName::FIXEDDOWN;
+    }
+}

libraries/controller_common/src/FSM/StatePassive.cpp

@@ -0,0 +1,43 @@
+//
+// Created by tlab-uav on 25-2-27.
+//
+
+#include "controller_common/FSM/StatePassive.h"
+
+#include <iostream>
+
+StatePassive::StatePassive(CtrlInterfaces &ctrl_interfaces) : FSMState(
+    FSMStateName::PASSIVE, "passive", ctrl_interfaces) {
+}
+
+void StatePassive::enter() {
+    for (auto i: ctrl_interfaces_.joint_torque_command_interface_) {
+        std::ignore = i.get().set_value(0);
+    }
+    for (auto i: ctrl_interfaces_.joint_position_command_interface_) {
+        std::ignore = i.get().set_value(0);
+    }
+    for (auto i: ctrl_interfaces_.joint_velocity_command_interface_) {
+        std::ignore = i.get().set_value(0);
+    }
+    for (auto i: ctrl_interfaces_.joint_kp_command_interface_) {
+        std::ignore = i.get().set_value(0);
+    }
+    for (auto i: ctrl_interfaces_.joint_kd_command_interface_) {
+        std::ignore = i.get().set_value(1);
+    }
+    ctrl_interfaces_.control_inputs_.command = 0;
+}
+
+void StatePassive::run() {
+}
+
+void StatePassive::exit() {
+}
+
+FSMStateName StatePassive::checkChange() {
+    if (ctrl_interfaces_.control_inputs_.command == 2) {
+        return FSMStateName::FIXEDDOWN;
+    }
+    return FSMStateName::PASSIVE;
+}