37 #include "ompl/geometric/planners/kpiece/LBKPIECE1.h"
38 #include "ompl/base/goals/GoalSampleableRegion.h"
39 #include "ompl/tools/config/SelfConfig.h"
43 dStart_(boost::bind(&
LBKPIECE1::freeMotion, this, _1)),
44 dGoal_(boost::bind(&
LBKPIECE1::freeMotion, this, _1))
57 ompl::geometric::LBKPIECE1::~LBKPIECE1(
void)
68 if (minValidPathFraction_ < std::numeric_limits<double>::epsilon() || minValidPathFraction_ > 1.0)
69 throw Exception(
"The minimum valid path fraction must be in the range (0,1]");
71 dStart_.setDimension(projectionEvaluator_->getDimension());
72 dGoal_.setDimension(projectionEvaluator_->getDimension());
82 OMPL_ERROR(
"%s: Unknown type of goal", getName().c_str());
91 si_->copyState(motion->
state, st);
94 projectionEvaluator_->computeCoordinates(motion->
state, xcoord);
95 dStart_.addMotion(motion, xcoord);
98 if (dStart_.getMotionCount() == 0)
100 OMPL_ERROR(
"%s: Motion planning start tree could not be initialized!", getName().c_str());
106 OMPL_ERROR(
"%s: Insufficient states in sampleable goal region", getName().c_str());
111 sampler_ = si_->allocStateSampler();
113 OMPL_INFORM(
"%s: Starting with %d states", getName().c_str(), (
int)(dStart_.getMotionCount() + dGoal_.getMotionCount()));
116 bool startTree =
true;
122 startTree = !startTree;
124 disc.countIteration();
127 if (dGoal_.getMotionCount() == 0 || pis_.getSampledGoalsCount() < dGoal_.getMotionCount() / 2)
129 const base::State *st = dGoal_.getMotionCount() == 0 ? pis_.nextGoal(ptc) : pis_.nextGoal();
133 si_->copyState(motion->
state, st);
135 motion->
valid =
true;
136 projectionEvaluator_->computeCoordinates(motion->
state, xcoord);
137 dGoal_.addMotion(motion, xcoord);
139 if (dGoal_.getMotionCount() == 0)
141 OMPL_ERROR(
"%s: Unable to sample any valid states for goal tree", getName().c_str());
150 sampler_->sampleUniformNear(xstate, existing->
state, maxDistance_);
154 si_->copyState(motion->
state, xstate);
155 motion->
parent = existing;
157 existing->
children.push_back(motion);
158 projectionEvaluator_->computeCoordinates(motion->
state, xcoord);
163 if (ocell && !ocell->
data->motions.empty())
165 Motion* connectOther = ocell->
data->motions[rng_.uniformInt(0, ocell->
data->motions.size() - 1)];
170 si_->copyState(connect->
state, connectOther->
state);
173 motion->
children.push_back(connect);
174 projectionEvaluator_->computeCoordinates(connect->
state, xcoord);
177 if (isPathValid(disc, connect, xstate) && isPathValid(otherDisc, connectOther, xstate))
180 connectionPoint_ = std::make_pair(connectOther->
state, motion->
state);
182 connectionPoint_ = std::make_pair(motion->
state, connectOther->
state);
186 std::vector<Motion*> mpath1;
187 while (motion != NULL)
189 mpath1.push_back(motion);
193 std::vector<Motion*> mpath2;
194 while (connectOther != NULL)
196 mpath2.push_back(connectOther);
197 connectOther = connectOther->
parent;
204 path->
getStates().reserve(mpath1.size() + mpath2.size());
205 for (
int i = mpath1.size() - 1 ; i >= 0 ; --i)
206 path->
append(mpath1[i]->state);
207 for (
unsigned int i = 0 ; i < mpath2.size() ; ++i)
208 path->
append(mpath2[i]->state);
218 si_->freeState(xstate);
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(),
224 dGoal_.getCellCount(), dGoal_.getGrid().countExternal());
231 std::vector<Motion*> mpath;
234 while (motion != NULL)
236 mpath.push_back(motion);
240 std::pair<base::State*, double> lastValid;
241 lastValid.first = temp;
244 for (
int i = mpath.size() - 1 ; i >= 0 ; --i)
245 if (!mpath[i]->valid)
247 if (si_->checkMotion(mpath[i]->parent->state, mpath[i]->state, lastValid))
248 mpath[i]->valid =
true;
251 Motion *parent = mpath[i]->parent;
252 removeMotion(disc, mpath[i]);
255 if (lastValid.second > minValidPathFraction_)
258 si_->copyState(reAdd->
state, lastValid.first);
264 projectionEvaluator_->computeCoordinates(reAdd->
state, coord);
279 projectionEvaluator_->computeCoordinates(motion->
state, coord);
280 disc.removeMotion(motion, coord);
286 for (
unsigned int i = 0 ; i < motion->
parent->
children.size() ; ++i)
295 for (
unsigned int i = 0 ; i < motion->
children.size() ; ++i)
298 removeMotion(disc, motion->
children[i]);
308 si_->freeState(motion->
state);
319 connectionPoint_ = std::make_pair<base::State*, base::State*>(NULL, NULL);
324 Planner::getPlannerData(data);
325 dStart_.getPlannerData(data, 1,
true, NULL);
326 dGoal_.getPlannerData(data, 2,
false, NULL);
virtual void getPlannerData(base::PlannerData &data) const
Get information about the current run of the motion planner. Repeated calls to this function will upd...
Grid::Coord Coord
The datatype for the maintained grid coordinates.
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique...
Motion * parent
The parent motion in the exploration tree.
Representation of a motion for this algorithm.
virtual void setup(void)
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
double getBorderFraction(void) const
Get the fraction of time to focus exploration on boundary.
std::vector< Motion * > children
The set of motions descending from the current motion.
The planner failed to find a solution.
GoalType recognizedGoal
The type of goal specification the planner can use.
base::State * state
The state contained by this motion.
void removeMotion(Discretization< Motion > &disc, Motion *motion)
Remove a motion from a tree of motions.
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
_T data
The data we store in the cell.
double maxDistance_
The maximum length of a motion to be added to a tree.
std::vector< base::State * > & getStates(void)
Get the states that make up the path (as a reference, so it can be modified, hence the function is no...
void append(const base::State *state)
Append state to the end of this path. The memory for state is copied.
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.
Invalid start state or no start state specified.
virtual void clear(void)
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Abstract definition of a goal region that can be sampled.
The goal is of a type that a planner does not recognize.
One-level discretization used for KPIECE.
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
void setRange(double distance)
Set the range the planner is supposed to use.
The planner found an exact solution.
double getMinValidPathFraction(void) const
Get the value of the fraction set by setMinValidPathFraction()
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...
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 ...
A class to store the exit status of Planner::solve()
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...
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...
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...
virtual bool couldSample(void) 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 of an abstract state.
bool valid
Flag indicating whether this motion has been checked for validity.
void freeMotion(Motion *motion)
Free the memory for a motion.
PlannerSpecs specs_
The specifications of the planner (its capabilities)
Definition of a cell in this grid.
Lazy Bi-directional KPIECE with one level of discretization.
The exception type for ompl.
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...
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...
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.
std::pair< base::State *, base::State * > connectionPoint_
The pair of states in each tree connected during planning. Used for PlannerData computation.
double getRange(void) const
Get the range the planner is using.
Definition of a geometric path.
const base::State * root
The root state (start state) that leads to this motion.
void setBorderFraction(double bp)
Set the fraction of time for focusing on the border (between 0 and 1). This is the minimum fraction u...
A boost shared pointer wrapper for ompl::base::Path.
LBKPIECE1(const base::SpaceInformationPtr &si)
Constructor.
This bit is set if casting to sampleable goal regions (ompl::base::GoalSampleableRegion) is possible...
#define OMPL_INFORM(fmt,...)
Log a formatted information string.