Merge pull request #40 from MGBla/fix_combined_speed_scaling_factor

Fixed the calculation of the combined speed scaling factor

Author: livanov93

ur_robot_driver/include/ur_robot_driver/hardware_interface.h

@@ -43,6 +43,7 @@
 // UR stuff
 #include <ur_client_library/ur/ur_driver.h>
 #include <ur_robot_driver/dashboard_client_ros.h>
+#include <ur_dashboard_msgs/msg/robot_mode.hpp>
 
 // ROS
 #include "rclcpp/macros.hpp"
@@ -55,6 +56,13 @@ using hardware_interface::status;
 
 namespace ur_robot_driver
 {
+enum class PausingState
+{
+  PAUSED,
+  RUNNING,
+  RAMPUP
+};
+
 /*!
  * \brief The HardwareInterface class handles the interface between the ROS system and the main
  * driver. It contains the read and write methods of the main control loop and registers various ROS
@@ -113,9 +121,11 @@ class URPositionHardwareInterface : public hardware_interface::SystemInterface
   urcl::vector6d_t urcl_joint_efforts_;
   urcl::vector6d_t urcl_ft_sensor_measurements_;
   urcl::vector6d_t urcl_tcp_pose_;
+
   bool packet_read_;
 
   uint32_t runtime_state_;
+  bool controllers_initialized_;
 
   std::bitset<18> actual_dig_out_bits_;
   std::bitset<18> actual_dig_in_bits_;
@@ -140,6 +150,9 @@ class URPositionHardwareInterface : public hardware_interface::SystemInterface
   bool non_blocking_read_;
   bool position_interface_in_use_;
 
+  PausingState pausing_state_;
+  double pausing_ramp_up_increment_;
+
   std::unique_ptr<urcl::UrDriver> ur_driver_;
 };
 }  // namespace ur_robot_driver

ur_robot_driver/src/hardware_interface.cpp

@@ -21,6 +21,7 @@
  *
  * \author  Lovro Ivanov [email protected]
  * \author  Andy Zelenak [email protected]
+ * \author  Marvin Große Besselmann [email protected]
  * \date    2020-11-9
  *
  */
@@ -49,6 +50,10 @@ hardware_interface::return_type URPositionHardwareInterface::configure(const Har
   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_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;
+  pausing_ramp_up_increment_ = 0.01;
+  controllers_initialized_ = false;
 
   for (const hardware_interface::ComponentInfo& joint : info_.joints)
   {
@@ -376,6 +381,41 @@ return_type URPositionHardwareInterface::read()
 
     // TODO logic for sending other stuff to higher level interface
 
+    // pausing state follows runtime state when pausing
+    if (runtime_state_ == static_cast<uint32_t>(rtde::RUNTIME_STATE::PAUSED))
+    {
+      pausing_state_ = PausingState::PAUSED;
+    }
+    // When the robot resumed program execution and pausing state was PAUSED, we enter RAMPUP
+    else if (runtime_state_ == static_cast<uint32_t>(rtde::RUNTIME_STATE::PLAYING) &&
+             pausing_state_ == PausingState::PAUSED)
+    {
+      speed_scaling_combined_ = 0.0;
+      pausing_state_ = PausingState::RAMPUP;
+    }
+
+    if (pausing_state_ == PausingState::RAMPUP)
+    {
+      double speed_scaling_ramp = speed_scaling_combined_ + pausing_ramp_up_increment_;
+      speed_scaling_combined_ = std::min(speed_scaling_ramp, speed_scaling_ * target_speed_fraction_);
+
+      if (speed_scaling_ramp > speed_scaling_ * target_speed_fraction_)
+      {
+        pausing_state_ = PausingState::RUNNING;
+      }
+    }
+    else if (runtime_state_ == static_cast<uint32_t>(rtde::RUNTIME_STATE::RESUMING))
+    {
+      // We have to keep speed scaling on ROS side at 0 during RESUMING to prevent controllers from
+      // continuing to interpolate
+      speed_scaling_combined_ = 0.0;
+    }
+    else
+    {
+      // Normal case
+      speed_scaling_combined_ = speed_scaling_ * target_speed_fraction_;
+    }
+
     return return_type::OK;
   }