diff --git a/examples/run_recorded_traj/package.xml b/examples/run_recorded_traj/package.xml
index 26eab51a..8cb1bc41 100644
--- a/examples/run_recorded_traj/package.xml
+++ b/examples/run_recorded_traj/package.xml
@@ -52,6 +52,7 @@
   <build_depend>roscpp</build_depend>
   <build_depend>xarm_api</build_depend>
   <build_depend>xarm_msgs</build_depend>
+  <build_depend>std_msgs</build_depend>
   
   <build_export_depend>roscpp</build_export_depend>
   <build_export_depend>xarm_api</build_export_depend>
@@ -59,6 +60,7 @@
   <exec_depend>roscpp</exec_depend>
   <exec_depend>xarm_api</exec_depend>
   <exec_depend>xarm_msgs</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
 
 
   <!-- The export tag contains other, unspecified, tags -->
diff --git a/examples/run_recorded_traj/src/xarm_traj_test.cpp b/examples/run_recorded_traj/src/xarm_traj_test.cpp
index 10d5224b..06b11df9 100644
--- a/examples/run_recorded_traj/src/xarm_traj_test.cpp
+++ b/examples/run_recorded_traj/src/xarm_traj_test.cpp
@@ -6,9 +6,6 @@
  ============================================================================*/
 #include "ros/ros.h"
 #include <std_msgs/Bool.h>
-#include <xarm_planner/pose_plan.h>
-#include <xarm_planner/joint_plan.h>
-#include <xarm_planner/exec_plan.h>
 #include <xarm_msgs/SetInt16.h>
 #include <xarm_msgs/SetAxis.h>
 #include <xarm_msgs/SetString.h>
@@ -16,33 +13,6 @@
 #include <stdlib.h>
 #include <vector>
 
-bool request_plan(ros::ServiceClient& client, xarm_planner::joint_plan& srv)
-{
-	if(client.call(srv))
-	{
-		return srv.response.success;
-	}
-	else
-	{
-		ROS_ERROR("Failed to call service joint_plan");
-		return false;
-	}
-}
-
-bool request_exec(ros::ServiceClient& client, xarm_planner::exec_plan& srv)
-{
-	if(client.call(srv))
-	{
-		return srv.response.success;
-	}
-	else
-	{
-		ROS_ERROR("Failed to call service exec_plan");
-		return false;
-	}
-}
-
-
 int main(int argc, char** argv)
 {	
 	
diff --git a/xarm_controller/src/xarm_hw.cpp b/xarm_controller/src/xarm_hw.cpp
index 661f0bef..4f614288 100755
--- a/xarm_controller/src/xarm_hw.cpp
+++ b/xarm_controller/src/xarm_hw.cpp
@@ -270,23 +270,30 @@ namespace xarm_control
 
 		_enforce_limits(period);
 
+		int cmd_ret = 0;
 		switch (ctrl_method_)
 		{
 		case VELOCITY:
 			{
 				for (int k = 0; k < dof_; k++) { velocity_cmd_float_[k] = (float)velocity_cmd_[k]; }
-				xarm.veloMoveJoint(velocity_cmd_float_, true);
+				cmd_ret = xarm.veloMoveJoint(velocity_cmd_float_, true);
 			}
 			break;
 		case POSITION:		
 		default:
 			{
 				for (int k = 0; k < dof_; k++) { position_cmd_float_[k] = (float)position_cmd_[k]; }
-				xarm.setServoJ(position_cmd_float_);
+				cmd_ret = xarm.setServoJ(position_cmd_float_);
 			}
 			break;
 		}
 
+		if(cmd_ret)
+		{
+			xarm.setState(XARM_STATE::STOP);
+			ROS_ERROR("XArmHW::Write() Error! Code: %d, Setting Robot State to STOP...", cmd_ret);
+		}
+
 		// for(int k=0; k<dof_; k++)
 		// {
 		// 	// make sure no abnormal command will be written into joints, check if cmd velocity > [180 deg/sec * (1+10%)]