diff --git a/src/dynamixel_servo.cpp b/src/dynamixel_servo.cpp
index 3bba671d441a81f8e2e865cf445923014b9ac4b4..2e53d922f393b31618dceaf571aef08f454612bc 100644
--- a/src/dynamixel_servo.cpp
+++ b/src/dynamixel_servo.cpp
@@ -807,6 +807,7 @@ namespace dynamixel_robot_gazebo
       }
       if(update)
       {
+        this->memory[123]=0x03;
         this->trajectory.clear();
         this->traj_index=0;
         if((!this->is_time_based() && speed!=0.0 && accel!=0.0) || (this->is_time_based() && traj_time!=0.0 && accel_time!=0.0))
@@ -859,6 +860,7 @@ namespace dynamixel_robot_gazebo
       }
       if(update)
       {
+        this->memory[123]=0x03;
         this->trajectory.clear();
         this->traj_index=0;
         if(speed!=0.0)
@@ -895,6 +897,10 @@ namespace dynamixel_robot_gazebo
     {
       this->traj_access.enter();
       target_angle=this->trajectory[this->traj_index];
+      if(fabs(this->get_target_angle()-real_angle)>0.02)
+        this->memory[123]=0x03;
+      else
+        this->memory[123]=0x00;
       if(this->pid!=NULL)
         command=this->pid->computeCommand(target_angle-real_angle,period);
       else if(this->compliance!=NULL)
@@ -908,6 +914,10 @@ namespace dynamixel_robot_gazebo
     else
     {
       target_pwm=this->get_target_pwm();
+      if(fabs(target_pwm)>0.1)
+        this->memory[123]=0x03;
+      else
+        this->memory[123]=0x00;
       command=this->saturate_command(target_pwm);
       this->joint.setCommand(command);
     }