Merge pull request #63 from MGBla/fix_move_to_home_bug

Quick fix for move-to-home bug

Author: destogl

ur_robot_driver/src/hardware_interface.cpp

@@ -45,6 +45,7 @@ hardware_interface::return_type URPositionHardwareInterface::configure(const Har
   urcl_ft_sensor_measurements_ = { { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 } };
   urcl_tcp_pose_ = { { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 } };
   urcl_position_commands_ = { { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 } };
+  urcl_position_commands_old_ = { { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 } };
   urcl_velocity_commands_ = { { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 } };
   runtime_state_ = static_cast<uint32_t>(rtde::RUNTIME_STATE::STOPPED);
   pausing_state_ = PausingState::RUNNING;
@@ -448,6 +449,8 @@ return_type URPositionHardwareInterface::write()
     if (position_interface_in_use_)
     {
       ur_driver_->writeJointCommand(urcl_position_commands_, urcl::comm::ControlMode::MODE_SERVOJ);
+      // remember old values
+      urcl_position_commands_old_ = urcl_position_commands_;
     }
     else
     {
@@ -456,9 +459,6 @@ return_type URPositionHardwareInterface::write()
 
     packet_read_ = false;
 
-    // remember old values
-    urcl_position_commands_old_ = urcl_position_commands_;
-
     return return_type::OK;
   }
   else
@@ -472,7 +472,8 @@ void URPositionHardwareInterface::handleRobotProgramState(bool program_running)
 {
   if (!robot_program_running_ && program_running)
   {
-    // TODO how to set controller reset flag
+    urcl_position_commands_old_ = urcl_position_commands_;
+    position_interface_in_use_ = false;
   }
   robot_program_running_ = program_running;
 }