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 
42 #include <ompl/base/StateStorage.h>
43 #include <boost/serialization/map.hpp>
44 
45 namespace ompl_interface
46 {
47 typedef std::pair<std::vector<std::size_t>, std::map<std::size_t, std::pair<std::size_t, std::size_t> > >
49 typedef ompl::base::StateStorageWithMetadata<ConstrainedStateMetadata> ConstraintApproximationStateStorage;
50 
51 MOVEIT_CLASS_FORWARD(ConstraintApproximation);
52 
53 class ConstraintApproximation
54 {
55 public:
56  ConstraintApproximation(std::string group, std::string state_space_parameterization, bool explicit_motions,
57  moveit_msgs::Constraints msg, std::string filename, ompl::base::StateStoragePtr storage,
58  std::size_t milestones = 0);
59 
60  virtual ~ConstraintApproximation()
61  {
62  }
63 
64  const std::string& getName() const
65  {
66  return constraint_msg_.name;
67  }
68 
69  ompl::base::StateSamplerAllocator getStateSamplerAllocator(const moveit_msgs::Constraints& msg) const;
70 
72 
73  const std::vector<int>& getSpaceSignature() const
74  {
75  return space_signature_;
76  }
77 
78  const std::string& getGroup() const
79  {
80  return group_;
81  }
82 
83  bool hasExplicitMotions() const
84  {
85  return explicit_motions_;
86  }
87 
88  std::size_t getMilestoneCount() const
89  {
90  return milestones_;
91  }
92 
93  const std::string& getStateSpaceParameterization() const
94  {
96  }
97 
98  const moveit_msgs::Constraints& getConstraintsMsg() const
99  {
100  return constraint_msg_;
101  }
102 
103  const ompl::base::StateStoragePtr& getStateStorage() const
104  {
106  }
107 
108  const std::string& getFilename() const
109  {
111  }
112 
113 protected:
114  std::string group_;
116  bool explicit_motions_;
117 
118  moveit_msgs::Constraints constraint_msg_;
119 
120  std::vector<int> space_signature_;
121 
122  std::string ompldb_filename_;
123  ompl::base::StateStoragePtr state_storage_ptr_;
125  std::size_t milestones_;
126 };
127 
129 {
131  : samples(0)
132  , edges_per_sample(0)
133  , max_edge_length(std::numeric_limits<double>::infinity())
134  , explicit_motions(false)
137  {
138  }
139 
141  unsigned int samples;
142  unsigned int edges_per_sample;
143  double max_edge_length;
144  bool explicit_motions;
146  unsigned int max_explicit_points;
147 };
148 
150 {
151  ConstraintApproximationPtr approx;
152  std::size_t milestones;
153  double state_sampling_time;
154  double state_connection_time;
155  double sampling_success_rate;
156 };
157 
158 MOVEIT_CLASS_FORWARD(ConstraintsLibrary); // Defines ConstraintsLibraryPtr, ConstPtr, WeakPtr... etc
159 
161 {
162 public:
164  {
165  }
166 
167  void loadConstraintApproximations(const std::string& path);
168 
169  void saveConstraintApproximations(const std::string& path);
170 
172  addConstraintApproximation(const moveit_msgs::Constraints& constr_sampling,
173  const moveit_msgs::Constraints& constr_hard, const std::string& group,
174  const planning_scene::PlanningSceneConstPtr& scene,
176 
178  addConstraintApproximation(const moveit_msgs::Constraints& constr, const std::string& group,
179  const planning_scene::PlanningSceneConstPtr& scene,
181 
182  void printConstraintApproximations(std::ostream& out = std::cout) const;
184 
185  void registerConstraintApproximation(const ConstraintApproximationPtr& approx)
186  {
188  }
189 
190  const ConstraintApproximationPtr& getConstraintApproximation(const moveit_msgs::Constraints& msg) const;
191 
192 private:
193  ompl::base::StateStoragePtr
194  constructConstraintApproximation(ModelBasedPlanningContext* pcontext, const moveit_msgs::Constraints& constr_sampling,
195  const moveit_msgs::Constraints& constr_hard,
198 
200  std::map<std::string, ConstraintApproximationPtr> constraint_approximations_;
201 };
202 } // namespace ompl_interface
ompl_interface::ConstraintApproximationConstructionOptions::max_explicit_points
unsigned int max_explicit_points
Definition: constraints_library.h:178
ompl_interface::ConstraintsLibrary::addConstraintApproximation
ConstraintApproximationConstructionResults addConstraintApproximation(const moveit_msgs::Constraints &constr_sampling, const moveit_msgs::Constraints &constr_hard, const std::string &group, const planning_scene::PlanningSceneConstPtr &scene, const ConstraintApproximationConstructionOptions &options)
Definition: constraints_library.cpp:424
ompl_interface::ConstraintApproximation::group_
std::string group_
Definition: constraint_approximations.h:94
ompl_interface::ConstraintApproximation::constraint_msg_
moveit_msgs::Constraints constraint_msg_
Definition: constraint_approximations.h:97
ompl_interface::ConstraintsLibrary::loadConstraintApproximations
void loadConstraintApproximations(const std::string &path)
Definition: constraints_library.cpp:277
ompl_interface::ConstraintApproximation::getGroup
const std::string & getGroup() const
Definition: constraints_library.h:110
ompl_interface::ConstraintApproximation::getFilename
const std::string & getFilename() const
Definition: constraints_library.h:140
ompl_interface::ConstraintApproximationConstructionOptions::samples
unsigned int samples
Definition: constraints_library.h:173
ompl_interface::ConstraintApproximationConstructionResults::state_connection_time
double state_connection_time
Definition: constraints_library.h:186
model_based_planning_context.h
ompl_interface::ModelBasedPlanningContext
Definition: model_based_planning_context.h:105
ompl_interface::ConstraintApproximation::ompldb_filename_
std::string ompldb_filename_
Definition: constraint_approximations.h:102
ompl_interface::ConstraintApproximation::~ConstraintApproximation
virtual ~ConstraintApproximation()
Definition: constraints_library.h:92
ompl_interface::ConstraintApproximation::state_storage_ptr_
ompl::base::StateStoragePtr state_storage_ptr_
Definition: constraint_approximations.h:103
ompl_interface::ConstraintsLibrary::constructConstraintApproximation
ompl::base::StateStoragePtr constructConstraintApproximation(ModelBasedPlanningContext *pcontext, const moveit_msgs::Constraints &constr_sampling, const moveit_msgs::Constraints &constr_hard, const ConstraintApproximationConstructionOptions &options, ConstraintApproximationConstructionResults &result)
Definition: constraints_library.cpp:464
ompl_interface
The MoveIt interface to OMPL.
Definition: constrained_goal_sampler.h:46
ompl_interface::ConstraintApproximation::ConstraintApproximation
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)
ompl_interface::ConstraintApproximation::milestones_
std::size_t milestones_
Definition: constraints_library.h:157
ompl_interface::ConstraintApproximation::getInterpolationFunction
InterpolationFunction getInterpolationFunction() const
Definition: constraints_library.cpp:216
ompl_interface::ConstraintApproximation::getSpaceSignature
const std::vector< int > & getSpaceSignature() const
Definition: constraints_library.h:105
ompl_interface::ConstraintApproximationConstructionResults::approx
ConstraintApproximationPtr approx
Definition: constraints_library.h:183
ompl_interface::ConstraintApproximation::state_space_parameterization_
std::string state_space_parameterization_
Definition: constraints_library.h:147
ompl_interface::ConstraintApproximation::space_signature_
std::vector< int > space_signature_
Definition: constraint_approximations.h:100
ompl_interface::ConstrainedStateMetadata
std::pair< std::vector< std::size_t >, std::map< std::size_t, std::pair< std::size_t, std::size_t > > > ConstrainedStateMetadata
Definition: constraints_library.h:80
ompl_interface::ConstraintApproximationConstructionResults
Definition: constraints_library.h:181
ompl_interface::ConstraintApproximation::state_storage_
ConstraintApproximationStateStorage * state_storage_
Definition: constraint_approximations.h:104
ompl_interface::ConstraintApproximationConstructionOptions::ConstraintApproximationConstructionOptions
ConstraintApproximationConstructionOptions()
Definition: constraints_library.h:162
ompl_interface::ConstraintApproximation::getStateSamplerAllocator
ompl::base::StateSamplerAllocator getStateSamplerAllocator(const moveit_msgs::Constraints &msg) const
Definition: constraints_library.cpp:226
ompl_interface::ConstraintApproximationStateStorage
ompl::base::StateStorageWithMetadata< std::vector< std::size_t > > ConstraintApproximationStateStorage
Definition: constraint_approximations.h:79
ompl_interface::ConstraintApproximation::getConstraintsMsg
const moveit_msgs::Constraints & getConstraintsMsg() const
Definition: constraints_library.h:130
ompl_interface::ConstraintApproximation::hasExplicitMotions
bool hasExplicitMotions() const
Definition: constraints_library.h:115
ompl_interface::ConstraintApproximationConstructionOptions
Definition: constraints_library.h:160
ompl_interface::ConstraintsLibrary::saveConstraintApproximations
void saveConstraintApproximations(const std::string &path)
Definition: constraints_library.cpp:351
ompl_interface::ConstraintsLibrary::printConstraintApproximations
void printConstraintApproximations(std::ostream &out=std::cout) const
Definition: constraints_library.cpp:389
ompl_interface::ConstraintApproximation::explicit_motions_
bool explicit_motions_
Definition: constraints_library.h:148
ompl_interface::ConstraintsLibrary::clearConstraintApproximations
void clearConstraintApproximations()
Definition: constraints_library.cpp:384
ompl_interface::ConstraintApproximationConstructionResults::sampling_success_rate
double sampling_success_rate
Definition: constraints_library.h:187
ompl_interface::ConstraintApproximationConstructionOptions::edges_per_sample
unsigned int edges_per_sample
Definition: constraints_library.h:174
kinematic_constraint.h
ompl_interface::ConstraintsLibrary::ConstraintsLibrary
ConstraintsLibrary(ModelBasedPlanningContext *pcontext)
Definition: constraints_library.h:195
ompl_interface::ConstraintApproximationConstructionOptions::explicit_points_resolution
double explicit_points_resolution
Definition: constraints_library.h:177
ompl_interface::ConstraintsLibrary::getConstraintApproximation
const ConstraintApproximationPtr & getConstraintApproximation(const moveit_msgs::Constraints &msg) const
Definition: constraints_library.cpp:404
ompl_interface::ConstraintApproximation::getName
const std::string & getName() const
Definition: constraints_library.h:96
ompl_interface::ConstraintsLibrary::registerConstraintApproximation
void registerConstraintApproximation(const ConstraintApproximationPtr &approx)
Definition: constraints_library.h:217
ompl_interface::ConstraintApproximation::getStateSpaceParameterization
const std::string & getStateSpaceParameterization() const
Definition: constraints_library.h:125
ompl_interface::ConstraintApproximation::getStateStorage
const ompl::base::StateStoragePtr & getStateStorage() const
Definition: constraints_library.h:135
class_forward.h
std
ompl_interface::ConstraintApproximation::getMilestoneCount
std::size_t getMilestoneCount() const
Definition: constraints_library.h:120
ompl_interface::MOVEIT_CLASS_FORWARD
MOVEIT_CLASS_FORWARD(ConstraintApproximation)
ompl_interface::ConstraintApproximationConstructionResults::state_sampling_time
double state_sampling_time
Definition: constraints_library.h:185
approx
bool approx(S x, S y)
ompl_interface::ConstraintApproximationConstructionOptions::state_space_parameterization
std::string state_space_parameterization
Definition: constraints_library.h:172
ompl_interface::ConstraintsLibrary
Definition: constraints_library.h:192
ompl_interface::InterpolationFunction
std::function< bool(const ompl::base::State *from, const ompl::base::State *to, const double t, ompl::base::State *state)> InterpolationFunction
Definition: model_based_state_space.h:81
ompl_interface::ConstraintApproximationConstructionOptions::explicit_motions
bool explicit_motions
Definition: constraints_library.h:176
ompl_interface::ConstraintsLibrary::context_
ModelBasedPlanningContext * context_
Definition: constraints_library.h:231
ompl_interface::ConstraintsLibrary::constraint_approximations_
std::map< std::string, ConstraintApproximationPtr > constraint_approximations_
Definition: constraints_library.h:232
ompl_interface::ConstraintApproximationConstructionResults::milestones
std::size_t milestones
Definition: constraints_library.h:184
ompl_interface::ConstraintApproximationConstructionOptions::max_edge_length
double max_edge_length
Definition: constraints_library.h:175


ompl
Author(s): Ioan Sucan
autogenerated on Sun Mar 3 2024 03:24:34