diff --git a/include/suiryoku/locomotion/model/robot.hpp b/include/suiryoku/locomotion/model/robot.hpp
index 86c4517..d710fdf 100644
--- a/include/suiryoku/locomotion/model/robot.hpp
+++ b/include/suiryoku/locomotion/model/robot.hpp
@@ -51,6 +51,8 @@ class Robot
   double x_amplitude;
   double y_amplitude;
   double a_amplitude;
+  
+  keisan::Vector<3> rpy;
 
   // member for setting
   double x_speed;
diff --git a/include/suiryoku/locomotion/process/locomotion.hpp b/include/suiryoku/locomotion/process/locomotion.hpp
index e12dd0a..7e74b38 100755
--- a/include/suiryoku/locomotion/process/locomotion.hpp
+++ b/include/suiryoku/locomotion/process/locomotion.hpp
@@ -42,42 +42,45 @@ class Locomotion
   bool walk_in_position();
   bool walk_in_position_until_stop();
 
-  void move_backward(const keisan::Angle<double> & direction);
-  bool move_backward_to(const keisan::Point2 & target);
+  void move_backward(const keisan::Angle<double> & direction, float delta_sec);
+  bool move_backward_to(const keisan::Point2 & target, float delta_sec);
 
-  void move_forward(const keisan::Angle<double> & direction);
-  bool move_forward_to(const keisan::Point2 & target);
+  void move_forward(const keisan::Angle<double> & direction, float delta_sec);
+  bool move_forward_to(const keisan::Point2 & target, float delta_sec);
 
-  bool rotate_to_target(const keisan::Angle<double> & direction);
-  bool rotate_to(const keisan::Angle<double> & direction, bool a_move_only);
+  bool rotate_to_target(const keisan::Angle<double> & direction, float delta_sec);
+  bool rotate_to(const keisan::Angle<double> & direction, bool a_move_only, float delta_sec);
 
-  bool move_follow_head();
-  bool move_follow_head(const keisan::Angle<double> & min_tilt);
+  bool move_follow_head(float delta_sec);
+  bool move_follow_head(const keisan::Angle<double> & min_tilt, float delta_sec);
 
-  bool move_skew(const keisan::Angle<double> & direction);
-  bool move_skew(const keisan::Angle<double> & direction, bool skew_left);
+  bool move_skew(const keisan::Angle<double> & direction, float delta_sec);
+  bool move_skew(const keisan::Angle<double> & direction, bool skew_left, float delta_sec);
 
-  bool dribble(const keisan::Angle<double> & direction);
-  bool pivot(const keisan::Angle<double> & direction);
-  bool pivot_new(const keisan::Angle<double> & direction);
+  bool dribble(const keisan::Angle<double> & direction, float delta_sec);
+  bool pivot(const keisan::Angle<double> & direction, float delta_sec);
+  bool pivot_new(const keisan::Angle<double> & direction, float delta_sec);
 
   bool position_until(
     const keisan::Angle<double> & target_pan,
     const keisan::Angle<double> & target_tilt,
-    const keisan::Angle<double> & direction);
-  bool position_left_kick(const keisan::Angle<double> & direction);
-  bool position_right_kick(const keisan::Angle<double> & direction);
+    const keisan::Angle<double> & direction,
+    float delta_sec);
+  bool position_left_kick(const keisan::Angle<double> & direction, float delta_sec);
+  bool position_right_kick(const keisan::Angle<double> & direction, float delta_sec);
   bool position_kick_custom_pan_tilt(const keisan::Angle<double> & direction, const keisan::Angle<double> & min_pan, 
                                      const keisan::Angle<double> & max_pan, const keisan::Angle<double> & min_tilt, 
-                                     const keisan::Angle<double> & max_tilt);
-  bool position_kick_general(const keisan::Angle<double> & direction);
-  bool position_kick_range_pan_tilt(const keisan::Angle<double> & direction, bool precise_kick, bool left_kick, bool is_positioning_center);
+                                     const keisan::Angle<double> & max_tilt, float delta_sec);
+  bool position_kick_general(const keisan::Angle<double> & direction, float delta_sec);
+  bool position_kick_range_pan_tilt(const keisan::Angle<double> & direction, bool precise_kick, bool left_kick, bool is_positioning_center, float delta_sec);
 
   bool is_time_to_follow();
   bool pivot_fulfilled();
   bool in_pan_kick_range();
   bool in_tilt_kick_range();
 
