diff --git a/controllers/unitree_guide_controller/CMakeLists.txt b/controllers/unitree_guide_controller/CMakeLists.txt
index 3b770ed..57c4a93 100644
--- a/controllers/unitree_guide_controller/CMakeLists.txt
+++ b/controllers/unitree_guide_controller/CMakeLists.txt
@@ -45,6 +45,10 @@ add_library(${PROJECT_NAME} SHARED
         src/quadProgpp/Array.cc
         src/quadProgpp/QuadProg++.cc
 
+        src/gait/WaveGenerator.cpp
+        src/gait/FootEndCtrl.cpp
+        src/gait/GaitGenerator.cpp
+
 )
 target_include_directories(${PROJECT_NAME}
         PUBLIC
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 32d011b..7017525 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,12 +22,15 @@ public:
 private:
     void calcTorque();
 
-    KDL::Vector pcd_;
-    KDL::Vector pcdInit_;
+    Estimator& estimator_;
+    QuadrupedRobot& robot_model_;
+    BalanceCtrl& balance_ctrl_;
+
+    Vec3 pcd_, pcd_init_;
     RotMat Rd_;
     KDL::Rotation init_rotation_;
 
-    KDL::Vector pose_body_, vel_body_;
+    Vec3 pose_body_, vel_body_;
 
     double kp_w_;
     Mat3 Kp_p_, Kd_p_, Kd_w_;
diff --git a/controllers/unitree_guide_controller/include/unitree_guide_controller/common/enumClass.h b/controllers/unitree_guide_controller/include/unitree_guide_controller/common/enumClass.h
index 810859f..ecddf66 100644
--- a/controllers/unitree_guide_controller/include/unitree_guide_controller/common/enumClass.h
+++ b/controllers/unitree_guide_controller/include/unitree_guide_controller/common/enumClass.h
@@ -23,10 +23,16 @@ enum class FSMMode {
     CHANGE
 };
 
-enum class FrameType{
+enum class FrameType {
     BODY,
     HIP,
     GLOBAL
 };
 
+enum class WaveStatus {
+    STANCE_ALL,
+    SWING_ALL,
+    WAVE_ALL
+};
+
 #endif //ENUMCLASS_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 820b6a8..6750019 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
@@ -5,8 +5,6 @@
 #ifndef BALANCECTRL_H
 #define BALANCECTRL_H
 
-#include <kdl/frames.hpp>
-
 #include "unitree_guide_controller/common/mathTypes.h"
 class QuadrupedRobot;
 
@@ -18,11 +16,20 @@ public:
 
     void init(const QuadrupedRobot &robot);
 
-    Vec34 calF(const Vec3 &ddPcd, const Vec3 &dWbd, const RotMat &rotM,
-               const std::vector<KDL::Vector> &feetPos2B, const std::vector<int> &contact);
+    /**
+     * Calculate the desired feet end force
+     * @param ddPcd desired body acceleration
+     * @param dWbd desired body angular acceleration
+     * @param rot_matrix current body rotation matrix
+     * @param feet_pos_2_body feet positions to body under world frame
+     * @param contact feet contact
+     * @return
+     */
+    Vec34 calF(const Vec3 &ddPcd, const Vec3 &dWbd, const RotMat &rot_matrix,
+               const Vec34 &feet_pos_2_body, const std::vector<int> &contact);
 
 private:
-    void calMatrixA(const std::vector<KDL::Vector> &feetPos2B, const RotMat &rotM);
+    void calMatrixA(const Vec34 &feet_pos_2_body, const RotMat &rotM);
 
     void calVectorBd(const Vec3 &ddPcd, const Vec3 &dWbd, const RotMat &rotM);
 
@@ -34,7 +41,7 @@ private:
     Mat6 S_;
     Mat3 Ib_;
     Vec6 bd_;
-    Vec3 _g, _pcb;
+    Vec3 g_, pcb_;
     Vec12 F_, F_prev_, g0T_;
     double mass_, alpha_, beta_, friction_ratio_;
     Eigen::MatrixXd CE_, CI_;
diff --git a/controllers/unitree_guide_controller/include/unitree_guide_controller/control/CtrlComponent.h b/controllers/unitree_guide_controller/include/unitree_guide_controller/control/CtrlComponent.h
index d393fde..f1023f7 100644
--- a/controllers/unitree_guide_controller/include/unitree_guide_controller/control/CtrlComponent.h
+++ b/controllers/unitree_guide_controller/include/unitree_guide_controller/control/CtrlComponent.h
@@ -9,6 +9,7 @@
 #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"
@@ -50,8 +51,11 @@ struct CtrlComponent {
     BalanceCtrl default_balance_ctrl_;
     std::reference_wrapper<BalanceCtrl> balance_ctrl_;
 
+    WaveGenerator default_wave_generator_;
+    std::reference_wrapper<WaveGenerator> wave_generator_;
+
     CtrlComponent() : control_inputs_(default_inputs_), robot_model_(default_robot_model_),
-                      estimator_(default_estimator_), balance_ctrl_(default_balance_ctrl_) {
+                      estimator_(default_estimator_), balance_ctrl_(default_balance_ctrl_), wave_generator_(default_wave_generator_) {
     }
 
     void clear() {
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 8771e8a..3a3d197 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
@@ -4,10 +4,9 @@
 
 #ifndef ESTIMATOR_H
 #define ESTIMATOR_H
-#include <iostream>
 #include <memory>
-#include <eigen3/Eigen/Dense>
 #include <kdl/frames.hpp>
+#include <unitree_guide_controller/common/mathTypes.h>
 
 #include "LowPassFilter.h"
 
@@ -23,16 +22,16 @@ public:
      * Get the estimated robot central position
      * @return robot central position
      */
-    KDL::Vector getPosition() {
-        return {x_hat_(0), x_hat_(1), x_hat_(2)};
+    Vec3 getPosition() {
+        return x_hat_.segment(0, 3);
     }
 
     /**
      * Get the estimated robot central velocity
      * @return robot central velocity
      */
-    KDL::Vector getVelocity() {
-        return {x_hat_(3), x_hat_(4), x_hat_(5)};
+    Vec3 getVelocity() {
+        return x_hat_.segment(3, 3);
     }
 
     /**
@@ -40,33 +39,19 @@ public:
      * @param index leg index
      * @return foot position in world frame
      */
-    KDL::Vector getFootPos(const int index) {
-        return getPosition() + rotation_ * foot_poses_[index].p;
-    }
-
-    /**
-     * Get all estimated foot positions in world frame
-     * @return all foot positions in world frame
-     */
-    std::vector<KDL::Vector> getFootPos() {
-        std::vector<KDL::Vector> foot_pos;
-        foot_pos.resize(4);
-        for (int i = 0; i < 4; i++) {
-            foot_pos[i] = getFootPos(i);
-        }
-        return foot_pos;
+    Vec3 getFootPos(const int index) {
+        return getPosition() + Vec3((rotation_ * foot_poses_[index].p).data);
     }
 
     /**
      * Get the estimated foot position in body frame
      * @return
      */
-    std::vector<KDL::Vector> getFootPos2Body() {
-        std::vector<KDL::Vector> foot_pos;
-        foot_pos.resize(4);
-        const KDL::Vector body_pos = getPosition();
+    Vec34 getFootPos2Body() {
+        Vec34 foot_pos;
+        const Vec3 body_pos = getPosition();
         for (int i = 0; i < 4; i++) {
-            foot_pos[i] = getFootPos(i) - body_pos;
+            foot_pos.col(i) = getFootPos(i) - body_pos;
         }
         return foot_pos;
     }
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
new file mode 100644
index 0000000..1d6abca
--- /dev/null
+++ b/controllers/unitree_guide_controller/include/unitree_guide_controller/gait/FootEndCtrl.h
@@ -0,0 +1,24 @@
+//
+// 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
new file mode 100644
index 0000000..a5a2823
--- /dev/null
+++ b/controllers/unitree_guide_controller/include/unitree_guide_controller/gait/GaitGenerator.h
@@ -0,0 +1,15 @@
+//
+// Created by biao on 24-9-18.
+//
+
+
+#ifndef GAITGENERATOR_H
+#define GAITGENERATOR_H
+
+
+class Gaitgenerator {
+
+};
+
+
+#endif //GAITGENERATOR_H
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
new file mode 100644
index 0000000..7d1e58d
--- /dev/null
+++ b/controllers/unitree_guide_controller/include/unitree_guide_controller/gait/WaveGenerator.h
@@ -0,0 +1,55 @@
+//
+// Created by biao on 24-9-18.
+//
+
+
+#ifndef WAVEGENERATOR_H
+#define WAVEGENERATOR_H
+#include <sys/time.h>
+#include <unitree_guide_controller/common/enumClass.h>
+#include <unitree_guide_controller/common/mathTypes.h>
+
+inline long long getSystemTime() {
+    timeval t{};
+    gettimeofday(&t, nullptr);
+    return 1000000 * t.tv_sec + t.tv_usec;
+}
+
+class WaveGenerator {
+public:
+    WaveGenerator();
+
+    ~WaveGenerator() = default;
+
+    void init(double period, double st_ratio, const Vec4 &bias);
+
+    void calculate(Vec4 &phase_result, VecInt4 &contact_result, WaveStatus status);
+
+    [[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_; }
+
+private:
+    /**
+     * Update phase, contact and status based on current time.
+     * @param phase foot phase
+     * @param contact foot contact
+     * @param status Wave Status
+     */
+    void calcWave(Vec4 &phase, VecInt4 &contact, WaveStatus status);
+
+    double period_{};
+    double st_ratio_{}; // stance phase ratio
+    Vec4 bias_;
+
+    Vec4 normal_t_; // normalize time [0,1)
+    Vec4 phase_, phase_past_; // foot phase
+    VecInt4 contact_, contact_past_; // foot contact
+    VecInt4 switch_status_;
+    WaveStatus status_past_;
+
+    long start_t_{};
+};
+
+
+#endif //WAVEGENERATOR_H
diff --git a/controllers/unitree_guide_controller/src/FSM/StateBalanceTest.cpp b/controllers/unitree_guide_controller/src/FSM/StateBalanceTest.cpp
index 3523949..251e9f1 100644
--- a/controllers/unitree_guide_controller/src/FSM/StateBalanceTest.cpp
+++ b/controllers/unitree_guide_controller/src/FSM/StateBalanceTest.cpp
@@ -7,7 +7,10 @@
 #include <unitree_guide_controller/common/mathTools.h>
 
 StateBalanceTest::StateBalanceTest(CtrlComponent ctrlComp) : FSMState(FSMStateName::BALANCETEST, "balance test",
-                                                                      std::move(ctrlComp)) {
+                                                                      std::move(ctrlComp)),
+                                                             estimator_(ctrlComp.estimator_.get()),
+                                                             robot_model_(ctrlComp.robot_model_.get()),
+                                                             balance_ctrl_(ctrlComp.balance_ctrl_.get()) {
     _xMax = 0.05;
     _xMin = -_xMax;
     _yMax = 0.05;
@@ -25,20 +28,20 @@ StateBalanceTest::StateBalanceTest(CtrlComponent ctrlComp) : FSMState(FSMStateNa
 }
 
 void StateBalanceTest::enter() {
-    pcdInit_ = ctrl_comp_.estimator_.get().getPosition();
-    pcd_ = pcdInit_;
-    init_rotation_ = ctrl_comp_.estimator_.get().getRotation();
+    pcd_init_ = estimator_.getPosition();
+    pcd_ = pcd_init_;
+    init_rotation_ = estimator_.getRotation();
 }
 
 void StateBalanceTest::run() {
-    pcd_(0) = pcdInit_(0) + invNormalize(ctrl_comp_.control_inputs_.get().ly, _xMin, _xMax);
-    pcd_(1) = pcdInit_(1) - invNormalize(ctrl_comp_.control_inputs_.get().lx, _yMin, _yMax);
-    pcd_(2) = pcdInit_(2) + invNormalize(ctrl_comp_.control_inputs_.get().ry, _zMin, _zMax);
+    pcd_(0) = pcd_init_(0) + invNormalize(ctrl_comp_.control_inputs_.get().ly, _xMin, _xMax);
+    pcd_(1) = pcd_init_(1) - invNormalize(ctrl_comp_.control_inputs_.get().lx, _yMin, _yMax);
+    pcd_(2) = pcd_init_(2) + invNormalize(ctrl_comp_.control_inputs_.get().ry, _zMin, _zMax);
     const float yaw = invNormalize(ctrl_comp_.control_inputs_.get().rx, _yawMin, _yawMax);
     Rd_ = Mat3((KDL::Rotation::RPY(0, 0, yaw).Inverse() * init_rotation_).data);
 
-    pose_body_ = ctrl_comp_.estimator_.get().getPosition();
-    vel_body_ = ctrl_comp_.estimator_.get().getVelocity();
+    pose_body_ = estimator_.getPosition();
+    vel_body_ = estimator_.getVelocity();
 
     for (int i = 0; i < 12; i++) {
         ctrl_comp_.joint_kp_command_interface_[i].get().set_value(0.8);
@@ -63,25 +66,24 @@ FSMStateName StateBalanceTest::checkChange() {
 }
 
 void StateBalanceTest::calcTorque() {
-    const auto B2G_Rotation = Eigen::Matrix3d(ctrl_comp_.estimator_.get().getRotation().data);
+    const auto B2G_Rotation = Eigen::Matrix3d(estimator_.getRotation().data);
     const RotMat G2B_Rotation = B2G_Rotation.transpose();
 
     // expected body acceleration
-    dd_pcd_ = Kp_p_ * Vec3((pcd_ - pose_body_).data) + Kd_p_ * Vec3((-vel_body_).data);
+    dd_pcd_ = Kp_p_ * (pcd_ - pose_body_) + Kd_p_ * -vel_body_;
 
     // expected body angular acceleration
-    d_wbd_ = kp_w_ * rotMatToExp(Rd_ * G2B_Rotation) +
-             Kd_w_ * (Vec3(0, 0, 0) - Vec3((-ctrl_comp_.estimator_.get().getGlobalGyro()).data));
+    d_wbd_ = -kp_w_ * rotMatToExp(Rd_ * G2B_Rotation) +
+             Kd_w_ * Vec3((-estimator_.getGlobalGyro()).data);
 
     // calculate foot force
     const std::vector contact(4, 1);
-    const Vec34 foot_force = G2B_Rotation * ctrl_comp_.balance_ctrl_.get().calF(dd_pcd_, -d_wbd_, B2G_Rotation,
-                                 ctrl_comp_.estimator_.get().
-                                 getFootPos2Body(), contact);
+    const Vec34 foot_force = G2B_Rotation * balance_ctrl_.calF(dd_pcd_, d_wbd_, B2G_Rotation,
+                                                               estimator_.getFootPos2Body(), contact);
 
-    std::vector<KDL::JntArray> current_joints = ctrl_comp_.robot_model_.get().current_joint_pos_;
+    std::vector<KDL::JntArray> current_joints = robot_model_.current_joint_pos_;
     for (int i = 0; i < 4; i++) {
-        KDL::JntArray torque = ctrl_comp_.robot_model_.get().getTorque(-foot_force.col(i), i);
+        KDL::JntArray torque = robot_model_.getTorque(-foot_force.col(i), i);
         for (int j = 0; j < 3; j++) {
             ctrl_comp_.joint_effort_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));
diff --git a/controllers/unitree_guide_controller/src/control/BalanceCtrl.cpp b/controllers/unitree_guide_controller/src/control/BalanceCtrl.cpp
index 7dbb5cd..0fe51b1 100644
--- a/controllers/unitree_guide_controller/src/control/BalanceCtrl.cpp
+++ b/controllers/unitree_guide_controller/src/control/BalanceCtrl.cpp
@@ -13,7 +13,7 @@ BalanceCtrl::BalanceCtrl() {
     mass_ = 15;
     alpha_ = 0.001;
     beta_ = 0.1;
-    _g << 0, 0, -9.81;
+    g_ << 0, 0, -9.81;
     friction_ratio_ = 0.4;
     friction_mat_ << 1, 0, friction_ratio_, -1, 0, friction_ratio_, 0, 1, friction_ratio_, 0, -1,
             friction_ratio_, 0, 0, 1;
@@ -21,7 +21,7 @@ BalanceCtrl::BalanceCtrl() {
 
 void BalanceCtrl::init(const QuadrupedRobot &robot) {
     mass_ = robot.mass_;
-    _pcb = Vec3(0.0, 0.0, 0.0);
+    pcb_ = Vec3(0.0, 0.0, 0.0);
     Ib_ = Vec3(0.0792, 0.2085, 0.2265).asDiagonal();
 
     Vec6 s;
@@ -37,10 +37,10 @@ void BalanceCtrl::init(const QuadrupedRobot &robot) {
     F_prev_.setZero();
 }
 
-Vec34 BalanceCtrl::calF(const Vec3 &ddPcd, const Vec3 &dWbd, const RotMat &rotM,
-                        const std::vector<KDL::Vector> &feetPos2B, const std::vector<int> &contact) {
-    calMatrixA(feetPos2B, rotM);
-    calVectorBd(ddPcd, dWbd, rotM);
+Vec34 BalanceCtrl::calF(const Vec3 &ddPcd, const Vec3 &dWbd, const RotMat &rot_matrix,
+                        const Vec34 &feet_pos_2_body, const std::vector<int> &contact) {
+    calMatrixA(feet_pos_2_body, rot_matrix);
+    calVectorBd(ddPcd, dWbd, rot_matrix);
     calConstraints(contact);
 
     G_ = A_.transpose() * S_ * A_ + alpha_ * W_ + beta_ * U_;
@@ -52,15 +52,15 @@ Vec34 BalanceCtrl::calF(const Vec3 &ddPcd, const Vec3 &dWbd, const RotMat &rotM,
     return vec12ToVec34(F_);
 }
 
-void BalanceCtrl::calMatrixA(const std::vector<KDL::Vector> &feetPos2B, const RotMat &rotM) {
+void BalanceCtrl::calMatrixA(const Vec34 &feet_pos_2_body, const RotMat &rotM) {
     for (int i = 0; i < 4; ++i) {
         A_.block(0, 3 * i, 3, 3) = I3;
-        A_.block(3, 3 * i, 3, 3) = skew(Vec3(feetPos2B[i].data) - rotM * _pcb);
+        A_.block(3, 3 * i, 3, 3) = skew(Vec3(feet_pos_2_body.col(i)) - rotM * pcb_);
     }
 }
 
 void BalanceCtrl::calVectorBd(const Vec3 &ddPcd, const Vec3 &dWbd, const RotMat &rotM) {
-    bd_.head(3) = mass_ * (ddPcd - _g);
+    bd_.head(3) = mass_ * (ddPcd - g_);
     bd_.tail(3) = rotM * Ib_ * rotM.transpose() * dWbd;
 }
 
diff --git a/controllers/unitree_guide_controller/src/gait/FootEndCtrl.cpp b/controllers/unitree_guide_controller/src/gait/FootEndCtrl.cpp
new file mode 100644
index 0000000..c994d4a
--- /dev/null
+++ b/controllers/unitree_guide_controller/src/gait/FootEndCtrl.cpp
@@ -0,0 +1,14 @@
+//
+// 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
new file mode 100644
index 0000000..53ee5fa
--- /dev/null
+++ b/controllers/unitree_guide_controller/src/gait/GaitGenerator.cpp
@@ -0,0 +1,5 @@
+//
+// Created by biao on 24-9-18.
+//
+
+#include "unitree_guide_controller/gait/GaitGenerator.h"
diff --git a/controllers/unitree_guide_controller/src/gait/WaveGenerator.cpp b/controllers/unitree_guide_controller/src/gait/WaveGenerator.cpp
new file mode 100644
index 0000000..d0b4d51
--- /dev/null
+++ b/controllers/unitree_guide_controller/src/gait/WaveGenerator.cpp
@@ -0,0 +1,100 @@
+//
+// Created by biao on 24-9-18.
+//
+
+#include <utility>
+
+#include "unitree_guide_controller/gait/WaveGenerator.h"
+
+#include <iostream>
+
+WaveGenerator::WaveGenerator() {
+    phase_past_ << 0.5, 0.5, 0.5, 0.5;
+    contact_past_.setZero();
+    status_past_ = WaveStatus::SWING_ALL;
+}
+
+void WaveGenerator::init(const double period, const double st_ratio, const Vec4 &bias) {
+    period_ = period;
+    st_ratio_ = st_ratio;
+    bias_ = bias;
+
+    if (st_ratio_ >= 1 || st_ratio_ <= 0) {
+        std::cerr << "[ERROR] The stancePhaseRatio of WaveGenerator should between (0, 1)"
+                << std::endl;
+        exit(-1);
+    }
+
+    for (int i(0); i < bias_.rows(); i++) {
+        if (bias_(i) > 1 || bias_(i) < 0) {
+            std::cerr << "[ERROR] The bias of WaveGenerator should between [0, 1]"
+                    << std::endl;
+            exit(-1);
+        }
+    }
+    start_t_ = getSystemTime();
+}
+
+auto WaveGenerator::calculate(Vec4 &phase_result, VecInt4 &contact_result, const WaveStatus status) -> void {
+    calcWave(phase_, contact_, status);
+
+    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) {
+            contact_past_.setOnes();
+        } else if (status == WaveStatus::SWING_ALL && status_past_ == WaveStatus::STANCE_ALL) {
+            contact_past_.setZero();
+        }
+    }
+
+    if (switch_status_.sum() != 0) {
+        for (int i(0); i < 4; ++i) {
+            if (contact_(i) == contact_past_(i)) {
+                switch_status_(i) = 0;
+            } else {
+                contact_(i) = contact_past_(i);
+                phase_(i) = phase_past_(i);
+            }
+        }
+        if (switch_status_.sum() == 0) {
+            status_past_ = status;
+        }
+    }
+
+    phase_result = phase_;
+    contact_result = contact_;
+}
+
+void WaveGenerator::calcWave(Vec4 &phase, VecInt4 &contact, const WaveStatus status) {
+    switch (status) {
+        case WaveStatus::WAVE_ALL: {
+            const double past_t = static_cast<double>(getSystemTime() - start_t_) * 1e-6;
+            for (int i(0); i < 4; ++i) {
+                normal_t_(i) =
+                        fmod(past_t + period_ - period_ * bias_(i), period_) / period_;
+                if (normal_t_(i) < st_ratio_) {
+                    contact(i) = 1;
+                    phase(i) = normal_t_(i) / st_ratio_;
+                } else {
+                    contact(i) = 0;
+                    phase(i) = (normal_t_(i) - st_ratio_) / (1 - st_ratio_);
+                }
+            }
+            break;
+        }
+        case WaveStatus::SWING_ALL: {
+            contact.setZero();
+            phase << 0.5, 0.5, 0.5, 0.5;
+            break;
+        }
+        case WaveStatus::STANCE_ALL: {
+            contact.setOnes();
+            phase << 0.5, 0.5, 0.5, 0.5;
+            break;
+        }
+    }
+}