diff --git a/src/dynamixel_servo.cpp b/src/dynamixel_servo.cpp
index f6b0478704d5b89a8c03c80ec8390cdf592894ff..d8602b507ddec6caa4989192700942980b3a895f 100644
--- a/src/dynamixel_servo.cpp
+++ b/src/dynamixel_servo.cpp
@@ -839,6 +839,7 @@ namespace dynamixel_robot_gazebo
       if(update)
       {
         this->memory[123]=0x03;
+        this->memory[122]=0x01;
         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))
@@ -892,6 +893,7 @@ namespace dynamixel_robot_gazebo
       if(update)
       {
         this->memory[123]=0x03;
+        this->memory[122]=0x01;
         this->trajectory.clear();
         this->traj_index=0;
         if(speed!=0.0)
@@ -929,9 +931,15 @@ 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;
+        this->memory[122]=0x01;
+      }
       else
+      {
         this->memory[123]=0x00;
+        this->memory[122]=0x00;
+      }
       if(this->pid!=NULL)
         command=this->pid->computeCommand(target_angle-real_angle,period);
       else if(this->compliance!=NULL)
@@ -946,9 +954,15 @@ namespace dynamixel_robot_gazebo
     {
       target_pwm=this->get_target_pwm();
       if(fabs(target_pwm)>0.1)
+      {
         this->memory[123]=0x03;
+        this->memory[122]=0x01;
+      }
       else
+      {
         this->memory[123]=0x00;
+        this->memory[122]=0x00;
+      }
       command=this->saturate_command(target_pwm);
       this->joint.setCommand(command);
     }