+  void speed_control(double x_speed, double y_speed, double a_speed, bool aim_on, float delta_sec);
+
   std::shared_ptr<Robot> get_robot() const;
   void update_move_amplitude(double x_amplitude, double y_amplitude);
 
@@ -161,6 +164,13 @@ class Locomotion
   double skew_pan_comp;
   double skew_delta_direction_comp;
 
+  double pitch_unstable_threshold;
+  double roll_unstable_threshold;
+  bool unstable_pitch;
+  bool unstable_roll;
+  float stable_time;
+  bool using_speed_control;
+
   keisan::Angle<double> left_kick_target_pan;
   keisan::Angle<double> left_kick_target_tilt;
 
diff --git a/src/suiryoku/locomotion/control/node/control_node.cpp b/src/suiryoku/locomotion/control/node/control_node.cpp
index 557481c..2471c21 100644
--- a/src/suiryoku/locomotion/control/node/control_node.cpp
+++ b/src/suiryoku/locomotion/control/node/control_node.cpp
@@ -97,7 +97,7 @@ void ControlNode::run_locomotion_callback(const RunLocomotion::SharedPtr message
             auto direction = keisan::make_degree(val.get<double>());
 
             process = [this, direction]() {
-                this->locomotion->move_backward(direction);
+                this->locomotion->move_backward(direction, 0.0);
 
                 return false;
               };
@@ -108,7 +108,7 @@ void ControlNode::run_locomotion_callback(const RunLocomotion::SharedPtr message
               val["x"].get<double>(), val["y"].get<double>());
 
             process = [this, target]() {
-                return this->locomotion->move_backward_to(target);
+                return this->locomotion->move_backward_to(target, 0.0);
               };
 
             break;
@@ -125,7 +125,7 @@ void ControlNode::run_locomotion_callback(const RunLocomotion::SharedPtr message
             auto direction = keisan::make_degree(val.get<double>());
 
             process = [this, direction]() {
-                this->locomotion->move_forward(direction);
+                this->locomotion->move_forward(direction, 0.0);
 
                 return false;
               };
@@ -136,7 +136,7 @@ void ControlNode::run_locomotion_callback(const RunLocomotion::SharedPtr message
               val["x"].get<double>(), val["y"].get<double>());
 
             process = [this, target]() {
-                return this->locomotion->move_forward_to(target);
+                return this->locomotion->move_forward_to(target, 0.0);
               };
           }
         }
@@ -158,7 +158,7 @@ void ControlNode::run_locomotion_callback(const RunLocomotion::SharedPtr message
         }
 
         process = [this, direction, a_move_only]() {
-            return this->locomotion->rotate_to(direction, a_move_only);
+            return this->locomotion->rotate_to(direction, a_move_only, 0.0);
           };
 
         break;
@@ -178,11 +178,11 @@ void ControlNode::run_locomotion_callback(const RunLocomotion::SharedPtr message
 
         if (is_default) {
           process = [this]() {
-              return this->locomotion->move_follow_head();
+              return this->locomotion->move_follow_head(0.0);
             };
         } else {
           process = [this, min_tilt]() {
-              return this->locomotion->move_follow_head(min_tilt);
+              return this->locomotion->move_follow_head(min_tilt, 0.0);
             };
         }
 
@@ -196,7 +196,7 @@ void ControlNode::run_locomotion_callback(const RunLocomotion::SharedPtr message
             auto direction = keisan::make_degree(val.get<double>());
 
             process = [this, direction]() {
-                return !this->locomotion->move_skew(direction);
+                return !this->locomotion->move_skew(direction, 0.0);
               };
           }
         }
