1+ /* ********************************************************************
2+ * Software License Agreement (BSD License)
3+ *
4+ * Copyright (c) 2015, PickNik LLC
5+ * All rights reserved.
6+ *
7+ * Redistribution and use in source and binary forms, with or without
8+ * modification, are permitted provided that the following conditions
9+ * are met:
10+ *
11+ * * Redistributions of source code must retain the above copyright
12+ * notice, this list of conditions and the following disclaimer.
13+ * * Redistributions in binary form must reproduce the above
14+ * copyright notice, this list of conditions and the following
15+ * disclaimer in the documentation and/or other materials provided
16+ * with the distribution.
17+ * * Neither the name of PickNik LLC nor the names of its
18+ * contributors may be used to endorse or promote products derived
19+ * from this software without specific prior written permission.
20+ *
21+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32+ * POSSIBILITY OF SUCH DAMAGE.
33+ *********************************************************************/
34+
35+ /* Author: Dave Coleman
36+ Desc: Helper ros_control hardware interface that loads configurations
37+ */
38+
39+ #include " ur_robot_driver/ros/hardware_interface.h"
40+
41+ namespace ur_driver
42+ {
43+ void HardwareInterface::registerJointLimits (ros::NodeHandle& robot_hw_nh,
44+ const hardware_interface::JointHandle& joint_handle_position,
45+ const hardware_interface::JointHandle& joint_handle_velocity,
46+ std::size_t joint_id)
47+ {
48+ // Default values
49+ joint_position_lower_limits_[joint_id] = -std::numeric_limits<double >::max ();
50+ joint_position_upper_limits_[joint_id] = std::numeric_limits<double >::max ();
51+ joint_velocity_limits_[joint_id] = std::numeric_limits<double >::max ();
52+
53+ // Limits datastructures
54+ joint_limits_interface::JointLimits joint_limits; // Position
55+ joint_limits_interface::SoftJointLimits soft_limits; // Soft Position
56+ bool has_joint_limits = false ;
57+ bool has_soft_limits = false ;
58+
59+ // Get limits from URDF
60+ if (urdf_model_ == nullptr )
61+ {
62+ ROS_WARN_STREAM (" No URDF model loaded, unable to get joint limits"
63+ return ;
64+ }
65+
66+ // Get limits from URDF
67+ urdf::JointConstSharedPtr urdf_joint = urdf_model_->getJoint (joint_names_[joint_id]);
68+
69+ // Get main joint limits
70+ if (urdf_joint == nullptr )
71+ {
72+ ROS_ERROR_STREAM (" URDF joint not found "
73+ return ;
74+ }
75+
76+ // Get limits from URDF
77+ if (joint_limits_interface::getJointLimits (urdf_joint, joint_limits))
78+ {
79+ has_joint_limits = true ;
80+ ROS_DEBUG_STREAM (" Joint " " has URDF position limits [" min_position
81+ << " , " max_position << " ]"
82+ if (joint_limits.has_velocity_limits )
83+ ROS_DEBUG_STREAM (" Joint " " has URDF velocity limit [" max_velocity
84+ << " ]"
85+ }
86+ else
87+ {
88+ if (urdf_joint->type != urdf::Joint::CONTINUOUS)
89+ ROS_WARN_STREAM (" No URDF limits are configured for joint "
90+ }
91+
92+ // Get limits from ROS param
93+ if (joint_limits_interface::getJointLimits (joint_names_[joint_id], robot_hw_nh, joint_limits))
94+ {
95+ has_joint_limits = true ;
96+ ROS_DEBUG_STREAM (" Joint " " has rosparam position limits ["
97+ << joint_limits.min_position << " , " max_position << " ]"
98+ if (joint_limits.has_velocity_limits )
99+ ROS_DEBUG_STREAM (" Joint " " has rosparam velocity limit ["
100+ << joint_limits.max_velocity << " ]"
101+ } // the else debug message provided internally by joint_limits_interface
102+
103+ // Get soft limits from URDF
104+ if (joint_limits_interface::getSoftJointLimits (urdf_joint, soft_limits))
105+ {
106+ has_soft_limits = true ;
107+ ROS_DEBUG_STREAM (" Joint " " has soft joint limits."
108+ }
109+ else
110+ {
111+ ROS_DEBUG_STREAM (" Joint "
112+ << " does not have soft joint "
113+ " limits"
114+ }
115+
116+ // Quit we we haven't found any limits in URDF or rosparam server
117+ if (!has_joint_limits)
118+ {
119+ return ;
120+ }
121+
122+ // Copy position limits if available
123+ if (joint_limits.has_position_limits )
124+ {
125+ // Slighly reduce the joint limits to prevent floating point errors
126+ joint_limits.min_position += std::numeric_limits<double >::epsilon ();
127+ joint_limits.max_position -= std::numeric_limits<double >::epsilon ();
128+
129+ joint_position_lower_limits_[joint_id] = joint_limits.min_position ;
130+ joint_position_upper_limits_[joint_id] = joint_limits.max_position ;
131+ }
132+
133+ // Copy velocity limits if available
134+ if (joint_limits.has_velocity_limits )
135+ {
136+ joint_velocity_limits_[joint_id] = joint_limits.max_velocity ;
137+ }
138+
139+ if (has_soft_limits) // Use soft limits
140+ {
141+ ROS_DEBUG_STREAM (" Using soft saturation limits"
142+ const joint_limits_interface::PositionJointSoftLimitsHandle soft_handle_position (joint_handle_position,
143+ joint_limits, soft_limits);
144+ pos_jnt_soft_interface_.registerHandle (soft_handle_position);
145+ const joint_limits_interface::VelocityJointSoftLimitsHandle soft_handle_velocity (joint_handle_velocity,
146+ joint_limits, soft_limits);
147+ vel_jnt_soft_interface_.registerHandle (soft_handle_velocity);
148+ }
149+ else // Use saturation limits
150+ {
151+ ROS_DEBUG_STREAM (" Using saturation limits (not soft limits)"
152+
153+ const joint_limits_interface::PositionJointSaturationHandle sat_handle_position (joint_handle_position,
154+ joint_limits);
155+ pos_jnt_sat_interface_.registerHandle (sat_handle_position);
156+
157+ const joint_limits_interface::VelocityJointSaturationHandle sat_handle_velocity (joint_handle_velocity,
158+ joint_limits);
159+ vel_jnt_sat_interface_.registerHandle (sat_handle_velocity);
160+ }
161+ }
162+
163+ void HardwareInterface::loadURDF (ros::NodeHandle& nh, std::string param_name)
164+ {
165+ std::string urdf_string;
166+ urdf_model_ = new urdf::Model ();
167+
168+ // search and wait for robot_description on param server
169+ while (urdf_string.empty () && ros::ok ())
170+ {
171+ std::string search_param_name;
172+ if (nh.searchParam (param_name, search_param_name))
173+ {
174+ ROS_INFO_STREAM (" Waiting for model URDF on the ROS param server at location: " getNamespace ()
175+ << search_param_name);
176+ nh.getParam (search_param_name, urdf_string);
177+ }
178+
179+ usleep (100000 );
180+ }
181+
182+ if (!urdf_model_->initString (urdf_string))
183+ ROS_ERROR_STREAM (" Unable to load URDF model"
184+ else
185+ ROS_DEBUG_STREAM (" Received URDF from param server"
186+ }
187+ }
0 commit comments