37 #ifndef OMPL_CONTRIB_LAZY_LBTRRT_
38 #define OMPL_CONTRIB_LAZY_LBTRRT_
40 #include "ompl/geometric/planners/PlannerIncludes.h"
41 #include "ompl/datastructures/NearestNeighbors.h"
42 #include "ompl/base/goals/GoalSampleableRegion.h"
43 #include "ompl/datastructures/LPAstarOnGraph.h"
50 #include <boost/graph/graph_traits.hpp>
51 #include <boost/graph/adjacency_list.hpp>
62 LazyLBTRRT(
const base::SpaceInformationPtr &si);
70 void clear()
override;
109 template <
template <
typename T>
class NN>
112 if (
nn_ &&
nn_->size() != 0)
113 OMPL_WARN(
"Calling setNearestNeighbors will clear all states.");
115 nn_ = std::make_shared<NN<Motion *>>();
119 void setup()
override;
129 std::string getIterationCount()
const
133 std::string getBestCost()
const
159 using WeightProperty = boost::property<boost::edge_weight_t, double>;
160 using BoostGraph = boost::adjacency_list<boost::vecS,
167 friend class CostEstimatorApx;
174 double operator()(std::size_t i)
176 double lb_estimate = (*(alg_->LPAstarLb_))(i);
177 if (lb_estimate != std::numeric_limits<double>::infinity())
191 : goal_(goal), idToMotionMap_(idToMotionMap)
194 double operator()(std::size_t i)
197 goal_->
isSatisfied(idToMotionMap_[i]->state_, &dist);
204 std::vector<Motion *> &idToMotionMap_;
219 return si_->distance(a, b);
223 return si_->distance(a->state_, b->state_);
227 return si_->checkMotion(a, b);
229 bool checkMotion(
const Motion *a,
const Motion *b)
const
231 return si_->checkMotion(a->state_, b->state_);
234 Motion *getMotion(std::size_t
id)
const
236 assert(idToMotionMap_.size() >
id);
237 return idToMotionMap_[id];
239 void addVertex(
const Motion *a)
241 boost::add_vertex(a->id_, graphApx_);
242 boost::add_vertex(a->id_, graphLb_);
245 void addEdgeApx(Motion *a, Motion *b,
double c)
248 boost::add_edge(a->id_, b->id_, w, graphApx_);
249 LPAstarApx_->insertEdge(a->id_, b->id_, c);
250 LPAstarApx_->insertEdge(b->id_, a->id_, c);
252 void addEdgeLb(
const Motion *a,
const Motion *b,
double c)
255 boost::add_edge(a->id_, b->id_, w, graphLb_);
256 LPAstarLb_->insertEdge(a->id_, b->id_, c);
257 LPAstarLb_->insertEdge(b->id_, a->id_, c);
259 bool edgeExistsApx(std::size_t a, std::size_t b)
261 return boost::edge(a, b, graphApx_).second;
263 bool edgeExistsApx(
const Motion *a,
const Motion *b)
265 return edgeExistsApx(a->id_, b->id_);
267 bool edgeExistsLb(
const Motion *a,
const Motion *b)
269 return boost::edge(a->id_, b->id_, graphLb_).second;
271 void removeEdgeLb(
const Motion *a,
const Motion *b)
273 boost::remove_edge(a->id_, b->id_, graphLb_);
274 LPAstarLb_->removeEdge(a->id_, b->id_);
275 LPAstarLb_->removeEdge(b->id_, a->id_);
277 std::tuple<Motion *, base::State *, double> rrtExtend(
const base::GoalSampleableRegion *goal_s,
278 base::State *xstate, Motion *rmotion,
280 void rrt(
const base::PlannerTerminationCondition &ptc, base::GoalSampleableRegion *goal_s,
281 base::State *xstate, Motion *rmotion,
double &approxdif);
282 Motion *createMotion(
const base::GoalSampleableRegion *goal_s,
const base::State *st);
283 Motion *createGoalMotion(
const base::GoalSampleableRegion *goal_s);
285 void closeBounds(
const base::PlannerTerminationCondition &ptc);
297 std::shared_ptr<NearestNeighbors<Motion *>>
nn_;
316 BoostGraph graphApx_;
317 Motion *startMotion_;
318 Motion *goalMotion_{
nullptr};
319 LPAstarApx *LPAstarApx_{
nullptr};
320 LPAstarLb *LPAstarLb_{
nullptr};
321 std::vector<Motion *> idToMotionMap_;
Random number generation. An instance of this class cannot be used by multiple threads at once (membe...
Abstract definition of a goal region that can be sampled.
Abstract definition of goals.
virtual bool isSatisfied(const State *st) const =0
Return true if the state satisfies the goal constraints.
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique,...
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
Base class for a planner.
SpaceInformationPtr si_
The space information for which planning is done.
Definition of an abstract state.
Representation of a motion.
base::State * state_
The state contained by the motion.
Motion(const base::SpaceInformationPtr &si)
Constructor that allocates memory for the state.
std::size_t id_
The id of the motion.
Rapidly-exploring Random Trees.
RNG rng_
The random number generator.
double getApproximationFactor() const
Get the apprimation factor.
void setApproximationFactor(double epsilon)
Set the apprimation factor.
void sampleBiased(const base::GoalSampleableRegion *goal_s, base::State *rstate)
sample with goal biasing
double goalBias_
The fraction of time the goal is picked as the state to expand towards (if such a state is available)
void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Motion * lastGoalMotion_
The most recent goal motion. Used for PlannerData computation.
void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
base::StateSamplerPtr sampler_
State sampler.
double distanceFunction(const base::State *a, const base::State *b) const
Compute distance between motions (actually distance between contained states)
double getRange() const
Get the range the planner is using.
void setGoalBias(double goalBias)
Set the goal bias.
double maxDistance_
The maximum length of a motion to be added to a tree.
void setNearestNeighbors()
Set a different nearest neighbors datastructure.
double getGoalBias() const
Get the goal bias the planner is using.
std::shared_ptr< NearestNeighbors< Motion * > > nn_
A nearest-neighbors datastructure containing the tree of motions.
void getPlannerData(base::PlannerData &data) const override
Get information about the current run of the motion planner. Repeated calls to this function will upd...
void setRange(double distance)
Set the range the planner is supposed to use.
void freeMemory()
Free the memory allocated by this planner.
unsigned int iterations_
Number of iterations the algorithm performed.
double epsilon_
approximation factor
double bestCost_
Best cost found so far by algorithm.
LazyLBTRRT(const base::SpaceInformationPtr &si)
Constructor.
base::PlannerStatus solve(const base::PlannerTerminationCondition &ptc) override
Function that can solve the motion planning problem. This function can be called multiple times on th...
#define OMPL_WARN(fmt,...)
Log a formatted warning string.
Main namespace. Contains everything in this library.
A class to store the exit status of Planner::solve()