@@ -211,7 +211,7 @@ void ControlNode::run_locomotion_callback(const RunLocomotion::SharedPtr message
             auto direction = keisan::make_degree(val.get<double>());
 
             process = [this, direction]() {
-                return !this->locomotion->dribble(direction);
+                return !this->locomotion->dribble(direction, 0.0);
               };
           }
         }
@@ -226,7 +226,7 @@ void ControlNode::run_locomotion_callback(const RunLocomotion::SharedPtr message
             auto direction = keisan::make_degree(val.get<double>());
 
             process = [this, direction]() {
-                return this->locomotion->pivot(direction);
+                return this->locomotion->pivot(direction, 0.0);
               };
           }
         }
@@ -257,19 +257,19 @@ void ControlNode::run_locomotion_callback(const RunLocomotion::SharedPtr message
 
         if (is_left_kick && is_right_kick) {
           process = [this, direction]() {
-              return this->locomotion->position_kick_general(direction);
+              return this->locomotion->position_kick_general(direction, 0.0);
             };
         } else if (is_left_kick) {
           process = [this, direction]() {
-              return this->locomotion->position_left_kick(direction);
+              return this->locomotion->position_left_kick(direction, 0.0);
             };
         } else if (is_right_kick) {
           process = [this, direction]() {
-              return this->locomotion->position_right_kick(direction);
+              return this->locomotion->position_right_kick(direction, 0.0);
             };
         } else {
           process = [this, target_pan, target_tilt, direction]() {
-              return this->locomotion->position_until(target_pan, target_tilt, direction);
+              return this->locomotion->position_until(target_pan, target_tilt, direction, 0.0); // TODO
             };
         }
 
diff --git a/src/suiryoku/locomotion/model/robot.cpp b/src/suiryoku/locomotion/model/robot.cpp
index 6e18c78..78ecb0a 100644
--- a/src/suiryoku/locomotion/model/robot.cpp
+++ b/src/suiryoku/locomotion/model/robot.cpp
@@ -33,7 +33,8 @@ Robot::Robot()
 : pan(0_deg), tilt(0_deg), pan_center(0_deg), tilt_center(0_deg), x_speed(0.0),
   y_speed(0.0), a_speed(0.0), aim_on(false), is_walking(false),
   orientation(0_deg), position(0.0, 0.0), x_amplitude(0.0), y_amplitude(0.0),
-  a_amplitude(0.0), is_calibrated(false)
+  a_amplitude(0.0), is_calibrated(false),
+  rpy(0.0, 0.0, 0.0)
 {
 }
 
diff --git a/src/suiryoku/locomotion/node/locomotion_node.cpp b/src/suiryoku/locomotion/node/locomotion_node.cpp
index ad21df9..5c5bc21 100644
--- a/src/suiryoku/locomotion/node/locomotion_node.cpp
+++ b/src/suiryoku/locomotion/node/locomotion_node.cpp
@@ -24,6 +24,7 @@
 #include "suiryoku/locomotion/node/locomotion_node.hpp"
 
 #include "aruku/walking/walking.hpp"
+#include "tachimawari/imu/imu.hpp"
 #include "kansei/measurement/measurement.hpp"
 #include "keisan/keisan.hpp"
 #include "nlohmann/json.hpp"
@@ -50,6 +51,7 @@ LocomotionNode::LocomotionNode(
     [this](const MeasurementStatus::SharedPtr message) {
       this->robot->is_calibrated = message->is_calibrated;
       this->robot->orientation = keisan::make_degree(message->orientation.yaw);
+      this->robot->rpy = keisan::Vector<3>(message->orientation.roll, message->orientation.pitch, message->orientation.yaw);
     });
 
   set_odometry_publisher = node->create_publisher<Point2>(
diff --git a/src/suiryoku/locomotion/process/locomotion.cpp b/src/suiryoku/locomotion/process/locomotion.cpp
index aaa40b2..4d07350 100755
--- a/src/suiryoku/locomotion/process/locomotion.cpp
+++ b/src/suiryoku/locomotion/process/locomotion.cpp
@@ -287,6 +287,22 @@ void Locomotion::set_config(const nlohmann::json & json)
     valid_config = false;
   }
 
+  nlohmann::json speed_control_section;
+  if (jitsuyo::assign_val(json, "speed_control", speed_control_section)) {
+    bool valid_section = true;
+
+    valid_section &= jitsuyo::assign_val(speed_control_section, "pitch_unstable_threshold", pitch_unstable_threshold);
+    valid_section &= jitsuyo::assign_val(speed_control_section, "roll_unstable_threshold", roll_unstable_threshold);
+    valid_section &= jitsuyo::assign_val(speed_control_section, "using_speed_control", using_speed_control);
+
+    if (!valid_section) {
+      std::cout << "Error found at section `speed_control`" << std::endl;
+      valid_config = false;
+    }
+  } else {
+    valid_config = false;
+  }
+
   if (!valid_config) {
     throw std::runtime_error("Failed to load config file `locomotion.json`");
   }
@@ -328,7 +344,7 @@ bool Locomotion::walk_in_position_until_stop()
   return !robot->is_walking;
 }
 
