constraints_library.h
Go to the documentation of this file.
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2011, Willow Garage, Inc.
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 *
11 * * Redistributions of source code must retain the above copyright
12 * notice, this list of conditions and the following disclaimer.
13 * * Redistributions in binary form must reproduce the above
14 * copyright notice, this list of conditions and the following
15 * disclaimer in the documentation and/or other materials provided
16 * with the distribution.
17 * * Neither the name of Willow Garage nor the names of its
18 * contributors may be used to endorse or promote products derived
19 * from this software without specific prior written permission.
20 *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 * POSSIBILITY OF SUCH DAMAGE.
33 *********************************************************************/
34 
35 /* Author: Ioan Sucan */
36 
37 #ifndef MOVEIT_OMPL_INTERFACE_CONSTRAINTS_LIBRARY_
38 #define MOVEIT_OMPL_INTERFACE_CONSTRAINTS_LIBRARY_
39 
43 #include <ompl/base/StateStorage.h>
44 #include <boost/function.hpp>
45 #include <boost/serialization/map.hpp>
46 
47 namespace ompl_interface
48 {
49 typedef std::pair<std::vector<std::size_t>, std::map<std::size_t, std::pair<std::size_t, std::size_t> > >
51 typedef ompl::base::StateStorageWithMetadata<ConstrainedStateMetadata> ConstraintApproximationStateStorage;
52 
54 
56 {
57 public:
58  ConstraintApproximation(const std::string& group, const std::string& state_space_parameterization,
59  bool explicit_motions, const moveit_msgs::Constraints& msg, const std::string& filename,
60  const ompl::base::StateStoragePtr& storage, std::size_t milestones = 0);
61 
63  {
64  }
65 
66  const std::string& getName() const
67  {
68  return constraint_msg_.name;
69  }
70 
71  ompl::base::StateSamplerAllocator getStateSamplerAllocator(const moveit_msgs::Constraints& msg) const;
72 
73  InterpolationFunction getInterpolationFunction() const;
74 
75  const std::vector<int>& getSpaceSignature() const
76  {
77  return space_signature_;
78  }
79 
80  const std::string& getGroup() const
81  {
82  return group_;
83  }
84 
85  bool hasExplicitMotions() const
86  {
87  return explicit_motions_;
88  }
89 
90  std::size_t getMilestoneCount() const
91  {
92  return milestones_;
93  }
94 
95  const std::string& getStateSpaceParameterization() const
96  {
97  return state_space_parameterization_;
98  }
99 
100  const moveit_msgs::Constraints& getConstraintsMsg() const
101  {
102  return constraint_msg_;
103  }
104 
105  const ompl::base::StateStoragePtr& getStateStorage() const
106  {
107  return state_storage_ptr_;
108  }
109 
110  const std::string& getFilename() const
111  {
112  return ompldb_filename_;
113  }
114 
115 protected:
116  std::string group_;
119 
120  moveit_msgs::Constraints constraint_msg_;
121 
122  std::vector<int> space_signature_;
123 
124  std::string ompldb_filename_;
125  ompl::base::StateStoragePtr state_storage_ptr_;
126  ConstraintApproximationStateStorage* state_storage_;
127  std::size_t milestones_;
128 };
129 
131 {
133  : samples(0)
134  , edges_per_sample(0)
135  , max_edge_length(std::numeric_limits<double>::infinity())
136  , explicit_motions(false)
139  {
140  }
141 
143  unsigned int samples;
144  unsigned int edges_per_sample;
148  unsigned int max_explicit_points;
149 };
150 
152 {
153  ConstraintApproximationPtr approx;
154  std::size_t milestones;
158 };
159 
161 
163 {
164 public:
165  ConstraintsLibrary(const PlanningContextManager& pcontext) : context_manager_(pcontext)
166  {
167  }
168 
169  void loadConstraintApproximations(const std::string& path);
170 
171  void saveConstraintApproximations(const std::string& path);
172 
173  ConstraintApproximationConstructionResults addConstraintApproximation(
174  const moveit_msgs::Constraints& constr_sampling, const moveit_msgs::Constraints& constr_hard,
175  const std::string& group, const planning_scene::PlanningSceneConstPtr& scene,
177 
178  ConstraintApproximationConstructionResults addConstraintApproximation(
179  const moveit_msgs::Constraints& constr, const std::string& group,
180  const planning_scene::PlanningSceneConstPtr& scene, const ConstraintApproximationConstructionOptions& options);
181 
182  void printConstraintApproximations(std::ostream& out = std::cout) const;
183  void clearConstraintApproximations();
184 
185  void registerConstraintApproximation(const ConstraintApproximationPtr& approx)
186  {
187  constraint_approximations_[approx->getName()] = approx;
188  }
189 
190  const ConstraintApproximationPtr& getConstraintApproximation(const moveit_msgs::Constraints& msg) const;
191 
192 private:
193  ompl::base::StateStoragePtr constructConstraintApproximation(
194  const ModelBasedPlanningContextPtr& pcontext, const moveit_msgs::Constraints& constr_sampling,
195  const moveit_msgs::Constraints& constr_hard, const ConstraintApproximationConstructionOptions& options,
197 
199  std::map<std::string, ConstraintApproximationPtr> constraint_approximations_;
200 };
201 }
202 
203 #endif
std::pair< std::vector< std::size_t >, std::map< std::size_t, std::pair< std::size_t, std::size_t > > > ConstrainedStateMetadata
filename
const std::string & getGroup() const
The MoveIt interface to OMPL.
const moveit_msgs::Constraints & getConstraintsMsg() const
ConstraintApproximationStateStorage * state_storage_
MOVEIT_CLASS_FORWARD(ConstraintsLibrary)
ompl::base::StateStoragePtr state_storage_ptr_
options
const std::string & getStateSpaceParameterization() const
void registerConstraintApproximation(const ConstraintApproximationPtr &approx)
ConstraintsLibrary(const PlanningContextManager &pcontext)
const std::string & getName() const
const std::vector< int > & getSpaceSignature() const
ompl::base::StateStorageWithMetadata< ConstrainedStateMetadata > ConstraintApproximationStateStorage
const PlanningContextManager & context_manager_
std::map< std::string, ConstraintApproximationPtr > constraint_approximations_
const std::string & getFilename() const
const ompl::base::StateStoragePtr & getStateStorage() const
boost::function< bool(const ompl::base::State *from, const ompl::base::State *to, const double t, ompl::base::State *state)> InterpolationFunction


ompl
Author(s): Ioan Sucan
autogenerated on Mon Jan 15 2018 03:51:37