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> 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 typedef boost::property<boost::edge_weight_t, double> WeightProperty;
160 typedef boost::adjacency_list<boost::vecS,
168 friend class CostEstimatorApx;
175 double operator()(std::size_t i)
177 double lb_estimate = (*(alg_->LPAstarLb_))(i);
178 if (lb_estimate != std::numeric_limits<double>::infinity())
193 : goal_(goal), idToMotionMap_(idToMotionMap)
196 double operator()(std::size_t i)
199 goal_->
isSatisfied(idToMotionMap_[i]->state_, &dist);
206 std::vector<Motion *> &idToMotionMap_;
221 return si_->distance(a, b);
225 return si_->distance(a->state_, b->state_);
229 return si_->checkMotion(a, b);
231 bool checkMotion(
const Motion *a,
const Motion *b)
const 233 return si_->checkMotion(a->state_, b->state_);
236 Motion *getMotion(std::size_t
id)
const 238 assert(idToMotionMap_.size() > id);
239 return idToMotionMap_[id];
241 void addVertex(
const Motion *a)
243 boost::add_vertex(a->id_, graphApx_);
244 boost::add_vertex(a->id_, graphLb_);
247 void addEdgeApx(Motion *a, Motion *b,
double c)
250 boost::add_edge(a->id_, b->id_, w, graphApx_);
251 LPAstarApx_->insertEdge(a->id_, b->id_, c);
252 LPAstarApx_->insertEdge(b->id_, a->id_, c);
254 void addEdgeLb(
const Motion *a,
const Motion *b,
double c)
257 boost::add_edge(a->id_, b->id_, w, graphLb_);
258 LPAstarLb_->insertEdge(a->id_, b->id_, c);
259 LPAstarLb_->insertEdge(b->id_, a->id_, c);
261 bool edgeExistsApx(std::size_t a, std::size_t b)
263 return boost::edge(a, b, graphApx_).second;
265 bool edgeExistsApx(
const Motion *a,
const Motion *b)
267 return edgeExistsApx(a->id_, b->id_);
269 bool edgeExistsLb(
const Motion *a,
const Motion *b)
271 return boost::edge(a->id_, b->id_, graphLb_).second;
273 void removeEdgeLb(
const Motion *a,
const Motion *b)
275 boost::remove_edge(a->id_, b->id_, graphLb_);
276 LPAstarLb_->removeEdge(a->id_, b->id_);
277 LPAstarLb_->removeEdge(b->id_, a->id_);
279 std::tuple<Motion *, base::State *, double> rrtExtend(
const base::GoalSampleableRegion *goal_s,
280 base::State *xstate, Motion *rmotion,
282 void rrt(
const base::PlannerTerminationCondition &ptc, base::GoalSampleableRegion *goal_s,
283 base::State *xstate, Motion *rmotion,
double &approxdif);
284 Motion *createMotion(
const base::GoalSampleableRegion *goal_s,
const base::State *st);
285 Motion *createGoalMotion(
const base::GoalSampleableRegion *goal_s);
287 void closeBounds(
const base::PlannerTerminationCondition &ptc);
299 std::shared_ptr<NearestNeighbors<Motion *>>
nn_;
318 BoostGraph graphApx_;
319 Motion *startMotion_;
320 Motion *goalMotion_{
nullptr};
321 LPAstarApx *LPAstarApx_{
nullptr};
322 LPAstarLb *LPAstarLb_{
nullptr};
323 std::vector<Motion *> idToMotionMap_;
335 #endif // OMPL_CONTRIB_LAZY_LBTRRT_ base::State * state_
The state contained by the motion.
base::StateSamplerPtr sampler_
State sampler.
unsigned int iterations_
Number of iterations the algorithm performed.
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique...
void freeMemory()
Free the memory allocated by this planner.
A shared pointer wrapper for ompl::base::StateSampler.
Motion(const base::SpaceInformationPtr &si)
Constructor that allocates memory for the state.
void setGoalBias(double goalBias)
Set the goal bias.
Abstract definition of goals.
double getGoalBias() const
Get the goal bias the planner is using.
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
Motion * lastGoalMotion_
The most recent goal motion. Used for PlannerData computation.
double getRange() const
Get the range the planner is using.
Representation of a motion.
void setRange(double distance)
Set the range the planner is supposed to use.
Abstract definition of a goal region that can be sampled.
Main namespace. Contains everything in this library.
Random number generation. An instance of this class cannot be used by multiple threads at once (membe...
Base class for a planner.
Rapidly-exploring Random Trees.
std::size_t id_
The id of the motion.
A class to store the exit status of Planner::solve()
Definition of an abstract state.
virtual bool isSatisfied(const State *st) const =0
Return true if the state satisfies the goal constraints.
#define OMPL_WARN(fmt,...)
Log a formatted warning string.
double epsilon_
approximation factor
LazyLBTRRT(const base::SpaceInformationPtr &si)
Constructor.
double goalBias_
The fraction of time the goal is picked as the state to expand towards (if such a state is available)...
void setApproximationFactor(double epsilon)
Set the apprimation factor.
void setNearestNeighbors()
Set a different nearest neighbors datastructure.
void sampleBiased(const base::GoalSampleableRegion *goal_s, base::State *rstate)
sample with goal biasing
void getPlannerData(base::PlannerData &data) const override
Get information about the current run of the motion planner. Repeated calls to this function will upd...
double bestCost_
Best cost found so far by algorithm.
std::shared_ptr< NearestNeighbors< Motion * > > nn_
A nearest-neighbors datastructure containing the tree of motions.
void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
SpaceInformationPtr si_
The space information for which planning is done.
double maxDistance_
The maximum length of a motion to be added to a tree.
RNG rng_
The random number generator.
double getApproximationFactor() const
Get the apprimation factor.
double distanceFunction(const base::State *a, const base::State *b) const
Compute distance between motions (actually distance between contained states)
void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
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...