diff --git a/src/dynamixel_motor.cpp b/src/dynamixel_motor.cpp
index 6648e9813929c37978e943a00954e7d4a521a2b4..aff4877010bde091cc8c84970c8773e2ec028856 100644
--- a/src/dynamixel_motor.cpp
+++ b/src/dynamixel_motor.cpp
@@ -312,6 +312,7 @@ void CDynamixelMotor::set_control_mode(control_mode mode)
     {
       value=mode;
       this->write_register(this->registers[op_mode],value);
+      this->mode=mode;
     }
     else
     {