default_constraint_samplers.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_CONSTRAINT_SAMPLERS_DEFAULT_CONSTRAINT_SAMPLERS_
00038 #define MOVEIT_CONSTRAINT_SAMPLERS_DEFAULT_CONSTRAINT_SAMPLERS_
00039 
00040 #include <moveit/constraint_samplers/constraint_sampler.h>
00041 #include <random_numbers/random_numbers.h>
00042 
00043 namespace constraint_samplers
00044 {
00045 
00055 class JointConstraintSampler : public ConstraintSampler
00056 {
00057 public:
00058 
00069   JointConstraintSampler(const planning_scene::PlanningSceneConstPtr &scene,
00070                          const std::string &group_name) :
00071     ConstraintSampler(scene, group_name)
00072   {
00073   }
00091   virtual bool configure(const moveit_msgs::Constraints &constr);
00092 
00118   bool configure(const std::vector<kinematic_constraints::JointConstraint> &jc);
00119 
00120   virtual bool sample(robot_state::JointStateGroup *jsg,
00121                       const robot_state::RobotState &ks,
00122                       unsigned int max_attempts);
00123 
00124   virtual bool project(robot_state::JointStateGroup *jsg,
00125                        const robot_state::RobotState &reference_state,
00126                        unsigned int max_attempts);
00127 
00135   std::size_t getConstrainedJointCount() const
00136   {
00137     return bounds_.size();
00138   }
00139 
00146   std::size_t getUnconstrainedJointCount() const
00147   {
00148     return unbounded_.size();
00149   }
00150 
00151 protected:
00152 
00154   struct JointInfo
00155   {
00161     JointInfo()
00162     {
00163       min_bound_ = -std::numeric_limits<double>::max();
00164       max_bound_ = std::numeric_limits<double>::max();
00165     }
00166 
00176     void potentiallyAdjustMinMaxBounds(double min, double max)
00177     {
00178       min_bound_ = std::max(min, min_bound_);
00179       max_bound_ = std::min(max, max_bound_);
00180     }
00181 
00182     double min_bound_;          
00183     double max_bound_;          
00184     std::size_t index_;         
00185   };
00186 
00187   virtual void clear();
00188 
00189   random_numbers::RandomNumberGenerator           random_number_generator_; 
00190   std::vector<JointInfo>                          bounds_; 
00192   std::vector<const robot_model::JointModel*> unbounded_; 
00193   std::vector<unsigned int>                       uindex_; 
00194   std::vector<double>                             values_; 
00195 };
00196 
00202 struct IKSamplingPose
00203 {
00209   IKSamplingPose();
00210 
00217   IKSamplingPose(const kinematic_constraints::PositionConstraint &pc);
00218 
00226   IKSamplingPose(const kinematic_constraints::OrientationConstraint &oc);
00227 
00237   IKSamplingPose(const kinematic_constraints::PositionConstraint &pc,
00238                  const kinematic_constraints::OrientationConstraint &oc);
00239 
00247   IKSamplingPose(const boost::shared_ptr<kinematic_constraints::PositionConstraint> &pc);
00248 
00256   IKSamplingPose(const boost::shared_ptr<kinematic_constraints::OrientationConstraint> &oc);
00257 
00258 
00267   IKSamplingPose(const boost::shared_ptr<kinematic_constraints::PositionConstraint> &pc,
00268                  const boost::shared_ptr<kinematic_constraints::OrientationConstraint> &oc);
00269 
00270   boost::shared_ptr<kinematic_constraints::PositionConstraint>    position_constraint_; 
00271   boost::shared_ptr<kinematic_constraints::OrientationConstraint> orientation_constraint_; 
00272 };
00273 
00283 class IKConstraintSampler : public ConstraintSampler
00284 {
00285 public:
00286 
00297   IKConstraintSampler(const planning_scene::PlanningSceneConstPtr &scene,
00298                       const std::string &group_name) :
00299     ConstraintSampler(scene, group_name)
00300   {
00301   }
00302 
00327   virtual bool configure(const moveit_msgs::Constraints &constr);
00328 
00353   bool configure(const IKSamplingPose &sp);
00354 
00361   double getIKTimeout() const
00362   {
00363     return ik_timeout_;
00364   }
00365 
00371   void setIKTimeout(double timeout)
00372   {
00373     ik_timeout_ = timeout;
00374   }
00375 
00382   const boost::shared_ptr<kinematic_constraints::PositionConstraint>& getPositionConstraint() const
00383   {
00384     return sampling_pose_.position_constraint_;
00385   }
00392   const boost::shared_ptr<kinematic_constraints::OrientationConstraint>& getOrientationConstraint() const
00393   {
00394     return sampling_pose_.orientation_constraint_;
00395   }
00396 
00410   double getSamplingVolume() const;
00411 
00418   const std::string& getLinkName() const;
00419 
00441   virtual bool sample(robot_state::JointStateGroup *jsg, const robot_state::RobotState &ks, unsigned int max_attempts);
00442 
00443   virtual bool project(robot_state::JointStateGroup *jsg,
00444                        const robot_state::RobotState &reference_state,
00445                        unsigned int max_attempts);
00468   bool samplePose(Eigen::Vector3d &pos, Eigen::Quaterniond &quat, const robot_state::RobotState &ks, unsigned int max_attempts);
00469 
00470 protected:
00471 
00472   virtual void clear();
00473 
00479   bool loadIKSolver();
00480 
00491   bool callIK(const geometry_msgs::Pose &ik_query, const kinematics::KinematicsBase::IKCallbackFn &adapted_ik_validity_callback, double timeout,
00492               robot_state::JointStateGroup *jsg, bool use_as_seed);
00493 
00494   bool sampleHelper(robot_state::JointStateGroup *jsg, const robot_state::RobotState &ks, unsigned int max_attempts, bool project);
00495 
00496   random_numbers::RandomNumberGenerator random_number_generator_; 
00497   IKSamplingPose                        sampling_pose_; 
00498   kinematics::KinematicsBaseConstPtr    kb_; 
00499   double                                ik_timeout_; 
00500   std::string                           ik_frame_; 
00501   bool                                  transform_ik_; 
00502 };
00503 
00504 
00505 }
00506 
00507 
00508 #endif


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