-void Locomotion::move_backward(const keisan::Angle<double> & direction)
+void Locomotion::move_backward(const keisan::Angle<double> & direction, float delta_sec)
 {
   auto delta_direction = (direction - robot->orientation).normalize().degree();
 
@@ -346,14 +362,10 @@ void Locomotion::move_backward(const keisan::Angle<double> & direction)
     x_speed = keisan::map(std::abs(delta_direction), 0.0, 15.0, backward_max_x, backward_min_x);
   }
 
-  robot->x_speed = x_speed;
-  robot->y_speed = 0.0;
-  robot->a_speed = a_speed;
-  robot->aim_on = false;
-  start();
+  speed_control(x_speed, 0.0, a_speed, false, delta_sec);
 }
 
-bool Locomotion::move_backward_to(const keisan::Point2 & target)
+bool Locomotion::move_backward_to(const keisan::Point2 & target, float delta_sec)
 {
   double delta_x = (robot->position.x - target.x);
   double delta_y = (robot->position.y - target.y);
@@ -377,16 +389,11 @@ bool Locomotion::move_backward_to(const keisan::Point2 & target)
 
   x_speed = keisan::map(target_distance, 0.0, 25.0, backward_min_x, x_speed);
 
-  robot->x_speed = x_speed;
-  robot->y_speed = 0.0;
-  robot->a_speed = a_speed;
-  robot->aim_on = false;
-  start();
-
+  speed_control(x_speed, 0.0, a_speed, false, delta_sec);
   return false;
 }
 
-void Locomotion::move_forward(const keisan::Angle<double> & direction)
+void Locomotion::move_forward(const keisan::Angle<double> & direction, float delta_sec)
 {
   auto delta_direction = (direction - robot->orientation).normalize().degree();
 
@@ -398,14 +405,10 @@ void Locomotion::move_forward(const keisan::Angle<double> & direction)
     x_speed = 0.0;
   }
 
-  robot->x_speed = x_speed;
-  robot->y_speed = 0.0;
-  robot->a_speed = a_speed;
-  robot->aim_on = false;
-  start();
+  speed_control(x_speed, 0.0, a_speed, false, delta_sec);
 }
 
-bool Locomotion::move_forward_to(const keisan::Point2 & target)
+bool Locomotion::move_forward_to(const keisan::Point2 & target, float delta_sec)
 {
   double delta_x = (target.x - robot->position.x);
   double delta_y = (target.y - robot->position.y);
@@ -432,16 +435,12 @@ bool Locomotion::move_forward_to(const keisan::Point2 & target)
 
   x_speed = keisan::smooth(robot->x_speed, x_speed, 0.4);
 
-  robot->x_speed = x_speed;
-  robot->y_speed = 0.0;
-  robot->a_speed = a_speed;
-  robot->aim_on = false;
-  start();
+  speed_control(x_speed, 0.0, a_speed, false, delta_sec);
 
   return false;
 }
 
