[WIP] Enforce joint limits

ur_robot_driver/include/ur_robot_driver/ros/hardware_interface.h

@@ -45,8 +45,13 @@
 #include <ur_msgs/SetIO.h>
 #include "ur_msgs/SetSpeedSliderFraction.h"
 
+#include <urdf/model.h>
 #include <ur_controllers/speed_scaling_interface.h>
 #include <ur_controllers/scaled_joint_command_interface.h>
+#include <joint_limits_interface/joint_limits.h>
+#include <joint_limits_interface/joint_limits_interface.h>
+#include <joint_limits_interface/joint_limits_rosparam.h>
+#include <joint_limits_interface/joint_limits_urdf.h>
 
 #include "ur_robot_driver/ur/ur_driver.h"
 #include <ur_robot_driver/ros/dashboard_client_ros.h>
@@ -156,6 +161,19 @@ class HardwareInterface : public hardware_interface::RobotHW
   bool shouldResetControllers();
 
 protected:
+  /*!
+   * \brief Applies joint limits & soft joint limits on position, velocity & effort defined in URDF / parameter server
+   *
+   */
+  void registerJointLimits(ros::NodeHandle& robot_hw_nh, const hardware_interface::JointHandle& joint_handle_position,
+                           const hardware_interface::JointHandle& joint_handle_velocity, std::size_t joint_id);
+
+  /*!
+   * \brief Loads URDF model from robot_description parameter
+   *
+   * Requires robot_description paramter to be set on the parameter server
+   */
+  void loadURDF(ros::NodeHandle& nh, std::string param_name);
   /*!
    * \brief Transforms force-torque measurements reported from the robot from base to tool frame.
    *
@@ -223,6 +241,21 @@ class HardwareInterface : public hardware_interface::RobotHW
   ur_controllers::ScaledVelocityJointInterface svj_interface_;
   hardware_interface::ForceTorqueSensorInterface fts_interface_;
 
+  // Joint limits interfaces - Saturation
+  joint_limits_interface::PositionJointSaturationInterface pos_jnt_sat_interface_;
+  joint_limits_interface::VelocityJointSaturationInterface vel_jnt_sat_interface_;
+
+  // Joint limits interfaces - Soft limits
+  joint_limits_interface::PositionJointSoftLimitsInterface pos_jnt_soft_interface_;
+  joint_limits_interface::VelocityJointSoftLimitsInterface vel_jnt_soft_interface_;
+
+  // Copy of limits, in case we need them later in our control stack
+  std::vector<double> joint_position_lower_limits_;
+  std::vector<double> joint_position_upper_limits_;
+  std::vector<double> joint_velocity_limits_;
+
+  urdf::Model* urdf_model_;
+
   vector6d_t joint_position_command_;
   vector6d_t joint_velocity_command_;
   vector6d_t joint_positions_;

ur_robot_driver/src/ros/hardware_interface.cpp

@@ -72,6 +72,9 @@ bool HardwareInterface::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw
   std::string output_recipe_filename;
   std::string input_recipe_filename;
 
+  // Load URDF file from parameter server
+  loadURDF(robot_hw_nh, "robot_description");
+
   // The robot's IP address.
   if (!robot_hw_nh.getParam("robot_ip", robot_ip_))
   {
@@ -295,14 +298,18 @@ bool HardwareInterface::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw
                                                                       &joint_velocities_[i], &joint_efforts_[i]));
 
     // Create joint position control interface
-    pj_interface_.registerHandle(
-        hardware_interface::JointHandle(js_interface_.getHandle(joint_names_[i]), &joint_position_command_[i]));
-    vj_interface_.registerHandle(
-        hardware_interface::JointHandle(js_interface_.getHandle(joint_names_[i]), &joint_velocity_command_[i]));
+    hardware_interface::JointHandle joint_handle_position =
+        hardware_interface::JointHandle(js_interface_.getHandle(joint_names_[i]), &joint_position_command_[i]);
+    pj_interface_.registerHandle(joint_handle_position);
+    hardware_interface::JointHandle joint_handle_velocity =
+        hardware_interface::JointHandle(js_interface_.getHandle(joint_names_[i]), &joint_velocity_command_[i]);
+    vj_interface_.registerHandle(joint_handle_velocity);
     spj_interface_.registerHandle(ur_controllers::ScaledJointHandle(
         js_interface_.getHandle(joint_names_[i]), &joint_position_command_[i], &speed_scaling_combined_));
     svj_interface_.registerHandle(ur_controllers::ScaledJointHandle(
         js_interface_.getHandle(joint_names_[i]), &joint_velocity_command_[i], &speed_scaling_combined_));
+
+    registerJointLimits(robot_hw_nh, joint_handle_position, joint_handle_velocity, i);
   }
 
   speedsc_interface_.registerHandle(
@@ -382,6 +389,128 @@ bool HardwareInterface::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw
   return true;
 }
 
+void HardwareInterface::registerJointLimits(ros::NodeHandle& robot_hw_nh,
+                                            const hardware_interface::JointHandle& joint_handle_position,
+                                            const hardware_interface::JointHandle& joint_handle_velocity,
+                                            std::size_t joint_id)
+{
+  // Default values
+  joint_position_lower_limits_[joint_id] = -std::numeric_limits<double>::max();
+  joint_position_upper_limits_[joint_id] = std::numeric_limits<double>::max();
+  joint_velocity_limits_[joint_id] = std::numeric_limits<double>::max();
+
+  // Limits datastructures
+  joint_limits_interface::JointLimits joint_limits;     // Position
+  joint_limits_interface::SoftJointLimits soft_limits;  // Soft Position
+  bool has_joint_limits = false;
+  bool has_soft_limits = false;
+
+  // Get limits from URDF
+  if (urdf_model_ == NULL)
+  {
+    ROS_WARN_STREAM("No URDF model loaded, unable to get joint limits");
+    return;
+  }
+
+  // Get limits from URDF
+  urdf::JointConstSharedPtr urdf_joint = urdf_model_->getJoint(joint_names_[joint_id]);
+
+  // Get main joint limits
+  if (urdf_joint == NULL)
+  {
+    ROS_ERROR_STREAM("URDF joint not found " << joint_names_[joint_id]);
+    return;
+  }
+
+  // Get limits from URDF
+  if (joint_limits_interface::getJointLimits(urdf_joint, joint_limits))
+  {
+    has_joint_limits = true;
+    ROS_DEBUG_STREAM("Joint " << joint_names_[joint_id] << " has URDF position limits [" << joint_limits.min_position
+                              << ", " << joint_limits.max_position << "]");
+    if (joint_limits.has_velocity_limits)
+      ROS_DEBUG_STREAM("Joint " << joint_names_[joint_id] << " has URDF velocity limit [" << joint_limits.max_velocity
+                                << "]");
+  }
+  else
+  {
+    if (urdf_joint->type != urdf::Joint::CONTINUOUS)
+      ROS_WARN_STREAM("Joint " << joint_names_[joint_id]
+                               << " does not have a URDF "
+                                  "position limit");
+  }
+
+  // Get limits from ROS param
+  if (joint_limits_interface::getJointLimits(joint_names_[joint_id], robot_hw_nh, joint_limits))
+  {
+    has_joint_limits = true;
+    ROS_DEBUG_STREAM("Joint " << joint_names_[joint_id] << " has rosparam position limits ["
+                              << joint_limits.min_position << ", " << joint_limits.max_position << "]");
+    if (joint_limits.has_velocity_limits)
+      ROS_DEBUG_STREAM("Joint " << joint_names_[joint_id] << " has rosparam velocity limit ["
+                                << joint_limits.max_velocity << "]");
+  }  // the else debug message provided internally by joint_limits_interface
+
+  // Get soft limits from URDF
+  if (joint_limits_interface::getSoftJointLimits(urdf_joint, soft_limits))
+  {
+    has_soft_limits = true;
+    ROS_DEBUG_STREAM("Joint " << joint_names_[joint_id] << " has soft joint limits.");
+  }
+  else
+  {
+    ROS_DEBUG_STREAM("Joint " << joint_names_[joint_id]
+                              << " does not have soft joint "
+                                 "limits");
+  }
+
+  // Quit we we haven't found any limits in URDF or rosparam server
+  if (!has_joint_limits)
+  {
+    return;
+  }
+
+  // Copy position limits if available
+  if (joint_limits.has_position_limits)
+  {
+    // Slighly reduce the joint limits to prevent floating point errors
+    joint_limits.min_position += std::numeric_limits<double>::epsilon();
+    joint_limits.max_position -= std::numeric_limits<double>::epsilon();
+
+    joint_position_lower_limits_[joint_id] = joint_limits.min_position;
+    joint_position_upper_limits_[joint_id] = joint_limits.max_position;
+  }
+
+  // Copy velocity limits if available
+  if (joint_limits.has_velocity_limits)
+  {
+    joint_velocity_limits_[joint_id] = joint_limits.max_velocity;
+  }
+
+  if (has_soft_limits)  // Use soft limits
+  {
+    ROS_DEBUG_STREAM("Using soft saturation limits");
+    const joint_limits_interface::PositionJointSoftLimitsHandle soft_handle_position(joint_handle_position,
+                                                                                     joint_limits, soft_limits);
+    pos_jnt_soft_interface_.registerHandle(soft_handle_position);
+    const joint_limits_interface::VelocityJointSoftLimitsHandle soft_handle_velocity(joint_handle_velocity,
+                                                                                     joint_limits, soft_limits);
+    vel_jnt_soft_interface_.registerHandle(soft_handle_velocity);
+  }
+  else  // Use saturation limits
+  {
+    ROS_DEBUG_STREAM("Using saturation limits (not soft limits)");
+
+    const joint_limits_interface::PositionJointSaturationHandle sat_handle_position(joint_handle_position,
+                                                                                    joint_limits);
+    pos_jnt_sat_interface_.registerHandle(sat_handle_position);
+
+    const joint_limits_interface::VelocityJointSaturationHandle sat_handle_velocity(joint_handle_velocity,
+                                                                                    joint_limits);
+    vel_jnt_sat_interface_.registerHandle(sat_handle_velocity);
+  }
+}
+
 template <typename T>
 void HardwareInterface::readData(const std::unique_ptr<rtde_interface::DataPackage>& data_pkg,
                                  const std::string& var_name, T& data)
@@ -519,10 +648,14 @@ void HardwareInterface::write(const ros::Time& time, const ros::Duration& period
   {
     if (position_controller_running_)
     {
+      pos_jnt_soft_interface_.enforceLimits(period);
+      pos_jnt_sat_interface_.enforceLimits(period);
       ur_driver_->writeJointCommand(joint_position_command_, comm::ControlMode::MODE_SERVOJ);
     }
     else if (velocity_controller_running_)
     {
+      vel_jnt_soft_interface_.enforceLimits(period);
+      vel_jnt_sat_interface_.enforceLimits(period);
       ur_driver_->writeJointCommand(joint_velocity_command_, comm::ControlMode::MODE_SPEEDJ);
     }
     else
@@ -956,6 +1089,37 @@ bool HardwareInterface::checkControllerClaims(const std::set<std::string>& claim
   }
   return false;
 }
+
+void HardwareInterface::loadURDF(ros::NodeHandle& nh, std::string param_name)
+{
+  std::string urdf_string;
+  urdf_model_ = new urdf::Model();
+
+  // search and wait for robot_description on param server
+  while (urdf_string.empty() && ros::ok())
+  {
+    std::string search_param_name;
+    if (nh.searchParam(param_name, search_param_name))
+    {
+      ROS_INFO_STREAM("Waiting for model URDF on the ROS param server at location: " << nh.getNamespace()
+                                                                                     << search_param_name);
+      nh.getParam(search_param_name, urdf_string);
+    }
+    else
+    {
+      ROS_INFO_STREAM("Waiting for model URDF on the ROS param server at location: " << nh.getNamespace()
+                                                                                     << param_name);
+      nh.getParam(param_name, urdf_string);
+    }
+
+    usleep(100000);
+  }
+
+  if (!urdf_model_->initString(urdf_string))
+    ROS_ERROR_STREAM("Unable to load URDF model");
+  else
+    ROS_DEBUG_STREAM("Received URDF from param server");
+}
 }  // namespace ur_driver
 
 PLUGINLIB_EXPORT_CLASS(ur_driver::HardwareInterface, hardware_interface::RobotHW)