LBKPIECE1.cpp
42 ompl::geometric::LBKPIECE1::LBKPIECE1(const base::SpaceInformationPtr &si) : base::Planner(si, "LBKPIECE1"),
52 Planner::declareParam<double>("range", this, &LBKPIECE1::setRange, &LBKPIECE1::getRange, "0.:1.:10000");
53 Planner::declareParam<double>("border_fraction", this, &LBKPIECE1::setBorderFraction, &LBKPIECE1::getBorderFraction, "0.:.05:1.");
54 Planner::declareParam<double>("min_valid_path_fraction", this, &LBKPIECE1::setMinValidPathFraction, &LBKPIECE1::getMinValidPathFraction);
68 if (minValidPathFraction_ < std::numeric_limits<double>::epsilon() || minValidPathFraction_ > 1.0)
75 ompl::base::PlannerStatus ompl::geometric::LBKPIECE1::solve(const base::PlannerTerminationCondition &ptc)
78 base::GoalSampleableRegion *goal = dynamic_cast<base::GoalSampleableRegion*>(pdef_->getGoal().get());
113 OMPL_INFORM("%s: Starting planning with %d states already in datastructure", getName().c_str(), (int)(dStart_.getMotionCount() + dGoal_.getMotionCount()));
165 Motion *connectOther = ocell->data->motions[rng_.uniformInt(0, ocell->data->motions.size() - 1)];
167 if (goal->isStartGoalPairValid(startTree ? connectOther->root : motion->root, startTree ? motion->root : connectOther->root))
220 OMPL_INFORM("%s: Created %u (%u start + %u goal) states in %u cells (%u start (%u on boundary) + %u goal (%u on boundary))",
222 dStart_.getMotionCount() + dGoal_.getMotionCount(), dStart_.getMotionCount(), dGoal_.getMotionCount(),
223 dStart_.getCellCount() + dGoal_.getCellCount(), dStart_.getCellCount(), dStart_.getGrid().countExternal(),
229 bool ompl::geometric::LBKPIECE1::isPathValid(Discretization<Motion> &disc, Motion *motion, base::State *temp)
329 data.addEdge (data.vertexIndex(connectionPoint_.first), data.vertexIndex(connectionPoint_.second));
virtual void getPlannerData(base::PlannerData &data) const
Get information about the current run of the motion planner. Repeated calls to this function will upd...
Definition: LBKPIECE1.cpp:322
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique...
Definition: PlannerData.h:164
Representation of a motion for this algorithm.
Definition: LBKPIECE1.h:168
virtual void clear()
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition: LBKPIECE1.cpp:312
std::vector< Motion * > children
The set of motions descending from the current motion.
Definition: LBKPIECE1.h:198
The planner failed to find a solution.
Definition: PlannerStatus.h:62
GoalType recognizedGoal
The type of goal specification the planner can use.
Definition: Planner.h:208
void removeMotion(Discretization< Motion > &disc, Motion *motion)
Remove a motion from a tree of motions.
Definition: LBKPIECE1.cpp:274
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
Definition: PlannerTerminationCondition.h:64
double maxDistance_
The maximum length of a motion to be added to a tree.
Definition: LBKPIECE1.h:234
void append(const base::State *state)
Append state to the end of this path. The memory for state is copied.
Definition: PathGeometric.cpp:432
double minValidPathFraction_
When extending a motion, the planner can decide to keep the first valid part of it, even if invalid states are found, as long as the valid part represents a sufficiently large fraction from the original motion.
Definition: LBKPIECE1.h:231
Invalid start state or no start state specified.
Definition: PlannerStatus.h:56
Abstract definition of a goal region that can be sampled.
Definition: GoalSampleableRegion.h:49
The goal is of a type that a planner does not recognize.
Definition: PlannerStatus.h:60
One-level discretization used for KPIECE.
Definition: Discretization.h:59
std::vector< base::State * > & getStates()
Get the states that make up the path (as a reference, so it can be modified, hence the function is no...
Definition: PathGeometric.h:219
void setRange(double distance)
Set the range the planner is supposed to use.
Definition: LBKPIECE1.h:112
The planner found an exact solution.
Definition: PlannerStatus.h:66
unsigned int vertexIndex(const PlannerDataVertex &v) const
Return the index for the vertex associated with the given data. INVALID_INDEX is returned if this ver...
Definition: PlannerData.cpp:328
unsigned int addMotion(Motion *motion, const Coord &coord, double dist=0.0)
Add a motion to the grid containing motions. As a hint, dist specifies the distance to the goal from ...
Definition: Discretization.h:200
A class to store the exit status of Planner::solve()
Definition: PlannerStatus.h:48
virtual bool addEdge(unsigned int v1, unsigned int v2, const PlannerDataEdge &edge=PlannerDataEdge(), Cost weight=Cost(1.0))
Adds a directed edge between the given vertex indexes. An optional edge structure and weight can be s...
Definition: PlannerData.cpp:444
virtual bool isStartGoalPairValid(const State *, const State *) const
Since there can be multiple starting states (and multiple goal states) it is possible certain pairs a...
Definition: Goal.h:136
A boost shared pointer wrapper for ompl::base::SpaceInformation.
bool isPathValid(Discretization< Motion > &disc, Motion *motion, base::State *temp)
Since solutions are computed in a lazy fashion, once trees are connected, the solution found needs to...
Definition: LBKPIECE1.cpp:229
bool valid
Flag indicating whether this motion has been checked for validity.
Definition: LBKPIECE1.h:195
Lazy Bi-directional KPIECE with one level of discretization.
Definition: LBKPIECE1.h:78
void configureProjectionEvaluator(base::ProjectionEvaluatorPtr &proj)
If proj is undefined, it is set to the default projection reported by base::StateSpace::getDefaultPro...
Definition: SelfConfig.cpp:238
virtual bool couldSample() const
Return true if samples could be generated by this sampler at some point in the future. By default this is equivalent to canSample(), but for GoalLazySamples, this call also reflects the fact that a sampling thread is active and although no samples are produced yet, some may become available at some point in the future.
Definition: GoalSampleableRegion.h:78
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...
Definition: LBKPIECE1.cpp:75
double getMinValidPathFraction() const
Get the value of the fraction set by setMinValidPathFraction()
Definition: LBKPIECE1.h:153
void selectMotion(Motion *&smotion, Cell *&scell)
Select a motion and the cell it is part of from the grid of motions. This is where preference is give...
Definition: Discretization.h:231
void setMinValidPathFraction(double fraction)
When extending a motion, the planner can decide to keep the first valid part of it, even if invalid states are found, as long as the valid part represents a sufficiently large fraction from the original motion. This function sets the minimum acceptable fraction.
Definition: LBKPIECE1.h:147
void configurePlannerRange(double &range)
Compute what a good length for motion segments is.
Definition: SelfConfig.cpp:232
This class contains methods that automatically configure various parameters for motion planning...
Definition: SelfConfig.h:58
std::pair< base::State *, base::State * > connectionPoint_
The pair of states in each tree connected during planning. Used for PlannerData computation.
Definition: LBKPIECE1.h:240
virtual void setup()
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition: LBKPIECE1.cpp:61
double getBorderFraction() const
Get the fraction of time to focus exploration on boundary.
Definition: LBKPIECE1.h:136
const base::State * root
The root state (start state) that leads to this motion.
Definition: LBKPIECE1.h:186
void setBorderFraction(double bp)
Set the fraction of time for focusing on the border (between 0 and 1). This is the minimum fraction u...
Definition: LBKPIECE1.h:128
A boost shared pointer wrapper for ompl::base::Path.
This bit is set if casting to sampleable goal regions (ompl::base::GoalSampleableRegion) is possible...
Definition: GoalTypes.h:55