37 #ifndef OMPL_GEOMETRIC_PLANNERS_RRT_LBT_RRT_
38 #define OMPL_GEOMETRIC_PLANNERS_RRT_LBT_RRT_
40 #include "ompl/geometric/planners/PlannerIncludes.h"
41 #include "ompl/datastructures/NearestNeighbors.h"
80 virtual void clear(
void);
119 template<
template<
typename T>
class NN>
122 nn_.reset(
new NN<Motion*>());
125 virtual void setup(
void);
173 double costLb_, costApx_;
175 std::vector<Motion*> childrenLb_;
176 std::vector<Motion*> childrenApx_;
185 bool operator() (
const Motion * motionA,
const Motion * motionB)
188 double distA = std::sqrt(sqDistA);
191 double distB = std::sqrt(sqDistB);
193 return (motionA->costLb_ + distA < motionB->costLb_ + distB);
230 boost::shared_ptr< NearestNeighbors<Motion*> >
nn_;
251 #endif //OMPL_GEOMETRIC_PLANNERS_RRT_LBT_RRT_
void updateChildCostsApx(Motion *m, double delta)
update the child cost of the approximation tree
void attemptNodeUpdate(Motion *potentialParent, Motion *child)
attempt to rewire the trees
void removeFromParentLb(Motion *m)
remove motion from its parent in the lower bound tree
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique...
double getRange(void) const
Get the range the planner is using.
void setApproximationFactor(double epsilon)
Set the apprimation factor.
virtual base::PlannerStatus solve(const base::PlannerTerminationCondition &ptc)
Function that can solve the motion planning problem. This function can be called multiple times on th...
LBTRRT(const base::SpaceInformationPtr &si)
Constructor.
Motion * parentLb_
The parent motion in the exploration tree.
A boost shared pointer wrapper for ompl::base::StateSampler.
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
virtual void getPlannerData(base::PlannerData &data) const
Get information about the current run of the motion planner. Repeated calls to this function will upd...
double maxDistance_
The maximum length of a motion to be added to a tree.
base::State * state
The state contained by the motion.
Lower Bound Tree Rapidly-exploring Random Trees.
double distanceFunction(const Motion *a, const Motion *b) const
Compute distance between motions (actually distance between contained states)
void removeFromParentApx(Motion *m)
remove motion from its parent in the approximation tree
double goalBias_
The fraction of time the goal is picked as the state to expand towards (if such a state is available)...
RNG rng_
The random number generator.
Random number generation. An instance of this class cannot be used by multiple threads at once (membe...
double getApproximationFactor(void) const
Get the apprimation factor.
Motion * lastGoalMotion_
The most recent goal motion. Used for PlannerData computation.
Base class for a planner.
void freeMemory(void)
Free the memory allocated by this planner.
void updateChildCostsLb(Motion *m, double delta)
update the child cost of the lower bound tree
A class to store the exit status of Planner::solve()
double epsilon_
approximation factor
Definition of an abstract state.
void setRange(double distance)
Set the range the planner is supposed to use.
virtual void clear(void)
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Motion * parentApx_
The parent motion in the exploration tree.
Motion(const base::SpaceInformationPtr &si)
Constructor that allocates memory for the state.
static const double kRRG
kRRG = 2e~5.5 is a valid choice for all problem instances
boost::shared_ptr< NearestNeighbors< Motion * > > nn_
A nearest-neighbors datastructure containing the tree of motions.
void setGoalBias(double goalBias)
Set the goal bias.
Representation of a motion.
SpaceInformationPtr si_
The space information for which planning is done.
void removeFromParent(const Motion *m, std::vector< Motion * > &vec)
remove motion from a vector
double getGoalBias(void) const
Get the goal bias the planner is using.
void setNearestNeighbors(void)
Set a different nearest neighbors datastructure.
base::StateSamplerPtr sampler_
State sampler.
virtual void setup(void)
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...