From 8828a43713bf6e5f46a7a661ef2a2ae89ca9aabb Mon Sep 17 00:00:00 2001 From: Harsh Deshpande Date: Tue, 7 Jul 2020 15:23:53 +0200 Subject: [PATCH 1/5] loads URDF model from parameter server --- .../ur_robot_driver/ros/hardware_interface.h | 11 ++++++ .../src/ros/hardware_interface.cpp | 34 +++++++++++++++++++ 2 files changed, 45 insertions(+) diff --git a/ur_robot_driver/include/ur_robot_driver/ros/hardware_interface.h b/ur_robot_driver/include/ur_robot_driver/ros/hardware_interface.h index 9d89f0f49..06b2a5d40 100644 --- a/ur_robot_driver/include/ur_robot_driver/ros/hardware_interface.h +++ b/ur_robot_driver/include/ur_robot_driver/ros/hardware_interface.h @@ -45,6 +45,7 @@ #include #include "ur_msgs/SetSpeedSliderFraction.h" +#include #include #include @@ -156,6 +157,13 @@ class HardwareInterface : public hardware_interface::RobotHW bool shouldResetControllers(); protected: + + /*! + * \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 +231,9 @@ class HardwareInterface : public hardware_interface::RobotHW ur_controllers::ScaledVelocityJointInterface svj_interface_; hardware_interface::ForceTorqueSensorInterface fts_interface_; + + urdf::Model* urdf_model_; + vector6d_t joint_position_command_; vector6d_t joint_velocity_command_; vector6d_t joint_positions_; diff --git a/ur_robot_driver/src/ros/hardware_interface.cpp b/ur_robot_driver/src/ros/hardware_interface.cpp index 74cfe983c..303ca8725 100644 --- a/ur_robot_driver/src/ros/hardware_interface.cpp +++ b/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_)) { @@ -956,6 +959,37 @@ bool HardwareInterface::checkControllerClaims(const std::set& 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) From 72463b38d38d02fc7eb175b4e9a0f3f6fd6aed35 Mon Sep 17 00:00:00 2001 From: Harsh Deshpande Date: Tue, 7 Jul 2020 15:30:00 +0200 Subject: [PATCH 2/5] enforcing joint limits from urdf / parameter server --- .../ur_robot_driver/ros/hardware_interface.h | 22 +++ .../src/ros/hardware_interface.cpp | 138 +++++++++++++++++- 2 files changed, 156 insertions(+), 4 deletions(-) diff --git a/ur_robot_driver/include/ur_robot_driver/ros/hardware_interface.h b/ur_robot_driver/include/ur_robot_driver/ros/hardware_interface.h index 06b2a5d40..c4ab0f1e3 100644 --- a/ur_robot_driver/include/ur_robot_driver/ros/hardware_interface.h +++ b/ur_robot_driver/include/ur_robot_driver/ros/hardware_interface.h @@ -48,6 +48,10 @@ #include #include #include +#include +#include +#include +#include #include "ur_robot_driver/ur/ur_driver.h" #include @@ -157,6 +161,12 @@ 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 @@ -231,6 +241,18 @@ 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 joint_position_lower_limits_; + std::vector joint_position_upper_limits_; + std::vector joint_velocity_limits_; urdf::Model* urdf_model_; diff --git a/ur_robot_driver/src/ros/hardware_interface.cpp b/ur_robot_driver/src/ros/hardware_interface.cpp index 303ca8725..a24c034b9 100644 --- a/ur_robot_driver/src/ros/hardware_interface.cpp +++ b/ur_robot_driver/src/ros/hardware_interface.cpp @@ -298,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( @@ -385,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::max(); + joint_position_upper_limits_[joint_id] = std::numeric_limits::max(); + joint_velocity_limits_[joint_id] = std::numeric_limits::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::epsilon(); + joint_limits.max_position -= std::numeric_limits::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 void HardwareInterface::readData(const std::unique_ptr& data_pkg, const std::string& var_name, T& data) @@ -522,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 From 7315fefd8ce1654dfbfd033b641c1d14e1e6fbdd Mon Sep 17 00:00:00 2001 From: Harsh Deshpande Date: Tue, 21 Jul 2020 14:52:01 +0200 Subject: [PATCH 3/5] Separated boilterplate example in a separate cpp with license --- ur_robot_driver/CMakeLists.txt | 2 + .../src/ros/hardware_interface.cpp | 156 +-------------- .../src/ros/joint_limit_interface.cpp | 187 ++++++++++++++++++ 3 files changed, 192 insertions(+), 153 deletions(-) create mode 100644 ur_robot_driver/src/ros/joint_limit_interface.cpp diff --git a/ur_robot_driver/CMakeLists.txt b/ur_robot_driver/CMakeLists.txt index eb89d7cf5..14a25fade 100644 --- a/ur_robot_driver/CMakeLists.txt +++ b/ur_robot_driver/CMakeLists.txt @@ -115,6 +115,7 @@ add_dependencies(ur_robot_driver ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EX add_library(ur_robot_driver_plugin src/ros/dashboard_client_ros.cpp src/ros/hardware_interface.cpp + src/ros/joint_limit_interface.cpp ) target_link_libraries(ur_robot_driver_plugin ur_robot_driver ${catkin_LIBRARIES}) add_dependencies(ur_robot_driver_plugin ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) @@ -123,6 +124,7 @@ add_executable(ur_robot_driver_node src/ros/dashboard_client_ros.cpp src/ros/hardware_interface.cpp src/ros/hardware_interface_node.cpp + src/ros/joint_limit_interface.cpp ) target_link_libraries(ur_robot_driver_node ${catkin_LIBRARIES} ur_robot_driver) add_dependencies(ur_robot_driver_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) diff --git a/ur_robot_driver/src/ros/hardware_interface.cpp b/ur_robot_driver/src/ros/hardware_interface.cpp index a24c034b9..6a7f421eb 100644 --- a/ur_robot_driver/src/ros/hardware_interface.cpp +++ b/ur_robot_driver/src/ros/hardware_interface.cpp @@ -61,6 +61,9 @@ HardwareInterface::HardwareInterface() , pausing_state_(PausingState::RUNNING) , pausing_ramp_up_increment_(0.01) , controllers_initialized_(false) + , joint_position_lower_limits_(6) + , joint_position_upper_limits_(6) + , joint_velocity_limits_(6) { } @@ -389,128 +392,6 @@ 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::max(); - joint_position_upper_limits_[joint_id] = std::numeric_limits::max(); - joint_velocity_limits_[joint_id] = std::numeric_limits::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::epsilon(); - joint_limits.max_position -= std::numeric_limits::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 void HardwareInterface::readData(const std::unique_ptr& data_pkg, const std::string& var_name, T& data) @@ -1089,37 +970,6 @@ bool HardwareInterface::checkControllerClaims(const std::set& 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) diff --git a/ur_robot_driver/src/ros/joint_limit_interface.cpp b/ur_robot_driver/src/ros/joint_limit_interface.cpp new file mode 100644 index 000000000..55fa45ea3 --- /dev/null +++ b/ur_robot_driver/src/ros/joint_limit_interface.cpp @@ -0,0 +1,187 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2015, PickNik LLC + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of PickNik LLC nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Dave Coleman + Desc: Helper ros_control hardware interface that loads configurations +*/ + +#include "ur_robot_driver/ros/hardware_interface.h" + +namespace ur_driver +{ +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::max(); + joint_position_upper_limits_[joint_id] = std::numeric_limits::max(); + joint_velocity_limits_[joint_id] = std::numeric_limits::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_ == nullptr) + { + 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 == nullptr) + { + 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("No URDF limits are configured for joint " << joint_names_[joint_id]); + } + + // 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::epsilon(); + joint_limits.max_position -= std::numeric_limits::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); + } +} + +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); + } + + 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 From a1422f1251e049bec5990b1217df16fa8d22920d Mon Sep 17 00:00:00 2001 From: Harsh Deshpande Date: Fri, 9 Oct 2020 12:08:18 +0200 Subject: [PATCH 4/5] Added note to document changes from original file --- .../include/ur_robot_driver/ros/hardware_interface.h | 2 +- ur_robot_driver/src/ros/hardware_interface.cpp | 2 +- ur_robot_driver/src/ros/joint_limit_interface.cpp | 7 +++++++ 3 files changed, 9 insertions(+), 2 deletions(-) diff --git a/ur_robot_driver/include/ur_robot_driver/ros/hardware_interface.h b/ur_robot_driver/include/ur_robot_driver/ros/hardware_interface.h index c4ab0f1e3..77f7af653 100644 --- a/ur_robot_driver/include/ur_robot_driver/ros/hardware_interface.h +++ b/ur_robot_driver/include/ur_robot_driver/ros/hardware_interface.h @@ -162,7 +162,7 @@ class HardwareInterface : public hardware_interface::RobotHW protected: /*! - * \brief Applies joint limits & soft joint limits on position, velocity & effort defined in URDF / parameter server + * \brief Registers 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, diff --git a/ur_robot_driver/src/ros/hardware_interface.cpp b/ur_robot_driver/src/ros/hardware_interface.cpp index 6a7f421eb..7143510f0 100644 --- a/ur_robot_driver/src/ros/hardware_interface.cpp +++ b/ur_robot_driver/src/ros/hardware_interface.cpp @@ -76,7 +76,7 @@ bool HardwareInterface::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw std::string input_recipe_filename; // Load URDF file from parameter server - loadURDF(robot_hw_nh, "robot_description"); + loadURDF(root_nh, "robot_description"); // The robot's IP address. if (!robot_hw_nh.getParam("robot_ip", robot_ip_)) diff --git a/ur_robot_driver/src/ros/joint_limit_interface.cpp b/ur_robot_driver/src/ros/joint_limit_interface.cpp index 55fa45ea3..5d81da712 100644 --- a/ur_robot_driver/src/ros/joint_limit_interface.cpp +++ b/ur_robot_driver/src/ros/joint_limit_interface.cpp @@ -34,6 +34,13 @@ /* Author: Dave Coleman Desc: Helper ros_control hardware interface that loads configurations + + Note: The code used here is originally from + https://github.com/PickNikRobotics/ros_control_boilerplate/blob/melodic-devel/src/generic_hw_interface.cpp + with slight modifications + 1. Removed references to EffortJointInterface, since it's not supported on URs + 2. Changed logic to get "robot_description" param from parameter server + - removed the additional getparam condition in case searchParam fails */ #include "ur_robot_driver/ros/hardware_interface.h" From 7a4fcfbd23b864f20c823506865e59d37af99905 Mon Sep 17 00:00:00 2001 From: Harsh Deshpande Date: Mon, 12 Oct 2020 10:50:16 +0200 Subject: [PATCH 5/5] restarting CI checks