diff --git a/commands/keyboard_input/include/keyboard_input/KeyboardInput.h b/commands/keyboard_input/include/keyboard_input/KeyboardInput.h
index 7daad93..e7a7aa2 100644
--- a/commands/keyboard_input/include/keyboard_input/KeyboardInput.h
+++ b/commands/keyboard_input/include/keyboard_input/KeyboardInput.h
@@ -41,6 +41,9 @@ private:
     rclcpp::Publisher<control_input_msgs::msg::Inputs>::SharedPtr publisher_;
     rclcpp::TimerBase::SharedPtr timer_;
 
+    bool just_published_ = false;
+    int reset_count_ = 0;
+
     float sensitivity_left_ = 0.05;
     float sensitivity_right_ = 0.05;
     termios old_tio_{}, new_tio_{};
diff --git a/commands/keyboard_input/src/KeyboardInput.cpp b/commands/keyboard_input/src/KeyboardInput.cpp
index 74d4da7..601395d 100644
--- a/commands/keyboard_input/src/KeyboardInput.cpp
+++ b/commands/keyboard_input/src/KeyboardInput.cpp
@@ -26,8 +26,21 @@ void KeyboardInput::timer_callback() {
             inputs_.ly = 0;
             inputs_.rx = 0;
             inputs_.ry = 0;
+            reset_count_ = 100;
         }
         publisher_->publish(inputs_);
