model_based_planning_context.h
Go to the documentation of this file.
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2012, 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_MODEL_BASED_PLANNING_CONTEXT_
38 #define MOVEIT_OMPL_INTERFACE_MODEL_BASED_PLANNING_CONTEXT_
39 
44 
45 #include <ompl/geometric/SimpleSetup.h>
46 #include <ompl/tools/benchmark/Benchmark.h>
47 #include <ompl/tools/multiplan/ParallelPlan.h>
48 #include <ompl/base/StateStorage.h>
49 
50 #include <boost/thread/mutex.hpp>
51 
52 namespace ompl_interface
53 {
54 namespace ob = ompl::base;
55 namespace og = ompl::geometric;
56 namespace ot = ompl::tools;
57 
58 MOVEIT_CLASS_FORWARD(ModelBasedPlanningContext);
59 MOVEIT_CLASS_FORWARD(ConstraintsLibrary);
60 
62 typedef boost::function<ob::PlannerPtr(const ompl::base::SpaceInformationPtr& si, const std::string& name,
65 typedef boost::function<ConfiguredPlannerAllocator(const std::string& planner_type)> ConfiguredPlannerSelector;
66 
68 {
69  std::map<std::string, std::string> config_;
70  ConfiguredPlannerSelector planner_selector_;
71  ConstraintsLibraryConstPtr constraints_library_;
72  constraint_samplers::ConstraintSamplerManagerPtr constraint_sampler_manager_;
73 
75  std::vector<ModelBasedStateSpacePtr> subspaces_;
76  og::SimpleSetupPtr ompl_simple_setup_; // pass in the correct simple setup type
77 };
78 
80 {
81 public:
82  ModelBasedPlanningContext(const std::string& name, const ModelBasedPlanningContextSpecification& spec);
83 
85  {
86  }
87 
88  virtual bool solve(planning_interface::MotionPlanResponse& res);
89  virtual bool solve(planning_interface::MotionPlanDetailedResponse& res);
90 
91  virtual void clear();
92  virtual bool terminate();
93 
95  {
96  return spec_;
97  }
98 
99  const std::map<std::string, std::string>& getSpecificationConfig() const
100  {
101  return spec_.config_;
102  }
103 
104  void setSpecificationConfig(const std::map<std::string, std::string>& config)
105  {
106  spec_.config_ = config;
107  }
108 
109  const robot_model::RobotModelConstPtr& getRobotModel() const
110  {
111  return spec_.state_space_->getRobotModel();
112  }
113 
114  const robot_model::JointModelGroup* getJointModelGroup() const
115  {
116  return spec_.state_space_->getJointModelGroup();
117  }
118 
119  const robot_state::RobotState& getCompleteInitialRobotState() const
120  {
121  return complete_initial_robot_state_;
122  }
123 
125  {
126  return spec_.state_space_;
127  }
128 
129  const og::SimpleSetupPtr& getOMPLSimpleSetup() const
130  {
131  return ompl_simple_setup_;
132  }
133 
134  og::SimpleSetupPtr& getOMPLSimpleSetup()
135  {
136  return ompl_simple_setup_;
137  }
138 
139  const ot::Benchmark& getOMPLBenchmark() const
140  {
141  return ompl_benchmark_;
142  }
143 
144  ot::Benchmark& getOMPLBenchmark()
145  {
146  return ompl_benchmark_;
147  }
148 
149  const kinematic_constraints::KinematicConstraintSetPtr& getPathConstraints() const
150  {
151  return path_constraints_;
152  }
153 
154  /* \brief Get the maximum number of sampling attempts allowed when sampling states is needed */
155  unsigned int getMaximumStateSamplingAttempts() const
156  {
157  return max_state_sampling_attempts_;
158  }
159 
160  /* \brief Set the maximum number of sampling attempts allowed when sampling states is needed */
161  void setMaximumStateSamplingAttempts(unsigned int max_state_sampling_attempts)
162  {
163  max_state_sampling_attempts_ = max_state_sampling_attempts;
164  }
165 
166  /* \brief Get the maximum number of sampling attempts allowed when sampling goals is needed */
167  unsigned int getMaximumGoalSamplingAttempts() const
168  {
169  return max_goal_sampling_attempts_;
170  }
171 
172  /* \brief Set the maximum number of sampling attempts allowed when sampling goals is needed */
173  void setMaximumGoalSamplingAttempts(unsigned int max_goal_sampling_attempts)
174  {
175  max_goal_sampling_attempts_ = max_goal_sampling_attempts;
176  }
177 
178  /* \brief Get the maximum number of valid goal samples to store */
179  unsigned int getMaximumGoalSamples() const
180  {
181  return max_goal_samples_;
182  }
183 
184  /* \brief Set the maximum number of valid goal samples to store */
185  void setMaximumGoalSamples(unsigned int max_goal_samples)
186  {
187  max_goal_samples_ = max_goal_samples;
188  }
189 
190  /* \brief Get the maximum number of planning threads allowed */
191  unsigned int getMaximumPlanningThreads() const
192  {
193  return max_planning_threads_;
194  }
195 
196  /* \brief Set the maximum number of planning threads */
197  void setMaximumPlanningThreads(unsigned int max_planning_threads)
198  {
199  max_planning_threads_ = max_planning_threads;
200  }
201 
202  /* \brief Get the maximum solution segment length */
204  {
205  return max_solution_segment_length_;
206  }
207 
208  /* \brief Set the maximum solution segment length */
210  {
211  max_solution_segment_length_ = mssl;
212  }
213 
214  unsigned int getMinimumWaypointCount() const
215  {
216  return minimum_waypoint_count_;
217  }
218 
220  void setMinimumWaypointCount(unsigned int mwc)
221  {
222  minimum_waypoint_count_ = mwc;
223  }
224 
225  const constraint_samplers::ConstraintSamplerManagerPtr& getConstraintSamplerManager()
226  {
227  return spec_.constraint_sampler_manager_;
228  }
229 
230  void setConstraintSamplerManager(const constraint_samplers::ConstraintSamplerManagerPtr& csm)
231  {
232  spec_.constraint_sampler_manager_ = csm;
233  }
234 
235  void setVerboseStateValidityChecks(bool flag);
236 
237  void setProjectionEvaluator(const std::string& peval);
238 
239  void setPlanningVolume(const moveit_msgs::WorkspaceParameters& wparams);
240 
241  void setCompleteInitialState(const robot_state::RobotState& complete_initial_robot_state);
242 
243  bool setGoalConstraints(const std::vector<moveit_msgs::Constraints>& goal_constraints,
244  const moveit_msgs::Constraints& path_constraints, moveit_msgs::MoveItErrorCodes* error);
245  bool setPathConstraints(const moveit_msgs::Constraints& path_constraints, moveit_msgs::MoveItErrorCodes* error);
246 
247  void setConstraintsApproximations(const ConstraintsLibraryConstPtr& constraints_library)
248  {
249  spec_.constraints_library_ = constraints_library;
250  }
251 
253  {
254  return use_state_validity_cache_;
255  }
256 
257  void useStateValidityCache(bool flag)
258  {
259  use_state_validity_cache_ = flag;
260  }
261 
262  bool simplifySolutions() const
263  {
264  return simplify_solutions_;
265  }
266 
267  void simplifySolutions(bool flag)
268  {
269  simplify_solutions_ = flag;
270  }
271 
272  /* @brief Solve the planning problem. Return true if the problem is solved
273  @param timeout The time to spend on solving
274  @param count The number of runs to combine the paths of, in an attempt to generate better quality paths
275  */
276  bool solve(double timeout, unsigned int count);
277 
278  /* @brief Benchmark the planning problem. Return true on succesful saving of benchmark results
279  @param timeout The time to spend on solving
280  @param count The number of runs to average in the computation of the benchmark
281  @param filename The name of the file to which the benchmark results are to be saved (automatic names can be
282  provided if a name is not specified)
283  */
284  bool benchmark(double timeout, unsigned int count, const std::string& filename = "");
285 
286  /* @brief Get the amount of time spent computing the last plan */
287  double getLastPlanTime() const
288  {
289  return last_plan_time_;
290  }
291 
292  /* @brief Get the amount of time spent simplifying the last plan */
293  double getLastSimplifyTime() const
294  {
295  return last_simplify_time_;
296  }
297 
298  /* @brief Apply smoothing and try to simplify the plan
299  @param timeout The amount of time allowed to be spent on simplifying the plan*/
300  void simplifySolution(double timeout);
301 
302  /* @brief Interpolate the solution*/
303  void interpolateSolution();
304 
305  /* @brief Get the solution as a RobotTrajectory object*/
306  bool getSolutionPath(robot_trajectory::RobotTrajectory& traj) const;
307 
308  void convertPath(const og::PathGeometric& pg, robot_trajectory::RobotTrajectory& traj) const;
309 
310  virtual void configure();
311 
312 protected:
313  void preSolve();
314  void postSolve();
315 
316  void startSampling();
317  void stopSampling();
318 
319  virtual ob::ProjectionEvaluatorPtr getProjectionEvaluator(const std::string& peval) const;
320  virtual ob::StateSamplerPtr allocPathConstrainedSampler(const ompl::base::StateSpace* ss) const;
321  virtual void useConfig();
322  virtual ob::GoalPtr constructGoal();
323 
324  void registerTerminationCondition(const ob::PlannerTerminationCondition& ptc);
325  void unregisterTerminationCondition();
326 
328 
329  robot_state::RobotState complete_initial_robot_state_;
330 
332  og::SimpleSetupPtr ompl_simple_setup_;
333 
335  ot::Benchmark ompl_benchmark_;
336 
338  ot::ParallelPlan ompl_parallel_plan_;
339 
340  std::vector<int> space_signature_;
341 
342  kinematic_constraints::KinematicConstraintSetPtr path_constraints_;
343  moveit_msgs::Constraints path_constraints_msg_;
344  std::vector<kinematic_constraints::KinematicConstraintSetPtr> goal_constraints_;
345 
346  const ob::PlannerTerminationCondition* ptc_;
347  boost::mutex ptc_lock_;
348 
351 
354 
357  unsigned int max_goal_samples_;
358 
362 
365 
367  unsigned int max_planning_threads_;
368 
372 
376 
378 
380 };
381 }
382 
383 #endif
double last_simplify_time_
the time spent simplifying the last plan
void setMaximumGoalSamples(unsigned int max_goal_samples)
boost::function< ConfiguredPlannerAllocator(const std::string &planner_type)> ConfiguredPlannerSelector
filename
ot::Benchmark ompl_benchmark_
the OMPL tool for benchmarking planners
void setConstraintSamplerManager(const constraint_samplers::ConstraintSamplerManagerPtr &csm)
void setMinimumWaypointCount(unsigned int mwc)
Get the minimum number of waypoints along the solution path.
const og::SimpleSetupPtr & getOMPLSimpleSetup() const
const robot_model::JointModelGroup * getJointModelGroup() const
const robot_state::RobotState & getCompleteInitialRobotState() const
The MoveIt! interface to OMPL.
void setSpecificationConfig(const std::map< std::string, std::string > &config)
unsigned int max_planning_threads_
when planning in parallel, this is the maximum number of threads to use at one time ...
MOVEIT_CLASS_FORWARD(ConstraintsLibrary)
same_shared_ptr< ModelBasedStateSpace, ompl::base::StateSpacePtr >::type ModelBasedStateSpacePtr
boost::function< ob::PlannerPtr(const ompl::base::SpaceInformationPtr &si, const std::string &name, const ModelBasedPlanningContextSpecification &spec)> ConfiguredPlannerAllocator
kinematic_constraints::KinematicConstraintSetPtr path_constraints_
const constraint_samplers::ConstraintSamplerManagerPtr & getConstraintSamplerManager()
double last_plan_time_
the time spent computing the last plan
const ob::PlannerTerminationCondition * ptc_
ot::ParallelPlan ompl_parallel_plan_
tool used to compute multiple plans in parallel; this uses the problem definition maintained by ompl_...
const kinematic_constraints::KinematicConstraintSetPtr & getPathConstraints() const
const std::map< std::string, std::string > & getSpecificationConfig() const
og::SimpleSetupPtr ompl_simple_setup_
the OMPL planning context; this contains the problem definition and the planner used ...
ModelBasedPlanningContextSpecification spec_
void setMaximumGoalSamplingAttempts(unsigned int max_goal_sampling_attempts)
void setMaximumPlanningThreads(unsigned int max_planning_threads)
constraint_samplers::ConstraintSamplerManagerPtr constraint_sampler_manager_
std::vector< kinematic_constraints::KinematicConstraintSetPtr > goal_constraints_
unsigned int max_goal_sampling_attempts_
maximum number of attempts to be made at sampling a goal states
const robot_model::RobotModelConstPtr & getRobotModel() const
void setMaximumStateSamplingAttempts(unsigned int max_state_sampling_attempts)
const ModelBasedPlanningContextSpecification & getSpecification() const
void setConstraintsApproximations(const ConstraintsLibraryConstPtr &constraints_library)
const ModelBasedStateSpacePtr & getOMPLStateSpace() const


ompl
Author(s): Ioan Sucan
autogenerated on Wed Jul 10 2019 04:03:46