model_based_planning_context.h
Go to the documentation of this file.
00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 *
00004 *  Copyright (c) 2012, 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_MODEL_BASED_PLANNING_CONTEXT_
00038 #define MOVEIT_OMPL_INTERFACE_MODEL_BASED_PLANNING_CONTEXT_
00039 
00040 #include <moveit/ompl_interface/parameterization/model_based_state_space.h>
00041 #include <moveit/ompl_interface/detail/constrained_valid_state_sampler.h>
00042 #include <moveit/constraint_samplers/constraint_sampler_manager.h>
00043 #include <moveit/planning_interface/planning_interface.h>
00044 
00045 #include <ompl/geometric/SimpleSetup.h>
00046 #include <ompl/tools/benchmark/Benchmark.h>
00047 #include <ompl/tools/multiplan/ParallelPlan.h>
00048 #include <ompl/base/StateStorage.h>
00049 
00050 #include <boost/thread/mutex.hpp>
00051 
00052 namespace ompl_interface
00053 {
00054 namespace ob = ompl::base;
00055 namespace og = ompl::geometric;
00056 namespace ot = ompl::tools;
00057 
00058 MOVEIT_CLASS_FORWARD(ModelBasedPlanningContext);
00059 MOVEIT_CLASS_FORWARD(ConstraintsLibrary);
00060 
00061 struct ModelBasedPlanningContextSpecification;
00062 typedef boost::function<ob::PlannerPtr(const ompl::base::SpaceInformationPtr& si, const std::string& name,
00063                                        const ModelBasedPlanningContextSpecification& spec)>
00064     ConfiguredPlannerAllocator;
00065 typedef boost::function<ConfiguredPlannerAllocator(const std::string& planner_type)> ConfiguredPlannerSelector;
00066 
00067 struct ModelBasedPlanningContextSpecification
00068 {
00069   std::map<std::string, std::string> config_;
00070   ConfiguredPlannerSelector planner_selector_;
00071   ConstraintsLibraryConstPtr constraints_library_;
00072   constraint_samplers::ConstraintSamplerManagerPtr constraint_sampler_manager_;
00073 
00074   ModelBasedStateSpacePtr state_space_;
00075   std::vector<ModelBasedStateSpacePtr> subspaces_;
00076   og::SimpleSetupPtr ompl_simple_setup_;  // pass in the correct simple setup type
00077 };
00078 
00079 class ModelBasedPlanningContext : public planning_interface::PlanningContext
00080 {
00081 public:
00082   ModelBasedPlanningContext(const std::string& name, const ModelBasedPlanningContextSpecification& spec);
00083 
00084   virtual ~ModelBasedPlanningContext()
00085   {
00086   }
00087 
00088   virtual bool solve(planning_interface::MotionPlanResponse& res);
00089   virtual bool solve(planning_interface::MotionPlanDetailedResponse& res);
00090 
00091   virtual void clear();
00092   virtual bool terminate();
00093 
00094   const ModelBasedPlanningContextSpecification& getSpecification() const
00095   {
00096     return spec_;
00097   }
00098 
00099   const std::map<std::string, std::string>& getSpecificationConfig() const
00100   {
00101     return spec_.config_;
00102   }
00103 
00104   void setSpecificationConfig(const std::map<std::string, std::string>& config)
00105   {
00106     spec_.config_ = config;
00107   }
00108 
00109   const robot_model::RobotModelConstPtr& getRobotModel() const
00110   {
00111     return spec_.state_space_->getRobotModel();
00112   }
00113 
00114   const robot_model::JointModelGroup* getJointModelGroup() const
00115   {
00116     return spec_.state_space_->getJointModelGroup();
00117   }
00118 
00119   const robot_state::RobotState& getCompleteInitialRobotState() const
00120   {
00121     return complete_initial_robot_state_;
00122   }
00123 
00124   const ModelBasedStateSpacePtr& getOMPLStateSpace() const
00125   {
00126     return spec_.state_space_;
00127   }
00128 
00129   const og::SimpleSetupPtr& getOMPLSimpleSetup() const
00130   {
00131     return ompl_simple_setup_;
00132   }
00133 
00134   og::SimpleSetupPtr& getOMPLSimpleSetup()
00135   {
00136     return ompl_simple_setup_;
00137   }
00138 
00139   const ot::Benchmark& getOMPLBenchmark() const
00140   {
00141     return ompl_benchmark_;
00142   }
00143 
00144   ot::Benchmark& getOMPLBenchmark()
00145   {
00146     return ompl_benchmark_;
00147   }
00148 
00149   const kinematic_constraints::KinematicConstraintSetPtr& getPathConstraints() const
00150   {
00151     return path_constraints_;
00152   }
00153 
00154   /* \brief Get the maximum number of sampling attempts allowed when sampling states is needed */
00155   unsigned int getMaximumStateSamplingAttempts() const
00156   {
00157     return max_state_sampling_attempts_;
00158   }
00159 
00160   /* \brief Set the maximum number of sampling attempts allowed when sampling states is needed */
00161   void setMaximumStateSamplingAttempts(unsigned int max_state_sampling_attempts)
00162   {
00163     max_state_sampling_attempts_ = max_state_sampling_attempts;
00164   }
00165 
00166   /* \brief Get the maximum number of sampling attempts allowed when sampling goals is needed */
00167   unsigned int getMaximumGoalSamplingAttempts() const
00168   {
00169     return max_goal_sampling_attempts_;
00170   }
00171 
00172   /* \brief Set the maximum number of sampling attempts allowed when sampling goals is needed */
00173   void setMaximumGoalSamplingAttempts(unsigned int max_goal_sampling_attempts)
00174   {
00175     max_goal_sampling_attempts_ = max_goal_sampling_attempts;
00176   }
00177 
00178   /* \brief Get the maximum number of valid goal samples to store */
00179   unsigned int getMaximumGoalSamples() const
00180   {
00181     return max_goal_samples_;
00182   }
00183 
00184   /* \brief Set the maximum number of valid goal samples to store */
00185   void setMaximumGoalSamples(unsigned int max_goal_samples)
00186   {
00187     max_goal_samples_ = max_goal_samples;
00188   }
00189 
00190   /* \brief Get the maximum number of planning threads allowed */
00191   unsigned int getMaximumPlanningThreads() const
00192   {
00193     return max_planning_threads_;
00194   }
00195 
00196   /* \brief Set the maximum number of planning threads */
00197   void setMaximumPlanningThreads(unsigned int max_planning_threads)
00198   {
00199     max_planning_threads_ = max_planning_threads;
00200   }
00201 
00202   /* \brief Get the maximum solution segment length */
00203   double getMaximumSolutionSegmentLength() const
00204   {
00205     return max_solution_segment_length_;
00206   }
00207 
00208   /* \brief Set the maximum solution segment length */
00209   void setMaximumSolutionSegmentLength(double mssl)
00210   {
00211     max_solution_segment_length_ = mssl;
00212   }
00213 
00214   unsigned int getMinimumWaypointCount() const
00215   {
00216     return minimum_waypoint_count_;
00217   }
00218 
00220   void setMinimumWaypointCount(unsigned int mwc)
00221   {
00222     minimum_waypoint_count_ = mwc;
00223   }
00224 
00225   const constraint_samplers::ConstraintSamplerManagerPtr& getConstraintSamplerManager()
00226   {
00227     return spec_.constraint_sampler_manager_;
00228   }
00229 
00230   void setConstraintSamplerManager(const constraint_samplers::ConstraintSamplerManagerPtr& csm)
00231   {
00232     spec_.constraint_sampler_manager_ = csm;
00233   }
00234 
00235   void setVerboseStateValidityChecks(bool flag);
00236 
00237   void setProjectionEvaluator(const std::string& peval);
00238 
00239   void setPlanningVolume(const moveit_msgs::WorkspaceParameters& wparams);
00240 
00241   void setCompleteInitialState(const robot_state::RobotState& complete_initial_robot_state);
00242 
00243   bool setGoalConstraints(const std::vector<moveit_msgs::Constraints>& goal_constraints,
00244                           const moveit_msgs::Constraints& path_constraints, moveit_msgs::MoveItErrorCodes* error);
00245   bool setPathConstraints(const moveit_msgs::Constraints& path_constraints, moveit_msgs::MoveItErrorCodes* error);
00246 
00247   void setConstraintsApproximations(const ConstraintsLibraryConstPtr& constraints_library)
00248   {
00249     spec_.constraints_library_ = constraints_library;
00250   }
00251 
00252   bool useStateValidityCache() const
00253   {
00254     return use_state_validity_cache_;
00255   }
00256 
00257   void useStateValidityCache(bool flag)
00258   {
00259     use_state_validity_cache_ = flag;
00260   }
00261 
00262   bool simplifySolutions() const
00263   {
00264     return simplify_solutions_;
00265   }
00266 
00267   void simplifySolutions(bool flag)
00268   {
00269     simplify_solutions_ = flag;
00270   }
00271 
00272   /* @brief Solve the planning problem. Return true if the problem is solved
00273      @param timeout The time to spend on solving
00274      @param count The number of runs to combine the paths of, in an attempt to generate better quality paths
00275   */
00276   bool solve(double timeout, unsigned int count);
00277 
00278   /* @brief Benchmark the planning problem. Return true on succesful saving of benchmark results
00279      @param timeout The time to spend on solving
00280      @param count The number of runs to average in the computation of the benchmark
00281      @param filename The name of the file to which the benchmark results are to be saved (automatic names can be
00282      provided if a name is not specified)
00283   */
00284   bool benchmark(double timeout, unsigned int count, const std::string& filename = "");
00285 
00286   /* @brief Get the amount of time spent computing the last plan */
00287   double getLastPlanTime() const
00288   {
00289     return last_plan_time_;
00290   }
00291 
00292   /* @brief Get the amount of time spent simplifying the last plan */
00293   double getLastSimplifyTime() const
00294   {
00295     return last_simplify_time_;
00296   }
00297 
00298   /* @brief Apply smoothing and try to simplify the plan
00299      @param timeout The amount of time allowed to be spent on simplifying the plan*/
00300   void simplifySolution(double timeout);
00301 
00302   /* @brief Interpolate the solution*/
00303   void interpolateSolution();
00304 
00305   /* @brief Get the solution as a RobotTrajectory object*/
00306   bool getSolutionPath(robot_trajectory::RobotTrajectory& traj) const;
00307 
00308   void convertPath(const og::PathGeometric& pg, robot_trajectory::RobotTrajectory& traj) const;
00309 
00310   virtual void configure();
00311 
00312 protected:
00313   void preSolve();
00314   void postSolve();
00315 
00316   void startSampling();
00317   void stopSampling();
00318 
00319   virtual ob::ProjectionEvaluatorPtr getProjectionEvaluator(const std::string& peval) const;
00320   virtual ob::StateSamplerPtr allocPathConstrainedSampler(const ompl::base::StateSpace* ss) const;
00321   virtual void useConfig();
00322   virtual ob::GoalPtr constructGoal();
00323 
00324   void registerTerminationCondition(const ob::PlannerTerminationCondition& ptc);
00325   void unregisterTerminationCondition();
00326 
00327   ModelBasedPlanningContextSpecification spec_;
00328 
00329   robot_state::RobotState complete_initial_robot_state_;
00330 
00332   og::SimpleSetupPtr ompl_simple_setup_;
00333 
00335   ot::Benchmark ompl_benchmark_;
00336 
00338   ot::ParallelPlan ompl_parallel_plan_;
00339 
00340   std::vector<int> space_signature_;
00341 
00342   kinematic_constraints::KinematicConstraintSetPtr path_constraints_;
00343   moveit_msgs::Constraints path_constraints_msg_;
00344   std::vector<kinematic_constraints::KinematicConstraintSetPtr> goal_constraints_;
00345 
00346   const ob::PlannerTerminationCondition* ptc_;
00347   boost::mutex ptc_lock_;
00348 
00350   double last_plan_time_;
00351 
00353   double last_simplify_time_;
00354 
00357   unsigned int max_goal_samples_;
00358 
00361   unsigned int max_state_sampling_attempts_;
00362 
00364   unsigned int max_goal_sampling_attempts_;
00365 
00367   unsigned int max_planning_threads_;
00368 
00371   double max_solution_segment_length_;
00372 
00375   unsigned int minimum_waypoint_count_;
00376 
00377   bool use_state_validity_cache_;
00378 
00379   bool simplify_solutions_;
00380 };
00381 }
00382 
00383 #endif


ompl
Author(s): Ioan Sucan
autogenerated on Mon Jul 24 2017 02:21:33