+        just_published_ = true;
+    } else {
+        if (just_published_) {
+            reset_count_ -= 1;
+            if (reset_count_ == 0) {
+                just_published_ = false;
+                if (inputs_.command != 0) {
+                    inputs_.command = 0;
+                    publisher_->publish(inputs_);
+                }
+            }
+        }
     }
 }
 
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 3c2475c..bce4941 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
@@ -38,8 +38,7 @@ struct CtrlComponent {
     std::vector<std::reference_wrapper<hardware_interface::LoanedStateInterface> >
     imu_state_interface_;
 
-    control_input_msgs::msg::Inputs default_inputs_;
-    std::reference_wrapper<control_input_msgs::msg::Inputs> control_inputs_;
+    control_input_msgs::msg::Inputs control_inputs_;
     int frequency_{};
 
     QuadrupedRobot robot_model_;
@@ -48,8 +47,7 @@ struct CtrlComponent {
 
     WaveGenerator wave_generator_;
 
-    CtrlComponent() : control_inputs_(default_inputs_),
-                      robot_model_(*this),
+    CtrlComponent() : robot_model_(*this),
                       estimator_(*this) {
     }
 
diff --git a/controllers/unitree_guide_controller/src/FSM/StateBalanceTest.cpp b/controllers/unitree_guide_controller/src/FSM/StateBalanceTest.cpp
index 7e6538f..a51a231 100644
--- a/controllers/unitree_guide_controller/src/FSM/StateBalanceTest.cpp
+++ b/controllers/unitree_guide_controller/src/FSM/StateBalanceTest.cpp
@@ -36,11 +36,11 @@ void StateBalanceTest::enter() {
 }
 
 void StateBalanceTest::run() {
-    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);
+    pcd_(0) = pcd_init_(0) + invNormalize(ctrl_comp_.control_inputs_.ly, _xMin, _xMax);
+    pcd_(1) = pcd_init_(1) - invNormalize(ctrl_comp_.control_inputs_.lx, _yMin, _yMax);
+    pcd_(2) = pcd_init_(2) + invNormalize(ctrl_comp_.control_inputs_.ry, _zMin, _zMax);
 
-    const float yaw = - invNormalize(ctrl_comp_.control_inputs_.get().rx, _yawMin, _yawMax);
+    const float yaw = - invNormalize(ctrl_comp_.control_inputs_.rx, _yawMin, _yawMax);
     Rd_ = rotz(yaw) * init_rotation_;
 
     for (int i = 0; i < 12; i++) {
@@ -56,7 +56,7 @@ void StateBalanceTest::exit() {
 }
 
 FSMStateName StateBalanceTest::checkChange() {
-    switch (ctrl_comp_.control_inputs_.get().command) {
+    switch (ctrl_comp_.control_inputs_.command) {
         case 1:
             return FSMStateName::FIXEDDOWN;
         case 2:
diff --git a/controllers/unitree_guide_controller/src/FSM/StateFixedDown.cpp b/controllers/unitree_guide_controller/src/FSM/StateFixedDown.cpp
index 2ca1761..442085c 100644
--- a/controllers/unitree_guide_controller/src/FSM/StateFixedDown.cpp
+++ b/controllers/unitree_guide_controller/src/FSM/StateFixedDown.cpp
@@ -15,7 +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;
+    ctrl_comp_.control_inputs_.command = 0;
 }
 
 void StateFixedDown::run() {
@@ -39,7 +39,7 @@ FSMStateName StateFixedDown::checkChange() {
     if (percent_ < 1.5) {
         return FSMStateName::FIXEDDOWN;
     }
-    switch (ctrl_comp_.control_inputs_.get().command) {
+    switch (ctrl_comp_.control_inputs_.command) {
         case 1:
             return FSMStateName::PASSIVE;
         case 2:
diff --git a/controllers/unitree_guide_controller/src/FSM/StateFixedStand.cpp b/controllers/unitree_guide_controller/src/FSM/StateFixedStand.cpp
index 92b4e04..f37f88c 100644
--- a/controllers/unitree_guide_controller/src/FSM/StateFixedStand.cpp
+++ b/controllers/unitree_guide_controller/src/FSM/StateFixedStand.cpp
@@ -15,7 +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;
+    ctrl_comp_.control_inputs_.command = 0;
 }
 
 void StateFixedStand::run() {
@@ -40,8 +40,10 @@ FSMStateName StateFixedStand::checkChange() {
     if (percent_ < 1.5) {
         return FSMStateName::FIXEDSTAND;
     }
-    switch (ctrl_comp_.control_inputs_.get().command) {
+    switch (ctrl_comp_.control_inputs_.command) {
         case 1:
+            return FSMStateName::PASSIVE;
+        case 2:
             return FSMStateName::FIXEDDOWN;
         case 3:
             return FSMStateName::FREESTAND;
diff --git a/controllers/unitree_guide_controller/src/FSM/StateFreeStand.cpp b/controllers/unitree_guide_controller/src/FSM/StateFreeStand.cpp
index eaedf75..45e53bb 100644
--- a/controllers/unitree_guide_controller/src/FSM/StateFreeStand.cpp
+++ b/controllers/unitree_guide_controller/src/FSM/StateFreeStand.cpp
@@ -33,23 +33,23 @@ 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;
+    ctrl_comp_.control_inputs_.command = 0;
 }
 
 void StateFreeStand::run() {
-    calc_body_target(invNormalize(ctrl_comp_.control_inputs_.get().lx, row_min_, row_max_),
-                     invNormalize(ctrl_comp_.control_inputs_.get().ly, pitch_min_, pitch_max_),
-                     invNormalize(ctrl_comp_.control_inputs_.get().rx, yaw_min_, yaw_max_),
-                     invNormalize(ctrl_comp_.control_inputs_.get().ry, height_min_, height_max_));
+    calc_body_target(invNormalize(ctrl_comp_.control_inputs_.lx, row_min_, row_max_),
+                     invNormalize(ctrl_comp_.control_inputs_.ly, pitch_min_, pitch_max_),
+                     invNormalize(ctrl_comp_.control_inputs_.rx, yaw_min_, yaw_max_),
+                     invNormalize(ctrl_comp_.control_inputs_.ry, height_min_, height_max_));
 }
 
 void StateFreeStand::exit() {
 }
 
 FSMStateName StateFreeStand::checkChange() {
-    switch (ctrl_comp_.control_inputs_.get().command) {
+    switch (ctrl_comp_.control_inputs_.command) {
         case 1:
-            return FSMStateName::FIXEDDOWN;
+            return FSMStateName::PASSIVE;
         case 2:
             return FSMStateName::FIXEDSTAND;
         default:
diff --git a/controllers/unitree_guide_controller/src/FSM/StatePassive.cpp b/controllers/unitree_guide_controller/src/FSM/StatePassive.cpp
index f4af125..e9381bf 100644
--- a/controllers/unitree_guide_controller/src/FSM/StatePassive.cpp
+++ b/controllers/unitree_guide_controller/src/FSM/StatePassive.cpp
@@ -27,7 +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;
+    ctrl_comp_.control_inputs_.command = 0;
 }
 
 void StatePassive::run() {
@@ -37,7 +37,7 @@ void StatePassive::exit() {
 }
 
 FSMStateName StatePassive::checkChange() {
-    if (ctrl_comp_.control_inputs_.get().command == 2) {
+    if (ctrl_comp_.control_inputs_.command == 2) {
         return FSMStateName::FIXEDDOWN;
     }
     return FSMStateName::PASSIVE;
diff --git a/controllers/unitree_guide_controller/src/FSM/StateSwingTest.cpp b/controllers/unitree_guide_controller/src/FSM/StateSwingTest.cpp
index d8c6cfe..09c7ae5 100644
--- a/controllers/unitree_guide_controller/src/FSM/StateSwingTest.cpp
+++ b/controllers/unitree_guide_controller/src/FSM/StateSwingTest.cpp
@@ -38,25 +38,25 @@ void StateSwingTest::enter() {
 }
 
 void StateSwingTest::run() {
-    if (ctrl_comp_.control_inputs_.get().ly > 0) {
-        fr_goal_pos_.p.x(invNormalize(ctrl_comp_.control_inputs_.get().ly, fr_init_pos_.p.x(),
+    if (ctrl_comp_.control_inputs_.ly > 0) {
+        fr_goal_pos_.p.x(invNormalize(ctrl_comp_.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_.get().ly, fr_init_pos_.p.x() + _xMin,
+        fr_goal_pos_.p.x(invNormalize(ctrl_comp_.control_inputs_.ly, fr_init_pos_.p.x() + _xMin,
                                       fr_init_pos_.p.x(), -1, 0));
     }
-    if (ctrl_comp_.control_inputs_.get().lx > 0) {
-        fr_goal_pos_.p.y(invNormalize(ctrl_comp_.control_inputs_.get().lx, fr_init_pos_.p.y(),
+    if (ctrl_comp_.control_inputs_.lx > 0) {
+        fr_goal_pos_.p.y(invNormalize(ctrl_comp_.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_.get().lx, fr_init_pos_.p.y() + _yMin,
+        fr_goal_pos_.p.y(invNormalize(ctrl_comp_.control_inputs_.lx, fr_init_pos_.p.y() + _yMin,
                                       fr_init_pos_.p.y(), -1, 0));
     }
-    if (ctrl_comp_.control_inputs_.get().ry > 0) {
-        fr_goal_pos_.p.z(invNormalize(ctrl_comp_.control_inputs_.get().ry, fr_init_pos_.p.z(),
+    if (ctrl_comp_.control_inputs_.ry > 0) {
+        fr_goal_pos_.p.z(invNormalize(ctrl_comp_.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_.get().ry, fr_init_pos_.p.z() + _zMin,
+        fr_goal_pos_.p.z(invNormalize(ctrl_comp_.control_inputs_.ry, fr_init_pos_.p.z() + _zMin,
                                       fr_init_pos_.p.z(), -1, 0));
     }
 
@@ -68,9 +68,9 @@ void StateSwingTest::exit() {
 }
 
 FSMStateName StateSwingTest::checkChange() {
-    switch (ctrl_comp_.control_inputs_.get().command) {
+    switch (ctrl_comp_.control_inputs_.command) {
         case 1:
-            return FSMStateName::FIXEDDOWN;
+            return FSMStateName::PASSIVE;
         case 2:
             return FSMStateName::FIXEDSTAND;
         default:
diff --git a/controllers/unitree_guide_controller/src/FSM/StateTrotting.cpp b/controllers/unitree_guide_controller/src/FSM/StateTrotting.cpp
index af646d7..deeccb0 100644
--- a/controllers/unitree_guide_controller/src/FSM/StateTrotting.cpp
+++ b/controllers/unitree_guide_controller/src/FSM/StateTrotting.cpp
@@ -34,7 +34,7 @@ void StateTrotting::enter() {
     Rd = rotz(yaw_cmd_);
     w_cmd_global_.setZero();
 
-    ctrl_comp_.control_inputs_.get().command = 0;
+    ctrl_comp_.control_inputs_.command = 0;
     gait_generator_.restart();
 }
 
@@ -68,9 +68,9 @@ void StateTrotting::exit() {
 }
 
 FSMStateName StateTrotting::checkChange() {
-    switch (ctrl_comp_.control_inputs_.get().command) {
+    switch (ctrl_comp_.control_inputs_.command) {
         case 1:
-            return FSMStateName::FIXEDDOWN;
+            return FSMStateName::PASSIVE;
         case 2:
             return FSMStateName::FIXEDSTAND;
         default:
@@ -80,12 +80,12 @@ FSMStateName StateTrotting::checkChange() {
 
 void StateTrotting::getUserCmd() {
     /* Movement */
-    v_cmd_body_(0) = invNormalize(ctrl_comp_.control_inputs_.get().ly, v_x_limit_(0), v_x_limit_(1));
-    v_cmd_body_(1) = -invNormalize(ctrl_comp_.control_inputs_.get().lx, v_y_limit_(0), v_y_limit_(1));
+    v_cmd_body_(0) = invNormalize(ctrl_comp_.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_(2) = 0;
 
     /* Turning */
-    d_yaw_cmd_ = -invNormalize(ctrl_comp_.control_inputs_.get().rx, w_yaw_limit_(0), w_yaw_limit_(1));
+    d_yaw_cmd_ = -invNormalize(ctrl_comp_.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_;
 }
diff --git a/controllers/unitree_guide_controller/src/UnitreeGuideController.cpp b/controllers/unitree_guide_controller/src/UnitreeGuideController.cpp
index 4e729ee..86b375f 100644
--- a/controllers/unitree_guide_controller/src/UnitreeGuideController.cpp
+++ b/controllers/unitree_guide_controller/src/UnitreeGuideController.cpp
@@ -105,11 +105,11 @@ 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_.get().command = msg->command;
-                ctrl_comp_.control_inputs_.get().lx = msg->lx;
-                ctrl_comp_.control_inputs_.get().ly = msg->ly;
-                ctrl_comp_.control_inputs_.get().rx = msg->rx;
-                ctrl_comp_.control_inputs_.get().ry = msg->ry;
+                ctrl_comp_.control_inputs_.command = msg->command;
+                ctrl_comp_.control_inputs_.lx = msg->lx;
+                ctrl_comp_.control_inputs_.ly = msg->ly;
+                ctrl_comp_.control_inputs_.rx = msg->rx;
+                ctrl_comp_.control_inputs_.ry = msg->ry;
             });
 
         robot_description_subscription_ = get_node()->create_subscription<std_msgs::msg::String>(