moveit2
The MoveIt Motion Planning Framework for ROS 2.
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 #pragma once
38 
39 #include <map>
43 #include <ompl/base/StateStorage.h>
44 #include <boost/serialization/map.hpp>
45 
46 namespace ompl_interface
47 {
48 typedef std::pair<std::vector<std::size_t>, std::map<std::size_t, std::pair<std::size_t, std::size_t> > >
50 typedef ompl::base::StateStorageWithMetadata<ConstrainedStateMetadata> ConstraintApproximationStateStorage;
51 
53 
55 {
56 public:
57  ConstraintApproximation(std::string group, std::string state_space_parameterization, bool explicit_motions,
58  moveit_msgs::msg::Constraints msg, std::string filename, ompl::base::StateStoragePtr storage,
59  std::size_t milestones = 0);
60 
62  {
63  }
64 
65  const std::string& getName() const
66  {
67  return constraint_msg_.name;
68  }
69 
70  ompl::base::StateSamplerAllocator getStateSamplerAllocator(const moveit_msgs::msg::Constraints& msg) const;
71 
73 
74  const std::vector<int>& getSpaceSignature() const
75  {
76  return space_signature_;
77  }
78 
79  const std::string& getGroup() const
80  {
81  return group_;
82  }
83 
84  bool hasExplicitMotions() const
85  {
86  return explicit_motions_;
87  }
88 
89  std::size_t getMilestoneCount() const
90  {
91  return milestones_;
92  }
93 
94  const std::string& getStateSpaceParameterization() const
95  {
97  }
98 
99  const moveit_msgs::msg::Constraints& getConstraintsMsg() const
100  {
101  return constraint_msg_;
102  }
103 
104  const ompl::base::StateStoragePtr& getStateStorage() const
105  {
106  return state_storage_ptr_;
107  }
108 
109  const std::string& getFilename() const
110  {
111  return ompldb_filename_;
112  }
113 
114 protected:
115  std::string group_;
118 
119  moveit_msgs::msg::Constraints constraint_msg_;
120 
121  std::vector<int> space_signature_;
122 
123  std::string ompldb_filename_;
124  ompl::base::StateStoragePtr state_storage_ptr_;
126  std::size_t milestones_;
127 };
128 
130 {
132  : samples(0)
133  , edges_per_sample(0)
134  , max_edge_length(std::numeric_limits<double>::infinity())
135  , explicit_motions(false)
138  {
139  }
140 
142  unsigned int samples;
143  unsigned int edges_per_sample;
147  unsigned int max_explicit_points;
148 };
149 
151 {
152  ConstraintApproximationPtr approx;
153  std::size_t milestones;
157 };
158 
159 MOVEIT_CLASS_FORWARD(ConstraintsLibrary); // Defines ConstraintsLibraryPtr, ConstPtr, WeakPtr... etc
160 
162 {
163 public:
164  ConstraintsLibrary(ModelBasedPlanningContext* pcontext) : context_(pcontext)
165  {
166  }
167 
168  void loadConstraintApproximations(const std::string& path);
169 
170  void saveConstraintApproximations(const std::string& path);
171 
173  addConstraintApproximation(const moveit_msgs::msg::Constraints& constr_sampling,
174  const moveit_msgs::msg::Constraints& constr_hard, const std::string& group,
175  const planning_scene::PlanningSceneConstPtr& scene,
177 
179  addConstraintApproximation(const moveit_msgs::msg::Constraints& constr, const std::string& group,
180  const planning_scene::PlanningSceneConstPtr& scene,
182 
183  void printConstraintApproximations(std::ostream& out = std::cout) const;
185 
186  void registerConstraintApproximation(const ConstraintApproximationPtr& approx)
187  {
188  constraint_approximations_[approx->getName()] = approx;
189  }
190 
191  const ConstraintApproximationPtr& getConstraintApproximation(const moveit_msgs::msg::Constraints& msg) const;
192 
193 private:
194  ompl::base::StateStoragePtr constructConstraintApproximation(ModelBasedPlanningContext* pcontext,
195  const moveit_msgs::msg::Constraints& constr_sampling,
196  const moveit_msgs::msg::Constraints& constr_hard,
199 
200  ModelBasedPlanningContext* context_;
201  std::map<std::string, ConstraintApproximationPtr> constraint_approximations_;
202 };
203 } // namespace ompl_interface
void registerConstraintApproximation(const ConstraintApproximationPtr &approx)
const ConstraintApproximationPtr & getConstraintApproximation(const moveit_msgs::msg::Constraints &msg) const
ConstraintApproximationConstructionResults addConstraintApproximation(const moveit_msgs::msg::Constraints &constr_sampling, const moveit_msgs::msg::Constraints &constr_hard, const std::string &group, const planning_scene::PlanningSceneConstPtr &scene, const ConstraintApproximationConstructionOptions &options)
void printConstraintApproximations(std::ostream &out=std::cout) const
ConstraintsLibrary(ModelBasedPlanningContext *pcontext)
void saveConstraintApproximations(const std::string &path)
void loadConstraintApproximations(const std::string &path)
The MoveIt interface to OMPL.
std::pair< std::vector< std::size_t >, std::map< std::size_t, std::pair< std::size_t, std::size_t > > > ConstrainedStateMetadata
MOVEIT_CLASS_FORWARD(ValidStateSampler)
std::function< bool(const ompl::base::State *from, const ompl::base::State *to, const double t, ompl::base::State *state)> InterpolationFunction
ompl::base::StateStorageWithMetadata< std::vector< std::size_t > > ConstraintApproximationStateStorage
scene
Definition: pick.py:52
ConstraintApproximationStateStorage * state_storage_
const ompl::base::StateStoragePtr & getStateStorage() const
const moveit_msgs::msg::Constraints & getConstraintsMsg() const
const std::string & getStateSpaceParameterization() const
ConstraintApproximation(const planning_models::RobotModelConstPtr &kinematic_model, const std::string &group, const std::string &factory, const std::string &serialization, const std::string &filename, const ompl::base::StateStoragePtr &storage)
const std::vector< int > & getSpaceSignature() const
ompl::base::StateSamplerAllocator getStateSamplerAllocator(const moveit_msgs::msg::Constraints &msg) const
InterpolationFunction getInterpolationFunction() const