Merge pull request #32 from PickNikRobotics/livanov/use_only_position…

…_interface Use only position command interface for Beta driver
UniversalRobots · Jan 25, 2021 · b7dde30 · b7dde30
2 parents eb2a01a + 4aad523
commit b7dde30
Show file tree

Hide file tree

Showing 2 changed files with 20 additions and 27 deletions.
diff --git a/ur_robot_driver/include/ur_robot_driver/hardware_interface.h b/ur_robot_driver/include/ur_robot_driver/hardware_interface.h
@@ -108,7 +108,6 @@ class URPositionHardwareInterface : public hardware_interface::SystemInterface
   urcl::vector6d_t urcl_position_commands_;
   urcl::vector6d_t urcl_position_commands_old_;
   urcl::vector6d_t urcl_velocity_commands_;
-  urcl::vector6d_t urcl_velocity_commands_old_;
   urcl::vector6d_t urcl_joint_positions_;
   urcl::vector6d_t urcl_joint_velocities_;
   urcl::vector6d_t urcl_joint_efforts_;
@@ -139,6 +138,7 @@ class URPositionHardwareInterface : public hardware_interface::SystemInterface
 
   bool robot_program_running_;
   bool non_blocking_read_;
+  bool position_interface_in_use_;
 
   std::unique_ptr<urcl::UrDriver> ur_driver_;
 };

diff --git a/ur_robot_driver/src/hardware_interface.cpp b/ur_robot_driver/src/hardware_interface.cpp
@@ -150,6 +150,8 @@ return_type URPositionHardwareInterface::start()
 {
   RCLCPP_INFO(rclcpp::get_logger("URPositionHardwareInterface"), "Starting ...please wait...");
 
+  position_interface_in_use_ = false;
+
   std::this_thread::sleep_for(std::chrono::seconds(2));
 
   // The robot's IP address.
@@ -288,6 +290,8 @@ return_type URPositionHardwareInterface::stop()
 {
   RCLCPP_INFO(rclcpp::get_logger("URPositionHardwareInterface"), "Stopping ...please wait...");
 
+  position_interface_in_use_ = false;
+
   std::this_thread::sleep_for(std::chrono::seconds(2));
 
   ur_driver_.reset();
@@ -325,8 +329,6 @@ void URPositionHardwareInterface::readBitsetData(const std::unique_ptr<rtde::Dat
 
 return_type URPositionHardwareInterface::read()
 {
-  RCLCPP_INFO(rclcpp::get_logger("URPositionHardwareInterface"), "Reading ...");
-
   std::unique_ptr<rtde::DataPackage> data_pkg = ur_driver_->getDataPackage();
 
   if (data_pkg)
@@ -374,36 +376,28 @@ return_type URPositionHardwareInterface::write()
        runtime_state_ == static_cast<uint32_t>(rtde::RUNTIME_STATE::PAUSING)) &&
       robot_program_running_ && (!non_blocking_read_ || packet_read_))
   {
-    RCLCPP_INFO(rclcpp::get_logger("URPositionHardwareInterface"), "Writing ...");
-
-    // create a lambda substract functor
-    std::function<double(double, double)> substractor = [](double a, double b) { return std::abs(a - b); };
+    if (!position_interface_in_use_)
+    {
+      // create a lambda substract functor
+      std::function<double(double, double)> substractor = [](double a, double b) { return std::abs(a - b); };
 
-    // create a position difference vector
-    std::vector<double> pos_diff;
-    pos_diff.resize(urcl_position_commands_.size());
-    std::transform(urcl_position_commands_.begin(), urcl_position_commands_.end(), urcl_position_commands_old_.begin(),
-                   pos_diff.begin(), substractor);
+      // create a position difference vector
+      std::vector<double> pos_diff;
+      pos_diff.resize(urcl_position_commands_.size());
+      std::transform(urcl_position_commands_.begin(), urcl_position_commands_.end(),
+                     urcl_position_commands_old_.begin(), pos_diff.begin(), substractor);
 
-    // create a velocity difference vector
-    std::vector<double> vel_diff;
-    vel_diff.resize(urcl_velocity_commands_.size());
-    std::transform(urcl_velocity_commands_.begin(), urcl_velocity_commands_.end(), urcl_velocity_commands_old_.begin(),
-                   vel_diff.begin(), substractor);
+      double pos_diff_sum = 0.0;
+      std::for_each(pos_diff.begin(), pos_diff.end(), [&pos_diff_sum](double a) { return pos_diff_sum += a; });
 
-    double pos_diff_sum = 0.0;
-    double vel_diff_sum = 0.0;
-    std::for_each(pos_diff.begin(), pos_diff.end(), [&pos_diff_sum](double a) { return pos_diff_sum += a; });
-    std::for_each(vel_diff.begin(), vel_diff.end(), [&vel_diff_sum](double a) { return vel_diff_sum += a; });
+      if (pos_diff_sum != 0.0)
+        position_interface_in_use_ = true;
+    }
 
-    if (pos_diff_sum != 0.0)
+    if (position_interface_in_use_)
     {
       ur_driver_->writeJointCommand(urcl_position_commands_, urcl::comm::ControlMode::MODE_SERVOJ);
     }
-    else if (vel_diff_sum != 0.0)
-    {
-      ur_driver_->writeJointCommand(urcl_velocity_commands_, urcl::comm::ControlMode::MODE_SPEEDJ);
-    }
     else
     {
       ur_driver_->writeKeepalive();
@@ -413,7 +407,6 @@ return_type URPositionHardwareInterface::write()
 
     // remember old values
     urcl_position_commands_old_ = urcl_position_commands_;
-    urcl_velocity_commands_old_ = urcl_velocity_commands_;
 
     return return_type::OK;
   }