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 the 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 
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


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Mon Oct 6 2014 02:24:47