void PropagatedValue<Id, Value, Merge>::propagate(const Graph<Id>& graph) {
all_values = fixed_values;
//Propagate the values
for(const std::pair<Id,Value>& v : fixed_values) {
propagateFrom(v.first, v.second, PropagatedValue::MAX_PONDERATION, graph);
}
//Normalize
for(const std::pair<Id,Value>& v : ponderations) {
all_values[v.first] /= v.second;
}
}
ompl::base::PlannerStatus ompl::control::PDST::solve(const base::PlannerTerminationCondition &ptc)
{
// Make sure the planner is configured correctly.
// ompl::base::Planner::checkValidity checks that there is at least one
// start state and an ompl::base::Goal object specified and throws an
// exception if this is not the case.
checkValidity();
// depending on how the planning problem is set up, this may be necessary
bsp_->bounds_ = projectionEvaluator_->getBounds();
base::Goal *goal = pdef_->getGoal().get();
goalSampler_ = dynamic_cast<ompl::base::GoalSampleableRegion *>(goal);
// Ensure that we have a state sampler AND a control sampler
if (!sampler_)
sampler_ = si_->allocStateSampler();
if (!controlSampler_)
controlSampler_ = siC_->allocDirectedControlSampler();
// Initialize to correct values depending on whether or not previous calls to solve
// generated an approximate or exact solution. If solve is being called for the first
// time then initializes hasSolution to false and isApproximate to true.
double distanceToGoal, closestDistanceToGoal = std::numeric_limits<double>::infinity();
bool hasSolution = lastGoalMotion_ != nullptr;
bool isApproximate = !hasSolution || !goal->isSatisfied(lastGoalMotion_->endState_, &closestDistanceToGoal);
unsigned int ndim = projectionEvaluator_->getDimension();
// If an exact solution has already been found, do not run for another iteration.
if (hasSolution && !isApproximate)
return ompl::base::PlannerStatus::EXACT_SOLUTION;
// Initialize tree with start state(s)
while (const base::State *st = pis_.nextStart())
{
auto *startMotion = new Motion(si_->cloneState(st));
bsp_->addMotion(startMotion);
startMotion->heapElement_ = priorityQueue_.insert(startMotion);
}
if (priorityQueue_.empty())
{
OMPL_ERROR("%s: There are no valid initial states!", getName().c_str());
return base::PlannerStatus::INVALID_START;
}
OMPL_INFORM("%s: Starting planning with %u states already in datastructure", getName().c_str(),
priorityQueue_.size());
base::State *tmpState1 = si_->allocState(), *tmpState2 = si_->allocState();
base::EuclideanProjection tmpProj1(ndim), tmpProj2(ndim);
while (!ptc)
{
// Get the top priority path.
Motion *motionSelected = priorityQueue_.top()->data;
motionSelected->updatePriority();
priorityQueue_.update(motionSelected->heapElement_);
Motion *newMotion = propagateFrom(motionSelected, tmpState1, tmpState2);
if (newMotion == nullptr)
continue;
addMotion(newMotion, bsp_, tmpState1, tmpState2, tmpProj1, tmpProj2);
// Check if the newMotion reached the goal.
hasSolution = goal->isSatisfied(newMotion->endState_, &distanceToGoal);
if (hasSolution)
{
closestDistanceToGoal = distanceToGoal;
lastGoalMotion_ = newMotion;
isApproximate = false;
break;
}
else if (distanceToGoal < closestDistanceToGoal)
{
closestDistanceToGoal = distanceToGoal;
lastGoalMotion_ = newMotion;
}
// subdivide cell that contained selected motion, put motions of that
// cell in subcells and split motions so that they contained within
// one subcell
Cell *cellSelected = motionSelected->cell_;
std::vector<Motion *> motions;
cellSelected->subdivide(ndim);
motions.swap(cellSelected->motions_);
for (auto &motion : motions)
addMotion(motion, cellSelected, tmpState1, tmpState2, tmpProj1, tmpProj2);
}
if (lastGoalMotion_ != nullptr)
hasSolution = true;
// If a solution path has been computed, save it in the problem definition object.
if (hasSolution)
{
Motion *m;
std::vector<unsigned int> durations(
1, findDurationAndAncestor(lastGoalMotion_, lastGoalMotion_->endState_, tmpState1, m));
std::vector<Motion *> mpath(1, m);
while (m->parent_)
{
durations.push_back(findDurationAndAncestor(m->parent_, m->startState_, tmpState1, m));
mpath.push_back(m);
}
auto path(std::make_shared<PathControl>(si_));
double dt = siC_->getPropagationStepSize();
path->append(mpath.back()->endState_);
for (int i = (int)mpath.size() - 2; i > 0; i--)
path->append(mpath[i - 1]->startState_, mpath[i]->control_, durations[i] * dt);
path->append(lastGoalMotion_->endState_, mpath[0]->control_, durations[0] * dt);
pdef_->addSolutionPath(path, isApproximate, closestDistanceToGoal, getName());
}
si_->freeState(tmpState1);
si_->freeState(tmpState2);
OMPL_INFORM("%s: Created %u states and %u cells", getName().c_str(), priorityQueue_.size(), bsp_->size());
return base::PlannerStatus(hasSolution, isApproximate);
}
