diff --git a/src/dynamixel_motor.cpp b/src/dynamixel_motor.cpp
index 5b6ca9fa5831fdb9da1f68e7d2ec06d52bb23852..c729a8d8a6a4e538c92e055670e9381bfc76e87b 100644
--- a/src/dynamixel_motor.cpp
+++ b/src/dynamixel_motor.cpp
@@ -1202,7 +1202,11 @@ void CDynamixelMotor::stop(void)
       if(this->mode==angle_ctrl)
       {
 	this->dynamixel_dev->read_word_register(this->registers[current_pos],&current_position);
-	this->dynamixel_dev->write_word_register(this->registers[goal_pos],current_position);
+        if(this->to_angles(current_position)>this->config.max_angle)
+          current_position=from_angles(this->config.max_angle);
+        else if(this->to_angles(current_position)<this->config.min_angle)
+          current_position=from_angles(this->config.min_angle);
+        this->dynamixel_dev->write_word_register(this->registers[goal_pos],current_position);
       }
       else
 	this->dynamixel_dev->write_word_register(this->registers[goal_speed],0);
diff --git a/src/dynamixel_motor_group.cpp b/src/dynamixel_motor_group.cpp
index 1ad63f80d50691cd00c1cb83351f03eee4873cfa..2ff5f4b446c0e8825475b6f01ab22fe2c7f0fc13 100644
--- a/src/dynamixel_motor_group.cpp
+++ b/src/dynamixel_motor_group.cpp
@@ -1152,6 +1152,10 @@ void CDynamixelMotorGroup::stop(void)
 	if(this->mode==angle_ctrl)
 	{
 	  this->dynamixel_dev[i]->read_word_register(this->registers[i][current_pos],&current_position);
+          if(current_position>this->to_angles(i,this->config[i].max_angle))
+            current_position=this->from_angles(i,this->config[i].max_angle);
+          else if(current_position<this->to_angles(i,this->config[i].min_angle))
+            current_position=this->from_angles(i,this->config[i].min_angle);
 	  this->dynamixel_dev[i]->write_word_register(this->registers[i][goal_pos],current_position);
 	}
 	else