-bool Locomotion::rotate_to_target(const keisan::Angle<double> & direction)
+bool Locomotion::rotate_to_target(const keisan::Angle<double> & direction, float delta_sec)
 {
   auto delta_direction = (direction - robot->orientation).normalize().degree();
 
@@ -453,16 +452,16 @@ bool Locomotion::rotate_to_target(const keisan::Angle<double> & direction)
   double y_speed = (delta_direction < 0.0) ? move_max_y : -move_max_y;
   double a_speed = (delta_direction < 0.0) ? rotate_max_a : -rotate_max_a;
 
-  robot->x_speed = keisan::smooth(robot->x_speed, 0.0, 0.8);
-  robot->y_speed = keisan::smooth(robot->y_speed, y_speed, 0.8);
-  robot->a_speed = keisan::smooth(robot->a_speed, a_speed, 0.8);
-  robot->aim_on = false;
-  start();
+  double x_speed = keisan::smooth(robot->x_speed, 0.0, 0.8);
+  y_speed = keisan::smooth(robot->y_speed, y_speed, 0.8);
+  a_speed = keisan::smooth(robot->a_speed, a_speed, 0.8);
+
+  speed_control(x_speed, y_speed, a_speed, false, delta_sec);
 
   return false;
 }
 
-bool Locomotion::rotate_to(const keisan::Angle<double> & direction, bool a_move_only)
+bool Locomotion::rotate_to(const keisan::Angle<double> & direction, bool a_move_only, float delta_sec)
 {
   auto delta_direction = (direction - robot->orientation).normalize().degree();
 
@@ -477,21 +476,17 @@ bool Locomotion::rotate_to(const keisan::Angle<double> & direction, bool a_move_
 
   double a_speed = keisan::sign(delta_direction) * -move_max_a;
 
-  robot->x_speed = 0.0;
-  robot->y_speed = y_speed;
-  robot->a_speed = a_speed;
-  robot->aim_on = false;
-  start();
+  speed_control(0.0, y_speed, a_speed, false, delta_sec);
 
   return false;
 }
 
-bool Locomotion::move_follow_head()
+bool Locomotion::move_follow_head(float delta_sec)
 {
-  return move_follow_head(follow_min_tilt);
+  return move_follow_head(follow_min_tilt, delta_sec);
 }
 
-bool Locomotion::move_follow_head(const keisan::Angle<double> & min_tilt)
+bool Locomotion::move_follow_head(const keisan::Angle<double> & min_tilt, float delta_sec)
 {
   double a_speed = 0.0;
   if (robot->get_pan().degree() < 0.0) {
@@ -523,23 +518,19 @@ bool Locomotion::move_follow_head(const keisan::Angle<double> & min_tilt)
 
   x_speed = keisan::smooth(robot->x_speed, x_speed, smooth_ratio);
 
-  robot->x_speed = x_speed;
-  robot->y_speed = y_speed;
-  robot->a_speed = a_speed;
-  robot->aim_on = false;
-  start();
+  speed_control(x_speed, y_speed, a_speed, false, delta_sec);
 
   return robot->get_tilt() + robot->tilt_center < min_tilt;
 }
 
-bool Locomotion::move_skew(const keisan::Angle<double> & direction)
+bool Locomotion::move_skew(const keisan::Angle<double> & direction, float delta_sec)
 {
   auto current_direction = (robot->orientation - robot->pan).normalize();
   auto delta_direction = (direction - current_direction).normalize().degree();
-  return move_skew(direction, delta_direction > 0);
+  return move_skew(direction, delta_direction > 0, delta_sec);
 }
 
-bool Locomotion::move_skew(const keisan::Angle<double> & direction, bool skew_left)
+bool Locomotion::move_skew(const keisan::Angle<double> & direction, bool skew_left, float delta_sec)
 {
   auto current_direction = (robot->orientation - robot->pan).normalize();
   double delta_direction = std::fabs((direction - current_direction).normalize().degree());
@@ -591,19 +582,15 @@ bool Locomotion::move_skew(const keisan::Angle<double> & direction, bool skew_le
       move_x = keisan::map(std::abs(move_a), 0.0, skew_max_a, skew_max_x, 0.0);
     }
 
-    robot->x_speed = move_x;
-    robot->y_speed = 0.0;
-    robot->a_speed = move_a;
-    robot->aim_on = false;
-    start();
+    speed_control(move_x, 0.0, move_a, false, delta_sec);
 
     return false;
   } else {
-    return move_follow_head();
+    return move_follow_head(delta_sec);
   }
 }
 
-bool Locomotion::dribble(const keisan::Angle<double> & direction)
+bool Locomotion::dribble(const keisan::Angle<double> & direction, float delta_sec)
 {
   double min_kick_tilt = std::min(left_kick_target_tilt.degree(), right_kick_target_tilt.degree());
   bool is_dribble = (robot->get_tilt() + robot->tilt_center).degree() <= min_kick_tilt;
@@ -651,17 +638,19 @@ bool Locomotion::dribble(const keisan::Angle<double> & direction)
   x_speed = keisan::smooth(robot->x_speed, x_speed, smooth_ratio);
   y_speed = keisan::smooth(robot->y_speed, y_speed, smooth_ratio);
 
-  robot->x_speed = x_speed;
-  robot->y_speed = y_speed;
-  robot->a_speed = a_speed;
-  robot->aim_on = false;
-  start();
+  speed_control(x_speed, y_speed, a_speed, false, delta_sec);
 
-  return is_dribble;
+  return false;
 }
 
-bool Locomotion::pivot(const keisan::Angle<double> & direction)
+bool Locomotion::pivot(const keisan::Angle<double> & direction, float delta_sec)
 {
+  if (initial_pivot) {
+    initial_pivot = false;
+    walk_in_position();
+    return false;
+  }
+
   auto delta_direction = (direction - robot->orientation).normalize().degree();
 
   if (std::abs(delta_direction) < pivot_max_delta_direction) {
@@ -669,7 +658,8 @@ bool Locomotion::pivot(const keisan::Angle<double> & direction)
     return true;
   }
 
-  double delta_tilt = (pivot_target_tilt - robot->tilt + robot->tilt_center).degree();
+  auto tilt = robot->get_tilt();
+  double delta_tilt = (pivot_target_tilt - tilt).degree();
 
   double x_speed = 0;
   if (delta_tilt > 0.0) {
@@ -691,17 +681,13 @@ bool Locomotion::pivot(const keisan::Angle<double> & direction)
   if (fabs(pan) > pivot_pan_range_a_speed) {
     a_speed = keisan::map(pan, -pivot_pan_range_a_speed, pivot_pan_range_a_speed, pivot_max_a, -pivot_max_a);
   }
-  
-  robot->x_speed = x_speed;
-  robot->y_speed = y_speed;
-  robot->a_speed = a_speed;
-  robot->aim_on = true;
-  start();
+
+  speed_control(x_speed, y_speed, a_speed, true, delta_sec);
 
   return false;
 }
 
-bool Locomotion::pivot_new(const keisan::Angle<double> & direction)
+bool Locomotion::pivot_new(const keisan::Angle<double> & direction, float delta_sec)
 {
   if (initial_pivot) {
     initial_pivot = false;
@@ -746,11 +732,7 @@ bool Locomotion::pivot_new(const keisan::Angle<double> & direction)
     : keisan::map(delta_direction, 180.0, 0.0, pivot_max_a, pivot_max_a * 0.9);
   }
 
-  robot->x_speed = x_speed;
-  robot->y_speed = y_speed;
-  robot->a_speed = a_speed;
-  robot->aim_on = true;
-  start();
+  speed_control(x_speed, y_speed, a_speed, true, delta_sec);
 
   return false;
 }
@@ -758,7 +740,8 @@ bool Locomotion::pivot_new(const keisan::Angle<double> & direction)
 bool Locomotion::position_until(
   const keisan::Angle<double> & target_pan,
   const keisan::Angle<double> & target_tilt,
-  const keisan::Angle<double> & direction)
+  const keisan::Angle<double> & direction,
+  float delta_sec)
 {
   auto pan = robot->get_pan() + robot->pan_center;
   auto tilt = robot->get_tilt() + robot->tilt_center;
@@ -799,11 +782,7 @@ bool Locomotion::position_until(
   x_speed = keisan::smooth(robot->x_speed, x_speed, smooth_ratio);
   y_speed = keisan::smooth(robot->y_speed, y_speed, smooth_ratio);
 
-  robot->x_speed = x_speed;
-  robot->y_speed = y_speed;
-  robot->a_speed = a_speed;
-  robot->aim_on = false;
-  start();
+  speed_control(x_speed, y_speed, a_speed, false, delta_sec);
 
   if (std::abs(delta_tilt) < position_min_delta_tilt.degree() && std::abs(delta_pan) < position_min_delta_pan.degree()) {
     printf("done by pan tilt\n");
@@ -813,27 +792,27 @@ bool Locomotion::position_until(
   return false;
 }
 
-bool Locomotion::position_left_kick(const keisan::Angle<double> & direction)
+bool Locomotion::position_left_kick(const keisan::Angle<double> & direction, float delta_sec)
 {
   return position_until(
-    left_kick_target_pan, left_kick_target_tilt, direction);
+    left_kick_target_pan, left_kick_target_tilt, direction, delta_sec);
 }
 
-bool Locomotion::position_right_kick(const keisan::Angle<double> & direction)
+bool Locomotion::position_right_kick(const keisan::Angle<double> & direction, float delta_sec)
 {
   return position_until(
-    right_kick_target_pan, right_kick_target_tilt, direction);
+    right_kick_target_pan, right_kick_target_tilt, direction, delta_sec);
 }
 
-bool Locomotion::position_kick_general(const keisan::Angle<double> & direction)
+bool Locomotion::position_kick_general(const keisan::Angle<double> & direction, float delta_sec)
 {
   return position_kick_custom_pan_tilt(direction, right_kick_target_pan, left_kick_target_pan,
     (left_kick_target_tilt < right_kick_target_tilt ? left_kick_target_tilt : right_kick_target_tilt) - position_min_delta_tilt,
-    (left_kick_target_tilt > right_kick_target_tilt ? left_kick_target_tilt : right_kick_target_tilt) + position_min_delta_tilt);
+    (left_kick_target_tilt > right_kick_target_tilt ? left_kick_target_tilt : right_kick_target_tilt) + position_min_delta_tilt, delta_sec);
 }
 
 bool Locomotion::position_kick_custom_pan_tilt(const keisan::Angle<double> & direction, const keisan::Angle<double> & min_pan, 
-  const keisan::Angle<double> & max_pan, const keisan::Angle<double> & min_tilt, const keisan::Angle<double> & max_tilt) 
+  const keisan::Angle<double> & max_pan, const keisan::Angle<double> & min_tilt, const keisan::Angle<double> & max_tilt, float delta_sec) 
 {
   double pan = (robot->get_pan() + robot->pan_center).degree();
   double tilt = (robot->get_tilt() + robot->tilt_center).degree();
@@ -880,6 +859,8 @@ bool Locomotion::position_kick_custom_pan_tilt(const keisan::Angle<double> & dir
   x_speed = keisan::smooth(robot->x_speed, x_speed, smooth_ratio);
   y_speed = keisan::smooth(robot->y_speed, y_speed, smooth_ratio);
 
+  speed_control(x_speed, y_speed, a_speed, false, delta_sec);
+
   printf("delta pan %.1f, delta tilt %.1f, delta direction %.1f\n", delta_pan, delta_tilt, delta_direction);
 
   if (tilt == keisan::clamp(tilt, min_tilt.degree(), max_tilt.degree()) && pan == keisan::clamp(pan, min_pan.degree(), max_pan.degree())) {
@@ -890,7 +871,7 @@ bool Locomotion::position_kick_custom_pan_tilt(const keisan::Angle<double> & dir
   return false;
 }
 
-bool Locomotion::position_kick_range_pan_tilt(const keisan::Angle<double> & direction, bool precise_kick, bool left_kick, bool is_positioning_center)
+bool Locomotion::position_kick_range_pan_tilt(const keisan::Angle<double> & direction, bool precise_kick, bool left_kick, bool is_positioning_center, float delta_sec)
 {
   auto tilt = robot->get_tilt();
   auto pan = robot->get_pan();
@@ -956,11 +937,11 @@ bool Locomotion::position_kick_range_pan_tilt(const keisan::Angle<double> & dire
   double smooth_ratio = 0.8;
   #endif
 
-  robot->x_speed = keisan::smooth(robot->x_speed, x_speed, smooth_ratio);
-  robot->y_speed = keisan::smooth(robot->y_speed, y_speed, smooth_ratio);
-  robot->a_speed = keisan::smooth(robot->a_speed, a_speed, smooth_ratio);
-  robot->aim_on = false;
-  start();
+  x_speed = keisan::smooth(robot->x_speed, x_speed, smooth_ratio);
+  y_speed = keisan::smooth(robot->y_speed, y_speed, smooth_ratio);
+  a_speed = keisan::smooth(robot->a_speed, a_speed, smooth_ratio);
+
+  speed_control(x_speed, y_speed, a_speed, false, delta_sec);
 
   printf("delta pan %.1f, delta tilt %.1f, delta direction %.1f\n", delta_pan, delta_tilt, delta_direction);
 
@@ -993,6 +974,57 @@ bool Locomotion::in_tilt_kick_range()
   return tilt > min_target_tilt && tilt < max_target_tilt;
 }
 
+void Locomotion::speed_control(double x_speed, double y_speed, double a_speed, bool aim_on, float delta_sec)
+{
+  enum rpy{roll, pitch, yaw};
+  //  Speed control process
+
+  if (!using_speed_control) {
+    robot->x_speed = x_speed;
+    robot->y_speed = y_speed;
+    robot->a_speed = a_speed;
+    robot->aim_on = aim_on;
+    start();
+    return;
+  }
+  std::cout << "pitch " << std::abs(robot->rpy[pitch]) << std::endl;
+  std::cout << "roll " << std::abs(robot->rpy[roll]) << std::endl;
+
+  if (std::abs(robot->rpy[pitch]) > pitch_unstable_threshold) {
+    std::cout << "unstable walking detected!" << std::endl;
+    unstable_pitch = true;
+    stable_time = 0.0;
+  } else if (std::abs(robot->rpy[roll]) > roll_unstable_threshold) {
+    std::cout << "unstable walking detected!" << std::endl;
+    unstable_roll = true;
+    stable_time = 0.0;
+  } else {
+    stable_time += delta_sec;
+    std::cout << "stable time: " << stable_time << std::endl; 
+    if (stable_time > 5.0) {
+      unstable_pitch = false;
+      unstable_roll = false;
+    }
+  }
+
+  if (unstable_pitch) {
+    std::cout << "pitch speed stabilization trigerred!" << std::endl;
+    x_speed = keisan::map(std::abs(robot->rpy[pitch]), 25.0, pitch_unstable_threshold, 0.0, x_speed - 5.0);
+    a_speed = keisan::map(std::abs(robot->rpy[pitch]), 25.0, pitch_unstable_threshold, 0.0, a_speed);
+  }
+  if (unstable_roll) {
+    std::cout << "roll speed stabilization trigerred!" << std::endl;
+    y_speed = keisan::map(std::abs(robot->rpy[roll]), 20.0, roll_unstable_threshold, 0.0, y_speed- 5.0);
+    a_speed = keisan::map(std::abs(robot->rpy[roll]), 20.0, roll_unstable_threshold, 0.0, a_speed);
+  }
+
+  robot->x_speed = x_speed;
+  robot->y_speed = y_speed;
+  robot->a_speed = a_speed;
+  robot->aim_on = aim_on;
+  start();
+}
+
 std::shared_ptr<Robot> Locomotion::get_robot() const
 {
   return robot;