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/include/ur_robot_driver/ros/hardware_interface.h b/ur_robot_driver/include/ur_robot_driver/ros/hardware_interface.h index 9d89f0f49..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 @@ -45,8 +45,13 @@ #include #include "ur_msgs/SetSpeedSliderFraction.h" +#include #include #include +#include +#include +#include +#include #include "ur_robot_driver/ur/ur_driver.h" #include @@ -156,6 +161,19 @@ class HardwareInterface : public hardware_interface::RobotHW bool shouldResetControllers(); protected: + /*! + * \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, + 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 joint_position_lower_limits_; + std::vector joint_position_upper_limits_; + std::vector joint_velocity_limits_; + + 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..7143510f0 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) { } @@ -72,6 +75,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(root_nh, "robot_description"); + // The robot's IP address. if (!robot_hw_nh.getParam("robot_ip", robot_ip_)) { @@ -295,14 +301,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( @@ -519,10 +529,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 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..5d81da712 --- /dev/null +++ b/ur_robot_driver/src/ros/joint_limit_interface.cpp @@ -0,0 +1,194 @@ +/********************************************************************* + * 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 + + 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" + +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