19 #ifndef HPP_RBPRM_PARABOLA_PLANNER_HH 20 #define HPP_RBPRM_PARABOLA_PLANNER_HH 22 #include <boost/tuple/tuple.hpp> 23 #include <hpp/core/path-planner.hh> 24 #include <hpp/core/steering-method.hh> 37 typedef boost::tuple<core::NodePtr_t, core::ConfigurationPtr_t, core::PathPtr_t>
46 const core::Problem& problem,
const core::RoadmapPtr_t&
roadmap);
48 static ParabolaPlannerPtr_t
create(
const core::Problem& problem);
78 core::ConfigurationPtr_t q_target,
79 DelayedEdges_t& delayedEdges);
86 virtual const core::RoadmapPtr_t&
roadmap()
const {
return roadmap_; }
93 const core::RoadmapPtr_t& roadmap);
97 void init(
const ParabolaPlannerWkPtr_t& weak);
101 virtual core::PathPtr_t
extend(
const core::NodePtr_t& near,
102 const core::ConfigurationPtr_t& target);
104 const core::NodePtr_t& near,
const core::ConfigurationPtr_t& target);
114 core::ValidationReportPtr_t report);
123 void computeGIWC(
const core::NodePtr_t x) {
124 computeGIWC(static_cast<core::RbprmNodePtr_t>(x));
127 void computeGIWC(
const core::NodePtr_t node,
128 core::ValidationReportPtr_t report) {
129 computeGIWC(static_cast<core::RbprmNodePtr_t>(node), report);
132 core::ConfigurationShooterPtr_t configurationShooter_;
133 mutable core::Configuration_t qProj_;
134 ParabolaPlannerWkPtr_t weakPtr_;
137 const core::RoadmapPtr_t roadmap_;
142 #endif // HPP_CORE_DIFFUSING_PLANNER_HH shared_ptr< RbprmRoadmap > RbprmRoadmapPtr_t
Definition: rbprm-roadmap.hh:13
Definition: algorithm.hh:26
ParabolaPlanner(const core::Problem &problem, const core::RoadmapPtr_t &roadmap)
Constructor.
std::vector< DelayedEdge_t > DelayedEdges_t
Definition: parabola-planner.hh:39
Definition: rbprm-node.hh:22
virtual core::PathPtr_t extend(const core::NodePtr_t &near, const core::ConfigurationPtr_t &target)
static ParabolaPlannerPtr_t createWithRoadmap(const core::Problem &problem, const core::RoadmapPtr_t &roadmap)
Return shared pointer to new object.
boost::tuple< core::NodePtr_t, core::ConfigurationPtr_t, core::PathPtr_t > DelayedEdge_t
Definition: parabola-planner.hh:38
virtual void oneStep()
One step of extension.
HPP_PREDEF_CLASS(RbPrmFullBody)
virtual core::PathPtr_t extendParabola(const core::NodePtr_t &near, const core::ConfigurationPtr_t &target)
void init(const ParabolaPlannerWkPtr_t &weak)
Store weak pointer to itself.
void configurationShooter(const core::ConfigurationShooterPtr_t &shooter)
Set configuration shooter.
virtual const core::RoadmapPtr_t & roadmap() const
Definition: parabola-planner.hh:86
core::PathPtr_t computeRandomParabola(core::NodePtr_t x_start, core::ConfigurationPtr_t q_target, DelayedEdges_t &delayedEdges)
computeRandomParabola
Generic implementation of RRT algorithm.
Definition: parabola-planner.hh:42
virtual void tryDirectPath()
virtual void startSolve()
const core::RbprmRoadmapPtr_t & rbprmRoadmap() const
Definition: parabola-planner.hh:88
shared_ptr< SteeringMethodParabola > SteeringMethodParabolaPtr_t
Definition: steering-method-parabola.hh:37
static ParabolaPlannerPtr_t create(const core::Problem &problem)
Return shared pointer to new object.
shared_ptr< ParabolaPlanner > ParabolaPlannerPtr_t
Definition: parabola-planner.hh:36