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 #include <moveit/macros/class_forward.h>
00045
00046 #include <geometric_shapes/bodies.h>
00047 #include <moveit_msgs/Constraints.h>
00048
00049 #include <iostream>
00050 #include <vector>
00051
00053 namespace kinematic_constraints
00054 {
00056 struct ConstraintEvaluationResult
00057 {
00066 ConstraintEvaluationResult(bool result_satisfied = false, double dist = 0.0)
00067 : satisfied(result_satisfied), distance(dist)
00068 {
00069 }
00070
00071 bool satisfied;
00072 double distance;
00073 };
00074
00075 MOVEIT_CLASS_FORWARD(KinematicConstraint);
00076
00078 class KinematicConstraint
00079 {
00080 public:
00082 enum ConstraintType
00083 {
00084 UNKNOWN_CONSTRAINT,
00085 JOINT_CONSTRAINT,
00086 POSITION_CONSTRAINT,
00087 ORIENTATION_CONSTRAINT,
00088 VISIBILITY_CONSTRAINT
00089 };
00090
00096 KinematicConstraint(const robot_model::RobotModelConstPtr& model);
00097 virtual ~KinematicConstraint();
00098
00100 virtual void clear() = 0;
00101
00110 virtual ConstraintEvaluationResult decide(const robot_state::RobotState& state, bool verbose = false) const = 0;
00111
00117 virtual bool enabled() const = 0;
00118
00130 virtual bool equal(const KinematicConstraint& other, double margin) const = 0;
00131
00138 ConstraintType getType() const
00139 {
00140 return type_;
00141 }
00142
00148 virtual void print(std::ostream& out = std::cout) const
00149 {
00150 }
00151
00159 double getConstraintWeight() const
00160 {
00161 return constraint_weight_;
00162 }
00163
00170 const robot_model::RobotModelConstPtr& getRobotModel() const
00171 {
00172 return robot_model_;
00173 }
00174
00175 protected:
00176 ConstraintType type_;
00177 robot_model::RobotModelConstPtr robot_model_;
00178 double constraint_weight_;
00180 };
00181
00182 MOVEIT_CLASS_FORWARD(JointConstraint);
00183
00203 class JointConstraint : public KinematicConstraint
00204 {
00205 public:
00211 JointConstraint(const robot_model::RobotModelConstPtr& model)
00212 : KinematicConstraint(model), joint_model_(NULL), joint_variable_index_(-1)
00213 {
00214 type_ = JOINT_CONSTRAINT;
00215 }
00216
00230 bool configure(const moveit_msgs::JointConstraint& jc);
00231
00247 virtual bool equal(const KinematicConstraint& other, double margin) const;
00248
00249 virtual ConstraintEvaluationResult decide(const robot_state::RobotState& state, bool verbose = false) const;
00250 virtual bool enabled() const;
00251 virtual void clear();
00252 virtual void print(std::ostream& out = std::cout) const;
00253
00259 const robot_model::JointModel* getJointModel() const
00260 {
00261 return joint_model_;
00262 }
00271 const std::string& getLocalVariableName() const
00272 {
00273 return local_variable_name_;
00274 }
00275
00283 const std::string& getJointVariableName() const
00284 {
00285 return joint_variable_name_;
00286 }
00287
00288 int getJointVariableIndex() const
00289 {
00290 return joint_variable_index_;
00291 }
00292
00299 double getDesiredJointPosition() const
00300 {
00301 return joint_position_;
00302 }
00303
00310 double getJointToleranceAbove() const
00311 {
00312 return joint_tolerance_above_;
00313 }
00314
00321 double getJointToleranceBelow() const
00322 {
00323 return joint_tolerance_below_;
00324 }
00325
00326 protected:
00327 const robot_model::JointModel* joint_model_;
00328 bool joint_is_continuous_;
00329 std::string local_variable_name_;
00330 std::string joint_variable_name_;
00331 int joint_variable_index_;
00332 double joint_position_, joint_tolerance_above_, joint_tolerance_below_;
00333 };
00334
00335 MOVEIT_CLASS_FORWARD(OrientationConstraint);
00336
00348 class OrientationConstraint : public KinematicConstraint
00349 {
00350 public:
00351 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00352
00353 public:
00359 OrientationConstraint(const robot_model::RobotModelConstPtr& model) : KinematicConstraint(model), link_model_(NULL)
00360 {
00361 type_ = ORIENTATION_CONSTRAINT;
00362 }
00363
00377 bool configure(const moveit_msgs::OrientationConstraint& oc, const robot_state::Transforms& tf);
00378
00395 virtual bool equal(const KinematicConstraint& other, double margin) const;
00396
00397 virtual void clear();
00398 virtual ConstraintEvaluationResult decide(const robot_state::RobotState& state, bool verbose = false) const;
00399 virtual bool enabled() const;
00400 virtual void print(std::ostream& out = std::cout) const;
00401
00408 const robot_model::LinkModel* getLinkModel() const
00409 {
00410 return link_model_;
00411 }
00412
00419 const std::string& getReferenceFrame() const
00420 {
00421 return desired_rotation_frame_id_;
00422 }
00423
00430 bool mobileReferenceFrame() const
00431 {
00432 return mobile_frame_;
00433 }
00434
00440 const Eigen::Matrix3d& getDesiredRotationMatrix() const
00441 {
00442 return desired_rotation_matrix_;
00443 }
00444
00451 double getXAxisTolerance() const
00452 {
00453 return absolute_x_axis_tolerance_;
00454 }
00455
00462 double getYAxisTolerance() const
00463 {
00464 return absolute_y_axis_tolerance_;
00465 }
00466
00473 double getZAxisTolerance() const
00474 {
00475 return absolute_z_axis_tolerance_;
00476 }
00477
00480 void swapLinkModel(const robot_model::LinkModel* new_link, const Eigen::Matrix3d& update);
00481
00482 protected:
00483 const robot_model::LinkModel* link_model_;
00484 Eigen::Matrix3d desired_rotation_matrix_;
00485 Eigen::Matrix3d desired_rotation_matrix_inv_;
00488 std::string desired_rotation_frame_id_;
00489 bool mobile_frame_;
00490 double absolute_x_axis_tolerance_, absolute_y_axis_tolerance_,
00491 absolute_z_axis_tolerance_;
00492 };
00493
00494 MOVEIT_CLASS_FORWARD(PositionConstraint);
00495
00509 class PositionConstraint : public KinematicConstraint
00510 {
00511 public:
00512 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00513
00514 public:
00520 PositionConstraint(const robot_model::RobotModelConstPtr& model) : KinematicConstraint(model), link_model_(NULL)
00521 {
00522 type_ = POSITION_CONSTRAINT;
00523 }
00524
00541 bool configure(const moveit_msgs::PositionConstraint& pc, const robot_state::Transforms& tf);
00542
00566 virtual bool equal(const KinematicConstraint& other, double margin) const;
00567
00568 virtual void clear();
00569 virtual ConstraintEvaluationResult decide(const robot_state::RobotState& state, bool verbose = false) const;
00570 virtual bool enabled() const;
00571 virtual void print(std::ostream& out = std::cout) const;
00572
00579 const robot_model::LinkModel* getLinkModel() const
00580 {
00581 return link_model_;
00582 }
00583
00590 const Eigen::Vector3d& getLinkOffset() const
00591 {
00592 return offset_;
00593 }
00594
00602 bool hasLinkOffset() const
00603 {
00604 if (!enabled())
00605 return false;
00606 return has_offset_;
00607 }
00608
00615 const std::vector<bodies::BodyPtr>& getConstraintRegions() const
00616 {
00617 return constraint_region_;
00618 }
00619
00626 const std::string& getReferenceFrame() const
00627 {
00628 return constraint_frame_id_;
00629 }
00630
00638 bool mobileReferenceFrame() const
00639 {
00640 if (!enabled())
00641 return false;
00642 return mobile_frame_;
00643 }
00644
00647 void swapLinkModel(const robot_model::LinkModel* new_link, const Eigen::Affine3d& update);
00648
00649 protected:
00650 Eigen::Vector3d offset_;
00651 bool has_offset_;
00652 std::vector<bodies::BodyPtr> constraint_region_;
00653 EigenSTL::vector_Affine3d constraint_region_pose_;
00654 bool mobile_frame_;
00655 std::string constraint_frame_id_;
00656 const robot_model::LinkModel* link_model_;
00657 };
00658
00659 MOVEIT_CLASS_FORWARD(VisibilityConstraint);
00660
00758 class VisibilityConstraint : public KinematicConstraint
00759 {
00760 public:
00761 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00762
00763 public:
00769 VisibilityConstraint(const robot_model::RobotModelConstPtr& model);
00770
00783 bool configure(const moveit_msgs::VisibilityConstraint& vc, const robot_state::Transforms& tf);
00784
00802 virtual bool equal(const KinematicConstraint& other, double margin) const;
00803 virtual void clear();
00804
00812 shapes::Mesh* getVisibilityCone(const robot_state::RobotState& state) const;
00813
00825 void getMarkers(const robot_state::RobotState& state, visualization_msgs::MarkerArray& markers) const;
00826
00827 virtual bool enabled() const;
00828 virtual ConstraintEvaluationResult decide(const robot_state::RobotState& state, bool verbose = false) const;
00829 virtual void print(std::ostream& out = std::cout) const;
00830
00831 protected:
00841 bool decideContact(const collision_detection::Contact& contact) const;
00842
00843 collision_detection::CollisionRobotPtr collision_robot_;
00845 bool mobile_sensor_frame_;
00846 bool mobile_target_frame_;
00847 std::string target_frame_id_;
00848 std::string sensor_frame_id_;
00849 Eigen::Affine3d sensor_pose_;
00850 int sensor_view_direction_;
00851 Eigen::Affine3d target_pose_;
00852 unsigned int cone_sides_;
00853 EigenSTL::vector_Vector3d points_;
00854 double target_radius_;
00855 double max_view_angle_;
00856 double max_range_angle_;
00857 };
00858
00859 MOVEIT_CLASS_FORWARD(KinematicConstraintSet);
00860
00869 class KinematicConstraintSet
00870 {
00871 public:
00872 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00873
00874 public:
00880 KinematicConstraintSet(const robot_model::RobotModelConstPtr& model) : robot_model_(model)
00881 {
00882 }
00883
00884 ~KinematicConstraintSet()
00885 {
00886 clear();
00887 }
00888
00890 void clear();
00891
00902 bool add(const moveit_msgs::Constraints& c, const robot_state::Transforms& tf);
00903
00911 bool add(const std::vector<moveit_msgs::JointConstraint>& jc);
00912
00920 bool add(const std::vector<moveit_msgs::PositionConstraint>& pc, const robot_state::Transforms& tf);
00921
00929 bool add(const std::vector<moveit_msgs::OrientationConstraint>& oc, const robot_state::Transforms& tf);
00930
00938 bool add(const std::vector<moveit_msgs::VisibilityConstraint>& vc, const robot_state::Transforms& tf);
00939
00951 ConstraintEvaluationResult decide(const robot_state::RobotState& state, bool verbose = false) const;
00952
00971 ConstraintEvaluationResult decide(const robot_state::RobotState& state,
00972 std::vector<ConstraintEvaluationResult>& results, bool verbose = false) const;
00973
00989 bool equal(const KinematicConstraintSet& other, double margin) const;
00990
00996 void print(std::ostream& out = std::cout) const;
00997
01004 const std::vector<moveit_msgs::PositionConstraint>& getPositionConstraints() const
01005 {
01006 return position_constraints_;
01007 }
01008
01015 const std::vector<moveit_msgs::OrientationConstraint>& getOrientationConstraints() const
01016 {
01017 return orientation_constraints_;
01018 }
01019
01026 const std::vector<moveit_msgs::JointConstraint>& getJointConstraints() const
01027 {
01028 return joint_constraints_;
01029 }
01030
01037 const std::vector<moveit_msgs::VisibilityConstraint>& getVisibilityConstraints() const
01038 {
01039 return visibility_constraints_;
01040 }
01041
01048 const moveit_msgs::Constraints& getAllConstraints() const
01049 {
01050 return all_constraints_;
01051 }
01052
01059 bool empty() const
01060 {
01061 return kinematic_constraints_.empty();
01062 }
01063
01064 protected:
01065 robot_model::RobotModelConstPtr robot_model_;
01066 std::vector<KinematicConstraintPtr>
01067 kinematic_constraints_;
01069 std::vector<moveit_msgs::JointConstraint> joint_constraints_;
01071 std::vector<moveit_msgs::PositionConstraint> position_constraints_;
01073 std::vector<moveit_msgs::OrientationConstraint> orientation_constraints_;
01075 std::vector<moveit_msgs::VisibilityConstraint> visibility_constraints_;
01077 moveit_msgs::Constraints all_constraints_;
01078 };
01079 }
01080
01081 #endif