bool RejectionInfSampler::sampleUniform(State *statePtr, const Cost &minCost, const Cost &maxCost)
{
// Variable
// Whether we were successful in creating an informed sample. Initially not:
bool foundSample = false;
// Spend numIters_ iterations trying to find an informed sample:
for (unsigned int i = 0u; i < InformedSampler::numIters_ && foundSample == false; ++i)
{
// Call the helper function for the larger cost. It will move our iteration counter:
foundSample = sampleUniform(statePtr, maxCost, &i);
// Did we find a sample?
if (foundSample == true)
{
// We did, but it only satisfied the upper bound. Check that it meets the lower bound.
// Variables
// The cost of the sample we found:
Cost sampledCost = InformedSampler::heuristicSolnCost(statePtr);
// Check if the sample's cost is greater than or equal to the lower bound
foundSample = InformedSampler::opt_->isCostEquivalentTo(minCost, sampledCost) || InformedSampler::opt_->isCostBetterThan(minCost, sampledCost);
}
// No else, no sample was found.
}
// One way or the other, we're done:
return foundSample;
}
void ompl::base::SO3StateSampler::sampleGaussian(State *state, const State * mean, const double stdDev)
{
// CDF of N(0, 1.17) at -pi/4 is approx. .25, so there's .25 probability
// weight in each tail. Since the maximum distance in SO(3) is pi/2, we're
// essentially as likely to sample a state within distance [0, pi/4] as
// within distance [pi/4, pi/2]. With most weight in the tails (that wrap
// around in case of quaternions) we might as well sample uniformly.
if (stdDev > 1.17)
{
sampleUniform(state);
return;
}
double x = rng_.gaussian(0, stdDev), y = rng_.gaussian(0, stdDev), z = rng_.gaussian(0, stdDev),
theta = std::sqrt(x*x + y*y + z*z);
if (theta < std::numeric_limits<double>::epsilon())
space_->copyState(state, mean);
else
{
SO3StateSpace::StateType q,
*qs = static_cast<SO3StateSpace::StateType*>(state);
const SO3StateSpace::StateType *qmu = static_cast<const SO3StateSpace::StateType*>(mean);
double s = sin(theta / 2) / theta;
q.w = cos(theta / 2);
q.x = s * x;
q.y = s * y;
q.z = s * z;
quaternionProduct(*qs, *qmu, q);
}
}
task main()
{
eraseDisplay();
// Array for accumulating Gaussian values
int accumulator[100];
for (int i = 0; i < 99; i++) {
accumulator[i] = 0;
}
// Infinite loop
while(1)
{
// Get and print uniform-distributed and Gaussian-distributed random numbers
float uniform_float = sampleUniform(1.0);
float gaussian_float = sampleGaussian(0.0, 1.0);
nxtDisplayString(1, "Uniform : %04f", uniform_float);
nxtDisplayString(2, "Gaussian: %04f", gaussian_float);
// Build a histogram of Gaussian numbers and plot on the NXT screen
// Scale random number to screen resolution
int gaussian_int = (int) (gaussian_float * 10.0) + 50;
accumulator[gaussian_int]++;
nxtSetPixel(gaussian_int, accumulator[gaussian_int]);
wait1Msec(10);
}
}
bool RejectionInfSampler::sampleUniform(State *statePtr, const Cost &maxCost)
{
// Variable
// The persistent iteration counter:
unsigned int iter = 0u;
//Call the sampleUniform helper function with my iteration counter:
return sampleUniform(statePtr, maxCost, &iter);
}
int GP_Hedge::update_hedge()
{
// We just care about the differences
double max_l = *std::max_element(loss_.begin(),loss_.end());
loss_ += svectord(loss_.size(),max_l);
// To avoid overflow
double mean_g = std::accumulate(gain_.begin(),gain_.end(),0.0)
/ static_cast<double>(gain_.size());
gain_ -= svectord(gain_.size(),mean_g);
// Optimal eta according to Shapire
double max_g = *std::max_element(gain_.begin(),gain_.end());
double eta = (std::min)(10.0,sqrt(2.0*log(3.0)/max_g));
// Compute probabilities
std::transform(gain_.begin(), gain_.end(), prob_.begin(),
boost::bind(softmax,_1,eta));
//Normalize
double sum_p =std::accumulate(prob_.begin(),prob_.end(),0.0);
prob_ /= sum_p;
//Update bandits gain
gain_ -= loss_;
std::partial_sum(prob_.begin(), prob_.end(), cumprob_.begin(),
std::plus<double>());
randFloat sampleUniform( *mtRandom, realUniformDist(0,1));
double u = sampleUniform();
for (size_t i=0; i < cumprob_.size(); ++i)
{
if (u < cumprob_(i))
return i;
}
FILE_LOG(logERROR) << "Error updating Hedge algorithm. "
<< "Selecting first criteria by default.";
return 0;
};
static void sample(T* tests, int cnt, int patch_sz, int type)
{
switch (type)
{
case 0: // uniform random, i.i.d.
sampleUniform(tests, patch_sz, cnt);
break;
case 1: // Gaussian random, i.i.d.
sampleGaussian(tests, patch_sz, cnt);
break;
default:
STDOUT_ERROR("Bad sample type");
}
}
void ompl::base::SO3StateSampler::sampleUniformNear(State *state, const State *near, const double distance)
{
if (distance >= .25 * boost::math::constants::pi<double>())
{
sampleUniform(state);
return;
}
double d = rng_.uniform01();
SO3StateSpace::StateType q,
*qs = static_cast<SO3StateSpace::StateType*>(state);
const SO3StateSpace::StateType *qnear = static_cast<const SO3StateSpace::StateType*>(near);
computeAxisAngle(q, rng_.gaussian01(), rng_.gaussian01(), rng_.gaussian01(), 2.*pow(d,1./3.)*distance);
quaternionProduct(*qs, *qnear, q);
}
int getRandomParticleIndex()
{
float randomFloat = 0;
float cumulativeWeight = 0;
int currentIndex = 0;
while (randomFloat == 0)
{
randomFloat = sampleUniform();
}
while (cumulativeWeight < randomFloat)
{
cumulativeWeight = cumulativeWeight + weightArray[currentIndex];
++currentIndex;
}
return currentIndex-1;
}
void ompl::base::SO3StateSampler::sampleGaussian(State *state, const State * mean, const double stdDev)
{
// The standard deviation of the individual components of the tangent
// perturbation needs to be scaled so that the expected quaternion distance
// between the sampled state and the mean state is stdDev. The factor 2 is
// due to the way we define distance (see also Matt Mason's lecture notes
// on quaternions at
// http://www.cs.cmu.edu/afs/cs/academic/class/16741-s07/www/lecture7.pdf).
// The 1/sqrt(3) factor is necessary because the distribution in the tangent
// space is a 3-dimensional Gaussian, so that the *length* of a tangent
// vector needs to be scaled by 1/sqrt(3).
double rotDev = (2. * stdDev) / boost::math::constants::root_three<double>();
// CDF of N(0, 1.17) at -pi/4 is approx. .25, so there's .25 probability
// weight in each tail. Since the maximum distance in SO(3) is pi/2, we're
// essentially as likely to sample a state within distance [0, pi/4] as
// within distance [pi/4, pi/2]. With most weight in the tails (that wrap
// around in case of quaternions) we might as well sample uniformly.
if (rotDev > 1.17)
{
sampleUniform(state);
return;
}
double x = rng_.gaussian(0, rotDev), y = rng_.gaussian(0, rotDev), z = rng_.gaussian(0, rotDev),
theta = std::sqrt(x*x + y*y + z*z);
if (theta < std::numeric_limits<double>::epsilon())
space_->copyState(state, mean);
else
{
SO3StateSpace::StateType q,
*qs = static_cast<SO3StateSpace::StateType*>(state);
const SO3StateSpace::StateType *qmu = static_cast<const SO3StateSpace::StateType*>(mean);
double half_theta = theta / 2.0;
double s = sin(half_theta) / theta;
q.w = cos(half_theta);
q.x = s * x;
q.y = s * y;
q.z = s * z;
quaternionProduct(*qs, *qmu, q);
}
}
rgbColor whittedRayTracer::_L(ray& r, const int& depth) const{
if(depth > MAX_DEPTH){
return rgbColor(0.f);
}
const intersection isect = parent.intersect(r);
//return rgbColor(0, isect.debugInfo / 1e8, 0);
if(!isect.hit){
return rgbColor(0.f);
}else if(isect.li != NULL){
return isect.li->L(r);
}
material& mat = isect.li ? *isect.li->getMaterial().get() : *isect.s->getMaterial().get();
const vec3& normal = isect.shadingNormal;
const bsdf& bsdf = mat.getBsdf(isect.uv);
const vec3 wo = worldToBsdf(-r.direction, isect);
bxdfType sampledType;
rgbColor colorSum(0.f);
if(isect.s->getMaterial()->isEmissive()){
return mat.Le();
}
// Diffuse calculations.
float lightPdf = 0.f;
for(int i=0; i<parent.numLights(); ++i){
const light& li = parent.getLight(i);
if(li.isPointSource()){
vec3 lightDir;
const rgbColor Li = li.sampleL(r.origin, lightDir, sampleUniform(), sampleUniform(), lightPdf);
const float lightDist = norm(lightDir);
lightDir = normalize(lightDir);
// Test for shadowing early.
ray shadowRay(r.origin, lightDir);
shadowRay.tMax = lightDist;
if(!parent.intersectB(shadowRay)){
const vec3 wi = worldToBsdf(lightDir, isect);
const rgbColor f = bsdf.f(wo, wi, bxdfType(DIFFUSE | GLOSSY | REFLECTION)) + mat.Le();
colorSum += f * dot(normal, lightDir) * (Li / lightPdf);
}
}else{
rgbColor areaContrib(0.f);
for(int j=0; j<areaSamples; ++j){
vec3 lightDir;
const rgbColor Li = li.sampleL(r.origin, lightDir, sampleUniform(), sampleUniform(), lightPdf);
ray shadowRay(r.origin, normalize(lightDir));
shadowRay.tMax = norm(lightDir) + EPSILON;
if(!parent.intersectB(shadowRay) && li.intersect(shadowRay).hit){
lightDir = normalize(lightDir);
const vec3 wi = worldToBsdf(lightDir, isect);
const rgbColor f = bsdf.f(wo, wi, bxdfType(DIFFUSE | GLOSSY | REFLECTION)) + mat.Le();
areaContrib += f * dot(normal, lightDir) * (Li / lightPdf);
}
}
colorSum += areaContrib / (float)areaSamples;
}
}
// Trace specular rays.
vec3 specDir;
float pdf;
const rgbColor fr =
bsdf.sampleF(sampleUniform(), sampleUniform(), sampleUniform(),
wo, specDir, bxdfType(GLOSSY | SPECULAR | REFLECTION), sampledType, pdf);
if(!fr.isBlack()){
specDir = bsdfToWorld(specDir, isect);
ray r2(r.origin, specDir);
colorSum += (fr / pdf) * _L(r2, depth+1) * abs(dot(specDir, normal));
}
const rgbColor ft =
bsdf.sampleF(sampleUniform(), sampleUniform(), sampleUniform(),
wo, specDir, bxdfType(GLOSSY | SPECULAR | TRANSMISSION), sampledType, pdf);
if(!ft.isBlack()){
specDir = bsdfToWorld(specDir, isect);
ray r2(r.origin, specDir);
colorSum += (ft / pdf) * _L(r2, depth+1) * abs(dot(specDir, normal));
}
if(!isFinite(colorSum.avg())) {
return ERROR_COLOR;
} else {
return colorSum;
}
}
void Nucleotide::sampleAmbiguity()
{
switch (rep_) {
case NT_A:
case NT_C:
case NT_G:
case NT_T:
case NT_GAP:
break;
case NT_M:
rep_ = sampleUniform(NT_A, NT_C); break;
case NT_R:
rep_ = sampleUniform(NT_A, NT_G); break;
case NT_W:
rep_ = sampleUniform(NT_A, NT_T); break;
case NT_S:
rep_ = sampleUniform(NT_C, NT_G); break;
case NT_Y:
rep_ = sampleUniform(NT_C, NT_T); break;
case NT_K:
rep_ = sampleUniform(NT_G, NT_T); break;
case NT_V:
rep_ = sampleUniform(NT_A, NT_C, NT_G); break;
case NT_H:
rep_ = sampleUniform(NT_A, NT_C, NT_T); break;
case NT_D:
rep_ = sampleUniform(NT_A, NT_G, NT_T); break;
case NT_B:
rep_ = sampleUniform(NT_C, NT_G, NT_T); break;
case NT_N:
rep_ = sampleUniform(NT_A, NT_C, NT_G, NT_T); break;
default:
std::cerr << rep_ << std::endl;
assert(false);
}
}
ompl::base::PlannerStatus ompl::geometric::RRTstar::solve(const base::PlannerTerminationCondition &ptc)
{
checkValidity();
base::Goal *goal = pdef_->getGoal().get();
base::GoalSampleableRegion *goal_s = dynamic_cast<base::GoalSampleableRegion*>(goal);
bool symCost = opt_->isSymmetric();
// Check if there are more starts
if (pis_.haveMoreStartStates() == true)
{
// There are, add them
while (const base::State *st = pis_.nextStart())
{
auto *motion = new Motion(si_);
si_->copyState(motion->state, st);
motion->cost = opt_->identityCost();
nn_->add(motion);
startMotions_.push_back(motion);
}
// And assure that, if we're using an informed sampler, it's reset
infSampler_.reset();
}
// No else
if (nn_->size() == 0)
{
OMPL_ERROR("%s: There are no valid initial states!", getName().c_str());
return base::PlannerStatus::INVALID_START;
}
//Allocate a sampler if necessary
if (!sampler_ && !infSampler_)
{
allocSampler();
}
OMPL_INFORM("%s: Starting planning with %u states already in datastructure", getName().c_str(), nn_->size());
if ((useTreePruning_ || useRejectionSampling_ || useInformedSampling_ || useNewStateRejection_) && !si_->getStateSpace()->isMetricSpace())
OMPL_WARN("%s: The state space (%s) is not metric and as a result the optimization objective may not satisfy the triangle inequality. "
"You may need to disable pruning or rejection."
, getName().c_str(), si_->getStateSpace()->getName().c_str());
const base::ReportIntermediateSolutionFn intermediateSolutionCallback = pdef_->getIntermediateSolutionCallback();
Motion *solution = lastGoalMotion_;
Motion *approximation = nullptr;
double approximatedist = std::numeric_limits<double>::infinity();
bool sufficientlyShort = false;
auto *rmotion = new Motion(si_);
base::State *rstate = rmotion->state;
base::State *xstate = si_->allocState();
std::vector<Motion*> nbh;
std::vector<base::Cost> costs;
std::vector<base::Cost> incCosts;
std::vector<std::size_t> sortedCostIndices;
std::vector<int> valid;
unsigned int rewireTest = 0;
unsigned int statesGenerated = 0;
if (solution)
OMPL_INFORM("%s: Starting planning with existing solution of cost %.5f", getName().c_str(), solution->cost.value());
if (useKNearest_)
OMPL_INFORM("%s: Initial k-nearest value of %u", getName().c_str(), (unsigned int)std::ceil(k_rrg_ * log((double)(nn_->size() + 1u))));
else
OMPL_INFORM("%s: Initial rewiring radius of %.2f", getName().c_str(), std::min(maxDistance_, r_rrg_*std::pow(log((double)(nn_->size() + 1u))/((double)(nn_->size() + 1u)), 1/(double)(si_->getStateDimension()))));
// our functor for sorting nearest neighbors
CostIndexCompare compareFn(costs, *opt_);
while (ptc == false)
{
iterations_++;
// sample random state (with goal biasing)
// Goal samples are only sampled until maxSampleCount() goals are in the tree, to prohibit duplicate goal states.
if (goal_s && goalMotions_.size() < goal_s->maxSampleCount() && rng_.uniform01() < goalBias_ && goal_s->canSample())
goal_s->sampleGoal(rstate);
else
{
// Attempt to generate a sample, if we fail (e.g., too many rejection attempts), skip the remainder of this loop and return to try again
if (!sampleUniform(rstate))
continue;
}
// find closest state in the tree
Motion *nmotion = nn_->nearest(rmotion);
if (intermediateSolutionCallback && si_->equalStates(nmotion->state, rstate))
continue;
base::State *dstate = rstate;
// find state to add to the tree
double d = si_->distance(nmotion->state, rstate);
if (d > maxDistance_)
{
si_->getStateSpace()->interpolate(nmotion->state, rstate, maxDistance_ / d, xstate);
dstate = xstate;
}
// Check if the motion between the nearest state and the state to add is valid
if (si_->checkMotion(nmotion->state, dstate))
{
// create a motion
auto *motion = new Motion(si_);
si_->copyState(motion->state, dstate);
motion->parent = nmotion;
motion->incCost = opt_->motionCost(nmotion->state, motion->state);
motion->cost = opt_->combineCosts(nmotion->cost, motion->incCost);
// Find nearby neighbors of the new motion
getNeighbors(motion, nbh);
rewireTest += nbh.size();
++statesGenerated;
// cache for distance computations
//
// Our cost caches only increase in size, so they're only
// resized if they can't fit the current neighborhood
if (costs.size() < nbh.size())
{
costs.resize(nbh.size());
incCosts.resize(nbh.size());
sortedCostIndices.resize(nbh.size());
}
// cache for motion validity (only useful in a symmetric space)
//
// Our validity caches only increase in size, so they're
// only resized if they can't fit the current neighborhood
if (valid.size() < nbh.size())
valid.resize(nbh.size());
std::fill(valid.begin(), valid.begin() + nbh.size(), 0);
// Finding the nearest neighbor to connect to
// By default, neighborhood states are sorted by cost, and collision checking
// is performed in increasing order of cost
if (delayCC_)
{
// calculate all costs and distances
for (std::size_t i = 0 ; i < nbh.size(); ++i)
{
incCosts[i] = opt_->motionCost(nbh[i]->state, motion->state);
costs[i] = opt_->combineCosts(nbh[i]->cost, incCosts[i]);
}
// sort the nodes
//
// we're using index-value pairs so that we can get at
// original, unsorted indices
for (std::size_t i = 0; i < nbh.size(); ++i)
sortedCostIndices[i] = i;
std::sort(sortedCostIndices.begin(), sortedCostIndices.begin() + nbh.size(),
compareFn);
// collision check until a valid motion is found
//
// ASYMMETRIC CASE: it's possible that none of these
// neighbors are valid. This is fine, because motion
// already has a connection to the tree through
// nmotion (with populated cost fields!).
for (std::vector<std::size_t>::const_iterator i = sortedCostIndices.begin();
i != sortedCostIndices.begin() + nbh.size();
++i)
{
if (nbh[*i] == nmotion || si_->checkMotion(nbh[*i]->state, motion->state))
{
motion->incCost = incCosts[*i];
motion->cost = costs[*i];
motion->parent = nbh[*i];
valid[*i] = 1;
break;
}
else valid[*i] = -1;
}
}
else // if not delayCC
{
motion->incCost = opt_->motionCost(nmotion->state, motion->state);
motion->cost = opt_->combineCosts(nmotion->cost, motion->incCost);
// find which one we connect the new state to
for (std::size_t i = 0 ; i < nbh.size(); ++i)
{
if (nbh[i] != nmotion)
{
incCosts[i] = opt_->motionCost(nbh[i]->state, motion->state);
costs[i] = opt_->combineCosts(nbh[i]->cost, incCosts[i]);
if (opt_->isCostBetterThan(costs[i], motion->cost))
{
if (si_->checkMotion(nbh[i]->state, motion->state))
{
motion->incCost = incCosts[i];
motion->cost = costs[i];
motion->parent = nbh[i];
valid[i] = 1;
}
else valid[i] = -1;
}
}
else
{
incCosts[i] = motion->incCost;
costs[i] = motion->cost;
valid[i] = 1;
}
}
}
if (useNewStateRejection_)
{
if (opt_->isCostBetterThan(solutionHeuristic(motion), bestCost_))
{
nn_->add(motion);
motion->parent->children.push_back(motion);
}
else // If the new motion does not improve the best cost it is ignored.
{
si_->freeState(motion->state);
delete motion;
continue;
}
}
else
{
// add motion to the tree
nn_->add(motion);
motion->parent->children.push_back(motion);
}
bool checkForSolution = false;
for (std::size_t i = 0; i < nbh.size(); ++i)
{
if (nbh[i] != motion->parent)
{
base::Cost nbhIncCost;
if (symCost)
nbhIncCost = incCosts[i];
else
nbhIncCost = opt_->motionCost(motion->state, nbh[i]->state);
base::Cost nbhNewCost = opt_->combineCosts(motion->cost, nbhIncCost);
if (opt_->isCostBetterThan(nbhNewCost, nbh[i]->cost))
{
bool motionValid;
if (valid[i] == 0)
{
motionValid = si_->checkMotion(motion->state, nbh[i]->state);
}
else
{
motionValid = (valid[i] == 1);
}
if (motionValid)
{
// Remove this node from its parent list
removeFromParent (nbh[i]);
// Add this node to the new parent
nbh[i]->parent = motion;
nbh[i]->incCost = nbhIncCost;
nbh[i]->cost = nbhNewCost;
nbh[i]->parent->children.push_back(nbh[i]);
// Update the costs of the node's children
updateChildCosts(nbh[i]);
checkForSolution = true;
}
}
}
}
// Add the new motion to the goalMotion_ list, if it satisfies the goal
double distanceFromGoal;
if (goal->isSatisfied(motion->state, &distanceFromGoal))
{
goalMotions_.push_back(motion);
checkForSolution = true;
}
// Checking for solution or iterative improvement
if (checkForSolution)
{
bool updatedSolution = false;
for (auto & goalMotion : goalMotions_)
{
if (opt_->isCostBetterThan(goalMotion->cost, bestCost_))
{
if (opt_->isFinite(bestCost_) == false)
{
OMPL_INFORM("%s: Found an initial solution with a cost of %.2f in %u iterations (%u vertices in the graph)", getName().c_str(), goalMotion->cost.value(), iterations_, nn_->size());
}
bestCost_ = goalMotion->cost;
updatedSolution = true;
}
sufficientlyShort = opt_->isSatisfied(goalMotion->cost);
if (sufficientlyShort)
{
solution = goalMotion;
break;
}
else if (!solution ||
opt_->isCostBetterThan(goalMotion->cost,solution->cost))
{
solution = goalMotion;
updatedSolution = true;
}
}
if (updatedSolution)
{
if (useTreePruning_)
{
pruneTree(bestCost_);
}
if (intermediateSolutionCallback)
{
std::vector<const base::State *> spath;
Motion *intermediate_solution = solution->parent; // Do not include goal state to simplify code.
//Push back until we find the start, but not the start itself
while (intermediate_solution->parent != nullptr)
{
spath.push_back(intermediate_solution->state);
intermediate_solution = intermediate_solution->parent;
}
intermediateSolutionCallback(this, spath, bestCost_);
}
}
}
// Checking for approximate solution (closest state found to the goal)
if (goalMotions_.size() == 0 && distanceFromGoal < approximatedist)
{
approximation = motion;
approximatedist = distanceFromGoal;
}
}
// terminate if a sufficient solution is found
if (solution && sufficientlyShort)
break;
}
bool approximate = (solution == nullptr);
bool addedSolution = false;
if (approximate)
solution = approximation;
else
lastGoalMotion_ = solution;
if (solution != nullptr)
{
ptc.terminate();
// construct the solution path
std::vector<Motion*> mpath;
while (solution != nullptr)
{
mpath.push_back(solution);
solution = solution->parent;
}
// set the solution path
auto *geoPath = new PathGeometric(si_);
for (int i = mpath.size() - 1 ; i >= 0 ; --i)
geoPath->append(mpath[i]->state);
base::PathPtr path(geoPath);
// Add the solution path.
base::PlannerSolution psol(path);
psol.setPlannerName(getName());
if (approximate)
psol.setApproximate(approximatedist);
// Does the solution satisfy the optimization objective?
psol.setOptimized(opt_, bestCost_, sufficientlyShort);
pdef_->addSolutionPath(psol);
addedSolution = true;
}
si_->freeState(xstate);
if (rmotion->state)
si_->freeState(rmotion->state);
delete rmotion;
OMPL_INFORM("%s: Created %u new states. Checked %u rewire options. %u goal states in tree. Final solution cost %.3f", getName().c_str(), statesGenerated, rewireTest, goalMotions_.size(), bestCost_.value());
return base::PlannerStatus(addedSolution, approximate);
}
ompl::base::PlannerStatus ompl::geometric::RRTXstatic::solve(const base::PlannerTerminationCondition &ptc)
{
checkValidity();
base::Goal *goal = pdef_->getGoal().get();
auto *goal_s = dynamic_cast<base::GoalSampleableRegion *>(goal);
// Check if there are more starts
if (pis_.haveMoreStartStates() == true)
{
// There are, add them
while (const base::State *st = pis_.nextStart())
{
auto *motion = new Motion(si_);
si_->copyState(motion->state, st);
motion->cost = opt_->identityCost();
nn_->add(motion);
}
// And assure that, if we're using an informed sampler, it's reset
infSampler_.reset();
}
// No else
if (nn_->size() == 0)
{
OMPL_ERROR("%s: There are no valid initial states!", getName().c_str());
return base::PlannerStatus::INVALID_START;
}
// Allocate a sampler if necessary
if (!sampler_ && !infSampler_)
{
allocSampler();
}
OMPL_INFORM("%s: Starting planning with %u states already in datastructure", getName().c_str(), nn_->size());
if (!si_->getStateSpace()->isMetricSpace())
OMPL_WARN("%s: The state space (%s) is not metric and as a result the optimization objective may not satisfy "
"the triangle inequality. "
"You may need to disable rejection.",
getName().c_str(), si_->getStateSpace()->getName().c_str());
const base::ReportIntermediateSolutionFn intermediateSolutionCallback = pdef_->getIntermediateSolutionCallback();
Motion *solution = lastGoalMotion_;
Motion *approximation = nullptr;
double approximatedist = std::numeric_limits<double>::infinity();
bool sufficientlyShort = false;
auto *rmotion = new Motion(si_);
base::State *rstate = rmotion->state;
base::State *xstate = si_->allocState();
base::State *dstate;
Motion *motion;
Motion *nmotion;
Motion *nb;
Motion *min;
Motion *c;
bool feas;
unsigned int rewireTest = 0;
unsigned int statesGenerated = 0;
base::Cost incCost, cost;
if (solution)
OMPL_INFORM("%s: Starting planning with existing solution of cost %.5f", getName().c_str(),
solution->cost.value());
if (useKNearest_)
OMPL_INFORM("%s: Initial k-nearest value of %u", getName().c_str(),
(unsigned int)std::ceil(k_rrt_ * log((double)(nn_->size() + 1u))));
else
OMPL_INFORM(
"%s: Initial rewiring radius of %.2f", getName().c_str(),
std::min(maxDistance_, r_rrt_ * std::pow(log((double)(nn_->size() + 1u)) / ((double)(nn_->size() + 1u)),
1 / (double)(si_->getStateDimension()))));
while (ptc == false)
{
iterations_++;
// Computes the RRG values for this iteration (number or radius of neighbors)
calculateRRG();
// sample random state (with goal biasing)
// Goal samples are only sampled until maxSampleCount() goals are in the tree, to prohibit duplicate goal
// states.
if (goal_s && goalMotions_.size() < goal_s->maxSampleCount() && rng_.uniform01() < goalBias_ &&
goal_s->canSample())
goal_s->sampleGoal(rstate);
else
{
// Attempt to generate a sample, if we fail (e.g., too many rejection attempts), skip the remainder of this
// loop and return to try again
if (!sampleUniform(rstate))
continue;
}
// find closest state in the tree
nmotion = nn_->nearest(rmotion);
if (intermediateSolutionCallback && si_->equalStates(nmotion->state, rstate))
continue;
dstate = rstate;
// find state to add to the tree
double d = si_->distance(nmotion->state, rstate);
if (d > maxDistance_)
{
si_->getStateSpace()->interpolate(nmotion->state, rstate, maxDistance_ / d, xstate);
dstate = xstate;
}
// Check if the motion between the nearest state and the state to add is valid
if (si_->checkMotion(nmotion->state, dstate))
{
// create a motion
motion = new Motion(si_);
si_->copyState(motion->state, dstate);
motion->parent = nmotion;
incCost = opt_->motionCost(nmotion->state, motion->state);
motion->cost = opt_->combineCosts(nmotion->cost, incCost);
// Find nearby neighbors of the new motion
getNeighbors(motion);
// find which one we connect the new state to
for (auto it = motion->nbh.begin(); it != motion->nbh.end();)
{
nb = it->first;
feas = it->second;
// Compute cost using nb as a parent
incCost = opt_->motionCost(nb->state, motion->state);
cost = opt_->combineCosts(nb->cost, incCost);
if (opt_->isCostBetterThan(cost, motion->cost))
{
// Check range and feasibility
if ((!useKNearest_ || distanceFunction(motion, nb) < maxDistance_) &&
si_->checkMotion(nb->state, motion->state))
{
// mark than the motino has been checked as valid
it->second = true;
motion->cost = cost;
motion->parent = nb;
++it;
}
else
{
// Remove unfeasible neighbor from the list of neighbors
it = motion->nbh.erase(it);
}
}
else
{
++it;
}
}
// Check if the vertex should included
if (!includeVertex(motion))
{
si_->freeState(motion->state);
delete motion;
continue;
}
// Update neighbor motions neighbor datastructure
for (auto it = motion->nbh.begin(); it != motion->nbh.end(); ++it)
{
it->first->nbh.emplace_back(motion, it->second);
}
// add motion to the tree
++statesGenerated;
nn_->add(motion);
if (updateChildren_)
motion->parent->children.push_back(motion);
// add the new motion to the queue to propagate the changes
updateQueue(motion);
bool checkForSolution = false;
// Add the new motion to the goalMotion_ list, if it satisfies the goal
double distanceFromGoal;
if (goal->isSatisfied(motion->state, &distanceFromGoal))
{
goalMotions_.push_back(motion);
checkForSolution = true;
}
// Process the elements in the queue and rewire the tree until epsilon-optimality
while (!q_.empty())
{
// Get element to update
min = q_.top()->data;
// Remove element from the queue and NULL the handle so that we know it's not in the queue anymore
q_.pop();
min->handle = nullptr;
// Stop cost propagation if it is not in the relevant region
if (opt_->isCostBetterThan(bestCost_, mc_.costPlusHeuristic(min)))
break;
// Try min as a parent to optimize each neighbor
for (auto it = min->nbh.begin(); it != min->nbh.end();)
{
nb = it->first;
feas = it->second;
// Neighbor culling: removes neighbors farther than the neighbor radius
if ((!useKNearest_ || min->nbh.size() > rrg_k_) && distanceFunction(min, nb) > rrg_r_)
{
it = min->nbh.erase(it);
continue;
}
// Calculate cost of nb using min as a parent
incCost = opt_->motionCost(min->state, nb->state);
cost = opt_->combineCosts(min->cost, incCost);
// If cost improvement is better than epsilon
if (opt_->isCostBetterThan(opt_->combineCosts(cost, epsilonCost_), nb->cost))
{
if (nb->parent != min)
{
// changing parent, check feasibility
if (!feas)
{
feas = si_->checkMotion(nb->state, min->state);
if (!feas)
{
// Remove unfeasible neighbor from the list of neighbors
it = min->nbh.erase(it);
continue;
}
else
{
// mark than the motino has been checked as valid
it->second = true;
}
}
if (updateChildren_)
{
// Remove this node from its parent list
removeFromParent(nb);
// add it as a children of min
min->children.push_back(nb);
}
// Add this node to the new parent
nb->parent = min;
++rewireTest;
}
nb->cost = cost;
// Add to the queue for more improvements
updateQueue(nb);
checkForSolution = true;
}
++it;
}
if (updateChildren_)
{
// Propagatino of the cost to the children
for (auto it = min->children.begin(), end = min->children.end(); it != end; ++it)
{
c = *it;
incCost = opt_->motionCost(min->state, c->state);
cost = opt_->combineCosts(min->cost, incCost);
c->cost = cost;
// Add to the queue for more improvements
updateQueue(c);
checkForSolution = true;
}
}
}
// empty q and reset handles
while (!q_.empty())
{
q_.top()->data->handle = nullptr;
q_.pop();
}
q_.clear();
// Checking for solution or iterative improvement
if (checkForSolution)
{
bool updatedSolution = false;
for (auto &goalMotion : goalMotions_)
{
if (opt_->isCostBetterThan(goalMotion->cost, bestCost_))
{
if (opt_->isFinite(bestCost_) == false)
{
OMPL_INFORM("%s: Found an initial solution with a cost of %.2f in %u iterations (%u "
"vertices in the graph)",
getName().c_str(), goalMotion->cost.value(), iterations_, nn_->size());
}
bestCost_ = goalMotion->cost;
updatedSolution = true;
}
sufficientlyShort = opt_->isSatisfied(goalMotion->cost);
if (sufficientlyShort)
{
solution = goalMotion;
break;
}
else if (!solution || opt_->isCostBetterThan(goalMotion->cost, solution->cost))
{
solution = goalMotion;
updatedSolution = true;
}
}
if (updatedSolution)
{
if (intermediateSolutionCallback)
{
std::vector<const base::State *> spath;
Motion *intermediate_solution =
solution->parent; // Do not include goal state to simplify code.
// Push back until we find the start, but not the start itself
while (intermediate_solution->parent != nullptr)
{
spath.push_back(intermediate_solution->state);
intermediate_solution = intermediate_solution->parent;
}
intermediateSolutionCallback(this, spath, bestCost_);
}
}
}
// Checking for approximate solution (closest state found to the goal)
if (goalMotions_.size() == 0 && distanceFromGoal < approximatedist)
{
approximation = motion;
approximatedist = distanceFromGoal;
}
}
// terminate if a sufficient solution is found
if (solution && sufficientlyShort)
break;
}
bool approximate = (solution == nullptr);
bool addedSolution = false;
if (approximate)
solution = approximation;
else
lastGoalMotion_ = solution;
if (solution != nullptr)
{
ptc.terminate();
// construct the solution path
std::vector<Motion *> mpath;
while (solution != nullptr)
{
mpath.push_back(solution);
solution = solution->parent;
}
// set the solution path
auto path = std::make_shared<PathGeometric>(si_);
for (int i = mpath.size() - 1; i >= 0; --i)
path->append(mpath[i]->state);
// Add the solution path.
base::PlannerSolution psol(path);
psol.setPlannerName(getName());
if (approximate)
psol.setApproximate(approximatedist);
// Does the solution satisfy the optimization objective?
psol.setOptimized(opt_, bestCost_, sufficientlyShort);
pdef_->addSolutionPath(psol);
addedSolution = true;
}
si_->freeState(xstate);
if (rmotion->state)
si_->freeState(rmotion->state);
delete rmotion;
OMPL_INFORM("%s: Created %u new states. Checked %u rewire options. %u goal states in tree. Final solution cost "
"%.3f",
getName().c_str(), statesGenerated, rewireTest, goalMotions_.size(), bestCost_.value());
return {addedSolution, approximate};
}
