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
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037 #ifndef MOVEIT_KINEMATIC_CONSTRAINTS_UTILS_
00038 #define MOVEIT_KINEMATIC_CONSTRAINTS_UTILS_
00039
00040 #include <moveit_msgs/MotionPlanRequest.h>
00041 #include <geometry_msgs/PointStamped.h>
00042 #include <geometry_msgs/PoseStamped.h>
00043 #include <geometry_msgs/QuaternionStamped.h>
00044 #include <moveit/robot_state/robot_state.h>
00045 #include <limits>
00046
00047 namespace kinematic_constraints
00048 {
00062 moveit_msgs::Constraints mergeConstraints(const moveit_msgs::Constraints& first,
00063 const moveit_msgs::Constraints& second);
00064
00066 bool isEmpty(const moveit_msgs::Constraints& constr);
00067
00068 std::size_t countIndividualConstraints(const moveit_msgs::Constraints& constr);
00069
00082 moveit_msgs::Constraints constructGoalConstraints(const robot_state::RobotState& state,
00083 const robot_model::JointModelGroup* jmg, double tolerance_below,
00084 double tolerance_above);
00085
00097 moveit_msgs::Constraints constructGoalConstraints(const robot_state::RobotState& state,
00098 const robot_model::JointModelGroup* jmg,
00099 double tolerance = std::numeric_limits<double>::epsilon());
00100
00116 moveit_msgs::Constraints constructGoalConstraints(const std::string& link_name, const geometry_msgs::PoseStamped& pose,
00117 double tolerance_pos = 1e-3, double tolerance_angle = 1e-2);
00118
00134 moveit_msgs::Constraints constructGoalConstraints(const std::string& link_name, const geometry_msgs::PoseStamped& pose,
00135 const std::vector<double>& tolerance_pos,
00136 const std::vector<double>& tolerance_angle);
00137
00149 moveit_msgs::Constraints constructGoalConstraints(const std::string& link_name,
00150 const geometry_msgs::QuaternionStamped& quat,
00151 double tolerance = 1e-2);
00152
00166 moveit_msgs::Constraints constructGoalConstraints(const std::string& link_name,
00167 const geometry_msgs::Point& reference_point,
00168 const geometry_msgs::PointStamped& goal_point,
00169 double tolerance = 1e-3);
00170
00183 moveit_msgs::Constraints constructGoalConstraints(const std::string& link_name,
00184 const geometry_msgs::PointStamped& goal_point,
00185 double tolerance = 1e-3);
00186 }
00187
00188 #endif