diff --git a/src/APIs/c_api/src/nav_c_api.cpp b/src/APIs/c_api/src/nav_c_api.cpp
index ab7ac21..fede502 100644
--- a/src/APIs/c_api/src/nav_c_api.cpp
+++ b/src/APIs/c_api/src/nav_c_api.cpp
@@ -572,6 +572,7 @@ extern "C" bool navigation_set_twist_linear(NavigationHandle handle,
     linear.x = linear_x;
     linear.y = linear_y;
     linear.z = linear_z;
+    robot::log_info("setTwistLinear %f", linear.x);
     return nav_ptr->setTwistLinear(linear);
   }
   catch (...)
diff --git a/src/Navigations/Packages/move_base/src/move_base.cpp b/src/Navigations/Packages/move_base/src/move_base.cpp
index 5073b7a..3730106 100644
--- a/src/Navigations/Packages/move_base/src/move_base.cpp
+++ b/src/Navigations/Packages/move_base/src/move_base.cpp
@@ -976,6 +976,12 @@ bool move_base::MoveBase::moveTo( const robot_geometry_msgs::PoseStamped &goal,
     
     robot::log_info("[MoveBase::moveTo] Processing goal through action server...");
     as_->processGoal(action_goal);
+    
+    setTwistLinear(old_linear_fwd_);
+    setTwistLinear(old_linear_bwd_);
+    setTwistAngular(old_angular_fwd_);
+    setTwistAngular(old_angular_rev_);
+
     robot::log_info("[MoveBase::moveTo] Goal processed successfully by action server");
   }
   catch (const std::exception &e)
@@ -1118,6 +1124,11 @@ bool move_base::MoveBase::moveTo(const robot_protocol_msgs::Order &msg,
     }
 
     as_->processGoal(action_goal);
+
+    setTwistLinear(old_linear_fwd_);
+    setTwistLinear(old_linear_bwd_);
+    setTwistAngular(old_angular_fwd_);
+    setTwistAngular(old_angular_rev_);
   }
   catch (const std::exception &e)
   {
@@ -1279,6 +1290,10 @@ bool move_base::MoveBase::dockTo(const std::string &marker, const robot_geometry
     
     robot::log_info("[MoveBase::moveTo] Processing goal through action server...");
     as_->processGoal(action_goal);
+    setTwistLinear(old_linear_fwd_);
+    setTwistLinear(old_linear_bwd_);
+    setTwistAngular(old_angular_fwd_);
+    setTwistAngular(old_angular_rev_);
     robot::log_info("[MoveBase::moveTo] Goal processed successfully by action server");
   }
   catch (const std::exception &e)
@@ -1424,8 +1439,11 @@ bool move_base::MoveBase::dockTo(const robot_protocol_msgs::Order &msg,
       lock.unlock();
       return false;
     }
-
     as_->processGoal(action_goal);
+    setTwistLinear(old_linear_fwd_);
+    setTwistLinear(old_linear_bwd_);
+    setTwistAngular(old_angular_fwd_);
+    setTwistAngular(old_angular_rev_);
   }
   catch (const std::exception &e)
   {