Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00027
00029
00030 #ifndef JOINT_LIMITS_INTERFACE_JOINT_LIMITS_URDF_H
00031 #define JOINT_LIMITS_INTERFACE_JOINT_LIMITS_URDF_H
00032
00033 #include <ros/common.h>
00034 #include <urdf/urdfdom_compatibility.h>
00035 #include <urdf_model/joint.h>
00036 #include <joint_limits_interface/joint_limits.h>
00037
00038 namespace joint_limits_interface
00039 {
00040
00048 inline bool getJointLimits(urdf::JointConstSharedPtr urdf_joint, JointLimits& limits)
00049 {
00050 if (!urdf_joint || !urdf_joint->limits)
00051 {
00052 return false;
00053 }
00054
00055 limits.has_position_limits = urdf_joint->type == urdf::Joint::REVOLUTE || urdf_joint->type == urdf::Joint::PRISMATIC;
00056 if (limits.has_position_limits)
00057 {
00058 limits.min_position = urdf_joint->limits->lower;
00059 limits.max_position = urdf_joint->limits->upper;
00060 }
00061
00062 if (!limits.has_position_limits && urdf_joint->type == urdf::Joint::CONTINUOUS)
00063 {
00064 limits.angle_wraparound = true;
00065 }
00066
00067 limits.has_velocity_limits = true;
00068 limits.max_velocity = urdf_joint->limits->velocity;
00069
00070 limits.has_acceleration_limits = false;
00071
00072 limits.has_effort_limits = true;
00073 limits.max_effort = urdf_joint->limits->effort;
00074
00075 return true;
00076 }
00077
00084 inline bool getSoftJointLimits(urdf::JointConstSharedPtr urdf_joint, SoftJointLimits& soft_limits)
00085 {
00086 if (!urdf_joint || !urdf_joint->safety)
00087 {
00088 return false;
00089 }
00090
00091 soft_limits.min_position = urdf_joint->safety->soft_lower_limit;
00092 soft_limits.max_position = urdf_joint->safety->soft_upper_limit;
00093 soft_limits.k_position = urdf_joint->safety->k_position;
00094 soft_limits.k_velocity = urdf_joint->safety->k_velocity;
00095
00096 return true;
00097 }
00098
00099 }
00100
00101 #endif