kinematic_constraint.h
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2011, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of Willow Garage nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *********************************************************************/
00034 
00035 /* Author: Ioan Sucan */
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


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Mon Jul 24 2017 02:20:43