37 #include "ompl/geometric/planners/rrt/LBTRRT.h"
38 #include "ompl/base/goals/GoalSampleableRegion.h"
39 #include "ompl/datastructures/NearestNeighborsGNAT.h"
40 #include "ompl/tools/config/SelfConfig.h"
47 : base::Planner(si,
"LBTRRT")
64 ompl::geometric::LBTRRT::~LBTRRT(
void)
76 lastGoalMotion_ = NULL;
94 std::vector<Motion*> motions;
96 for (
unsigned int i = 0 ; i < motions.size() ; ++i)
98 if (motions[i]->state)
99 si_->freeState(motions[i]->state);
114 si_->copyState(motion->
state, st);
118 if (nn_->size() == 0)
120 OMPL_ERROR(
"There are no valid initial states!");
125 sampler_ = si_->allocStateSampler();
127 OMPL_INFORM(
"Starting with %u states", nn_->size());
131 double approxdif = std::numeric_limits<double>::infinity();
137 while ( ptc() ==
false )
140 if (goal_s && rng_.uniform01() < goalBias_ && goal_s->
canSample())
143 sampler_->sampleUniform(rstate);
146 Motion *nmotion = nn_->nearest(rmotion);
150 double d = si_->distance(nmotion->
state, rstate);
151 if (d > maxDistance_)
153 si_->getStateSpace()->interpolate(nmotion->
state, rstate, maxDistance_ / d, xstate);
157 if (si_->checkMotion(nmotion->
state, dstate))
161 si_->copyState(motion->
state, dstate);
166 double distN = distanceFunction(nmotion, motion);
167 motion->costLb_ = nmotion->costLb_ + distN;
168 motion->costApx_ = nmotion->costApx_ + distN;
170 nmotion->childrenLb_.push_back(motion);
171 nmotion->childrenApx_.push_back(motion);
176 double k =
std::log(
double(nn_->size())) * kRRG;
177 std::vector<Motion *> nnVec;
178 nn_->nearestK(rmotion, static_cast<int>(k), nnVec);
181 std::sort (nnVec.begin(), nnVec.end(), isLessThan);
183 for (std::size_t i = 0; i < nnVec.size(); ++i)
184 attemptNodeUpdate(motion, nnVec[i]);
186 for (std::size_t i = 0; i < nnVec.size(); ++i)
187 attemptNodeUpdate(nnVec[i], motion);
191 if (sat || dist < approxdif)
200 bool approximate =
false;
202 if (solution == NULL)
204 solution = approxSol;
208 if (solution != NULL)
210 lastGoalMotion_ = solution;
213 std::vector<Motion*> mpath;
214 while (solution != NULL)
216 mpath.push_back(solution);
217 solution = solution->parentApx_;
222 for (
int i = mpath.size() - 1 ; i >= 0 ; --i)
223 path->
append(mpath[i]->state);
224 pdef_->addSolutionPath(
base::PathPtr(path), approximate, approxdif);
228 si_->freeState(xstate);
230 si_->freeState(rmotion->
state);
240 double dist = distanceFunction(potentialParent, child);
241 double potentialLb = potentialParent->costLb_ + dist;
242 double potentialApx = potentialParent->costApx_ + dist;
244 if (child->costLb_ <= potentialLb)
247 if (child->costApx_ > 1.0 + epsilon_ * potentialLb)
249 if (si_->checkMotion(potentialParent->
state, child->
state) ==
false)
252 removeFromParentLb(child);
253 double deltaLb = potentialLb - child->costLb_;
255 potentialParent->childrenLb_.push_back(child);
256 child->costLb_ = potentialLb;
257 updateChildCostsLb(child, deltaLb);
260 if (child->costApx_ <= potentialApx)
263 removeFromParentApx(child);
264 double deltaApx = potentialApx - child->costApx_;
266 potentialParent->childrenApx_.push_back(child);
267 child->costApx_ = potentialApx;
268 updateChildCostsApx(child, deltaApx);
272 removeFromParentLb(child);
273 double deltaLb = potentialLb - child->costLb_;
275 potentialParent->childrenLb_.push_back(child);
276 child->costLb_ = potentialLb;
277 updateChildCostsLb(child, deltaLb);
285 Planner::getPlannerData(data);
287 std::vector<Motion*> motions;
294 for (
unsigned int i = 0 ; i < motions.size() ; ++i)
296 if (motions[i]->parentApx_ == NULL)
307 for (
size_t i = 0; i < m->childrenLb_.size(); ++i)
309 m->childrenLb_[i]->costLb_ += delta;
310 updateChildCostsLb(m->childrenLb_[i], delta);
315 for (
size_t i = 0; i < m->childrenApx_.size(); ++i)
317 m->childrenApx_[i]->costApx_ += delta;
318 updateChildCostsApx(m->childrenApx_[i], delta);
324 return removeFromParent(m, m->
parentLb_->childrenLb_);
328 return removeFromParent(m, m->
parentApx_->childrenApx_);
332 std::vector<Motion*>::iterator it = vec.begin ();
333 while (it != vec.end ())
void updateChildCostsApx(Motion *m, double delta)
update the child cost of the approximation tree
bool approximateSolutions
Flag indicating whether the planner is able to compute approximate solutions.
void attemptNodeUpdate(Motion *potentialParent, Motion *child)
attempt to rewire the trees
bool canSample(void) const
Return true if maxSampleCount() > 0, since in this case samples can certainly be produced.
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.
void log(const char *file, int line, LogLevel level, const char *m,...)
Root level logging function. This should not be invoked directly, but rather used via a logging macro...
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.
unsigned int addGoalVertex(const PlannerDataVertex &v)
Adds the given vertex to the graph data, and marks it as a start vertex. The vertex index is returned...
Abstract definition of goals.
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
Geometric Near-neighbor Access Tree (GNAT), a data structure for nearest neighbor search...
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.
void append(const base::State *state)
Append state to the end of this path. The memory for state is copied.
bool directed
Flag indicating whether the planner is able to account for the fact that the validity of a motion fro...
virtual void sampleGoal(State *st) const =0
Sample a state in the goal region.
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)...
Base class for a vertex in the PlannerData structure. All derived classes must implement the clone an...
Invalid start state or no start state specified.
Abstract definition of a goal region that can be sampled.
double getApproximationFactor(void) const
Get the apprimation factor.
Motion * lastGoalMotion_
The most recent goal motion. Used for PlannerData computation.
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
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()
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...
double epsilon_
approximation factor
unsigned int addStartVertex(const PlannerDataVertex &v)
Adds the given vertex to the graph data, and marks it as a start vertex. The vertex index is returned...
Definition of an abstract state.
void setRange(double distance)
Set the range the planner is supposed to use.
virtual bool isSatisfied(const State *st) const =0
Return true if the state satisfies the goal constraints.
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.
PlannerSpecs specs_
The specifications of the planner (its capabilities)
static const double kRRG
kRRG = 2e~5.5 is a valid choice for all problem instances
Definition of a geometric path.
void setGoalBias(double goalBias)
Set the goal bias.
Representation of a motion.
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.
A boost shared pointer wrapper for ompl::base::Path.
virtual void setup(void)
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
#define OMPL_INFORM(fmt,...)
Log a formatted information string.