constraints_library.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_OMPL_INTERFACE_CONSTRAINTS_LIBRARY_
00038 #define MOVEIT_OMPL_INTERFACE_CONSTRAINTS_LIBRARY_
00039 
00040 #include <moveit/macros/class_forward.h>
00041 #include <moveit/ompl_interface/planning_context_manager.h>
00042 #include <moveit/kinematic_constraints/kinematic_constraint.h>
00043 #include <ompl/base/StateStorage.h>
00044 #include <boost/function.hpp>
00045 #include <boost/serialization/map.hpp>
00046 
00047 namespace ompl_interface
00048 {
00049 typedef std::pair<std::vector<std::size_t>, std::map<std::size_t, std::pair<std::size_t, std::size_t> > >
00050     ConstrainedStateMetadata;
00051 typedef ompl::base::StateStorageWithMetadata<ConstrainedStateMetadata> ConstraintApproximationStateStorage;
00052 
00053 MOVEIT_CLASS_FORWARD(ConstraintApproximation)
00054 
00055 class ConstraintApproximation
00056 {
00057 public:
00058   ConstraintApproximation(const std::string& group, const std::string& state_space_parameterization,
00059                           bool explicit_motions, const moveit_msgs::Constraints& msg, const std::string& filename,
00060                           const ompl::base::StateStoragePtr& storage, std::size_t milestones = 0);
00061 
00062   virtual ~ConstraintApproximation()
00063   {
00064   }
00065 
00066   const std::string& getName() const
00067   {
00068     return constraint_msg_.name;
00069   }
00070 
00071   ompl::base::StateSamplerAllocator getStateSamplerAllocator(const moveit_msgs::Constraints& msg) const;
00072 
00073   InterpolationFunction getInterpolationFunction() const;
00074 
00075   const std::vector<int>& getSpaceSignature() const
00076   {
00077     return space_signature_;
00078   }
00079 
00080   const std::string& getGroup() const
00081   {
00082     return group_;
00083   }
00084 
00085   bool hasExplicitMotions() const
00086   {
00087     return explicit_motions_;
00088   }
00089 
00090   std::size_t getMilestoneCount() const
00091   {
00092     return milestones_;
00093   }
00094 
00095   const std::string& getStateSpaceParameterization() const
00096   {
00097     return state_space_parameterization_;
00098   }
00099 
00100   const moveit_msgs::Constraints& getConstraintsMsg() const
00101   {
00102     return constraint_msg_;
00103   }
00104 
00105   const ompl::base::StateStoragePtr& getStateStorage() const
00106   {
00107     return state_storage_ptr_;
00108   }
00109 
00110   const std::string& getFilename() const
00111   {
00112     return ompldb_filename_;
00113   }
00114 
00115 protected:
00116   std::string group_;
00117   std::string state_space_parameterization_;
00118   bool explicit_motions_;
00119 
00120   moveit_msgs::Constraints constraint_msg_;
00121 
00122   std::vector<int> space_signature_;
00123 
00124   std::string ompldb_filename_;
00125   ompl::base::StateStoragePtr state_storage_ptr_;
00126   ConstraintApproximationStateStorage* state_storage_;
00127   std::size_t milestones_;
00128 };
00129 
00130 struct ConstraintApproximationConstructionOptions
00131 {
00132   ConstraintApproximationConstructionOptions()
00133     : samples(0)
00134     , edges_per_sample(0)
00135     , max_edge_length(std::numeric_limits<double>::infinity())
00136     , explicit_motions(false)
00137     , explicit_points_resolution(0.0)
00138     , max_explicit_points(0)
00139   {
00140   }
00141 
00142   std::string state_space_parameterization;
00143   unsigned int samples;
00144   unsigned int edges_per_sample;
00145   double max_edge_length;
00146   bool explicit_motions;
00147   double explicit_points_resolution;
00148   unsigned int max_explicit_points;
00149 };
00150 
00151 struct ConstraintApproximationConstructionResults
00152 {
00153   ConstraintApproximationPtr approx;
00154   std::size_t milestones;
00155   double state_sampling_time;
00156   double state_connection_time;
00157   double sampling_success_rate;
00158 };
00159 
00160 MOVEIT_CLASS_FORWARD(ConstraintsLibrary);
00161 
00162 class ConstraintsLibrary
00163 {
00164 public:
00165   ConstraintsLibrary(const PlanningContextManager& pcontext) : context_manager_(pcontext)
00166   {
00167   }
00168 
00169   void loadConstraintApproximations(const std::string& path);
00170 
00171   void saveConstraintApproximations(const std::string& path);
00172 
00173   ConstraintApproximationConstructionResults addConstraintApproximation(
00174       const moveit_msgs::Constraints& constr_sampling, const moveit_msgs::Constraints& constr_hard,
00175       const std::string& group, const planning_scene::PlanningSceneConstPtr& scene,
00176       const ConstraintApproximationConstructionOptions& options);
00177 
00178   ConstraintApproximationConstructionResults addConstraintApproximation(
00179       const moveit_msgs::Constraints& constr, const std::string& group,
00180       const planning_scene::PlanningSceneConstPtr& scene, const ConstraintApproximationConstructionOptions& options);
00181 
00182   void printConstraintApproximations(std::ostream& out = std::cout) const;
00183   void clearConstraintApproximations();
00184 
00185   void registerConstraintApproximation(const ConstraintApproximationPtr& approx)
00186   {
00187     constraint_approximations_[approx->getName()] = approx;
00188   }
00189 
00190   const ConstraintApproximationPtr& getConstraintApproximation(const moveit_msgs::Constraints& msg) const;
00191 
00192 private:
00193   ompl::base::StateStoragePtr constructConstraintApproximation(
00194       const ModelBasedPlanningContextPtr& pcontext, const moveit_msgs::Constraints& constr_sampling,
00195       const moveit_msgs::Constraints& constr_hard, const ConstraintApproximationConstructionOptions& options,
00196       ConstraintApproximationConstructionResults& result);
00197 
00198   const PlanningContextManager& context_manager_;
00199   std::map<std::string, ConstraintApproximationPtr> constraint_approximations_;
00200 };
00201 }
00202 
00203 #endif


ompl
Author(s): Ioan Sucan
autogenerated on Wed Jun 19 2019 19:24:27