joint_limits_urdf.h
Go to the documentation of this file.
1 // Copyright (C) 2013, PAL Robotics S.L.
3 //
4 // Redistribution and use in source and binary forms, with or without
5 // modification, are permitted provided that the following conditions are met:
6 // * Redistributions of source code must retain the above copyright notice,
7 // this list of conditions and the following disclaimer.
8 // * Redistributions in binary form must reproduce the above copyright
9 // notice, this list of conditions and the following disclaimer in the
10 // documentation and/or other materials provided with the distribution.
11 // * Neither the name of PAL Robotics S.L. nor the names of its
12 // contributors may be used to endorse or promote products derived from
13 // this software without specific prior written permission.
14 //
15 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
16 // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
17 // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
18 // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
19 // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
20 // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
21 // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
22 // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
23 // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
24 // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
25 // POSSIBILITY OF SUCH DAMAGE.
27 
29 
30 #ifndef JOINT_LIMITS_INTERFACE_JOINT_LIMITS_URDF_H
31 #define JOINT_LIMITS_INTERFACE_JOINT_LIMITS_URDF_H
32 
33 #include <ros/common.h>
34 #include <urdf_model/joint.h>
35 #include <urdf/urdfdom_compatibility.h>
37 
38 namespace joint_limits_interface
39 {
40 
48 inline bool getJointLimits(urdf::JointConstSharedPtr urdf_joint, JointLimits& limits)
49 {
50  if (!urdf_joint || !urdf_joint->limits)
51  {
52  return false;
53  }
54 
55  limits.has_position_limits = urdf_joint->type == urdf::Joint::REVOLUTE || urdf_joint->type == urdf::Joint::PRISMATIC;
56  if (limits.has_position_limits)
57  {
58  limits.min_position = urdf_joint->limits->lower;
59  limits.max_position = urdf_joint->limits->upper;
60  }
61 
62  if (!limits.has_position_limits && urdf_joint->type == urdf::Joint::CONTINUOUS)
63  {
64  limits.angle_wraparound = true;
65  }
66 
67  limits.has_velocity_limits = true;
68  limits.max_velocity = urdf_joint->limits->velocity;
69 
70  limits.has_acceleration_limits = false;
71 
72  limits.has_effort_limits = true;
73  limits.max_effort = urdf_joint->limits->effort;
74 
75  return true;
76 }
77 
84 inline bool getSoftJointLimits(urdf::JointConstSharedPtr urdf_joint, SoftJointLimits& soft_limits)
85 {
86  if (!urdf_joint || !urdf_joint->safety)
87  {
88  return false;
89  }
90 
91  soft_limits.min_position = urdf_joint->safety->soft_lower_limit;
92  soft_limits.max_position = urdf_joint->safety->soft_upper_limit;
93  soft_limits.k_position = urdf_joint->safety->k_position;
94  soft_limits.k_velocity = urdf_joint->safety->k_velocity;
95 
96  return true;
97 }
98 
99 }
100 
101 #endif
bool getSoftJointLimits(const std::string &joint_name, const ros::NodeHandle &nh, SoftJointLimits &soft_limits)
Populate a SoftJointLimits instance from the ROS parameter server.
bool getJointLimits(const std::string &joint_name, const ros::NodeHandle &nh, JointLimits &limits)
Populate a JointLimits instance from the ROS parameter server.


joint_limits_interface
Author(s): Adolfo Rodriguez Tsouroukdissian
autogenerated on Fri Jun 7 2019 22:00:13