Exotica
ompl_solver.h
Go to the documentation of this file.
1 //
2 // Copyright (c) 2018, University of Edinburgh
3 // All rights reserved.
4 //
5 // Redistribution and use in source and binary forms, with or without
6 // modification, are permitted provided that the following conditions are met:
7 //
8 // * Redistributions of source code must retain the above copyright notice,
9 // this list of conditions and the following disclaimer.
10 // * Redistributions in binary form must reproduce the above copyright
11 // notice, this list of conditions and the following disclaimer in the
12 // documentation and/or other materials provided with the distribution.
13 // * Neither the name of nor the names of its contributors may be used to
14 // endorse or promote products derived from this software without specific
15 // prior written permission.
16 //
17 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18 // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19 // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20 // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21 // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22 // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23 // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24 // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25 // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26 // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27 // POSSIBILITY OF SUCH DAMAGE.
28 //
29 
30 #ifndef EXOTICA_OMPL_SOLVER_OMPL_SOLVER_H_
31 #define EXOTICA_OMPL_SOLVER_OMPL_SOLVER_H_
32 
35 
36 typedef boost::function<ompl::base::PlannerPtr(const ompl::base::SpaceInformationPtr &si, const std::string &name)> ConfiguredPlannerAllocator;
37 
38 #if ROS_VERSION_MINIMUM(1, 12, 0) // if ROS version >= ROS_KINETIC
39 template <class T, class T1>
40 std::shared_ptr<T> ompl_cast(std::shared_ptr<T1> ptr)
41 {
42  return std::static_pointer_cast<T>(ptr);
43 }
44 template <class T>
45 using ompl_ptr = std::shared_ptr<T>;
46 #else
47 template <class T, class T1>
48 boost::shared_ptr<T> ompl_cast(boost::shared_ptr<T1> ptr)
49 {
50  return boost::static_pointer_cast<T>(ptr);
51 }
52 template <class T>
53 using ompl_ptr = boost::shared_ptr<T>;
54 #endif
55 
56 namespace exotica
57 {
58 template <class ProblemType>
59 class OMPLSolver : public MotionSolver
60 {
61 public:
62  OMPLSolver();
63  virtual ~OMPLSolver();
64 
65  void Solve(Eigen::MatrixXd &solution) override;
66  void SpecifyProblem(PlanningProblemPtr pointer) override;
67 
68  int GetRandomSeed() const;
69 
70  // StateSpace related methods exposed via solver
71  double GetMaximumExtent() const { return state_space_->getMaximumExtent(); }
72  double GetLongestValidSegmentLength() const { return state_space_->getLongestValidSegmentLength(); }
73  void SetLongestValidSegmentFraction(double segmentFraction) { state_space_->setLongestValidSegmentFraction(segmentFraction); }
74  void SetValidSegmentCountFactor(unsigned int factor) { state_space_->setValidSegmentCountFactor(factor); }
75  unsigned int GetValidSegmentCountFactor() const { return state_space_->getValidSegmentCountFactor(); }
76 
77 protected:
78  template <typename T>
79  static ompl::base::PlannerPtr AllocatePlanner(const ompl::base::SpaceInformationPtr &si, const std::string &new_name)
80  {
81  ompl::base::PlannerPtr planner(new T(si));
82  if (!new_name.empty())
83  planner->setName(new_name);
84  return planner;
85  }
86 
87  void SetGoalState(Eigen::VectorXdRefConst qT, const double eps = 0);
88  void PreSolve();
89  void PostSolve();
90  void GetPath(Eigen::MatrixXd &traj, ompl::base::PlannerTerminationCondition &ptc);
91  OMPLSolverInitializer init_;
92  std::shared_ptr<ProblemType> prob_;
93  ompl::geometric::SimpleSetupPtr ompl_simple_setup_;
94  ompl::base::StateSpacePtr state_space_;
96  std::string algorithm_;
97  bool multi_query_ = false;
98  std::vector<double> bounds_; // original bounds for locked state space
99 };
100 } // namespace exotica
101 
102 #endif // EXOTICA_OMPL_SOLVER_OMPL_SOLVER_H_
exotica::OMPLSolver::SetGoalState
void SetGoalState(Eigen::VectorXdRefConst qT, const double eps=0)
exotica::OMPLSolver::multi_query_
bool multi_query_
Definition: ompl_solver.h:97
ompl_ptr
boost::shared_ptr< T > ompl_ptr
Definition: ompl_solver.h:53
exotica::OMPLSolver::ompl_simple_setup_
ompl::geometric::SimpleSetupPtr ompl_simple_setup_
Definition: ompl_solver.h:93
exotica::OMPLSolver::GetPath
void GetPath(Eigen::MatrixXd &traj, ompl::base::PlannerTerminationCondition &ptc)
ompl_exo.h
ConfiguredPlannerAllocator
boost::function< ompl::base::PlannerPtr(const ompl::base::SpaceInformationPtr &si, const std::string &name)> ConfiguredPlannerAllocator
Definition: ompl_solver.h:36
exotica::OMPLSolver::PostSolve
void PostSolve()
ompl_cast
boost::shared_ptr< T > ompl_cast(boost::shared_ptr< T1 > ptr)
Definition: ompl_solver.h:48
exotica::OMPLSolver::GetRandomSeed
int GetRandomSeed() const
exotica::OMPLSolver::AllocatePlanner
static ompl::base::PlannerPtr AllocatePlanner(const ompl::base::SpaceInformationPtr &si, const std::string &new_name)
Definition: ompl_solver.h:79
exotica
Definition: cartpole_dynamics_solver.h:38
exotica::OMPLSolver::algorithm_
std::string algorithm_
Definition: ompl_solver.h:96
exotica::OMPLSolver::init_
OMPLSolverInitializer init_
Definition: ompl_solver.h:91
Eigen::VectorXdRefConst
const typedef Eigen::Ref< const Eigen::VectorXd > & VectorXdRefConst
Convenience wrapper for storing references to sub-matrices/vectors.
Definition: conversions.h:52
exotica::OMPLSolver::PreSolve
void PreSolve()
exotica::OMPLSolver::bounds_
std::vector< double > bounds_
Definition: ompl_solver.h:98
exotica::OMPLSolver::~OMPLSolver
virtual ~OMPLSolver()
exotica::ConfiguredPlannerAllocator
boost::function< ompl::base::PlannerPtr(const ompl::base::SpaceInformationPtr &si, const std::string &name)> ConfiguredPlannerAllocator
Definition: time_indexed_rrt_connect.h:104
exotica::OMPLSolver::prob_
std::shared_ptr< ProblemType > prob_
Definition: ompl_solver.h:92
motion_solver.h
exotica::MotionSolver
Definition: motion_solver.h:42
exotica::PlanningProblemPtr
std::shared_ptr< PlanningProblem > PlanningProblemPtr
Definition: planning_problem.h:116
exotica::OMPLSolver::SetLongestValidSegmentFraction
void SetLongestValidSegmentFraction(double segmentFraction)
Definition: ompl_solver.h:73
exotica::OMPLSolver::SpecifyProblem
void SpecifyProblem(PlanningProblemPtr pointer) override
exotica::OMPLSolver::OMPLSolver
OMPLSolver()
exotica::OMPLSolver::state_space_
ompl::base::StateSpacePtr state_space_
Definition: ompl_solver.h:94
exotica::OMPLSolver::planner_allocator_
ConfiguredPlannerAllocator planner_allocator_
Definition: ompl_solver.h:95
exotica::OMPLSolver
Definition: ompl_solver.h:59
exotica::OMPLSolver::GetLongestValidSegmentLength
double GetLongestValidSegmentLength() const
Definition: ompl_solver.h:72
exotica::OMPLSolver::Solve
void Solve(Eigen::MatrixXd &solution) override
exotica::OMPLSolver::SetValidSegmentCountFactor
void SetValidSegmentCountFactor(unsigned int factor)
Definition: ompl_solver.h:74
exotica::OMPLSolver::GetMaximumExtent
double GetMaximumExtent() const
Definition: ompl_solver.h:71
exotica::OMPLSolver::GetValidSegmentCountFactor
unsigned int GetValidSegmentCountFactor() const
Definition: ompl_solver.h:75