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}; }