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_KINEMATIC_CONSTRAINT_
00038 #define MOVEIT_KINEMATIC_CONSTRAINTS_KINEMATIC_CONSTRAINT_
00039
00040 #include <moveit/robot_model/robot_model.h>
00041 #include <moveit/robot_state/robot_state.h>
00042 #include <moveit/transforms/transforms.h>
00043 #include <moveit/collision_detection/collision_world.h>
00044
00045 #include <geometric_shapes/bodies.h>
00046 #include <moveit_msgs/Constraints.h>
00047
00048 #include <iostream>
00049 #include <vector>
00050 #include <boost/shared_ptr.hpp>
00051
00053 namespace kinematic_constraints
00054 {
00055
00057 struct ConstraintEvaluationResult
00058 {
00067 ConstraintEvaluationResult(bool result_satisfied = false, double dist = 0.0) : satisfied(result_satisfied), distance(dist)
00068 {
00069 }
00070
00071 bool satisfied;
00072 double distance;
00073 };
00074
00076 class KinematicConstraint
00077 {
00078 public:
00079
00081 enum ConstraintType
00082 {
00083 UNKNOWN_CONSTRAINT, JOINT_CONSTRAINT, POSITION_CONSTRAINT, ORIENTATION_CONSTRAINT, VISIBILITY_CONSTRAINT
00084 };
00085
00091 KinematicConstraint(const robot_model::RobotModelConstPtr &model);
00092 virtual ~KinematicConstraint();
00093
00095 virtual void clear() = 0;
00096
00105 virtual ConstraintEvaluationResult decide(const robot_state::RobotState &state, bool verbose = false) const = 0;
00106
00112 virtual bool enabled() const = 0;
00113
00125 virtual bool equal(const KinematicConstraint &other, double margin) const = 0;
00126
00133 ConstraintType getType() const
00134 {
00135 return type_;
00136 }
00137
00143 virtual void print(std::ostream &out = std::cout) const
00144 {
00145 }
00146
00153 double getConstraintWeight() const
00154 {
00155 return constraint_weight_;
00156 }
00157
00164 const robot_model::RobotModelConstPtr& getRobotModel() const
00165 {
00166 return robot_model_;
00167 }
00168
00169 protected:
00170
00171 ConstraintType type_;
00172 robot_model::RobotModelConstPtr robot_model_;
00173 double constraint_weight_;
00174 };
00175
00176 typedef boost::shared_ptr<KinematicConstraint> KinematicConstraintPtr;
00177 typedef boost::shared_ptr<const KinematicConstraint> KinematicConstraintConstPtr;
00198 class JointConstraint : public KinematicConstraint
00199 {
00200 public:
00206 JointConstraint(const robot_model::RobotModelConstPtr &model) :
00207 KinematicConstraint(model), joint_model_(NULL)
00208 {
00209 type_ = JOINT_CONSTRAINT;
00210 }
00211
00225 bool configure(const moveit_msgs::JointConstraint &jc);
00226
00242 virtual bool equal(const KinematicConstraint &other, double margin) const;
00243 virtual ConstraintEvaluationResult decide(const robot_state::RobotState &state, bool verbose = false) const;
00244 virtual bool enabled() const;
00245 virtual void clear();
00246 virtual void print(std::ostream &out = std::cout) const;
00247
00253 const robot_model::JointModel* getJointModel() const
00254 {
00255 return joint_model_;
00256 }
00265 const std::string& getLocalVariableName() const
00266 {
00267 return local_variable_name_;
00268 }
00269
00277 const std::string& getJointVariableName() const
00278 {
00279 return joint_variable_name_;
00280 }
00281
00288 double getDesiredJointPosition() const
00289 {
00290 return joint_position_;
00291 }
00292
00299 double getJointToleranceAbove() const
00300 {
00301 return joint_tolerance_above_;
00302 }
00303
00310 double getJointToleranceBelow() const
00311 {
00312 return joint_tolerance_below_;
00313 }
00314
00315 protected:
00316
00317 const robot_model::JointModel *joint_model_;
00318 bool joint_is_continuous_;
00319 std::string local_variable_name_;
00320 std::string joint_variable_name_;
00321 double joint_position_, joint_tolerance_above_, joint_tolerance_below_;
00322 };
00323
00335 class OrientationConstraint : public KinematicConstraint
00336 {
00337 public:
00338 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00339
00340 public:
00346 OrientationConstraint(const robot_model::RobotModelConstPtr &model) :
00347 KinematicConstraint(model), link_model_(NULL)
00348 {
00349 type_ = ORIENTATION_CONSTRAINT;
00350 }
00351
00365 bool configure(const moveit_msgs::OrientationConstraint &oc, const robot_state::Transforms &tf);
00366
00383 virtual bool equal(const KinematicConstraint &other, double margin) const;
00384 virtual void clear();
00385 virtual ConstraintEvaluationResult decide(const robot_state::RobotState &state, bool verbose = false) const;
00386 virtual bool enabled() const;
00387 virtual void print(std::ostream &out = std::cout) const;
00388
00395 const robot_model::LinkModel* getLinkModel() const
00396 {
00397 return link_model_;
00398 }
00399
00406 const std::string& getReferenceFrame() const
00407 {
00408 return desired_rotation_frame_id_;
00409 }
00410
00417 bool mobileReferenceFrame() const
00418 {
00419 return mobile_frame_;
00420 }
00421
00427 const Eigen::Matrix3d& getDesiredRotationMatrix() const
00428 {
00429 return desired_rotation_matrix_;
00430 }
00431
00438 double getXAxisTolerance() const
00439 {
00440 return absolute_x_axis_tolerance_;
00441 }
00442
00449 double getYAxisTolerance() const
00450 {
00451 return absolute_y_axis_tolerance_;
00452 }
00453
00460 double getZAxisTolerance() const
00461 {
00462 return absolute_z_axis_tolerance_;
00463 }
00464
00465 protected:
00466
00467 const robot_model::LinkModel *link_model_;
00468 Eigen::Matrix3d desired_rotation_matrix_;
00469 Eigen::Matrix3d desired_rotation_matrix_inv_;
00470 std::string desired_rotation_frame_id_;
00471 bool mobile_frame_;
00472 double absolute_x_axis_tolerance_, absolute_y_axis_tolerance_, absolute_z_axis_tolerance_;
00473 };
00474
00475
00489 class PositionConstraint : public KinematicConstraint
00490 {
00491 public:
00492 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00493
00494 public:
00495
00501 PositionConstraint(const robot_model::RobotModelConstPtr &model) :
00502 KinematicConstraint(model), link_model_(NULL)
00503 {
00504 type_ = POSITION_CONSTRAINT;
00505 }
00506
00523 bool configure(const moveit_msgs::PositionConstraint &pc, const robot_state::Transforms &tf);
00524
00548 virtual bool equal(const KinematicConstraint &other, double margin) const;
00549 virtual void clear();
00550 virtual ConstraintEvaluationResult decide(const robot_state::RobotState &state, bool verbose = false) const;
00551 virtual bool enabled() const;
00552 virtual void print(std::ostream &out = std::cout) const;
00553
00560 const robot_model::LinkModel* getLinkModel() const
00561 {
00562 return link_model_;
00563 }
00564
00571 const Eigen::Vector3d& getLinkOffset() const
00572 {
00573 return offset_;
00574 }
00575
00583 bool hasLinkOffset() const
00584 {
00585 if(!enabled()) return false;
00586 return has_offset_;
00587 }
00588
00589
00596 const std::vector<bodies::BodyPtr>& getConstraintRegions() const
00597 {
00598 return constraint_region_;
00599 }
00600
00607 const std::string& getReferenceFrame() const
00608 {
00609 return constraint_frame_id_;
00610 }
00611
00619 bool mobileReferenceFrame() const
00620 {
00621 if(!enabled()) return false;
00622 return mobile_frame_;
00623 }
00624
00625 protected:
00626
00627 Eigen::Vector3d offset_;
00628 bool has_offset_;
00629 std::vector<bodies::BodyPtr> constraint_region_;
00630 EigenSTL::vector_Affine3d constraint_region_pose_;
00631 bool mobile_frame_;
00632 std::string constraint_frame_id_;
00633 const robot_model::LinkModel *link_model_;
00634 };
00635
00732 class VisibilityConstraint : public KinematicConstraint
00733 {
00734 public:
00735 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00736
00737 public:
00743 VisibilityConstraint(const robot_model::RobotModelConstPtr &model);
00744
00757 bool configure(const moveit_msgs::VisibilityConstraint &vc, const robot_state::Transforms &tf);
00758
00776 virtual bool equal(const KinematicConstraint &other, double margin) const;
00777 virtual void clear();
00778
00786 shapes::Mesh* getVisibilityCone(const robot_state::RobotState &state) const;
00787
00799 void getMarkers(const robot_state::RobotState &state, visualization_msgs::MarkerArray &markers) const;
00800
00801 virtual bool enabled() const;
00802 virtual ConstraintEvaluationResult decide(const robot_state::RobotState &state, bool verbose = false) const;
00803 virtual void print(std::ostream &out = std::cout) const;
00804
00805 protected:
00806
00816 bool decideContact(const collision_detection::Contact &contact) const;
00817
00818 collision_detection::CollisionRobotPtr collision_robot_;
00819 bool mobile_sensor_frame_;
00820 bool mobile_target_frame_;
00821 std::string target_frame_id_;
00822 std::string sensor_frame_id_;
00823 Eigen::Affine3d sensor_pose_;
00824 int sensor_view_direction_;
00825 Eigen::Affine3d target_pose_;
00826 unsigned int cone_sides_;
00827 EigenSTL::vector_Vector3d points_;
00828 double target_radius_;
00829 double max_view_angle_;
00830 double max_range_angle_;
00831 };
00832
00841 class KinematicConstraintSet
00842 {
00843 public:
00844 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00845
00846 public:
00847
00853 KinematicConstraintSet(const robot_model::RobotModelConstPtr &model) :
00854 robot_model_(model)
00855 {
00856 }
00857
00858 ~KinematicConstraintSet()
00859 {
00860 clear();
00861 }
00862
00864 void clear();
00865
00876 bool add(const moveit_msgs::Constraints &c, const robot_state::Transforms &tf);
00877
00885 bool add(const std::vector<moveit_msgs::JointConstraint> &jc);
00886
00894 bool add(const std::vector<moveit_msgs::PositionConstraint> &pc, const robot_state::Transforms &tf);
00895
00903 bool add(const std::vector<moveit_msgs::OrientationConstraint> &oc, const robot_state::Transforms &tf);
00904
00912 bool add(const std::vector<moveit_msgs::VisibilityConstraint> &vc, const robot_state::Transforms &tf);
00913
00925 ConstraintEvaluationResult decide(const robot_state::RobotState &state, bool verbose = false) const;
00926
00945 ConstraintEvaluationResult decide(const robot_state::RobotState &state, std::vector<ConstraintEvaluationResult> &results, bool verbose = false) const;
00946
00962 bool equal(const KinematicConstraintSet &other, double margin) const;
00963
00969 void print(std::ostream &out = std::cout) const;
00970
00977 const std::vector<moveit_msgs::PositionConstraint>& getPositionConstraints() const
00978 {
00979 return position_constraints_;
00980 }
00981
00988 const std::vector<moveit_msgs::OrientationConstraint>& getOrientationConstraints() const
00989 {
00990 return orientation_constraints_;
00991 }
00992
00999 const std::vector<moveit_msgs::JointConstraint>& getJointConstraints() const
01000 {
01001 return joint_constraints_;
01002 }
01003
01010 const std::vector<moveit_msgs::VisibilityConstraint>& getVisibilityConstraints() const
01011 {
01012 return visibility_constraints_;
01013 }
01014
01021 const moveit_msgs::Constraints& getAllConstraints() const
01022 {
01023 return all_constraints_;
01024 }
01025
01032 bool empty() const
01033 {
01034 return kinematic_constraints_.empty();
01035 }
01036
01037 protected:
01038
01039 robot_model::RobotModelConstPtr robot_model_;
01040 std::vector<KinematicConstraintPtr> kinematic_constraints_;
01042 std::vector<moveit_msgs::JointConstraint> joint_constraints_;
01043 std::vector<moveit_msgs::PositionConstraint> position_constraints_;
01044 std::vector<moveit_msgs::OrientationConstraint> orientation_constraints_;
01045 std::vector<moveit_msgs::VisibilityConstraint> visibility_constraints_;
01046 moveit_msgs::Constraints all_constraints_;
01048 };
01049
01050 typedef boost::shared_ptr<KinematicConstraintSet> KinematicConstraintSetPtr;
01051 typedef boost::shared_ptr<const KinematicConstraintSet> KinematicConstraintSetConstPtr;
01053 }
01054
01055
01056 #endif