39 #include "ompl/geometric/planners/rrt/VFRRT.h"
40 #include "ompl/base/goals/GoalSampleableRegion.h"
52 double initial_lambda,
unsigned int update_freq)
55 , explorationSetting_(exploration)
56 , lambda_(initial_lambda)
57 , nth_step_(update_freq)
69 inefficientCount_ = 0;
70 explorationInefficiency_ = 0.;
77 vfdim_ = si_->getStateSpace()->getValueLocations().size();
86 sampler_->sampleUniform(rstate);
87 sum += vf_(rstate).norm();
89 si_->freeState(rstate);
96 Eigen::VectorXd vrand(vfdim_);
97 for (
unsigned int i = 0; i < vfdim_; i++)
98 vrand[i] = *si_->getStateSpace()->getValueAddressAtIndex(qrand, i) -
99 *si_->getStateSpace()->getValueAddressAtIndex(qnear, i);
100 vrand /= si_->distance(qnear, qrand);
103 Eigen::VectorXd vfield = vf_(qnear);
104 const double lambdaScale = vfield.norm();
107 if (lambdaScale < std::numeric_limits<float>::epsilon())
109 vfield /= lambdaScale;
111 const double omega = biasedSampling(vrand, vfield, lambdaScale);
113 return computeAlphaBeta(omega, vrand, vfield);
119 double sigma = .25 * (vrand - vfield).squaredNorm();
121 double scaledLambda = lambda_ * lambdaScale / meanNorm_;
122 double phi = scaledLambda / (1. - std::exp(-2. * scaledLambda));
123 double z = -
std::log(1. - sigma * scaledLambda / phi) / scaledLambda;
124 return std::sqrt(2. * z);
129 if (step_ == nth_step_)
131 lambda_ = lambda_ * (1 - explorationInefficiency_ + explorationSetting_);
132 efficientCount_ = inefficientCount_ = 0;
133 explorationInefficiency_ = 0;
141 const Eigen::VectorXd &vfield)
143 double w2 = omega * omega;
144 double c = vfield.dot(vrand);
145 double cc_1 = c * c - 1.;
146 double root = std::sqrt(cc_1 * w2 * (w2 - 4.));
147 double beta = -root / (2. * cc_1);
148 double sign = (beta < 0.) ? -1. : 1.;
150 double alpha = (sign * c * root + cc_1 * (2. - w2)) / (2. * cc_1);
151 return alpha * vfield + beta * vrand;
155 const Eigen::VectorXd &v)
158 si_->copyState(newState, m->
state);
160 double d = si_->distance(m->
state, rstate);
161 if (d > maxDistance_)
164 const base::StateSpacePtr &space = si_->getStateSpace();
165 for (
unsigned int i = 0; i < vfdim_; i++)
166 *space->getValueAddressAtIndex(newState, i) += d * v[i];
167 if (!v.hasNaN() && si_->checkMotion(m->
state, newState))
169 auto *motion =
new Motion(si_);
170 motion->state = newState;
172 updateExplorationEfficiency(motion);
178 si_->freeState(newState);
186 Motion *near = nn_->nearest(m);
187 if (distanceFunction(m, near) < si_->getStateValidityCheckingResolution())
191 explorationInefficiency_ = inefficientCount_ / (double)(efficientCount_ + inefficientCount_);
201 sampler_ = si_->allocStateSampler();
203 meanNorm_ = determineMeanNorm();
207 auto *motion =
new Motion(si_);
208 si_->copyState(motion->state, st);
212 if (nn_->size() == 0)
214 OMPL_ERROR(
"%s: There are no valid initial states!", getName().c_str());
218 OMPL_INFORM(
"%s: Starting planning with %u states already in datastructure", getName().c_str(), nn_->size());
220 Motion *solution =
nullptr;
221 Motion *approxsol =
nullptr;
222 double approxdif = std::numeric_limits<double>::infinity();
223 auto *rmotion =
new Motion(si_);
230 if (goal_s && rng_.uniform01() < goalBias_ && goal_s->canSample())
231 goal_s->sampleGoal(rstate);
233 sampler_->sampleUniform(rstate);
236 Motion *nmotion = nn_->nearest(rmotion);
239 Motion *motion = extendTree(nmotion, rstate, getNewDirection(nmotion->
state, rstate));
245 bool sat = goal->isSatisfied(motion->
state, &dist);
252 if (dist < approxdif)
260 bool approximate =
false;
261 if (solution ==
nullptr)
263 solution = approxsol;
267 if (solution !=
nullptr)
269 lastGoalMotion_ = solution;
272 std::vector<Motion *> mpath;
273 while (solution !=
nullptr)
275 mpath.push_back(solution);
276 solution = solution->
parent;
280 auto path(std::make_shared<PathGeometric>(si_));
281 for (
int i = mpath.size() - 1; i >= 0; --i)
282 path->append(mpath[i]->state);
283 pdef_->addSolutionPath(path, approximate, approxdif, name_);
287 si_->freeState(xstate);
289 si_->freeState(rmotion->state);
292 OMPL_INFORM(
"%s: Created %u states", getName().c_str(), nn_->size());
294 return {solved, approximate};
Abstract definition of a goal region that can be sampled.
Abstract definition of goals.
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
void setName(const std::string &name)
Set the name of the planner.
Definition of an abstract state.
Representation of a motion.
Motion * parent
The parent motion in the exploration tree.
base::State * state
The state contained by the motion.
Rapidly-exploring Random Trees.
void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
double maxDistance_
The maximum length of a motion to be added to a tree.
double biasedSampling(const Eigen::VectorXd &vrand, const Eigen::VectorXd &vfield, double lambdaScale)
VFRRT(const base::SpaceInformationPtr &si, VectorField vf, double exploration, double initial_lambda, unsigned int update_freq)
double determineMeanNorm()
base::PlannerStatus solve(const base::PlannerTerminationCondition &ptc) override
Eigen::VectorXd getNewDirection(const base::State *qnear, const base::State *qrand)
void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Motion * extendTree(Motion *m, base::State *rstate, const Eigen::VectorXd &v)
void updateExplorationEfficiency(Motion *m)
Eigen::VectorXd computeAlphaBeta(double omega, const Eigen::VectorXd &vrand, const Eigen::VectorXd &vfield)
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
static const unsigned int VFRRT_MEAN_NORM_SAMPLES
Number of sampler to determine mean vector field norm in gVFRRT.
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...
Main namespace. Contains everything in this library.
A class to store the exit status of Planner::solve()
@ INVALID_START
Invalid start state or no start state specified.