Disabled the servos when changing the control mode to avoid erratic motions.

src/dynamixel_motor.cpp

@@ -329,15 +329,16 @@ void CDynamixelMotor::set_control_mode(control_mode mode)
   else
   {
     try{
-      if(this->info.model=="XL_320")
+      if(this->mode!=mode)
       {
-        this->dynamixel_dev->write_byte_register(this->registers[dyn_control_mode],(unsigned char)mode);
-        usleep(10000);
-        this->mode=mode;
-      }
-      else
-      {
-        if(this->mode!=mode)
+        this->disable();
+        if(this->info.model=="XL_320")
+        {
+          this->dynamixel_dev->write_byte_register(this->registers[dyn_control_mode],(unsigned char)mode);
+          usleep(10000);
+          this->mode=mode;
+        }
+        else
         {
           if(mode==angle_ctrl)
           {

src/dynamixel_motor_group.cpp

@@ -840,6 +840,7 @@ void CDynamixelMotorGroup::set_control_mode(control_mode mode)
 
   if(this->mode!=mode)
   {
+    this->disable();
     for(i=0;i<this->servo_id.size();i++)
     {
       try{