Loading...
Searching...
No Matches
VFRRT.cpp
51ompl::geometric::VFRRT::VFRRT(const base::SpaceInformationPtr &si, VectorField vf, double exploration,
93Eigen::VectorXd ompl::geometric::VFRRT::getNewDirection(const base::State *qnear, const base::State *qrand)
116double ompl::geometric::VFRRT::biasedSampling(const Eigen::VectorXd &vrand, const Eigen::VectorXd &vfield,
140Eigen::VectorXd ompl::geometric::VFRRT::computeAlphaBeta(double omega, const Eigen::VectorXd &vrand,
194ompl::base::PlannerStatus ompl::geometric::VFRRT::solve(const base::PlannerTerminationCondition &ptc)
218 OMPL_INFORM("%s: Starting planning with %u states already in datastructure", getName().c_str(), nn_->size());
Abstract definition of a goal region that can be sampled.
Definition GoalSampleableRegion.h:48
virtual bool isSatisfied(const State *st) const =0
Return true if the state satisfies the goal constraints.
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
Definition PlannerTerminationCondition.h:64
virtual void checkValidity()
Check to see if the planner is in a working state (setup has been called, a goal was set,...
Definition Planner.cpp:106
void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition RRT.cpp:71
double distanceFunction(const Motion *a, const Motion *b) const
Compute distance between motions (actually distance between contained states)
Definition RRT.h:170
RRT(const base::SpaceInformationPtr &si, bool addIntermediateStates=false)
Constructor.
Definition RRT.cpp:42
void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition RRT.cpp:61
Motion * lastGoalMotion_
The most recent goal motion. Used for PlannerData computation.
Definition RRT.h:195
double goalBias_
The fraction of time the goal is picked as the state to expand towards (if such a state is available)
Definition RRT.h:183
std::shared_ptr< NearestNeighbors< Motion * > > nn_
A nearest-neighbors datastructure containing the tree of motions.
Definition RRT.h:179
double biasedSampling(const Eigen::VectorXd &vrand, const Eigen::VectorXd &vfield, double lambdaScale)
Definition VFRRT.cpp:116
VFRRT(const base::SpaceInformationPtr &si, VectorField vf, double exploration, double initial_lambda, unsigned int update_freq)
Definition VFRRT.cpp:51
~VFRRT() override
base::PlannerStatus solve(const base::PlannerTerminationCondition &ptc) override
Definition VFRRT.cpp:194
Eigen::VectorXd getNewDirection(const base::State *qnear, const base::State *qrand)
Definition VFRRT.cpp:93
void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition VFRRT.cpp:74
Motion * extendTree(Motion *m, base::State *rstate, const Eigen::VectorXd &v)
Definition VFRRT.cpp:154
void updateExplorationEfficiency(Motion *m)
Definition VFRRT.cpp:184
Eigen::VectorXd computeAlphaBeta(double omega, const Eigen::VectorXd &vrand, const Eigen::VectorXd &vfield)
Definition VFRRT.cpp:140
This namespace includes magic constants used in various places in OMPL.
Definition Constraint.h:52
static const unsigned int VFRRT_MEAN_NORM_SAMPLES
Number of sampler to determine mean vector field norm in gVFRRT.
Definition VFRRT.cpp:47
Main namespace. Contains everything in this library.
Definition MultiLevelPlanarManipulatorDemo.cpp:66
STL namespace.
A class to store the exit status of Planner::solve()
Definition PlannerStatus.h:49