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 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 <moveit/macros/class_forward.h>
00042 #include <random_numbers/random_numbers.h>
00043 
00044 namespace constraint_samplers
00045 {
00046 MOVEIT_CLASS_FORWARD(JointConstraintSampler);
00047 
00057 class JointConstraintSampler : public ConstraintSampler
00058 {
00059 public:
00070   JointConstraintSampler(const planning_scene::PlanningSceneConstPtr& scene, 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::RobotState& state, const robot_state::RobotState& ks, unsigned int max_attempts);
00121 
00122   virtual bool project(robot_state::RobotState& state, unsigned int max_attempts);
00123 
00131   std::size_t getConstrainedJointCount() const
00132   {
00133     return bounds_.size();
00134   }
00135 
00142   std::size_t getUnconstrainedJointCount() const
00143   {
00144     return unbounded_.size();
00145   }
00146 
00152   virtual const std::string& getName() const
00153   {
00154     static const std::string SAMPLER_NAME = "JointConstraintSampler";
00155     return SAMPLER_NAME;
00156   }
00157 
00158 protected:
00160   struct JointInfo
00161   {
00167     JointInfo()
00168     {
00169       min_bound_ = -std::numeric_limits<double>::max();
00170       max_bound_ = std::numeric_limits<double>::max();
00171     }
00172 
00182     void potentiallyAdjustMinMaxBounds(double min, double max)
00183     {
00184       min_bound_ = std::max(min, min_bound_);
00185       max_bound_ = std::min(max, max_bound_);
00186     }
00187 
00188     double min_bound_;  
00189     double max_bound_;  
00190     std::size_t index_; 
00191   };
00192 
00193   virtual void clear();
00194 
00195   random_numbers::RandomNumberGenerator random_number_generator_; 
00196   std::vector<JointInfo> bounds_; 
00199   std::vector<const robot_model::JointModel*> unbounded_; 
00201   std::vector<unsigned int> uindex_; 
00202   std::vector<double> values_;       
00203 };
00204 
00210 struct IKSamplingPose
00211 {
00217   IKSamplingPose();
00218 
00225   IKSamplingPose(const kinematic_constraints::PositionConstraint& pc);
00226 
00234   IKSamplingPose(const kinematic_constraints::OrientationConstraint& oc);
00235 
00245   IKSamplingPose(const kinematic_constraints::PositionConstraint& pc,
00246                  const kinematic_constraints::OrientationConstraint& oc);
00247 
00255   IKSamplingPose(const kinematic_constraints::PositionConstraintPtr& pc);
00256 
00264   IKSamplingPose(const kinematic_constraints::OrientationConstraintPtr& oc);
00265 
00274   IKSamplingPose(const kinematic_constraints::PositionConstraintPtr& pc,
00275                  const kinematic_constraints::OrientationConstraintPtr& oc);
00276 
00277   kinematic_constraints::PositionConstraintPtr position_constraint_; 
00279   kinematic_constraints::OrientationConstraintPtr
00280       orientation_constraint_; 
00281 };
00282 
00283 MOVEIT_CLASS_FORWARD(IKConstraintSampler);
00284 
00293 class IKConstraintSampler : public ConstraintSampler
00294 {
00295 public:
00306   IKConstraintSampler(const planning_scene::PlanningSceneConstPtr& scene, const std::string& group_name)
00307     : ConstraintSampler(scene, group_name)
00308   {
00309   }
00310 
00335   virtual bool configure(const moveit_msgs::Constraints& constr);
00336 
00361   bool configure(const IKSamplingPose& sp);
00362 
00369   double getIKTimeout() const
00370   {
00371     return ik_timeout_;
00372   }
00373 
00379   void setIKTimeout(double timeout)
00380   {
00381     ik_timeout_ = timeout;
00382   }
00383 
00390   const kinematic_constraints::PositionConstraintPtr& getPositionConstraint() const
00391   {
00392     return sampling_pose_.position_constraint_;
00393   }
00400   const kinematic_constraints::OrientationConstraintPtr& getOrientationConstraint() const
00401   {
00402     return sampling_pose_.orientation_constraint_;
00403   }
00404 
00418   double getSamplingVolume() const;
00419 
00426   const std::string& getLinkName() const;
00427 
00449   virtual bool sample(robot_state::RobotState& state, const robot_state::RobotState& reference_state,
00450                       unsigned int max_attempts);
00451 
00452   virtual bool project(robot_state::RobotState& state, unsigned int max_attempts);
00475   bool samplePose(Eigen::Vector3d& pos, Eigen::Quaterniond& quat, const robot_state::RobotState& ks,
00476                   unsigned int max_attempts);
00477 
00483   virtual const std::string& getName() const
00484   {
00485     static const std::string SAMPLER_NAME = "IKConstraintSampler";
00486     return SAMPLER_NAME;
00487   }
00488 
00489 protected:
00490   virtual void clear();
00491 
00498   bool loadIKSolver();
00499 
00510   bool callIK(const geometry_msgs::Pose& ik_query,
00511               const kinematics::KinematicsBase::IKCallbackFn& adapted_ik_validity_callback, double timeout,
00512               robot_state::RobotState& state, bool use_as_seed);
00513   bool sampleHelper(robot_state::RobotState& state, const robot_state::RobotState& reference_state,
00514                     unsigned int max_attempts, bool project);
00515   bool validate(robot_state::RobotState& state) const;
00516 
00517   random_numbers::RandomNumberGenerator random_number_generator_; 
00518   IKSamplingPose sampling_pose_;                                  
00519   kinematics::KinematicsBaseConstPtr kb_;                         
00520   double ik_timeout_;                                             
00521   std::string ik_frame_;                                          
00522   bool transform_ik_; 
00524 };
00525 }
00526 
00527 #endif


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Wed Jun 19 2019 19:23:49