diff --git a/controllers/unitree_guide_controller/CMakeLists.txt b/controllers/unitree_guide_controller/CMakeLists.txt
index 57c4a93..88b4561 100644
--- a/controllers/unitree_guide_controller/CMakeLists.txt
+++ b/controllers/unitree_guide_controller/CMakeLists.txt
@@ -34,6 +34,7 @@ add_library(${PROJECT_NAME} SHARED
         src/FSM/StateSwingTest.cpp
         src/FSM/StateFreeStand.cpp
         src/FSM/StateBalanceTest.cpp
+        src/FSM/StateTrotting.cpp
 
         src/robot/QuadrupedRobot.cpp
         src/robot/RobotLeg.cpp
@@ -46,7 +47,7 @@ add_library(${PROJECT_NAME} SHARED
         src/quadProgpp/QuadProg++.cc
 
         src/gait/WaveGenerator.cpp
-        src/gait/FootEndCtrl.cpp
+        src/gait/FeetEndCtrl.cpp
         src/gait/GaitGenerator.cpp
 
 )
diff --git a/controllers/unitree_guide_controller/include/unitree_guide_controller/FSM/StateBalanceTest.h b/controllers/unitree_guide_controller/include/unitree_guide_controller/FSM/StateBalanceTest.h
index 66a600f..4dcc673 100644
--- a/controllers/unitree_guide_controller/include/unitree_guide_controller/FSM/StateBalanceTest.h
+++ b/controllers/unitree_guide_controller/include/unitree_guide_controller/FSM/StateBalanceTest.h
@@ -22,9 +22,10 @@ public:
 private:
     void calcTorque();
 
-    Estimator& estimator_;
-    QuadrupedRobot& robot_model_;
-    BalanceCtrl& balance_ctrl_;
+    Estimator &estimator_;
+    QuadrupedRobot &robot_model_;
+    BalanceCtrl &balance_ctrl_;
+    WaveGenerator &wave_generator_;
 
     Vec3 pcd_, pcd_init_;
     RotMat Rd_;
diff --git a/controllers/unitree_guide_controller/include/unitree_guide_controller/FSM/StateTrotting.h b/controllers/unitree_guide_controller/include/unitree_guide_controller/FSM/StateTrotting.h
new file mode 100644
index 0000000..d542ac6
--- /dev/null
+++ b/controllers/unitree_guide_controller/include/unitree_guide_controller/FSM/StateTrotting.h
@@ -0,0 +1,81 @@
+//
+// Created by tlab-uav on 24-9-18.
+//
+
+#ifndef STATETROTTING_H
+#define STATETROTTING_H
+#include <unitree_guide_controller/gait/GaitGenerator.h>
+
+#include "FSMState.h"
+
+
+class StateTrotting final : public FSMState {
+public:
+    explicit StateTrotting(CtrlComponent &ctrlComp);
+
+    void enter() override;
+
+    void run() override;
+
+    void exit() override;
+
+    FSMStateName checkChange() override;
+
+private:
+    void getUserCmd();
+
+    void calcCmd();
+
+    void calcTau();
+
+    void calcQQd();
+
+    void calcGain() const;
+
+    /**
+     * Check whether the robot should take a step or not
+     * @return
+     */
+    bool checkStepOrNot();
+
+    Estimator &estimator_;
+    QuadrupedRobot &robot_model_;
+    BalanceCtrl &balance_ctrl_;
+    WaveGenerator &wave_generator_;
+
+    GaitGenerator gait_generator_;
+
+    // Robot State
+    Vec3 _posBody, _velBody;
+    double _yaw{}, _dYaw{};
+    Vec34 _posFeetGlobal, _velFeetGlobal;
+    Vec34 _posFeet2BGlobal;
+    RotMat B2G_RotMat, G2B_RotMat;
+    Vec12 _q;
+
+
+    // Robot command
+    Vec3 _pcd;
+    Vec3 _vCmdGlobal, _vCmdBody;
+    double dt_;
+    double _yawCmd{}, _dYawCmd{};
+    double _dYawCmdPast{};
+    Vec3 _wCmdGlobal;
+    Vec34 _posFeetGlobalGoal, _velFeetGlobalGoal;
+    Vec34 _posFeet2BGoal, _velFeet2BGoal;
+    RotMat Rd;
+    Vec3 _ddPcd, _dWbd;
+    Vec34 force_feet_global_, force_feet_body_;
+    Vec12 _tau;
+
+    // Control Parameters
+    double _gaitHeight;
+    Vec3 pos_error_, vel_error_;
+    Mat3 Kpp, Kdp, Kdw;
+    double _kpw;
+    Mat3 KpSwing, KdSwing;
+    Vec2 _vxLim, _vyLim, _wyawLim;
+};
+
+
+#endif //STATETROTTING_H
diff --git a/controllers/unitree_guide_controller/include/unitree_guide_controller/UnitreeGuideController.h b/controllers/unitree_guide_controller/include/unitree_guide_controller/UnitreeGuideController.h
index a6c481b..743e6db 100644
--- a/controllers/unitree_guide_controller/include/unitree_guide_controller/UnitreeGuideController.h
+++ b/controllers/unitree_guide_controller/include/unitree_guide_controller/UnitreeGuideController.h
@@ -16,6 +16,7 @@
 #include "FSM/StateFreeStand.h"
 #include "FSM/StatePassive.h"
 #include "FSM/StateSwingTest.h"
+#include "FSM/StateTrotting.h"
 
 namespace unitree_guide_controller {
     struct FSMStateList {
@@ -24,6 +25,7 @@ namespace unitree_guide_controller {
         std::shared_ptr<StateFixedDown> fixedDown;
         std::shared_ptr<StateFixedStand> fixedStand;
         std::shared_ptr<StateFreeStand> freeStand;
+        std::shared_ptr<StateTrotting> trotting;
 
         std::shared_ptr<StateSwingTest> swingTest;
         std::shared_ptr<StateBalanceTest> balanceTest;
diff --git a/controllers/unitree_guide_controller/include/unitree_guide_controller/common/mathTools.h b/controllers/unitree_guide_controller/include/unitree_guide_controller/common/mathTools.h
index b91f312..13bb47e 100644
--- a/controllers/unitree_guide_controller/include/unitree_guide_controller/common/mathTools.h
+++ b/controllers/unitree_guide_controller/include/unitree_guide_controller/common/mathTools.h
@@ -61,5 +61,25 @@ inline Vec3 rotMatToExp(const RotMat &rm) {
     return exp;
 }
 
+template<typename T>
+T saturation(const T a, Vec2 limits) {
+    T lowLim, highLim;
+    if (limits(0) > limits(1)) {
+        lowLim = limits(1);
+        highLim = limits(0);
+    } else {
+        lowLim = limits(0);
+        highLim = limits(1);
+    }
+
+    if (a < lowLim) {
+        return lowLim;
+    }
+    if (a > highLim) {
+        return highLim;
+    }
+    return a;
+}
+
 
 #endif //MATHTOOLS_H
diff --git a/controllers/unitree_guide_controller/include/unitree_guide_controller/control/BalanceCtrl.h b/controllers/unitree_guide_controller/include/unitree_guide_controller/control/BalanceCtrl.h
index 6750019..fba6376 100644
--- a/controllers/unitree_guide_controller/include/unitree_guide_controller/control/BalanceCtrl.h
+++ b/controllers/unitree_guide_controller/include/unitree_guide_controller/control/BalanceCtrl.h
@@ -26,14 +26,14 @@ public:
      * @return
      */
     Vec34 calF(const Vec3 &ddPcd, const Vec3 &dWbd, const RotMat &rot_matrix,
-               const Vec34 &feet_pos_2_body, const std::vector<int> &contact);
+               const Vec34 &feet_pos_2_body, const VecInt4 &contact);
 
 private:
     void calMatrixA(const Vec34 &feet_pos_2_body, const RotMat &rotM);
 
     void calVectorBd(const Vec3 &ddPcd, const Vec3 &dWbd, const RotMat &rotM);
 
-    void calConstraints(const std::vector<int> &contact);
+    void calConstraints(const VecInt4 &contact);
 
     void solveQP();
 
diff --git a/controllers/unitree_guide_controller/include/unitree_guide_controller/control/Estimator.h b/controllers/unitree_guide_controller/include/unitree_guide_controller/control/Estimator.h
index 0a0c7d9..b0ad6ed 100644
--- a/controllers/unitree_guide_controller/include/unitree_guide_controller/control/Estimator.h
+++ b/controllers/unitree_guide_controller/include/unitree_guide_controller/control/Estimator.h
@@ -44,11 +44,23 @@ public:
         return getPosition() + Vec3((rotation_ * foot_poses_[index].p).data);
     }
 
+    /**
+     * Get the estimated feet velocity in world frame
+     * @return feet velocity in world frame
+     */
+    Vec34 getFeetPos() {
+        Vec34 feetPos;
+        for (int i(0); i < 4; ++i) {
+            feetPos.col(i) = getFootPos(i);
+        }
+        return feetPos;
+    }
+
     /**
      * Get the estimated foot position in body frame
      * @return
      */
-    Vec34 getFootPos2Body() {
+    Vec34 getFeetPos2Body() {
         Vec34 foot_pos;
         const Vec3 body_pos = getPosition();
         for (int i = 0; i < 4; i++) {
@@ -65,8 +77,18 @@ public:
         return gyro_;
     }
 
-    [[nodiscard]] KDL::Vector getGlobalGyro() const {
-        return rotation_ * gyro_;
+    [[nodiscard]] Vec3 getGlobalGyro() const {
+        return Vec3((rotation_ * gyro_).data);
+    }
+
+    [[nodiscard]] double getYaw() const {
+        double roll, pitch, yaw;
+        rotation_.GetRPY(roll, pitch, yaw);
+        return yaw;
+    }
+
+    [[nodiscard]] double getDYaw() const {
+        return getGlobalGyro()(2);
     }
 
     void update();
diff --git a/controllers/unitree_guide_controller/include/unitree_guide_controller/gait/FeetEndCtrl.h b/controllers/unitree_guide_controller/include/unitree_guide_controller/gait/FeetEndCtrl.h
new file mode 100644
index 0000000..ca69a01
--- /dev/null
+++ b/controllers/unitree_guide_controller/include/unitree_guide_controller/gait/FeetEndCtrl.h
@@ -0,0 +1,38 @@
+//
+// Created by biao on 24-9-18.
+//
+
+
+#ifndef FOOTENDCTRL_H
+#define FOOTENDCTRL_H
+#include <unitree_guide_controller/common/mathTypes.h>
+
+
+class Estimator;
+struct CtrlComponent;
+class QuadrupedRobot;
+
+class FeetEndCtrl {
+public:
+    explicit FeetEndCtrl(CtrlComponent &ctrl_component);
+
+    void init();
+
+    ~FeetEndCtrl() = default;
+
+    Vec3 calcFootPos(int index, Vec2 vxy_goal_global, double d_yaw_global, double phase);
+
+private:
+    CtrlComponent &ctrl_component_;
+    QuadrupedRobot &robot_model_;
+    Estimator &estimator_;
+
+    Vec4 feet_radius_, feet_init_angle_;
+
+    double k_x_, k_y_, k_yaw_;
+    double t_stance_{}, t_swing_{};
+
+};
+
+
+#endif //FOOTENDCTRL_H
diff --git a/controllers/unitree_guide_controller/include/unitree_guide_controller/gait/FootEndCtrl.h b/controllers/unitree_guide_controller/include/unitree_guide_controller/gait/FootEndCtrl.h
deleted file mode 100644
index 1d6abca..0000000
--- a/controllers/unitree_guide_controller/include/unitree_guide_controller/gait/FootEndCtrl.h
+++ /dev/null
@@ -1,24 +0,0 @@
-//
-// Created by biao on 24-9-18.
-//
-
-
-#ifndef FOOTENDCTRL_H
-#define FOOTENDCTRL_H
-#include <unitree_guide_controller/common/mathTypes.h>
-
-
-class FootEndCtrl {
-public:
-    FootEndCtrl();
-
-    ~FootEndCtrl() = default;
-
-    Vec3 calcFootPos(int index, Vec2 vxy_goal_global, double d_yaw_global, double phase);
-
-private:
-    double k_x_, k_y_, k_yaw_;
-};
-
-
-#endif //FOOTENDCTRL_H
diff --git a/controllers/unitree_guide_controller/include/unitree_guide_controller/gait/GaitGenerator.h b/controllers/unitree_guide_controller/include/unitree_guide_controller/gait/GaitGenerator.h
index a5a2823..14661e7 100644
--- a/controllers/unitree_guide_controller/include/unitree_guide_controller/gait/GaitGenerator.h
+++ b/controllers/unitree_guide_controller/include/unitree_guide_controller/gait/GaitGenerator.h
@@ -5,10 +5,76 @@
 
 #ifndef GAITGENERATOR_H
 #define GAITGENERATOR_H
+#include <unitree_guide_controller/common/mathTypes.h>
+
+#include "FeetEndCtrl.h"
 
 
-class Gaitgenerator {
+class Estimator;
+class WaveGenerator;
+struct CtrlComponent;
 
+class GaitGenerator {
+public:
+    explicit GaitGenerator(CtrlComponent &ctrl_component);
+
+    ~GaitGenerator() = default;
+
+    void setGait(Vec2 vxy_goal_global, double d_yaw_goal, double gait_height);
+
+    void run(Vec34 &feet_pos, Vec34 &feet_vel);
+
+    void restart();
+
+private:
+    Vec3 getFootPos(int i);
+
+    Vec3 getFootVel(int i);
+
+    /**
+     * Calculate the position of the foot in the XY plane
+     * @param startXY
+     * @param endXY
+     * @param phase
+     * @return
+     */
+    static double cycloidXYPosition(double startXY, double endXY, double phase);
+
+    /**
+     * Calculate the position of the foot in the Z direction
+     * @param startZ
+     * @param height
+     * @param phase
+     * @return
+     */
+    static double cycloidZPosition(double startZ, double height, double phase);
+
+    /**
+     * Calculate the velocity of the foot in the XY plane
+     * @param startXY
+     * @param endXY
+     * @param phase
+     * @return
+     */
+    [[nodiscard]] double cycloidXYVelocity(double startXY, double endXY, double phase) const;
+
+    /**
+     * Calculate the velocity of the foot in the Z direction
+     * @param height
+     * @param phase
+     * @return
+     */
+    [[nodiscard]] double cycloidZVelocity(double height, double phase) const;
+
+    WaveGenerator &wave_generator_;
+    Estimator &estimator_;
+    FeetEndCtrl feet_end_ctrl_;
+
+    double gait_height_{};
+    Vec2 vxy_goal_;
+    double d_yaw_goal_{};
+    Vec34 start_p_, end_p_, ideal_p_, past_p_;
+    bool first_run_;
 };
 
 
diff --git a/controllers/unitree_guide_controller/include/unitree_guide_controller/gait/WaveGenerator.h b/controllers/unitree_guide_controller/include/unitree_guide_controller/gait/WaveGenerator.h
index 7d1e58d..885f2aa 100644
--- a/controllers/unitree_guide_controller/include/unitree_guide_controller/gait/WaveGenerator.h
+++ b/controllers/unitree_guide_controller/include/unitree_guide_controller/gait/WaveGenerator.h
@@ -23,12 +23,16 @@ public:
 
     void init(double period, double st_ratio, const Vec4 &bias);
 
-    void calculate(Vec4 &phase_result, VecInt4 &contact_result, WaveStatus status);
+    void update();
 
     [[nodiscard]] double get_t_stance() const { return period_ * st_ratio_; }
     [[nodiscard]] double get_t_swing() const { return period_ * (1 - st_ratio_); }
     [[nodiscard]] double get_t() const { return period_; }
 
+    Vec4 phase_;
+    VecInt4 contact_;
+    WaveStatus status_{};
+
 private:
     /**
      * Update phase, contact and status based on current time.
@@ -43,8 +47,8 @@ private:
     Vec4 bias_;
 
     Vec4 normal_t_; // normalize time [0,1)
-    Vec4 phase_, phase_past_; // foot phase
-    VecInt4 contact_, contact_past_; // foot contact
+    Vec4 phase_past_; // foot phase
+    VecInt4 contact_past_; // foot contact
     VecInt4 switch_status_;
     WaveStatus status_past_;
 
diff --git a/controllers/unitree_guide_controller/include/unitree_guide_controller/robot/QuadrupedRobot.h b/controllers/unitree_guide_controller/include/unitree_guide_controller/robot/QuadrupedRobot.h
index 73ca436..cc3d32c 100644
--- a/controllers/unitree_guide_controller/include/unitree_guide_controller/robot/QuadrupedRobot.h
+++ b/controllers/unitree_guide_controller/include/unitree_guide_controller/robot/QuadrupedRobot.h
@@ -30,6 +30,10 @@ public:
      */
     [[nodiscard]] std::vector<KDL::JntArray> getQ(const std::vector<KDL::Frame> &pEe_list) const;
 
+    [[nodiscard]] Vec12 getQ(const Vec34 &vecP) const;
+
+    Vec12 getQd(const std::vector<KDL::Frame> &pos, const Vec34 &vel);
+
     /**
      * Calculate the foot end position based on joint positions
      * @return vector of foot-end position
diff --git a/controllers/unitree_guide_controller/src/FSM/StateBalanceTest.cpp b/controllers/unitree_guide_controller/src/FSM/StateBalanceTest.cpp
index 18514b5..41ccee3 100644
--- a/controllers/unitree_guide_controller/src/FSM/StateBalanceTest.cpp
+++ b/controllers/unitree_guide_controller/src/FSM/StateBalanceTest.cpp
@@ -10,7 +10,8 @@ StateBalanceTest::StateBalanceTest(CtrlComponent &ctrlComp) : FSMState(FSMStateN
                                                                        ctrlComp),
                                                               estimator_(ctrlComp.estimator_),
                                                               robot_model_(ctrlComp.robot_model_),
-                                                              balance_ctrl_(ctrlComp.balance_ctrl_) {
+                                                              balance_ctrl_(ctrlComp.balance_ctrl_),
+                                                              wave_generator_(ctrl_comp_.wave_generator_) {
     _xMax = 0.05;
     _xMin = -_xMax;
     _yMax = 0.05;
@@ -31,6 +32,7 @@ void StateBalanceTest::enter() {
     pcd_init_ = estimator_.getPosition();
     pcd_ = pcd_init_;
     init_rotation_ = estimator_.getRotation();
+    wave_generator_.status_ = WaveStatus::STANCE_ALL;
 }
 
 void StateBalanceTest::run() {
@@ -74,12 +76,11 @@ void StateBalanceTest::calcTorque() {
 
     // expected body angular acceleration
     d_wbd_ = -kp_w_ * rotMatToExp(Rd_ * G2B_Rotation) +
-             Kd_w_ * Vec3((-estimator_.getGlobalGyro()).data);
+             Kd_w_ * -estimator_.getGlobalGyro();
 
     // calculate foot force
-    const std::vector contact(4, 1);
     const Vec34 foot_force = G2B_Rotation * balance_ctrl_.calF(dd_pcd_, d_wbd_, B2G_Rotation,
-                                                               estimator_.getFootPos2Body(), contact);
+                                                               estimator_.getFeetPos2Body(), wave_generator_.contact_);
 
     std::vector<KDL::JntArray> current_joints = robot_model_.current_joint_pos_;
     for (int i = 0; i < 4; i++) {
diff --git a/controllers/unitree_guide_controller/src/FSM/StateFixedDown.cpp b/controllers/unitree_guide_controller/src/FSM/StateFixedDown.cpp
index 68d0af3..af2adf4 100644
--- a/controllers/unitree_guide_controller/src/FSM/StateFixedDown.cpp
+++ b/controllers/unitree_guide_controller/src/FSM/StateFixedDown.cpp
@@ -15,6 +15,7 @@ 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_.get().command = 0;
 }
 
 void StateFixedDown::run() {
diff --git a/controllers/unitree_guide_controller/src/FSM/StateFixedStand.cpp b/controllers/unitree_guide_controller/src/FSM/StateFixedStand.cpp
index 2458d2d..4512683 100644
--- a/controllers/unitree_guide_controller/src/FSM/StateFixedStand.cpp
+++ b/controllers/unitree_guide_controller/src/FSM/StateFixedStand.cpp
@@ -15,6 +15,7 @@ void StateFixedStand::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_.get().command = 0;
 }
 
 void StateFixedStand::run() {
@@ -44,6 +45,8 @@ FSMStateName StateFixedStand::checkChange() {
             return FSMStateName::FIXEDDOWN;
         case 3:
             return FSMStateName::FREESTAND;
+        case 4:
+            return FSMStateName::TROTTING;
         case 9:
             return FSMStateName::SWINGTEST;
         case 10:
diff --git a/controllers/unitree_guide_controller/src/FSM/StateFreeStand.cpp b/controllers/unitree_guide_controller/src/FSM/StateFreeStand.cpp
index 974e37f..3c1e13f 100644
--- a/controllers/unitree_guide_controller/src/FSM/StateFreeStand.cpp
+++ b/controllers/unitree_guide_controller/src/FSM/StateFreeStand.cpp
@@ -33,6 +33,7 @@ void StateFreeStand::enter() {
         foot_pos.p -= fr_init_pos_.p;
         foot_pos.M = KDL::Rotation::RPY(0, 0, 0);
     }
+    ctrl_comp_.control_inputs_.get().command = 0;
 }
 
 void StateFreeStand::run() {
diff --git a/controllers/unitree_guide_controller/src/FSM/StatePassive.cpp b/controllers/unitree_guide_controller/src/FSM/StatePassive.cpp
index bbd88dd..ff87aca 100644
--- a/controllers/unitree_guide_controller/src/FSM/StatePassive.cpp
+++ b/controllers/unitree_guide_controller/src/FSM/StatePassive.cpp
@@ -27,6 +27,7 @@ void StatePassive::enter() {
     for (auto i: ctrl_comp_.joint_kd_command_interface_) {
         i.get().set_value(1);
     }
+    ctrl_comp_.control_inputs_.get().command = 0;
 }
 
 void StatePassive::run() {
diff --git a/controllers/unitree_guide_controller/src/FSM/StateTrotting.cpp b/controllers/unitree_guide_controller/src/FSM/StateTrotting.cpp
new file mode 100644
index 0000000..86c92df
--- /dev/null
+++ b/controllers/unitree_guide_controller/src/FSM/StateTrotting.cpp
@@ -0,0 +1,196 @@
+//
+// Created by tlab-uav on 24-9-18.
+//
+
+#include "unitree_guide_controller/FSM/StateTrotting.h"
+
+#include <unitree_guide_controller/common/mathTools.h>
+
+StateTrotting::StateTrotting(CtrlComponent &ctrlComp) : FSMState(FSMStateName::TROTTING, "trotting", ctrlComp),
+                                                        estimator_(ctrlComp.estimator_),
+                                                        robot_model_(ctrlComp.robot_model_),
+                                                        balance_ctrl_(ctrlComp.balance_ctrl_),
+                                                        wave_generator_(ctrl_comp_.wave_generator_),
+                                                        gait_generator_(ctrlComp) {
+    _gaitHeight = 0.08;
+    Kpp = Vec3(70, 70, 70).asDiagonal();
+    Kdp = Vec3(10, 10, 10).asDiagonal();
+    _kpw = 780;
+    Kdw = Vec3(70, 70, 70).asDiagonal();
+    KpSwing = Vec3(400, 400, 400).asDiagonal();
+    KdSwing = Vec3(10, 10, 10).asDiagonal();
+
+    _vxLim << -0.4, 0.4;
+    _vyLim << -0.3, 0.3;
+    _wyawLim << -0.5, 0.5;
+    dt_ = 1.0 / ctrl_comp_.frequency_;
+}
+
+void StateTrotting::enter() {
+    _pcd = estimator_.getPosition();
+    _vCmdBody.setZero();
+    _yawCmd = estimator_.getYaw();
+    Rd = Mat3(KDL::Rotation::RPY(0, 0, _yawCmd).Inverse().data);
+    _wCmdGlobal.setZero();
+
+    ctrl_comp_.control_inputs_.get().command = 0;
+    gait_generator_.restart();
+}
+
+void StateTrotting::run() {
+    _posBody = estimator_.getPosition();
+    _velBody = estimator_.getVelocity();
+    _posFeet2BGlobal = estimator_.getFeetPos2Body();
+    _posFeetGlobal = estimator_.getFeetPos();
+    B2G_RotMat = Eigen::Matrix3d(estimator_.getRotation().data);
+    G2B_RotMat = B2G_RotMat.transpose();
+
+    getUserCmd();
+    calcCmd();
+
+    gait_generator_.setGait(_vCmdGlobal.segment(0, 2), _wCmdGlobal(2), _gaitHeight);
+    gait_generator_.run(_posFeetGlobalGoal, _velFeetGlobalGoal);
+
+    calcTau();
+    calcQQd();
+
+    if (checkStepOrNot()) {
+        wave_generator_.status_ = WaveStatus::WAVE_ALL;
+    } else {
+        wave_generator_.status_ = WaveStatus::STANCE_ALL;
+    }
+
+    calcGain();
+}
+
+void StateTrotting::exit() {
+    wave_generator_.status_ = WaveStatus::SWING_ALL;
+}
+
+FSMStateName StateTrotting::checkChange() {
+    switch (ctrl_comp_.control_inputs_.get().command) {
+        case 1:
+            return FSMStateName::FIXEDDOWN;
+        case 2:
+            return FSMStateName::FIXEDSTAND;
+        default:
+            return FSMStateName::TROTTING;
+    }
+}
+
+void StateTrotting::getUserCmd() {
+    /* Movement */
+    _vCmdBody(0) = invNormalize(ctrl_comp_.control_inputs_.get().ly, _vxLim(0), _vxLim(1));
+    _vCmdBody(1) = -invNormalize(ctrl_comp_.control_inputs_.get().lx, _vyLim(0), _vyLim(1));
+    _vCmdBody(2) = 0;
+
+    /* Turning */
+    _dYawCmd = -invNormalize(ctrl_comp_.control_inputs_.get().rx, _wyawLim(0), _wyawLim(1));
+    _dYawCmd = 0.9 * _dYawCmdPast + (1 - 0.9) * _dYawCmd;
+    _dYawCmdPast = _dYawCmd;
+}
+
+void StateTrotting::calcCmd() {
+    /* Movement */
+    _vCmdGlobal = B2G_RotMat * _vCmdBody;
+
+    _vCmdGlobal(0) =
+            saturation(_vCmdGlobal(0), Vec2(_velBody(0) - 0.2, _velBody(0) + 0.2));
+    _vCmdGlobal(1) =
+            saturation(_vCmdGlobal(1), Vec2(_velBody(1) - 0.2, _velBody(1) + 0.2));
+
+    _pcd(0) = saturation(_pcd(0) + _vCmdGlobal(0) * dt_,
+                         Vec2(_posBody(0) - 0.05, _posBody(0) + 0.05));
+    _pcd(1) = saturation(_pcd(1) + _vCmdGlobal(1) * dt_,
+                         Vec2(_posBody(1) - 0.05, _posBody(1) + 0.05));
+
+    _vCmdGlobal(2) = 0;
+
+    /* Turning */
+    _yawCmd = _yawCmd + _dYawCmd * dt_;
+    Rd = Mat3(KDL::Rotation::RPY(0, 0, _yawCmd).Inverse().data);
+    _wCmdGlobal(2) = _dYawCmd;
+}
+
+void StateTrotting::calcTau() {
+    pos_error_ = _pcd - _posBody;
+    vel_error_ = _vCmdGlobal - _velBody;
+
+    _ddPcd = Kpp * pos_error_ + Kdp * vel_error_;
+    _dWbd = _kpw * rotMatToExp(Rd * G2B_RotMat) +
+            Kdw * (_wCmdGlobal - estimator_.getGlobalGyro());
+
+    _ddPcd(0) = saturation(_ddPcd(0), Vec2(-3, 3));
+    _ddPcd(1) = saturation(_ddPcd(1), Vec2(-3, 3));
+    _ddPcd(2) = saturation(_ddPcd(2), Vec2(-5, 5));
+
+    _dWbd(0) = saturation(_dWbd(0), Vec2(-40, 40));
+    _dWbd(1) = saturation(_dWbd(1), Vec2(-40, 40));
+    _dWbd(2) = saturation(_dWbd(2), Vec2(-10, 10));
+
+    force_feet_global_ =
+            -balance_ctrl_.calF(_ddPcd, _dWbd, B2G_RotMat, _posFeet2BGlobal, wave_generator_.contact_);
+
+    for (int i(0); i < 4; ++i) {
+        if (wave_generator_.contact_(i) == 0) {
+            force_feet_global_.col(i) =
+                    KpSwing * (_posFeetGlobalGoal.col(i) - _posFeetGlobal.col(i)) +
+                    KdSwing * (_velFeetGlobalGoal.col(i) - _velFeetGlobal.col(i));
+        }
+    }
+
+    force_feet_body_ = G2B_RotMat * force_feet_global_;
+
+    std::vector<KDL::JntArray> current_joints = robot_model_.current_joint_pos_;
+    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_effort_command_interface_[i * 3 + j].get().set_value(torque(j));
+        }
+    }
+}
+
+void StateTrotting::calcQQd() {
+    const std::vector<KDL::Frame> _posFeet2B = robot_model_.getFeet2BPositions();
+
+    for (int i(0); i < 4; ++i) {
+        _posFeet2BGoal.col(i) = G2B_RotMat * (_posFeetGlobalGoal.col(i) - _posBody);
+        _velFeet2BGoal.col(i) = G2B_RotMat * (_velFeetGlobalGoal.col(i) - _velBody);
+        // _velFeet2BGoal.col(i) = _G2B_RotMat * (_velFeetGlobalGoal.col(i) -
+        // _velBody - _B2G_RotMat * (skew(_lowState->getGyro()) * _posFeet2B.col(i))
+        // );  //  c.f formula (6.12)
+    }
+
+    Vec12 q_goal = robot_model_.getQ(_posFeet2BGoal);
+    Vec12 qd_goal = robot_model_.getQd(_posFeet2B, _velFeet2BGoal);
+    for (int i = 0; i < 12; i++) {
+        ctrl_comp_.joint_position_command_interface_[i].get().set_value(q_goal(i));
+        ctrl_comp_.joint_velocity_command_interface_[i].get().set_value(qd_goal(i));
+    }
+}
+
+void StateTrotting::calcGain() const {
+    for (int i(0); i < 4; ++i) {
+        if (wave_generator_.contact_(i) == 0) {
+            for (int j = 0; j < 3; j++) {
+                ctrl_comp_.joint_kp_command_interface_[i * 3 + j].get().set_value(3);
+                ctrl_comp_.joint_kd_command_interface_[i * 3 + j].get().set_value(2);
+            }
+        } else {
+            for (int j = 0; j < 3; j++) {
+                ctrl_comp_.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);
+            }
+        }
+    }
+}
+
+bool StateTrotting::checkStepOrNot() {
+    if (fabs(_vCmdBody(0)) > 0.03 || fabs(_vCmdBody(1)) > 0.03 ||
+        fabs(pos_error_(0)) > 0.08 || fabs(pos_error_(1)) > 0.08 ||
+        fabs(vel_error_(0)) > 0.05 || fabs(vel_error_(1)) > 0.05 ||
+        fabs(_dYawCmd) > 0.20) {
+        return true;
+    }
+    return false;
+}
diff --git a/controllers/unitree_guide_controller/src/UnitreeGuideController.cpp b/controllers/unitree_guide_controller/src/UnitreeGuideController.cpp
index 87d3b5c..b30bbc8 100644
--- a/controllers/unitree_guide_controller/src/UnitreeGuideController.cpp
+++ b/controllers/unitree_guide_controller/src/UnitreeGuideController.cpp
@@ -51,6 +51,7 @@ namespace unitree_guide_controller {
 
 
         ctrl_comp_.robot_model_.update();
+        ctrl_comp_.wave_generator_.update();
         ctrl_comp_.estimator_.update();
 
         if (mode_ == FSMMode::NORMAL) {
@@ -114,6 +115,8 @@ namespace unitree_guide_controller {
         get_node()->get_parameter("update_rate", ctrl_comp_.frequency_);
         RCLCPP_INFO(get_node()->get_logger(), "Controller Manager Update Rate: %d Hz", ctrl_comp_.frequency_);
 
+        ctrl_comp_.wave_generator_.init(0.45, 0.5, Vec4(0, 0.5, 0.5, 0));
+
         return CallbackReturn::SUCCESS;
     }
 
@@ -142,6 +145,7 @@ namespace unitree_guide_controller {
         state_list_.swingTest = std::make_shared<StateSwingTest>(ctrl_comp_);
         state_list_.freeStand = std::make_shared<StateFreeStand>(ctrl_comp_);
         state_list_.balanceTest = std::make_shared<StateBalanceTest>(ctrl_comp_);
+        state_list_.trotting = std::make_shared<StateTrotting>(ctrl_comp_);
 
         // Initialize FSM
         current_state_ = state_list_.passive;
@@ -186,8 +190,8 @@ namespace unitree_guide_controller {
                 return state_list_.fixedStand;
             case FSMStateName::FREESTAND:
                 return state_list_.freeStand;
-            // case FSMStateName::TROTTING:
-            //     return state_list_.trotting;
+            case FSMStateName::TROTTING:
+                return state_list_.trotting;
             case FSMStateName::SWINGTEST:
                 return state_list_.swingTest;
             case FSMStateName::BALANCETEST:
diff --git a/controllers/unitree_guide_controller/src/control/BalanceCtrl.cpp b/controllers/unitree_guide_controller/src/control/BalanceCtrl.cpp
index 0fe51b1..ae147ae 100644
--- a/controllers/unitree_guide_controller/src/control/BalanceCtrl.cpp
+++ b/controllers/unitree_guide_controller/src/control/BalanceCtrl.cpp
@@ -38,7 +38,7 @@ void BalanceCtrl::init(const QuadrupedRobot &robot) {
 }
 
 Vec34 BalanceCtrl::calF(const Vec3 &ddPcd, const Vec3 &dWbd, const RotMat &rot_matrix,
-                        const Vec34 &feet_pos_2_body, const std::vector<int> &contact) {
+                        const Vec34 &feet_pos_2_body, const VecInt4 &contact) {
     calMatrixA(feet_pos_2_body, rot_matrix);
     calVectorBd(ddPcd, dWbd, rot_matrix);
     calConstraints(contact);
@@ -64,7 +64,7 @@ void BalanceCtrl::calVectorBd(const Vec3 &ddPcd, const Vec3 &dWbd, const RotMat
     bd_.tail(3) = rotM * Ib_ * rotM.transpose() * dWbd;
 }
 
-void BalanceCtrl::calConstraints(const std::vector<int> &contact) {
+void BalanceCtrl::calConstraints(const VecInt4 &contact) {
     int contactLegNum = 0;
     for (int i(0); i < 4; ++i) {
         if (contact[i] == 1) {
diff --git a/controllers/unitree_guide_controller/src/gait/FeetEndCtrl.cpp b/controllers/unitree_guide_controller/src/gait/FeetEndCtrl.cpp
new file mode 100644
index 0000000..661ad59
--- /dev/null
+++ b/controllers/unitree_guide_controller/src/gait/FeetEndCtrl.cpp
@@ -0,0 +1,56 @@
+//
+// Created by biao on 24-9-18.
+//
+
+#include "unitree_guide_controller/gait/FeetEndCtrl.h"
+
+#include "unitree_guide_controller/control/CtrlComponent.h"
+
+FeetEndCtrl::FeetEndCtrl(CtrlComponent &ctrl_component) : ctrl_component_(ctrl_component),
+                                                          robot_model_(ctrl_component.robot_model_),
+                                                          estimator_(ctrl_component.estimator_) {
+    k_x_ = 0.005;
+    k_y_ = 0.005;
+    k_yaw_ = 0.005;
+}
+
+void FeetEndCtrl::init() {
+    t_stance_ = ctrl_component_.wave_generator_.get_t_stance();
+    t_swing_ = ctrl_component_.wave_generator_.get_t_swing();
+
+    Vec34 feet_pos_body = estimator_.getFeetPos2Body();
+    for (int i(0); i < 4; ++i) {
+        feet_radius_(i) =
+                sqrt(pow(feet_pos_body(0, i), 2) + pow(feet_pos_body(1, i), 2));
+        feet_init_angle_(i) = atan2(feet_pos_body(1, i), feet_pos_body(0, i));
+    }
+}
+
+Vec3 FeetEndCtrl::calcFootPos(const int index, Vec2 vxy_goal_global, const double d_yaw_global, const double phase) {
+    Vec3 body_vel_global = estimator_.getVelocity();
+    Vec3 next_step;
+
+    next_step(0) = body_vel_global(0) * (1 - phase) * t_swing_ +
+                   body_vel_global(0) * t_stance_ / 2 +
+                   k_x_ * (body_vel_global(0) - vxy_goal_global(0));
+    next_step(1) = body_vel_global(1) * (1 - phase) * t_swing_ +
+                   body_vel_global(1) * t_stance_ / 2 +
+                   k_y_ * (body_vel_global(1) - vxy_goal_global(1));
+    next_step(2) = 0;
+
+    const double yaw = estimator_.getYaw();
+    const double d_yaw = estimator_.getDYaw();
+    const double next_yaw = d_yaw * (1 - phase) * t_swing_ + d_yaw * t_stance_ / 2 +
+                            k_yaw_ * (d_yaw_global - d_yaw);
+
+
+    next_step(0) +=
+            feet_radius_(index) * cos(yaw + feet_init_angle_(index) + next_yaw);
+    next_step(1) +=
+            feet_radius_(index) * sin(yaw + feet_init_angle_(index) + next_yaw);
+
+    Vec3 foot_pos = estimator_.getPosition() + next_step;
+    foot_pos(2) = 0.0;
+
+    return foot_pos;
+}
diff --git a/controllers/unitree_guide_controller/src/gait/FootEndCtrl.cpp b/controllers/unitree_guide_controller/src/gait/FootEndCtrl.cpp
deleted file mode 100644
index c994d4a..0000000
--- a/controllers/unitree_guide_controller/src/gait/FootEndCtrl.cpp
+++ /dev/null
@@ -1,14 +0,0 @@
-//
-// Created by biao on 24-9-18.
-//
-
-#include "unitree_guide_controller/gait/FootEndCtrl.h"
-
-FootEndCtrl::FootEndCtrl() {
-    k_x_ = 0.005;
-    k_y_ = 0.005;
-    k_yaw_ = 0.005;
-}
-
-Vec3 FootEndCtrl::calcFootPos(int index, Vec2 vxy_goal_global, double d_yaw_global, double phase) {
-}
diff --git a/controllers/unitree_guide_controller/src/gait/GaitGenerator.cpp b/controllers/unitree_guide_controller/src/gait/GaitGenerator.cpp
index 53ee5fa..6b3ec77 100644
--- a/controllers/unitree_guide_controller/src/gait/GaitGenerator.cpp
+++ b/controllers/unitree_guide_controller/src/gait/GaitGenerator.cpp
@@ -3,3 +3,92 @@
 //
 
 #include "unitree_guide_controller/gait/GaitGenerator.h"
+
+#include <utility>
+
+#include "unitree_guide_controller/control/CtrlComponent.h"
+
+GaitGenerator::GaitGenerator(CtrlComponent &ctrl_component) : estimator_(ctrl_component.estimator_),
+                                                              feet_end_ctrl_(ctrl_component),
+                                                              wave_generator_(ctrl_component.wave_generator_) {
+    first_run_ = true;
+}
+
+void GaitGenerator::setGait(Vec2 vxy_goal_global, const double d_yaw_goal, const double gait_height) {
+    vxy_goal_ = std::move(vxy_goal_global);
+    d_yaw_goal_ = d_yaw_goal;
+    gait_height_ = gait_height;
+}
+
+void GaitGenerator::run(Vec34 &feet_pos, Vec34 &feet_vel) {
+    if (first_run_) {
+        start_p_ = estimator_.getFeetPos();
+        first_run_ = false;
+    }
+
+    for (int i = 0; i < 4; i++) {
+        if (wave_generator_.contact_(i) == 1) {
+            // foot contact the ground
+            if (wave_generator_.phase_(i) < 0.5) {
+                start_p_.col(i) = estimator_.getFootPos(i);
+            }
+            feet_pos.col(i) = start_p_.col(i);
+            feet_vel.col(i).setZero();
+        } else {
+            // foot not contact, swing
+            end_p_.col(i) = feet_end_ctrl_.calcFootPos(i, vxy_goal_, d_yaw_goal_, wave_generator_.phase_(i));
+            feet_pos.col(i) = getFootPos(i);
+            feet_vel.col(i) = getFootVel(i);
+        }
+    }
+}
+
+void GaitGenerator::restart() {
+    first_run_ = true;
+    vxy_goal_.setZero();
+    feet_end_ctrl_.init();
+}
+
+Vec3 GaitGenerator::getFootPos(const int i) {
+    Vec3 foot_pos;
+
+    foot_pos(0) =
+            cycloidXYPosition(start_p_.col(i)(0), end_p_.col(i)(0), wave_generator_.phase_(i));
+    foot_pos(1) =
+            cycloidXYPosition(start_p_.col(i)(1), end_p_.col(i)(1), wave_generator_.phase_(i));
+    foot_pos(2) = cycloidZPosition(start_p_.col(i)(2), gait_height_, wave_generator_.phase_(i));
+
+    return foot_pos;
+}
+
+Vec3 GaitGenerator::getFootVel(const int i) {
+    Vec3 foot_vel;
+
+    foot_vel(0) =
+            cycloidXYVelocity(start_p_.col(i)(0), end_p_.col(i)(0), wave_generator_.phase_(i));
+    foot_vel(1) =
+            cycloidXYVelocity(start_p_.col(i)(1), end_p_.col(i)(1), wave_generator_.phase_(i));
+    foot_vel(2) = cycloidZVelocity(gait_height_, wave_generator_.phase_(i));
+
+    return foot_vel;
+}
+
+double GaitGenerator::cycloidXYPosition(const double startXY, const double endXY, const double phase) {
+    const double phase_pi = 2 * M_PI * phase;
+    return (endXY - startXY) * (phase_pi - sin(phase_pi)) / (2 * M_PI) + startXY;
+}
+
+double GaitGenerator::cycloidZPosition(const double startZ, const double height, const double phase) {
+    const double phase_pi = 2 * M_PI * phase;
+    return height * (1 - cos(phase_pi)) / 2 + startZ;
+}
+
+double GaitGenerator::cycloidXYVelocity(const double startXY, const double endXY, const double phase) const {
+    const double phase_pi = 2 * M_PI * phase;
+    return (endXY - startXY) * (1 - cos(phase_pi)) / wave_generator_.get_t_swing();
+}
+
+double GaitGenerator::cycloidZVelocity(const double height, const double phase) const {
+    const double phase_pi = 2 * M_PI * phase;
+    return height * M_PI * sin(phase_pi) / wave_generator_.get_t_swing();
+}
diff --git a/controllers/unitree_guide_controller/src/gait/WaveGenerator.cpp b/controllers/unitree_guide_controller/src/gait/WaveGenerator.cpp
index d0b4d51..9c7423e 100644
--- a/controllers/unitree_guide_controller/src/gait/WaveGenerator.cpp
+++ b/controllers/unitree_guide_controller/src/gait/WaveGenerator.cpp
@@ -12,6 +12,7 @@ WaveGenerator::WaveGenerator() {
     phase_past_ << 0.5, 0.5, 0.5, 0.5;
     contact_past_.setZero();
     status_past_ = WaveStatus::SWING_ALL;
+    status_ = WaveStatus::SWING_ALL;
 }
 
 void WaveGenerator::init(const double period, const double st_ratio, const Vec4 &bias) {
@@ -35,18 +36,18 @@ void WaveGenerator::init(const double period, const double st_ratio, const Vec4
     start_t_ = getSystemTime();
 }
 
-auto WaveGenerator::calculate(Vec4 &phase_result, VecInt4 &contact_result, const WaveStatus status) -> void {
-    calcWave(phase_, contact_, status);
+auto WaveGenerator::update() -> void {
+    calcWave(phase_, contact_, status_);
 
-    if (status != status_past_) {
+    if (status_ != status_past_) {
         if (switch_status_.sum() == 0) {
             switch_status_.setOnes();
         }
         calcWave(phase_past_, contact_past_, status_past_);
 
-        if (status == WaveStatus::STANCE_ALL && status_past_ == WaveStatus::SWING_ALL) {
+        if (status_ == WaveStatus::STANCE_ALL && status_past_ == WaveStatus::SWING_ALL) {
             contact_past_.setOnes();
-        } else if (status == WaveStatus::SWING_ALL && status_past_ == WaveStatus::STANCE_ALL) {
+        } else if (status_ == WaveStatus::SWING_ALL && status_past_ == WaveStatus::STANCE_ALL) {
             contact_past_.setZero();
         }
     }
@@ -61,12 +62,9 @@ auto WaveGenerator::calculate(Vec4 &phase_result, VecInt4 &contact_result, const
             }
         }
         if (switch_status_.sum() == 0) {
-            status_past_ = status;
+            status_past_ = status_;
         }
     }
-
-    phase_result = phase_;
-    contact_result = contact_;
 }
 
 void WaveGenerator::calcWave(Vec4 &phase, VecInt4 &contact, const WaveStatus status) {
diff --git a/controllers/unitree_guide_controller/src/robot/QuadrupedRobot.cpp b/controllers/unitree_guide_controller/src/robot/QuadrupedRobot.cpp
index f1f5f52..a18336a 100644
--- a/controllers/unitree_guide_controller/src/robot/QuadrupedRobot.cpp
+++ b/controllers/unitree_guide_controller/src/robot/QuadrupedRobot.cpp
@@ -42,6 +42,26 @@ std::vector<KDL::JntArray> QuadrupedRobot::getQ(const std::vector<KDL::Frame> &p
     return result;
 }
 
+Vec12 QuadrupedRobot::getQ(const Vec34 &vecP) const {
+    Vec12 q;
+    for (int i(0); i < 4; ++i) {
+        KDL::Frame frame;
+        frame.p = KDL::Vector(vecP.col(i)[0], vecP.col(i)[1], vecP.col(i)[2]);
+        q.segment(3 * i, 3) = robot_legs_[i]->calcQ(frame, current_joint_pos_[i]).data;
+    }
+    return q;
+}
+
+Vec12 QuadrupedRobot::getQd(const std::vector<KDL::Frame> &pos, const Vec34 &vel) {
+    Vec12 qd;
+    std::vector<KDL::JntArray> q = getQ(pos);
+    for (int i(0); i < 4; ++i) {
+        Mat3 jacobian = robot_legs_[i]->calcJaco(q[i]).data.topRows(3);
+        qd.segment(3 * i, 3) = jacobian.inverse() * vel.col(i);
+    }
+    return qd;
+}
+
 std::vector<KDL::Frame> QuadrupedRobot::getFeet2BPositions() const {
     std::vector<KDL::Frame> result;
     result.resize(4);