ompl::base::PlannerStatus ompl::control::EST::solve(const base::PlannerTerminationCondition &ptc)
{
checkValidity();
base::Goal *goal = pdef_->getGoal().get();
base::GoalSampleableRegion *goal_s = dynamic_cast<base::GoalSampleableRegion*>(goal);
// Initializing tree with start state(s)
while (const base::State *st = pis_.nextStart())
{
Motion *motion = new Motion(siC_);
si_->copyState(motion->state, st);
siC_->nullControl(motion->control);
addMotion(motion);
}
if (tree_.grid.size() == 0)
{
OMPL_ERROR("%s: There are no valid initial states!", getName().c_str());
return base::PlannerStatus::INVALID_START;
}
// Ensure that we have a state sampler AND a control sampler
if (!sampler_)
sampler_ = si_->allocValidStateSampler();
if (!controlSampler_)
controlSampler_ = siC_->allocDirectedControlSampler();
OMPL_INFORM("%s: Starting with %u states", getName().c_str(), tree_.size);
Motion *solution = NULL;
Motion *approxsol = NULL;
double approxdif = std::numeric_limits<double>::infinity();
Motion *rmotion = new Motion(siC_);
bool solved = false;
while (!ptc)
{
// Select a state to expand the tree from
Motion *existing = selectMotion();
assert (existing);
// sample a random state (with goal biasing) near the state selected for expansion
if (goal_s && rng_.uniform01() < goalBias_ && goal_s->canSample())
goal_s->sampleGoal(rmotion->state);
else
{
if (!sampler_->sampleNear(rmotion->state, existing->state, maxDistance_))
continue;
}
// Extend a motion toward the state we just sampled
unsigned int duration = controlSampler_->sampleTo(rmotion->control, existing->control,
existing->state, rmotion->state);
// If the system was propagated for a meaningful amount of time, save into the tree
if (duration >= siC_->getMinControlDuration())
{
// create a motion to the resulting state
Motion *motion = new Motion(siC_);
si_->copyState(motion->state, rmotion->state);
siC_->copyControl(motion->control, rmotion->control);
motion->steps = duration;
motion->parent = existing;
// save the state
addMotion(motion);
// Check if this state is the goal state, or improves the best solution so far
double dist = 0.0;
solved = goal->isSatisfied(motion->state, &dist);
if (solved)
{
approxdif = dist;
solution = motion;
break;
}
if (dist < approxdif)
{
approxdif = dist;
approxsol = motion;
}
}
}
bool approximate = false;
if (solution == NULL)
{
solution = approxsol;
approximate = true;
}
// Constructing the solution path
if (solution != NULL)
{
lastGoalMotion_ = solution;
std::vector<Motion*> mpath;
while (solution != NULL)
{
mpath.push_back(solution);
solution = solution->parent;
}
PathControl *path = new PathControl(si_);
for (int i = mpath.size() - 1 ; i >= 0 ; --i)
if (mpath[i]->parent)
path->append(mpath[i]->state, mpath[i]->control, mpath[i]->steps * siC_->getPropagationStepSize());
else
path->append(mpath[i]->state);
solved = true;
pdef_->addSolutionPath(base::PathPtr(path), approximate, approxdif);
}
// Cleaning up memory
if (rmotion->state)
si_->freeState(rmotion->state);
if (rmotion->control)
siC_->freeControl(rmotion->control);
delete rmotion;
OMPL_INFORM("%s: Created %u states in %u cells", getName().c_str(), tree_.size, tree_.grid.size());
return base::PlannerStatus(solved, approximate);
}
ompl::base::PlannerStatus ompl::geometric::SBL::solve(const base::PlannerTerminationCondition &ptc)
{
checkValidity();
auto *goal = dynamic_cast<base::GoalSampleableRegion *>(pdef_->getGoal().get());
if (!goal)
{
OMPL_ERROR("%s: Unknown type of goal", getName().c_str());
return base::PlannerStatus::UNRECOGNIZED_GOAL_TYPE;
}
while (const base::State *st = pis_.nextStart())
{
auto *motion = new Motion(si_);
si_->copyState(motion->state, st);
motion->valid = true;
motion->root = motion->state;
addMotion(tStart_, motion);
}
if (tStart_.size == 0)
{
OMPL_ERROR("%s: Motion planning start tree could not be initialized!", getName().c_str());
return base::PlannerStatus::INVALID_START;
}
if (!goal->couldSample())
{
OMPL_ERROR("%s: Insufficient states in sampleable goal region", getName().c_str());
return base::PlannerStatus::INVALID_GOAL;
}
if (!sampler_)
sampler_ = si_->allocValidStateSampler();
OMPL_INFORM("%s: Starting planning with %d states already in datastructure", getName().c_str(),
(int)(tStart_.size + tGoal_.size));
std::vector<Motion *> solution;
base::State *xstate = si_->allocState();
bool startTree = true;
bool solved = false;
while (ptc == false)
{
TreeData &tree = startTree ? tStart_ : tGoal_;
startTree = !startTree;
TreeData &otherTree = startTree ? tStart_ : tGoal_;
// if we have not sampled too many goals already
if (tGoal_.size == 0 || pis_.getSampledGoalsCount() < tGoal_.size / 2)
{
const base::State *st = tGoal_.size == 0 ? pis_.nextGoal(ptc) : pis_.nextGoal();
if (st)
{
auto *motion = new Motion(si_);
si_->copyState(motion->state, st);
motion->root = motion->state;
motion->valid = true;
addMotion(tGoal_, motion);
}
if (tGoal_.size == 0)
{
OMPL_ERROR("%s: Unable to sample any valid states for goal tree", getName().c_str());
break;
}
}
Motion *existing = selectMotion(tree);
assert(existing);
if (!sampler_->sampleNear(xstate, existing->state, maxDistance_))
continue;
/* create a motion */
auto *motion = new Motion(si_);
si_->copyState(motion->state, xstate);
motion->parent = existing;
motion->root = existing->root;
existing->children.push_back(motion);
addMotion(tree, motion);
if (checkSolution(!startTree, tree, otherTree, motion, solution))
{
auto path(std::make_shared<PathGeometric>(si_));
for (auto &i : solution)
path->append(i->state);
pdef_->addSolutionPath(path, false, 0.0, getName());
solved = true;
break;
}
}
si_->freeState(xstate);
OMPL_INFORM("%s: Created %u (%u start + %u goal) states in %u cells (%u start + %u goal)", getName().c_str(),
tStart_.size + tGoal_.size, tStart_.size, tGoal_.size, tStart_.grid.size() + tGoal_.grid.size(),
tStart_.grid.size(), tGoal_.grid.size());
return solved ? base::PlannerStatus::EXACT_SOLUTION : base::PlannerStatus::TIMEOUT;
}
bool ompl::control::KPIECE1::solve(const base::PlannerTerminationCondition &ptc)
{
checkValidity();
base::Goal *goal = pdef_->getGoal().get();
while (const base::State *st = pis_.nextStart())
{
Motion *motion = new Motion(siC_);
si_->copyState(motion->state, st);
siC_->nullControl(motion->control);
addMotion(motion, 1.0);
}
if (tree_.grid.size() == 0)
{
msg_.error("There are no valid initial states!");
return false;
}
if (!controlSampler_)
controlSampler_ = siC_->allocControlSampler();
msg_.inform("Starting with %u states", tree_.size);
Motion *solution = NULL;
Motion *approxsol = NULL;
double approxdif = std::numeric_limits<double>::infinity();
Control *rctrl = siC_->allocControl();
std::vector<base::State*> states(siC_->getMaxControlDuration() + 1);
std::vector<Grid::Coord> coords(states.size());
std::vector<Grid::Cell*> cells(coords.size());
for (unsigned int i = 0 ; i < states.size() ; ++i)
states[i] = si_->allocState();
// samples that were found to be the best, so far
CloseSamples closeSamples(nCloseSamples_);
while (ptc() == false)
{
tree_.iteration++;
/* Decide on a state to expand from */
Motion *existing = NULL;
Grid::Cell *ecell = NULL;
if (closeSamples.canSample() && rng_.uniform01() < goalBias_)
{
if (!closeSamples.selectMotion(existing, ecell))
selectMotion(existing, ecell);
}
else
selectMotion(existing, ecell);
assert(existing);
/* sample a random control */
controlSampler_->sampleNext(rctrl, existing->control, existing->state);
/* propagate */
unsigned int cd = controlSampler_->sampleStepCount(siC_->getMinControlDuration(), siC_->getMaxControlDuration());
cd = siC_->propagateWhileValid(existing->state, rctrl, cd, states, false);
/* if we have enough steps */
if (cd >= siC_->getMinControlDuration())
{
std::size_t avgCov_two_thirds = (2 * tree_.size) / (3 * tree_.grid.size());
bool interestingMotion = false;
// split the motion into smaller ones, so we do not cross cell boundaries
for (unsigned int i = 0 ; i < cd ; ++i)
{
projectionEvaluator_->computeCoordinates(states[i], coords[i]);
cells[i] = tree_.grid.getCell(coords[i]);
if (!cells[i])
interestingMotion = true;
else
{
if (!interestingMotion && cells[i]->data->motions.size() <= avgCov_two_thirds)
interestingMotion = true;
}
}
if (interestingMotion || rng_.uniform01() < 0.05)
{
unsigned int index = 0;
while (index < cd)
{
unsigned int nextIndex = findNextMotion(coords, index, cd);
Motion *motion = new Motion(siC_);
si_->copyState(motion->state, states[nextIndex]);
siC_->copyControl(motion->control, rctrl);
motion->steps = nextIndex - index + 1;
motion->parent = existing;
double dist = 0.0;
bool solv = goal->isSatisfied(motion->state, &dist);
Grid::Cell *toCell = addMotion(motion, dist);
if (solv)
{
approxdif = dist;
solution = motion;
break;
}
if (dist < approxdif)
{
approxdif = dist;
approxsol = motion;
}
closeSamples.consider(toCell, motion, dist);
// new parent will be the newly created motion
existing = motion;
index = nextIndex + 1;
}
if (solution)
break;
}
// update cell score
ecell->data->score *= goodScoreFactor_;
}
else
ecell->data->score *= badScoreFactor_;
tree_.grid.update(ecell);
}
bool solved = false;
bool approximate = false;
if (solution == NULL)
{
solution = approxsol;
approximate = true;
}
if (solution != NULL)
{
/* construct the solution path */
std::vector<Motion*> mpath;
while (solution != NULL)
{
mpath.push_back(solution);
solution = solution->parent;
}
/* set the solution path */
PathControl *path = new PathControl(si_);
for (int i = mpath.size() - 1 ; i >= 0 ; --i)
if (mpath[i]->parent)
path->append(mpath[i]->state, mpath[i]->control, mpath[i]->steps * siC_->getPropagationStepSize());
else
path->append(mpath[i]->state);
goal->addSolutionPath(base::PathPtr(path), approximate, approxdif);
solved = true;
}
siC_->freeControl(rctrl);
for (unsigned int i = 0 ; i < states.size() ; ++i)
si_->freeState(states[i]);
msg_.inform("Created %u states in %u cells (%u internal + %u external)", tree_.size, tree_.grid.size(),
tree_.grid.countInternal(), tree_.grid.countExternal());
return solved;
}
ompl::base::PlannerStatus ompl::geometric::ProjEST::solve(const base::PlannerTerminationCondition &ptc)
{
checkValidity();
base::Goal *goal = pdef_->getGoal().get();
base::GoalSampleableRegion *goal_s = dynamic_cast<base::GoalSampleableRegion *>(goal);
while (const base::State *st = pis_.nextStart())
{
auto *motion = new Motion(si_);
si_->copyState(motion->state, st);
addMotion(motion);
}
if (tree_.grid.size() == 0)
{
OMPL_ERROR("%s: There are no valid initial states!", getName().c_str());
return base::PlannerStatus::INVALID_START;
}
if (!sampler_)
sampler_ = si_->allocValidStateSampler();
OMPL_INFORM("%s: Starting planning with %u states already in datastructure", getName().c_str(), tree_.size);
Motion *solution = nullptr;
Motion *approxsol = nullptr;
double approxdif = std::numeric_limits<double>::infinity();
base::State *xstate = si_->allocState();
while (ptc == false)
{
/* Decide on a state to expand from */
Motion *existing = selectMotion();
assert(existing);
/* sample random state (with goal biasing) */
if (goal_s && rng_.uniform01() < goalBias_ && goal_s->canSample())
goal_s->sampleGoal(xstate);
else if (!sampler_->sampleNear(xstate, existing->state, maxDistance_))
continue;
if (si_->checkMotion(existing->state, xstate))
{
/* create a motion */
auto *motion = new Motion(si_);
si_->copyState(motion->state, xstate);
motion->parent = existing;
addMotion(motion);
double dist = 0.0;
bool solved = goal->isSatisfied(motion->state, &dist);
if (solved)
{
approxdif = dist;
solution = motion;
break;
}
if (dist < approxdif)
{
approxdif = dist;
approxsol = motion;
}
}
}
bool solved = false;
bool approximate = false;
if (solution == nullptr)
{
solution = approxsol;
approximate = true;
}
if (solution != nullptr)
{
lastGoalMotion_ = solution;
/* 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);
pdef_->addSolutionPath(path, approximate, approxdif, getName());
solved = true;
}
si_->freeState(xstate);
OMPL_INFORM("%s: Created %u states in %u cells", getName().c_str(), tree_.size, tree_.grid.size());
return base::PlannerStatus(solved, approximate);
}
void ompl::geometric::pSBL::threadSolve(unsigned int tid, const base::PlannerTerminationCondition &ptc, SolutionInfo *sol)
{
RNG rng;
std::vector<Motion*> solution;
base::State *xstate = si_->allocState();
bool startTree = rng.uniformBool();
while (!sol->found && ptc == false)
{
bool retry = true;
while (retry && !sol->found && ptc == false)
{
removeList_.lock.lock();
if (!removeList_.motions.empty())
{
if (loopLock_.try_lock())
{
retry = false;
std::map<Motion*, bool> seen;
for (unsigned int i = 0 ; i < removeList_.motions.size() ; ++i)
if (seen.find(removeList_.motions[i].motion) == seen.end())
removeMotion(*removeList_.motions[i].tree, removeList_.motions[i].motion, seen);
removeList_.motions.clear();
loopLock_.unlock();
}
}
else
retry = false;
removeList_.lock.unlock();
}
if (sol->found || ptc)
break;
loopLockCounter_.lock();
if (loopCounter_ == 0)
loopLock_.lock();
loopCounter_++;
loopLockCounter_.unlock();
TreeData &tree = startTree ? tStart_ : tGoal_;
startTree = !startTree;
TreeData &otherTree = startTree ? tStart_ : tGoal_;
Motion *existing = selectMotion(rng, tree);
if (!samplerArray_[tid]->sampleNear(xstate, existing->state, maxDistance_))
continue;
/* create a motion */
Motion *motion = new Motion(si_);
si_->copyState(motion->state, xstate);
motion->parent = existing;
motion->root = existing->root;
existing->lock.lock();
existing->children.push_back(motion);
existing->lock.unlock();
addMotion(tree, motion);
if (checkSolution(rng, !startTree, tree, otherTree, motion, solution))
{
sol->lock.lock();
if (!sol->found)
{
sol->found = true;
PathGeometric *path = new PathGeometric(si_);
for (unsigned int i = 0 ; i < solution.size() ; ++i)
path->append(solution[i]->state);
pdef_->addSolutionPath(base::PathPtr(path), false, 0.0, getName());
}
sol->lock.unlock();
}
loopLockCounter_.lock();
loopCounter_--;
if (loopCounter_ == 0)
loopLock_.unlock();
loopLockCounter_.unlock();
}
si_->freeState(xstate);
}