void ompl::geometric::PathHybridization::computeHybridPath()
{
boost::vector_property_map<Vertex> prev(boost::num_vertices(g_));
boost::dijkstra_shortest_paths(g_, root_, boost::predecessor_map(prev));
if (prev[goal_] != goal_)
{
PathGeometric *h = new PathGeometric(si_);
for (Vertex pos = prev[goal_]; prev[pos] != pos; pos = prev[pos])
h->append(stateProperty_[pos]);
h->reverse();
hpath_.reset(h);
}
}
void ompl::geometric::FMT::traceSolutionPathThroughTree(Motion *goalMotion)
{
std::vector<Motion*> mpath;
Motion *solution = goalMotion;
// Construct the solution path
while (solution != nullptr)
{
mpath.push_back(solution);
solution = solution->getParent();
}
// Set the solution path
PathGeometric *path = new PathGeometric(si_);
int mPathSize = mpath.size();
for (int i = mPathSize - 1 ; i >= 0 ; --i)
path->append(mpath[i]->getState());
pdef_->addSolutionPath(base::PathPtr(path), false, -1.0, getName());
}
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);
}
base::PlannerStatus BFRRT::solve(
const base::PlannerTerminationCondition &ptc)
{
checkValidity();
base::GoalSampleableRegion *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;
}
TreeGrowingInfo tgi;
while (const base::State *st = pis_.nextStart())
{
Motion *motion = new Motion(si_);
si_->copyState(motion->state, st);
motion->root = motion->state;
tStart_->add(motion);
tgi.last_s = 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_->allocStateSampler();
OMPL_INFORM(
"%s: Starting planning with %d states already in datastructure",
getName().c_str(), (int )(tStart_->size() + tGoal_->size()));
Motion *rmotion = new Motion(si_);
bool startTree = true;
bool solved = false;
while (ptc == false)
{
TreeData &tree = startTree ? tStart_ : tGoal_;
tgi.start = startTree;
startTree = !startTree;
TreeData &otherTree = startTree ? tStart_ : tGoal_;
if (tGoal_->size() == 0
|| pis_.getSampledGoalsCount() < tGoal_->size() / 2)
{
const base::State *st =
tGoal_->size() == 0 ? pis_.nextGoal(ptc) : pis_.nextGoal();
if (st)
{
Motion *motion = new Motion(si_);
si_->copyState(motion->state, st);
motion->root = motion->state;
tGoal_->add(motion);
tgi.last_g = motion;
}
if (tGoal_->size() == 0)
{
OMPL_ERROR("%s: Unable to sample any valid states for goal tree",
getName().c_str());
break;
}
}
static double nearest_r = 0.05
* distanceFunction(tgi.last_s, tgi.last_g);
/// Get a random state
bool r_ok = false;
do
{
sampler_->sampleUniform(rmotion->state);
r_ok = si_->getStateValidityChecker()->isValid(rmotion->state);
} while (!r_ok && ptc == false);
Motion *nearest_s = tStart_->nearest(rmotion);
Motion *nearest_g = tGoal_->nearest(rmotion);
Motion *last_valid_motion = new Motion(si_);
std::pair<ompl::base::State*, double> last_valid(
last_valid_motion->state, 0);
Motion *new_s = NULL;
Motion *new_g = NULL;
/// Connect random node to start tree
bool succ_s = si_->checkMotion(nearest_s->state, rmotion->state,
last_valid);
if (succ_s)
{
Motion *motion = new Motion(si_);
si_->copyState(motion->state, rmotion->state);
motion->parent = nearest_s;
tStart_->add(motion);
new_s = motion;
}
else
{
if (last_valid.second == 0) last_valid_motion->state = NULL;
Eigen::VectorXd eigen_g((int) si_->getStateDimension());
memcpy(eigen_g.data(),
rmotion->state->as<ompl::base::RealVectorStateSpace::StateType>()->values,
sizeof(double) * eigen_g.rows());
local_map_->jointRef = eigen_g;
local_solver_->getProblem()->setTau(1e-4);
Motion *new_motion = new Motion(si_);
if (localSolve(nearest_s, last_valid_motion->state, new_motion))
{
new_s = new_motion;
tStart_->add(new_motion);
succ_s = true;
}
else if (new_motion->internal_path)
{
si_->copyState(rmotion->state, new_motion->state);
bool addNew = true;
// std::vector<Motion*> n_motions;
// tStart_->nearestR(new_motion, nearest_r, n_motions);
// for (int i = 0; i < n_motions.size(); i++)
// if (!n_motions[i]->global_valid_)
// {
// addNew = false;
// break;
// }
if (addNew)
{
new_motion->global_valid_ = false;
tStart_->add(new_motion);
si_->copyState(rmotion->state, new_motion->state);
}
}
}
/// For goal tree, do the same thing
last_valid.second = 0;
bool succ_g = si_->checkMotion(nearest_g->state, rmotion->state,
last_valid);
if (succ_g)
{
Motion *motion = new Motion(si_);
si_->copyState(motion->state, rmotion->state);
motion->parent = nearest_g;
tGoal_->add(motion);
new_g = motion;
}
else
{
if (last_valid.second == 0) last_valid_motion->state = NULL;
Eigen::VectorXd eigen_g((int) si_->getStateDimension());
memcpy(eigen_g.data(),
rmotion->state->as<ompl::base::RealVectorStateSpace::StateType>()->values,
sizeof(double) * eigen_g.rows());
local_map_->jointRef = eigen_g;
local_solver_->getProblem()->setTau(1e-4);
Motion *new_motion = new Motion(si_);
if (localSolve(nearest_g, last_valid_motion->state, new_motion))
{
new_g = new_motion;
succ_g = true;
}
else if (new_motion->internal_path)
{
si_->copyState(rmotion->state, new_motion->state);
bool addNew = true;
// std::vector<Motion*> n_motions;
// tGoal_->nearestR(new_motion, nearest_r, n_motions);
// for (int i = 0; i < n_motions.size(); i++)
// if (!n_motions[i]->global_valid_)
// {
// addNew = false;
// break;
// }
if (addNew)
{
new_motion->global_valid_ = false;
tGoal_->add(new_motion);
}
}
}
/// If succeeded both ways, the a solution is found
if (succ_s && succ_g)
{
connectionPoint_ = std::make_pair(new_s->state, new_g->state);
Motion *solution = new_s;
std::vector<Motion*> mpath1;
while (solution != NULL)
{
if (solution->internal_path != nullptr)
{
for (int i = solution->internal_path->rows() - 1; i > 0; i--)
{
Motion *local_motion = new Motion(si_);
Eigen::VectorXd tmp = solution->internal_path->row(i);
memcpy(
local_motion->state->as<
ompl::base::RealVectorStateSpace::StateType>()->values,
tmp.data(),
sizeof(double) * (int) si_->getStateDimension());
mpath1.push_back(local_motion);
}
if (solution->inter_state != NULL)
{
Motion *local_motion = new Motion(si_);
si_->copyState(local_motion->state, solution->inter_state);
mpath1.push_back(local_motion);
}
}
else
mpath1.push_back(solution);
solution = solution->parent;
}
solution = new_g;
std::vector<Motion*> mpath2;
while (solution != NULL)
{
if (solution->internal_path != nullptr)
{
for (int i = solution->internal_path->rows() - 1; i > 0; i--)
{
Motion *local_motion = new Motion(si_);
Eigen::VectorXd tmp = solution->internal_path->row(i);
memcpy(
local_motion->state->as<
ompl::base::RealVectorStateSpace::StateType>()->values,
tmp.data(),
sizeof(double) * (int) si_->getStateDimension());
mpath2.push_back(local_motion);
}
if (solution->inter_state != NULL)
{
Motion *local_motion = new Motion(si_);
si_->copyState(local_motion->state, solution->inter_state);
mpath2.push_back(local_motion);
}
}
else
mpath2.push_back(solution);
solution = solution->parent;
}
PathGeometric *path = new PathGeometric(si_);
path->getStates().reserve(mpath1.size() + mpath2.size());
for (int i = mpath1.size() - 1; i >= 0; --i)
path->append(mpath1[i]->state);
for (unsigned int i = 0; i < mpath2.size(); ++i)
path->append(mpath2[i]->state);
pdef_->addSolutionPath(base::PathPtr(path), false, 0.0, getName());
solved = true;
break;
}
}
si_->freeState(rmotion->state);
delete rmotion;
OMPL_INFORM("%s: Created %u states (%u start + %u goal)",
getName().c_str(), tStart_->size() + tGoal_->size(), tStart_->size(),
tGoal_->size());
return
solved ?
base::PlannerStatus::EXACT_SOLUTION : base::PlannerStatus::TIMEOUT;
}
bool ompl::geometric::BKPIECE1::solve(const base::PlannerTerminationCondition &ptc)
{
checkValidity();
base::GoalSampleableRegion *goal = dynamic_cast<base::GoalSampleableRegion*>(pdef_->getGoal().get());
if (!goal)
{
msg_.error("Unknown type of goal (or goal undefined)");
return false;
}
Discretization<Motion>::Coord xcoord;
while (const base::State *st = pis_.nextStart())
{
Motion* motion = new Motion(si_);
si_->copyState(motion->state, st);
motion->root = motion->state;
projectionEvaluator_->computeCoordinates(motion->state, xcoord);
dStart_.addMotion(motion, xcoord);
}
if (dStart_.getMotionCount() == 0)
{
msg_.error("Motion planning start tree could not be initialized!");
return false;
}
if (!goal->couldSample())
{
msg_.error("Insufficient states in sampleable goal region");
return false;
}
if (!sampler_)
sampler_ = si_->allocValidStateSampler();
msg_.inform("Starting with %d states", (int)(dStart_.getMotionCount() + dGoal_.getMotionCount()));
std::vector<Motion*> solution;
base::State *xstate = si_->allocState();
bool startTree = true;
bool solved = false;
while (ptc() == false)
{
Discretization<Motion> &disc = startTree ? dStart_ : dGoal_;
startTree = !startTree;
Discretization<Motion> &otherDisc = startTree ? dStart_ : dGoal_;
disc.countIteration();
// if we have not sampled too many goals already
if (dGoal_.getMotionCount() == 0 || pis_.getSampledGoalsCount() < dGoal_.getMotionCount() / 2)
{
const base::State *st = dGoal_.getMotionCount() == 0 ? pis_.nextGoal(ptc) : pis_.nextGoal();
if (st)
{
Motion* motion = new Motion(si_);
si_->copyState(motion->state, st);
motion->root = motion->state;
projectionEvaluator_->computeCoordinates(motion->state, xcoord);
dGoal_.addMotion(motion, xcoord);
}
if (dGoal_.getMotionCount() == 0)
{
msg_.error("Unable to sample any valid states for goal tree");
break;
}
}
Discretization<Motion>::Cell *ecell = NULL;
Motion *existing = NULL;
disc.selectMotion(existing, ecell);
assert(existing);
if (sampler_->sampleNear(xstate, existing->state, maxDistance_))
{
std::pair<base::State*, double> fail(xstate, 0.0);
bool keep = si_->checkMotion(existing->state, xstate, fail);
if (!keep && fail.second > minValidPathFraction_)
keep = true;
if (keep)
{
/* create a motion */
Motion *motion = new Motion(si_);
si_->copyState(motion->state, xstate);
motion->root = existing->root;
motion->parent = existing;
projectionEvaluator_->computeCoordinates(motion->state, xcoord);
disc.addMotion(motion, xcoord);
Discretization<Motion>::Cell* cellC = otherDisc.getGrid().getCell(xcoord);
if (cellC && !cellC->data->motions.empty())
{
Motion* connectOther = cellC->data->motions[rng_.uniformInt(0, cellC->data->motions.size() - 1)];
if (goal->isStartGoalPairValid(startTree ? connectOther->root : motion->root, startTree ? motion->root : connectOther->root) &&
si_->checkMotion(motion->state, connectOther->state))
{
/* extract the motions and put them in solution vector */
std::vector<Motion*> mpath1;
while (motion != NULL)
{
mpath1.push_back(motion);
motion = motion->parent;
}
std::vector<Motion*> mpath2;
while (connectOther != NULL)
{
mpath2.push_back(connectOther);
connectOther = connectOther->parent;
}
if (startTree)
mpath1.swap(mpath2);
PathGeometric *path = new PathGeometric(si_);
path->getStates().reserve(mpath1.size() + mpath2.size());
for (int i = mpath1.size() - 1 ; i >= 0 ; --i)
path->append(mpath1[i]->state);
for (unsigned int i = 0 ; i < mpath2.size() ; ++i)
path->append(mpath2[i]->state);
goal->addSolutionPath(base::PathPtr(path), false, 0.0);
solved = true;
break;
}
}
}
else
ecell->data->score *= failedExpansionScoreFactor_;
}
else
ecell->data->score *= failedExpansionScoreFactor_;
disc.updateCell(ecell);
}
si_->freeState(xstate);
msg_.inform("Created %u (%u start + %u goal) states in %u cells (%u start (%u on boundary) + %u goal (%u on boundary))",
dStart_.getMotionCount() + dGoal_.getMotionCount(), dStart_.getMotionCount(), dGoal_.getMotionCount(),
dStart_.getCellCount() + dGoal_.getCellCount(), dStart_.getCellCount(), dStart_.getGrid().countExternal(),
dGoal_.getCellCount(), dGoal_.getGrid().countExternal());
return solved;
}
ompl::base::PlannerStatus ompl::geometric::LBTRRT::solve(const base::PlannerTerminationCondition &ptc)
{
checkValidity();
// update goal and check validity
base::Goal *goal = pdef_->getGoal().get();
base::GoalSampleableRegion *goal_s = dynamic_cast<base::GoalSampleableRegion*>(goal);
if (!goal)
{
OMPL_ERROR("%s: Goal undefined", getName().c_str());
return base::PlannerStatus::INVALID_GOAL;
}
// update start and check validity
while (const base::State *st = pis_.nextStart())
{
Motion *motion = new Motion(si_);
si_->copyState(motion->state_, st);
motion->id_ = nn_->size();
idToMotionMap_.push_back(motion);
nn_->add(motion);
lowerBoundGraph_.addVertex(motion->id_);
}
if (nn_->size() == 0)
{
OMPL_ERROR("%s: There are no valid initial states!", getName().c_str());
return base::PlannerStatus::INVALID_START;
}
if (nn_->size() > 1)
{
OMPL_ERROR("%s: There are multiple start states - currently not supported!", getName().c_str());
return base::PlannerStatus::INVALID_START;
}
if (!sampler_)
sampler_ = si_->allocStateSampler();
OMPL_INFORM("%s: Starting planning with %u states already in datastructure", getName().c_str(), nn_->size());
Motion *solution = lastGoalMotion_;
Motion *approxSol = nullptr;
double approxdif = std::numeric_limits<double>::infinity();
// e*(1+1/d) K-nearest constant, as used in RRT*
double k_rrg = boost::math::constants::e<double>() +
boost::math::constants::e<double>() / (double)si_->getStateDimension();
Motion *rmotion = new Motion(si_);
base::State *rstate = rmotion->state_;
base::State *xstate = si_->allocState();
unsigned int statesGenerated = 0;
bestCost_ = lastGoalMotion_ ? lastGoalMotion_->costApx_ : std::numeric_limits<double>::infinity();
while (ptc() == false)
{
iterations_++;
/* sample random state (with goal biasing) */
if (goal_s && rng_.uniform01() < goalBias_ && goal_s->canSample())
goal_s->sampleGoal(rstate);
else
sampler_->sampleUniform(rstate);
/* find closest state in the tree */
Motion *nmotion = nn_->nearest(rmotion);
base::State *dstate = rstate;
/* find state to add */
double d = si_->distance(nmotion->state_, rstate);
if (d == 0) // this takes care of the case that the goal is a single point and we re-sample it multiple times
continue;
if (d > maxDistance_)
{
si_->getStateSpace()->interpolate(nmotion->state_, rstate, maxDistance_ / d, xstate);
dstate = xstate;
}
if (checkMotion(nmotion->state_, dstate))
{
statesGenerated++;
/* create a motion */
Motion *motion = new Motion(si_);
si_->copyState(motion->state_, dstate);
/* update fields */
double distN = distanceFunction(nmotion, motion);
motion->id_ = nn_->size();
idToMotionMap_.push_back(motion);
lowerBoundGraph_.addVertex(motion->id_);
motion->parentApx_ = nmotion;
std::list<std::size_t> dummy;
lowerBoundGraph_.addEdge(nmotion->id_, motion->id_, distN, false, dummy);
motion->costLb_ = nmotion->costLb_ + distN;
motion->costApx_ = nmotion->costApx_ + distN;
nmotion->childrenApx_.push_back(motion);
std::vector<Motion*> nnVec;
unsigned int k = std::ceil(k_rrg * log((double)(nn_->size() + 1)));
nn_->nearestK(motion, k, nnVec);
nn_->add(motion); // if we add the motion before the nearestK call, we will get ourselves...
IsLessThan isLessThan(this, motion);
std::sort(nnVec.begin(), nnVec.end(), isLessThan);
//-------------------------------------------------//
// Rewiring Part (i) - find best parent of motion //
//-------------------------------------------------//
if (motion->parentApx_ != nnVec.front())
{
for (std::size_t i(0); i < nnVec.size(); ++i)
{
Motion *potentialParent = nnVec[i];
double dist = distanceFunction(potentialParent, motion);
considerEdge(potentialParent, motion, dist);
}
}
//------------------------------------------------------------------//
// Rewiring Part (ii) //
// check if motion may be a better parent to one of its neighbors //
//------------------------------------------------------------------//
for (std::size_t i(0); i < nnVec.size(); ++i)
{
Motion *child = nnVec[i];
double dist = distanceFunction(motion, child);
considerEdge(motion, child, dist);
}
double dist = 0.0;
bool sat = goal->isSatisfied(motion->state_, &dist);
if (sat)
{
approxdif = dist;
solution = motion;
}
if (dist < approxdif)
{
approxdif = dist;
approxSol = motion;
}
if (solution != nullptr && bestCost_ != solution->costApx_)
{
OMPL_INFORM("%s: approximation cost = %g", getName().c_str(),
solution->costApx_);
bestCost_ = solution->costApx_;
}
}
}
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->parentApx_;
}
/* set the solution path */
PathGeometric *path = new PathGeometric(si_);
for (int i = mpath.size() - 1 ; i >= 0 ; --i)
path->append(mpath[i]->state_);
// Add the solution path.
base::PathPtr bpath(path);
base::PlannerSolution psol(bpath);
psol.setPlannerName(getName());
if (approximate)
psol.setApproximate(approxdif);
pdef_->addSolutionPath(psol);
solved = true;
}
si_->freeState(xstate);
if (rmotion->state_)
si_->freeState(rmotion->state_);
delete rmotion;
OMPL_INFORM("%s: Created %u states", getName().c_str(), statesGenerated);
return base::PlannerStatus(solved, approximate);
}
bool ompl::geometric::BallTreeRRTstar::solve(const base::PlannerTerminationCondition &ptc)
{
checkValidity();
base::Goal *goal = pdef_->getGoal().get();
base::GoalSampleableRegion *goal_s = dynamic_cast<base::GoalSampleableRegion*>(goal);
if (!goal)
{
msg_.error("Goal undefined");
return false;
}
while (const base::State *st = pis_.nextStart())
{
Motion *motion = new Motion(si_, rO_);
si_->copyState(motion->state, st);
addMotion(motion);
}
if (nn_->size() == 0)
{
msg_.error("There are no valid initial states!");
return false;
}
if (!sampler_)
sampler_ = si_->allocStateSampler();
msg_.inform("Starting with %u states", nn_->size());
Motion *solution = NULL;
Motion *approximation = NULL;
double approximatedist = std::numeric_limits<double>::infinity();
bool sufficientlyShort = false;
Motion *rmotion = new Motion(si_, rO_);
Motion *toTrim = NULL;
base::State *rstate = rmotion->state;
base::State *xstate = si_->allocState();
base::State *tstate = si_->allocState();
std::vector<Motion*> solCheck;
std::vector<Motion*> nbh;
std::vector<double> dists;
std::vector<int> valid;
long unsigned int rewireTest = 0;
std::pair<base::State*,double> lastValid(tstate, 0.0);
while (ptc() == false)
{
bool rejected = false;
/* sample until a state not within any of the existing volumes is found */
do
{
/* sample random state (with goal biasing) */
if (goal_s && rng_.uniform01() < goalBias_ && goal_s->canSample())
goal_s->sampleGoal(rstate);
else
sampler_->sampleUniform(rstate);
/* check to see if it is inside an existing volume */
if (inVolume(rstate))
{
rejected = true;
/* see if the state is valid */
if(!si_->isValid(rstate))
{
/* if it's not, reduce the size of the nearest volume to the distance
between its center and the rejected state */
toTrim = nn_->nearest(rmotion);
double newRad = si_->distance(toTrim->state, rstate);
if (newRad < toTrim->volRadius)
toTrim->volRadius = newRad;
}
}
else
rejected = false;
}
while (rejected);
/* find closest state in the tree */
Motion *nmotion = nn_->nearest(rmotion);
base::State *dstate = rstate;
/* find state to add */
double d = si_->distance(nmotion->state, rstate);
if (d > maxDistance_)
{
si_->getStateSpace()->interpolate(nmotion->state, rstate, maxDistance_ / d, xstate);
dstate = xstate;
}
if (si_->checkMotion(nmotion->state, dstate, lastValid))
{
/* create a motion */
double distN = si_->distance(dstate, nmotion->state);
Motion *motion = new Motion(si_, rO_);
si_->copyState(motion->state, dstate);
motion->parent = nmotion;
motion->cost = nmotion->cost + distN;
/* find nearby neighbors */
double r = std::min(ballRadiusConst_ * (sqrt(log((double)(1 + nn_->size())) / ((double)(nn_->size())))),
ballRadiusMax_);
nn_->nearestR(motion, r, nbh);
rewireTest += nbh.size();
// cache for distance computations
dists.resize(nbh.size());
// cache for motion validity
valid.resize(nbh.size());
std::fill(valid.begin(), valid.end(), 0);
if (delayCC_)
{
// calculate all costs and distances
for (unsigned int i = 0 ; i < nbh.size() ; ++i)
if (nbh[i] != nmotion)
{
double c = nbh[i]->cost + si_->distance(nbh[i]->state, dstate);
nbh[i]->cost = c;
}
// sort the nodes
std::sort(nbh.begin(), nbh.end(), compareMotion);
for (unsigned int i = 0 ; i < nbh.size() ; ++i)
if (nbh[i] != nmotion)
{
dists[i] = si_->distance(nbh[i]->state, dstate);
nbh[i]->cost -= dists[i];
}
// collision check until a valid motion is found
for (unsigned int i = 0 ; i < nbh.size() ; ++i)
if (nbh[i] != nmotion)
{
dists[i] = si_->distance(nbh[i]->state, dstate);
double c = nbh[i]->cost + dists[i];
if (c < motion->cost)
{
if (si_->checkMotion(nbh[i]->state, dstate, lastValid))
{
motion->cost = c;
motion->parent = nbh[i];
valid[i] = 1;
break;
}
else
{
valid[i] = -1;
/* if a collision is found, trim radius to distance from motion to last valid state */
double nR = si_->distance(nbh[i]->state, lastValid.first);
if (nR < nbh[i]->volRadius)
nbh[i]->volRadius = nR;
}
}
}
else
{
valid[i] = 1;
dists[i] = distN;
break;
}
}
else{
/* find which one we connect the new state to*/
for (unsigned int i = 0 ; i < nbh.size() ; ++i)
if (nbh[i] != nmotion)
{
dists[i] = si_->distance(nbh[i]->state, dstate);
double c = nbh[i]->cost + dists[i];
if (c < motion->cost)
{
if (si_->checkMotion(nbh[i]->state, dstate, lastValid))
{
motion->cost = c;
motion->parent = nbh[i];
valid[i] = 1;
}
else
{
valid[i] = -1;
/* if a collision is found, trim radius to distance from motion to last valid state */
double newR = si_->distance(nbh[i]->state, lastValid.first);
if (newR < nbh[i]->volRadius)
nbh[i]->volRadius = newR;
}
}
}
else
{
valid[i] = 1;
dists[i] = distN;
}
}
/* add motion to tree */
addMotion(motion);
motion->parent->children.push_back(motion);
solCheck.resize(1);
solCheck[0] = motion;
/* rewire tree if needed */
for (unsigned int i = 0 ; i < nbh.size() ; ++i)
if (nbh[i] != motion->parent)
{
double c = motion->cost + dists[i];
if (c < nbh[i]->cost)
{
bool v = false;
if (valid[i] == 0)
{
if(!si_->checkMotion(nbh[i]->state, dstate, lastValid))
{
/* if a collision is found, trim radius to distance from motion to last valid state */
double R = si_->distance(nbh[i]->state, lastValid.first);
if (R < nbh[i]->volRadius)
nbh[i]->volRadius = R;
}
else
{
v = true;
}
}
if (valid[i] == 1)
v = true;
if (v)
{
// Remove this node from its parent list
removeFromParent (nbh[i]);
double delta = c - nbh[i]->cost;
nbh[i]->parent = motion;
nbh[i]->cost = c;
nbh[i]->parent->children.push_back(nbh[i]);
solCheck.push_back(nbh[i]);
// Update the costs of the node's children
updateChildCosts(nbh[i], delta);
}
}
}
// check if we found a solution
for (unsigned int i = 0 ; i < solCheck.size() ; ++i)
{
double dist = 0.0;
bool solved = goal->isSatisfied(solCheck[i]->state, &dist);
sufficientlyShort = solved ? goal->isPathLengthSatisfied(solCheck[i]->cost) : false;
if (solved)
{
if (sufficientlyShort)
{
solution = solCheck[i];
break;
}
else if (!solution || (solCheck[i]->cost < solution->cost))
{
solution = solCheck[i];
}
}
else if (!solution && dist < approximatedist)
{
approximation = solCheck[i];
approximatedist = dist;
}
}
// terminate if a sufficient solution is found
if (solution && sufficientlyShort)
break;
}
else
{
/* if a collision is found, trim radius to distance from motion to last valid state */
toTrim = nn_->nearest(nmotion);
double newRadius = si_->distance(toTrim->state, lastValid.first);
if (newRadius < toTrim->volRadius)
toTrim->volRadius = newRadius;
}
}
double solutionCost;
bool approximate = (solution == NULL);
bool addedSolution = false;
if (approximate)
{
solution = approximation;
solutionCost = approximatedist;
}
else
solutionCost = solution->cost;
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
PathGeometric *path = new PathGeometric(si_);
for (int i = mpath.size() - 1 ; i >= 0 ; --i)
path->append(mpath[i]->state);
goal->addSolutionPath(base::PathPtr(path), approximate, solutionCost);
addedSolution = true;
}
si_->freeState(xstate);
if (rmotion->state)
si_->freeState(rmotion->state);
delete rmotion;
msg_.inform("Created %u states. Checked %lu rewire options.", nn_->size(), rewireTest);
return addedSolution;
}
ompl::base::PlannerStatus ompl::geometric::LazyRRT::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())
{
Motion *motion = new Motion(si_);
si_->copyState(motion->state, st);
motion->valid = true;
nn_->add(motion);
}
if (nn_->size() == 0)
{
OMPL_ERROR("%s: There are no valid initial states!", getName().c_str());
return base::PlannerStatus::INVALID_START;
}
if (!sampler_)
sampler_ = si_->allocStateSampler();
OMPL_INFORM("%s: Starting planning with %u states already in datastructure", getName().c_str(), nn_->size());
Motion *solution = NULL;
double distsol = -1.0;
Motion *rmotion = new Motion(si_);
base::State *rstate = rmotion->state;
base::State *xstate = si_->allocState();
bool solutionFound = false;
while (ptc == false && !solutionFound)
{
/* sample random state (with goal biasing) */
if (goal_s && rng_.uniform01() < goalBias_ && goal_s->canSample())
goal_s->sampleGoal(rstate);
else
sampler_->sampleUniform(rstate);
/* find closest state in the tree */
Motion *nmotion = nn_->nearest(rmotion);
assert(nmotion != rmotion);
base::State *dstate = rstate;
/* find state to add */
double d = si_->distance(nmotion->state, rstate);
if (d > maxDistance_)
{
si_->getStateSpace()->interpolate(nmotion->state, rstate, maxDistance_ / d, xstate);
dstate = xstate;
}
/* create a motion */
Motion *motion = new Motion(si_);
si_->copyState(motion->state, dstate);
motion->parent = nmotion;
nmotion->children.push_back(motion);
nn_->add(motion);
double dist = 0.0;
if (goal->isSatisfied(motion->state, &dist))
{
distsol = dist;
solution = motion;
solutionFound = true;
lastGoalMotion_ = solution;
// Check that the solution is valid:
// construct the solution path
std::vector<Motion*> mpath;
while (solution != NULL)
{
mpath.push_back(solution);
solution = solution->parent;
}
// check each segment along the path for validity
for (int i = mpath.size() - 1 ; i >= 0 && solutionFound; --i)
if (!mpath[i]->valid)
{
if (si_->checkMotion(mpath[i]->parent->state, mpath[i]->state))
mpath[i]->valid = true;
else
{
removeMotion(mpath[i]);
solutionFound = false;
lastGoalMotion_ = NULL;
}
}
if (solutionFound)
{
// set the solution path
PathGeometric *path = new PathGeometric(si_);
for (int i = mpath.size() - 1 ; i >= 0 ; --i)
path->append(mpath[i]->state);
pdef_->addSolutionPath(base::PathPtr(path), false, distsol, getName());
}
}
}
si_->freeState(xstate);
si_->freeState(rstate);
delete rmotion;
OMPL_INFORM("%s: Created %u states", getName().c_str(), nn_->size());
return solutionFound ? base::PlannerStatus::EXACT_SOLUTION : base::PlannerStatus::TIMEOUT;
}
ompl::base::PlannerStatus ompl::geometric::RRTConnect::solve(const base::PlannerTerminationCondition &ptc)
{
checkValidity();
base::GoalSampleableRegion *goal = dynamic_cast<base::GoalSampleableRegion*>(pdef_->getGoal().get());
if (!goal)
{
logError("Unknown type of goal (or goal undefined)");
return base::PlannerStatus::UNRECOGNIZED_GOAL_TYPE;
}
while (const base::State *st = pis_.nextStart())
{
Motion *motion = new Motion(si_);
si_->copyState(motion->state, st);
motion->root = motion->state;
tStart_->add(motion);
}
if (tStart_->size() == 0)
{
logError("Motion planning start tree could not be initialized!");
return base::PlannerStatus::INVALID_START;
}
if (!goal->couldSample())
{
logError("Insufficient states in sampleable goal region");
return base::PlannerStatus::INVALID_GOAL;
}
if (!sampler_)
sampler_ = si_->allocStateSampler();
logInform("Starting with %d states", (int)(tStart_->size() + tGoal_->size()));
TreeGrowingInfo tgi;
tgi.xstate = si_->allocState();
Motion *rmotion = new Motion(si_);
base::State *rstate = rmotion->state;
bool startTree = true;
bool solved = false;
while (ptc() == false)
{
TreeData &tree = startTree ? tStart_ : tGoal_;
tgi.start = startTree;
startTree = !startTree;
TreeData &otherTree = startTree ? tStart_ : tGoal_;
if (tGoal_->size() == 0 || pis_.getSampledGoalsCount() < tGoal_->size() / 2)
{
const base::State *st = tGoal_->size() == 0 ? pis_.nextGoal(ptc) : pis_.nextGoal();
if (st)
{
Motion* motion = new Motion(si_);
si_->copyState(motion->state, st);
motion->root = motion->state;
tGoal_->add(motion);
}
if (tGoal_->size() == 0)
{
logError("Unable to sample any valid states for goal tree");
break;
}
}
/* sample random state */
sampler_->sampleUniform(rstate);
GrowState gs = growTree(tree, tgi, rmotion);
if (gs != TRAPPED)
{
/* remember which motion was just added */
Motion *addedMotion = tgi.xmotion;
/* attempt to connect trees */
/* if reached, it means we used rstate directly, no need top copy again */
if (gs != REACHED)
si_->copyState(rstate, tgi.xstate);
GrowState gsc = ADVANCED;
tgi.start = startTree;
while (gsc == ADVANCED)
gsc = growTree(otherTree, tgi, rmotion);
Motion *startMotion = startTree ? tgi.xmotion : addedMotion;
Motion *goalMotion = startTree ? addedMotion : tgi.xmotion;
/* if we connected the trees in a valid way (start and goal pair is valid)*/
if (gsc == REACHED && goal->isStartGoalPairValid(startMotion->root, goalMotion->root))
{
// it must be the case that either the start tree or the goal tree has made some progress
// so one of the parents is not NULL. We go one step 'back' to avoid having a duplicate state
// on the solution path
if (startMotion->parent)
startMotion = startMotion->parent;
else
goalMotion = goalMotion->parent;
connectionPoint_ = std::make_pair<base::State*, base::State*>(startMotion->state, goalMotion->state);
/* construct the solution path */
Motion *solution = startMotion;
std::vector<Motion*> mpath1;
while (solution != NULL)
{
mpath1.push_back(solution);
solution = solution->parent;
}
solution = goalMotion;
std::vector<Motion*> mpath2;
while (solution != NULL)
{
mpath2.push_back(solution);
solution = solution->parent;
}
PathGeometric *path = new PathGeometric(si_);
path->getStates().reserve(mpath1.size() + mpath2.size());
for (int i = mpath1.size() - 1 ; i >= 0 ; --i)
path->append(mpath1[i]->state);
for (unsigned int i = 0 ; i < mpath2.size() ; ++i)
path->append(mpath2[i]->state);
pdef_->addSolutionPath(base::PathPtr(path), false, 0.0);
solved = true;
break;
}
}
}
si_->freeState(tgi.xstate);
si_->freeState(rstate);
delete rmotion;
logInform("Created %u states (%u start + %u goal)", tStart_->size() + tGoal_->size(), tStart_->size(), tGoal_->size());
return solved ? base::PlannerStatus::EXACT_SOLUTION : base::PlannerStatus::TIMEOUT;
}
ompl::base::PlannerStatus ompl::geometric::LBKPIECE1::solve(const base::PlannerTerminationCondition &ptc)
{
checkValidity();
base::GoalSampleableRegion *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;
}
Discretization<Motion>::Coord xcoord;
while (const base::State *st = pis_.nextStart())
{
Motion* motion = new Motion(si_);
si_->copyState(motion->state, st);
motion->root = st;
motion->valid = true;
projectionEvaluator_->computeCoordinates(motion->state, xcoord);
dStart_.addMotion(motion, xcoord);
}
if (dStart_.getMotionCount() == 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_->allocStateSampler();
OMPL_INFORM("%s: Starting with %d states", getName().c_str(), (int)(dStart_.getMotionCount() + dGoal_.getMotionCount()));
base::State *xstate = si_->allocState();
bool startTree = true;
bool solved = false;
while (ptc == false)
{
Discretization<Motion> &disc = startTree ? dStart_ : dGoal_;
startTree = !startTree;
Discretization<Motion> &otherDisc = startTree ? dStart_ : dGoal_;
disc.countIteration();
// if we have not sampled too many goals already
if (dGoal_.getMotionCount() == 0 || pis_.getSampledGoalsCount() < dGoal_.getMotionCount() / 2)
{
const base::State *st = dGoal_.getMotionCount() == 0 ? pis_.nextGoal(ptc) : pis_.nextGoal();
if (st)
{
Motion* motion = new Motion(si_);
si_->copyState(motion->state, st);
motion->root = motion->state;
motion->valid = true;
projectionEvaluator_->computeCoordinates(motion->state, xcoord);
dGoal_.addMotion(motion, xcoord);
}
if (dGoal_.getMotionCount() == 0)
{
OMPL_ERROR("%s: Unable to sample any valid states for goal tree", getName().c_str());
break;
}
}
Discretization<Motion>::Cell *ecell = NULL;
Motion *existing = NULL;
disc.selectMotion(existing, ecell);
assert(existing);
sampler_->sampleUniformNear(xstate, existing->state, maxDistance_);
/* create a motion */
Motion* motion = new Motion(si_);
si_->copyState(motion->state, xstate);
motion->parent = existing;
motion->root = existing->root;
existing->children.push_back(motion);
projectionEvaluator_->computeCoordinates(motion->state, xcoord);
disc.addMotion(motion, xcoord);
/* attempt to connect trees */
Discretization<Motion>::Cell *ocell = otherDisc.getGrid().getCell(xcoord);
if (ocell && !ocell->data->motions.empty())
{
Motion* connectOther = ocell->data->motions[rng_.uniformInt(0, ocell->data->motions.size() - 1)];
if (goal->isStartGoalPairValid(startTree ? connectOther->root : motion->root, startTree ? motion->root : connectOther->root))
{
Motion* connect = new Motion(si_);
si_->copyState(connect->state, connectOther->state);
connect->parent = motion;
connect->root = motion->root;
motion->children.push_back(connect);
projectionEvaluator_->computeCoordinates(connect->state, xcoord);
disc.addMotion(connect, xcoord);
if (isPathValid(disc, connect, xstate) && isPathValid(otherDisc, connectOther, xstate))
{
if (startTree)
connectionPoint_ = std::make_pair(connectOther->state, motion->state);
else
connectionPoint_ = std::make_pair(motion->state, connectOther->state);
/* extract the motions and put them in solution vector */
std::vector<Motion*> mpath1;
while (motion != NULL)
{
mpath1.push_back(motion);
motion = motion->parent;
}
std::vector<Motion*> mpath2;
while (connectOther != NULL)
{
mpath2.push_back(connectOther);
connectOther = connectOther->parent;
}
if (startTree)
mpath1.swap(mpath2);
PathGeometric *path = new PathGeometric(si_);
path->getStates().reserve(mpath1.size() + mpath2.size());
for (int i = mpath1.size() - 1 ; i >= 0 ; --i)
path->append(mpath1[i]->state);
for (unsigned int i = 0 ; i < mpath2.size() ; ++i)
path->append(mpath2[i]->state);
pdef_->addSolutionPath(base::PathPtr(path), false, 0.0);
solved = true;
break;
}
}
}
}
si_->freeState(xstate);
OMPL_INFORM("%s: Created %u (%u start + %u goal) states in %u cells (%u start (%u on boundary) + %u goal (%u on boundary))",
getName().c_str(),
dStart_.getMotionCount() + dGoal_.getMotionCount(), dStart_.getMotionCount(), dGoal_.getMotionCount(),
dStart_.getCellCount() + dGoal_.getCellCount(), dStart_.getCellCount(), dStart_.getGrid().countExternal(),
dGoal_.getCellCount(), dGoal_.getGrid().countExternal());
return solved ? base::PlannerStatus::EXACT_SOLUTION : base::PlannerStatus::TIMEOUT;
}
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);
base::OptimizationObjective *opt = pdef_->getOptimizationObjective().get();
// when no optimization objective is specified, we create a temporary one (we should not modify the ProblemDefinition)
boost::scoped_ptr<base::OptimizationObjective> temporaryOptimizationObjective;
if (opt && !dynamic_cast<base::PathLengthOptimizationObjective*>(opt))
{
opt = NULL;
OMPL_WARN("Optimization objective '%s' specified, but such an objective is not appropriate for %s. Only path length can be optimized.", getName().c_str(), opt->getDescription().c_str());
}
if (!opt)
{
// by default, optimize path length and run until completion
opt = new base::PathLengthOptimizationObjective(si_, std::numeric_limits<double>::epsilon());
temporaryOptimizationObjective.reset(opt);
OMPL_INFORM("No optimization objective specified. Defaulting to optimization of path length for the allowed planning time.");
}
if (!goal)
{
OMPL_ERROR("Goal undefined");
return base::PlannerStatus::INVALID_GOAL;
}
while (const base::State *st = pis_.nextStart())
{
Motion *motion = new Motion(si_);
si_->copyState(motion->state, st);
nn_->add(motion);
}
if (nn_->size() == 0)
{
OMPL_ERROR("There are no valid initial states!");
return base::PlannerStatus::INVALID_START;
}
if (!sampler_)
sampler_ = si_->allocStateSampler();
OMPL_INFORM("Starting with %u states", nn_->size());
Motion *solution = NULL;
Motion *approximation = NULL;
double approximatedist = std::numeric_limits<double>::infinity();
bool sufficientlyShort = false;
Motion *rmotion = new Motion(si_);
base::State *rstate = rmotion->state;
base::State *xstate = si_->allocState();
std::vector<Motion*> solCheck;
std::vector<Motion*> nbh;
std::vector<double> dists;
std::vector<int> valid;
unsigned int rewireTest = 0;
double stateSpaceDimensionConstant = 1.0 / (double)si_->getStateSpace()->getDimension();
while (ptc == false)
{
// sample random state (with goal biasing)
if (goal_s && rng_.uniform01() < goalBias_ && goal_s->canSample())
goal_s->sampleGoal(rstate);
else
sampler_->sampleUniform(rstate);
// find closest state in the tree
Motion *nmotion = nn_->nearest(rmotion);
base::State *dstate = rstate;
// find state to add
double d = si_->distance(nmotion->state, rstate);
if (d > maxDistance_)
{
si_->getStateSpace()->interpolate(nmotion->state, rstate, maxDistance_ / d, xstate);
dstate = xstate;
}
if (si_->checkMotion(nmotion->state, dstate))
{
// create a motion
double distN = si_->distance(dstate, nmotion->state);
Motion *motion = new Motion(si_);
si_->copyState(motion->state, dstate);
motion->parent = nmotion;
motion->cost = nmotion->cost + distN;
// find nearby neighbors
double r = std::min(ballRadiusConst_ * pow(log((double)(1 + nn_->size())) / (double)(nn_->size()), stateSpaceDimensionConstant),
ballRadiusMax_);
nn_->nearestR(motion, r, nbh);
rewireTest += nbh.size();
// cache for distance computations
dists.resize(nbh.size());
// cache for motion validity
valid.resize(nbh.size());
std::fill(valid.begin(), valid.end(), 0);
if (delayCC_)
{
// calculate all costs and distances
for (unsigned int i = 0 ; i < nbh.size() ; ++i)
nbh[i]->cost += si_->distance(nbh[i]->state, dstate);
// sort the nodes
std::sort(nbh.begin(), nbh.end(), compareMotion);
for (unsigned int i = 0 ; i < nbh.size() ; ++i)
{
dists[i] = si_->distance(nbh[i]->state, dstate);
nbh[i]->cost -= dists[i];
}
// collision check until a valid motion is found
for (unsigned int i = 0 ; i < nbh.size() ; ++i)
{
if (nbh[i] != nmotion)
{
double c = nbh[i]->cost + dists[i];
if (c < motion->cost)
{
if (si_->checkMotion(nbh[i]->state, dstate))
{
motion->cost = c;
motion->parent = nbh[i];
valid[i] = 1;
break;
}
else
valid[i] = -1;
}
}
else
{
valid[i] = 1;
dists[i] = distN;
break;
}
}
}
else
{
// find which one we connect the new state to
for (unsigned int i = 0 ; i < nbh.size() ; ++i)
{
if (nbh[i] != nmotion)
{
dists[i] = si_->distance(nbh[i]->state, dstate);
double c = nbh[i]->cost + dists[i];
if (c < motion->cost)
{
if (si_->checkMotion(nbh[i]->state, dstate))
{
motion->cost = c;
motion->parent = nbh[i];
valid[i] = 1;
}
else
valid[i] = -1;
}
}
else
{
valid[i] = 1;
dists[i] = distN;
}
}
}
// add motion to the tree
nn_->add(motion);
motion->parent->children.push_back(motion);
solCheck.resize(1);
solCheck[0] = motion;
// rewire tree if needed
for (unsigned int i = 0 ; i < nbh.size() ; ++i)
if (nbh[i] != motion->parent)
{
double c = motion->cost + dists[i];
if (c < nbh[i]->cost)
{
bool v = valid[i] == 0 ? si_->checkMotion(nbh[i]->state, dstate) : valid[i] == 1;
if (v)
{
// Remove this node from its parent list
removeFromParent (nbh[i]);
double delta = c - nbh[i]->cost;
// Add this node to the new parent
nbh[i]->parent = motion;
nbh[i]->cost = c;
nbh[i]->parent->children.push_back(nbh[i]);
solCheck.push_back(nbh[i]);
// Update the costs of the node's children
updateChildCosts(nbh[i], delta);
}
}
}
// Make sure to check the existing solution for improvement
if (solution)
solCheck.push_back(solution);
// check if we found a solution
for (unsigned int i = 0 ; i < solCheck.size() ; ++i)
{
double dist = 0.0;
bool solved = goal->isSatisfied(solCheck[i]->state, &dist);
sufficientlyShort = solved ? opt->isSatisfied(solCheck[i]->cost) : false;
if (solved)
{
if (sufficientlyShort)
{
solution = solCheck[i];
break;
}
else if (!solution || (solCheck[i]->cost < solution->cost))
{
solution = solCheck[i];
}
}
else if (!solution && dist < approximatedist)
{
approximation = solCheck[i];
approximatedist = dist;
}
}
}
// terminate if a sufficient solution is found
if (solution && sufficientlyShort)
break;
}
double solutionCost;
bool approximate = (solution == NULL);
bool addedSolution = false;
if (approximate)
{
solution = approximation;
solutionCost = approximatedist;
}
else
solutionCost = solution->cost;
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
PathGeometric *path = new PathGeometric(si_);
for (int i = mpath.size() - 1 ; i >= 0 ; --i)
path->append(mpath[i]->state);
pdef_->addSolutionPath(base::PathPtr(path), approximate, solutionCost);
addedSolution = true;
}
si_->freeState(xstate);
if (rmotion->state)
si_->freeState(rmotion->state);
delete rmotion;
OMPL_INFORM("Created %u states. Checked %lu rewire options.", nn_->size(), rewireTest);
return base::PlannerStatus(addedSolution, approximate);
}
ompl::base::PlannerStatus ompl::geometric::KPIECE1::solve(const base::PlannerTerminationCondition &ptc)
{
checkValidity();
base::Goal *goal = pdef_->getGoal().get();
base::GoalSampleableRegion *goal_s = dynamic_cast<base::GoalSampleableRegion*>(goal);
Discretization<Motion>::Coord xcoord;
while (const base::State *st = pis_.nextStart())
{
Motion *motion = new Motion(si_);
si_->copyState(motion->state, st);
projectionEvaluator_->computeCoordinates(motion->state, xcoord);
disc_.addMotion(motion, xcoord, 1.0);
}
if (disc_.getMotionCount() == 0)
{
OMPL_ERROR("%s: There are no valid initial states!", getName().c_str());
return base::PlannerStatus::INVALID_START;
}
if (!sampler_)
sampler_ = si_->allocStateSampler();
OMPL_INFORM("%s: Starting with %u states", getName().c_str(), disc_.getMotionCount());
Motion *solution = NULL;
Motion *approxsol = NULL;
double approxdif = std::numeric_limits<double>::infinity();
base::State *xstate = si_->allocState();
while (ptc == false)
{
disc_.countIteration();
/* Decide on a state to expand from */
Motion *existing = NULL;
Discretization<Motion>::Cell *ecell = NULL;
disc_.selectMotion(existing, ecell);
assert(existing);
/* sample random state (with goal biasing) */
if (goal_s && rng_.uniform01() < goalBias_ && goal_s->canSample())
goal_s->sampleGoal(xstate);
else
sampler_->sampleUniformNear(xstate, existing->state, maxDistance_);
std::pair<base::State*, double> fail(xstate, 0.0);
bool keep = si_->checkMotion(existing->state, xstate, fail);
if (!keep && fail.second > minValidPathFraction_)
keep = true;
if (keep)
{
/* create a motion */
Motion *motion = new Motion(si_);
si_->copyState(motion->state, xstate);
motion->parent = existing;
double dist = 0.0;
bool solv = goal->isSatisfied(motion->state, &dist);
projectionEvaluator_->computeCoordinates(motion->state, xcoord);
disc_.addMotion(motion, xcoord, dist); // this will also update the discretization heaps as needed, so no call to updateCell() is needed
if (solv)
{
approxdif = dist;
solution = motion;
break;
}
if (dist < approxdif)
{
approxdif = dist;
approxsol = motion;
}
}
else
ecell->data->score *= failedExpansionScoreFactor_;
disc_.updateCell(ecell);
}
bool solved = false;
bool approximate = false;
if (solution == NULL)
{
solution = approxsol;
approximate = true;
}
if (solution != NULL)
{
lastGoalMotion_ = solution;
/* construct the solution path */
std::vector<Motion*> mpath;
while (solution != NULL)
{
mpath.push_back(solution);
solution = solution->parent;
}
/* set the solution path */
PathGeometric *path = new PathGeometric(si_);
for (int i = mpath.size() - 1 ; i >= 0 ; --i)
path->append(mpath[i]->state);
pdef_->addSolutionPath(base::PathPtr(path), approximate, approxdif);
solved = true;
}
si_->freeState(xstate);
OMPL_INFORM("%s: Created %u states in %u cells (%u internal + %u external)",
getName().c_str(),
disc_.getMotionCount(), disc_.getCellCount(),
disc_.getGrid().countInternal(), disc_.getGrid().countExternal());
return base::PlannerStatus(solved, approximate);
}
ompl::base::PlannerStatus ompl::geometric::BiTRRT::solve(const base::PlannerTerminationCondition &ptc)
{
// Basic error checking
checkValidity();
// Goal information
base::Goal *goal = pdef_->getGoal().get();
base::GoalSampleableRegion *gsr = dynamic_cast<base::GoalSampleableRegion*>(goal);
if (!gsr)
{
OMPL_ERROR("%s: Goal object does not derive from GoalSampleableRegion", getName().c_str());
return base::PlannerStatus::INVALID_GOAL;
}
// Loop through the (valid) input states and add them to the start tree
while (const base::State *state = pis_.nextStart())
{
Motion *motion = new Motion(si_);
si_->copyState(motion->state, state);
motion->cost = opt_->stateCost(motion->state);
motion->root = motion->state; // this state is the root of a tree
if (tStart_->size() == 0) // do not overwrite best/worst from a prior call to solve
worstCost_ = bestCost_ = motion->cost;
// Add start motion to the tree
tStart_->add(motion);
}
if (tStart_->size() == 0)
{
OMPL_ERROR("%s: Start tree has no valid states!", getName().c_str());
return base::PlannerStatus::INVALID_START;
}
// Do the same for the goal tree, if it is empty, but only once
if (tGoal_->size() == 0)
{
const base::State *state = pis_.nextGoal(ptc);
if (state)
{
Motion* motion = addMotion(state, tGoal_);
motion->root = motion->state; // this state is the root of a tree
}
}
if (tGoal_->size() == 0)
{
OMPL_ERROR("%s: Goal tree has no valid states!", getName().c_str());
return base::PlannerStatus::INVALID_GOAL;
}
OMPL_INFORM("%s: Planning started with %d states already in datastructure", getName().c_str(), (int)(tStart_->size() + tGoal_->size()));
base::StateSamplerPtr sampler = si_->allocStateSampler();
Motion *rmotion = new Motion(si_);
base::State *rstate = rmotion->state;
Motion *xmotion = new Motion(si_);
base::State *xstate = xmotion->state;
TreeData tree = tStart_;
TreeData otherTree = tGoal_;
bool solved = false;
// Planning loop
while (ptc == false)
{
// Check if there are more goal states
if (pis_.getSampledGoalsCount() < tGoal_->size() / 2)
{
if (const base::State *state = pis_.nextGoal())
{
Motion* motion = addMotion(state, tGoal_);
motion->root = motion->state; // this state is the root of a tree
}
}
// Sample a state uniformly at random
sampler->sampleUniform(rstate);
Motion* result; // the motion that gets added in extendTree
if (extendTree(rmotion, tree, result) != FAILED) // we added something new to the tree
{
// Try to connect the other tree to the node we just added
if (connectTrees(result, otherTree, xmotion))
{
// The trees have been connected. Construct the solution path
Motion *solution = connectionPoint_.first;
std::vector<Motion*> mpath1;
while (solution != NULL)
{
mpath1.push_back(solution);
solution = solution->parent;
}
solution = connectionPoint_.second;
std::vector<Motion*> mpath2;
while (solution != NULL)
{
mpath2.push_back(solution);
solution = solution->parent;
}
PathGeometric *path = new PathGeometric(si_);
path->getStates().reserve(mpath1.size() + mpath2.size());
for (int i = mpath1.size() - 1 ; i >= 0 ; --i)
path->append(mpath1[i]->state);
for (unsigned int i = 0 ; i < mpath2.size() ; ++i)
path->append(mpath2[i]->state);
pdef_->addSolutionPath(base::PathPtr(path), false, 0.0, getName());
solved = true;
break;
}
}
std::swap(tree, otherTree);
}
si_->freeState(rstate);
si_->freeState(xstate);
delete rmotion;
delete xmotion;
OMPL_INFORM("%s: Created %u states (%u start + %u goal)", getName().c_str(), tStart_->size() + tGoal_->size(), tStart_->size(), tGoal_->size());
return solved ? base::PlannerStatus::EXACT_SOLUTION : base::PlannerStatus::TIMEOUT;
}
bool ompl::geometric::PathSimplifier::findBetterGoal(PathGeometric &path, const base::PlannerTerminationCondition &ptc,
unsigned int samplingAttempts, double rangeRatio, double snapToVertex)
{
if (path.getStateCount() < 2)
return false;
if (!gsr_)
{
OMPL_WARN("%s: No goal sampleable object to sample a better goal from.", "PathSimplifier::findBetterGoal");
return false;
}
unsigned int maxGoals = std::min((unsigned)10, gsr_->maxSampleCount()); // the number of goals we will sample
unsigned int failedTries = 0;
bool betterGoal = false;
const base::StateSpacePtr& ss = si_->getStateSpace();
std::vector<base::State*> &states = path.getStates();
// dists[i] contains the cumulative length of the path up to and including state i
std::vector<double> dists(states.size(), 0.0);
for (unsigned int i = 1 ; i < dists.size() ; ++i)
dists[i] = dists[i-1] + si_->distance(states[i-1], states[i]);
// Sampled states closer than 'threshold' distance to any existing state in the path
// are snapped to the close state
double threshold = dists.back() * snapToVertex;
// The range (distance) of a single connection that will be attempted
double rd = rangeRatio * dists.back();
base::State* temp = si_->allocState();
base::State* tempGoal = si_->allocState();
while(!ptc && failedTries++ < maxGoals && !betterGoal)
{
gsr_->sampleGoal(tempGoal);
// Goal state is not compatible with the start state
if (!gsr_->isStartGoalPairValid(path.getState(0), tempGoal))
continue;
unsigned int numSamples = 0;
while (!ptc && numSamples++ < samplingAttempts && !betterGoal)
{
// sample a state within rangeRatio
double t = rng_.uniformReal(std::max(dists.back() - rd, 0.0), dists.back()); // Sample a random point within rd of the end of the path
std::vector<double>::iterator end = std::lower_bound(dists.begin(), dists.end(), t);
std::vector<double>::iterator start = end;
while(start != dists.begin() && *start >= t)
start -= 1;
unsigned int startIndex = start - dists.begin();
unsigned int endIndex = end - dists.begin();
// Snap the random point to the nearest vertex, if within the threshold
if (t - (*start) < threshold) // snap to the starting waypoint
endIndex = startIndex;
if ((*end) - t < threshold) // snap to the ending waypoint
startIndex = endIndex;
// Compute the state value and the accumulated cost up to that state
double costToCome = dists[startIndex];
base::State* state;
if (startIndex == endIndex)
{
state = states[startIndex];
}
else
{
double tSeg = (t - (*start)) / (*end - *start);
ss->interpolate(states[startIndex], states[endIndex], tSeg, temp);
state = temp;
costToCome += si_->distance(states[startIndex], state);
}
double costToGo = si_->distance(state, tempGoal);
double candidateCost = costToCome + costToGo;
// Make sure we improve before attempting validation
if (dists.back() - candidateCost > std::numeric_limits<float>::epsilon() && si_->checkMotion(state, tempGoal))
{
// insert the new states
if (startIndex == endIndex)
{
// new intermediate state
si_->copyState(states[startIndex], state);
// new goal state
si_->copyState(states[startIndex+1], tempGoal);
if (freeStates_)
{
for(size_t i = startIndex + 2; i < states.size(); ++i)
si_->freeState(states[i]);
}
states.erase(states.begin() + startIndex + 2, states.end());
}
else
{
// overwriting the end of the segment with the new state
si_->copyState(states[endIndex], state);
if (endIndex == states.size()-1)
{
path.append(tempGoal);
}
else
{
// adding goal new goal state
si_->copyState(states[endIndex + 1], tempGoal);
if (freeStates_)
{
for(size_t i = endIndex + 2; i < states.size(); ++i)
si_->freeState(states[i]);
}
states.erase(states.begin() + endIndex + 2, states.end());
}
}
// fix the helper variables
dists.resize(states.size(), 0.0);
for (unsigned int j = std::max(1u, startIndex); j < dists.size() ; ++j)
dists[j] = dists[j-1] + si_->distance(states[j-1], states[j]);
betterGoal = true;
}
}
}
si_->freeState(temp);
si_->freeState(tempGoal);
return betterGoal;
}
ompl::base::PlannerStatus ompl::geometric::RRTsharp::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();
while (const base::State *st = pis_.nextStart())
{
Motion *motion = new Motion(si_);
si_->copyState(motion->state, st);
motion->cost = opt_->identityCost();
nn_->add(motion);
startMotion_ = motion;
}
if (nn_->size() == 0)
{
OMPL_ERROR("%s: There are no valid initial states!", getName().c_str());
return base::PlannerStatus::INVALID_START;
}
if (!sampler_)
sampler_ = si_->allocStateSampler();
OMPL_INFORM("%s: Starting planning with %u states already in datastructure", getName().c_str(), nn_->size());
if (prune_ && !si_->getStateSpace()->isMetricSpace())
OMPL_WARN("%s: tree pruning was activated but since the state space %s is not a metric space, "
"the optimization objective might not satisfy the triangle inequality. You may need to disable pruning."
, getName().c_str(), si_->getStateSpace()->getName().c_str());
const base::ReportIntermediateSolutionFn intermediateSolutionCallback = pdef_->getIntermediateSolutionCallback();
Motion *solution = lastGoalMotion_;
// \todo Make this variable unnecessary, or at least have it
// persist across solve runs
base::Cost bestCost = opt_->infiniteCost();
bestCost_ = opt_->infiniteCost();
Motion *approximation = NULL;
double approximatedist = std::numeric_limits<double>::infinity();
bool sufficientlyShort = false;
Motion *rmotion = new Motion(si_);
base::State *rstate = rmotion->state;
base::State *xstate = si_->allocState();
// e+e/d. K-nearest RRT*
double k_rrg = boost::math::constants::e<double>() +
(boost::math::constants::e<double>() / (double)si_->getStateSpace()->getDimension());
std::vector<Motion*> nbh;
std::vector<base::Cost> costs;
std::vector<base::Cost> incCosts;
std::vector<std::size_t> sortedCostIndices;
std::vector<int> valid;
rewireTest = 0;
statesGenerated = 0;
if (solution)
OMPL_INFORM("%s: Starting planning with existing solution of cost %.5f", getName().c_str(), solution->cost.value());
OMPL_INFORM("%s: Initial k-nearest value of %u", getName().c_str(), (unsigned int)std::ceil(k_rrg * log((double)(nn_->size() + 1))));
// 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
{
sampler_->sampleUniform(rstate);
if (prune_ && opt_->isCostBetterThan(bestCost_, costToGo(rmotion)))
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
Motion *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 - k-nearest RRT*
unsigned int k = std::ceil(k_rrg * log((double)(nn_->size() + 1)));
nn_->nearestK(motion, k, 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 (prune_)
{
if (opt_->isCostBetterThan(costToGo(motion, false), bestCost_))
{
nn_->add(motion);
motion->parent->children.push_back(motion);
}
else // If the new motion does not improve the best cost it is ignored.
{
--statesGenerated;
si_->freeState(motion->state);
delete motion;
continue;
}
}
else
{
// add motion to the tree
nn_->add(motion);
motion->parent->children.push_back(motion);
}
this->nodesToAnalyzeForRewiring = std::priority_queue< Motion* ,std::vector<Motion*>, std::greater<Motion*>>();
assert(nodesToAnalyzeForRewiring.empty());
this->visitedMotions.clear();
this->toVisitMotions.clear();
bool checkForSolution = false;
toVisitMotions.insert(motion);
nodesToAnalyzeForRewiring.push(motion);
while (!nodesToAnalyzeForRewiring.empty())
{
// if (((int)nn_->size())>7000)
// usleep(200000);
Motion* mc = nodesToAnalyzeForRewiring.top();
nodesToAnalyzeForRewiring.pop();
visitedMotions.insert(mc);
toVisitMotions.erase(mc);
nn_->nearestK(mc, k, nbh);
// Cost minNbhCost = mc->cost;
bool updatedWiring = false;
if (mc!=motion){
for (std::size_t i = 0; i < nbh.size(); ++i){
rewireTest++;
// TODO: add if(symCost) option
base::Cost temp_incCost = opt_->motionCost(nbh[i]->state, mc->state);
base::Cost temp_Cost = opt_->combineCosts(nbh[i]->cost, temp_incCost);
if (opt_->isCostBetterThan(temp_Cost,mc->cost)){
if (si_->checkMotion(nbh[i]->state, mc->state))
{
removeFromParent (mc);
mc->parent = nbh[i];
mc->cost = temp_Cost;
mc->incCost = temp_incCost;
mc->parent->children.push_back(mc);
updatedWiring = true;
checkForSolution = true;
}
}
}
} else {
updatedWiring = true;
}
if (updatedWiring){
// add children to update list
for (std::size_t j = 0; j < mc->children.size(); ++j){
if (toVisitMotions.count(mc->children[j])==0 && visitedMotions.count(mc->children[j])==0){
nodesToAnalyzeForRewiring.push(mc->children[j]);
toVisitMotions.insert(mc->children[j]);
}
}
for (std::size_t i = 0; i < nbh.size(); ++i){
// TODO: avoid repeated calculation of same value
if (opt_->isCostBetterThan(opt_->combineCosts(mc->cost, opt_->motionCost(mc->state, nbh[i]->state)),nbh[i]->cost) && toVisitMotions.count(nbh[i])==0 && visitedMotions.count(nbh[i])==0){
nodesToAnalyzeForRewiring.push(nbh[i]);
toVisitMotions.insert(nbh[i]);
}
}
}
}
// 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 (size_t i = 0; i < goalMotions_.size(); ++i)
{
if (opt_->isCostBetterThan(goalMotions_[i]->cost, bestCost))
{
bestCost = goalMotions_[i]->cost;
bestCost_ = bestCost;
updatedSolution = true;
}
sufficientlyShort = opt_->isSatisfied(goalMotions_[i]->cost);
if (sufficientlyShort)
{
solution = goalMotions_[i];
break;
}
else if (!solution ||
opt_->isCostBetterThan(goalMotions_[i]->cost,solution->cost))
{
solution = goalMotions_[i];
updatedSolution = true;
}
}
if (updatedSolution)
{
if (prune_)
{
int n = pruneTree(bestCost_);
statesGenerated -= n;
}
if (intermediateSolutionCallback)
{
std::vector<const base::State *> spath;
Motion *intermediate_solution = solution->parent; // Do not include goal state to simplify code.
do
{
spath.push_back(intermediate_solution->state);
intermediate_solution = intermediate_solution->parent;
} while (intermediate_solution->parent != 0); // Do not include the start state.
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 == NULL);
bool addedSolution = false;
if (approximate)
solution = approximation;
else
lastGoalMotion_ = solution;
if (solution != NULL)
{
ptc.terminate();
// construct the solution path
std::vector<Motion*> mpath;
while (solution != NULL)
{
mpath.push_back(solution);
solution = solution->parent;
}
// set the solution path
PathGeometric *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.", getName().c_str(), statesGenerated, rewireTest, goalMotions_.size());
return base::PlannerStatus(addedSolution, approximate);
}
ompl::base::PathPtr ompl::geometric::LazyPRM::constructSolution(const Vertex &start, const Vertex &goal)
{
// Need to update the index map here, becuse nodes may have been removed and
// the numbering will not be 0 .. N-1 otherwise.
unsigned long int index = 0;
boost::graph_traits<Graph>::vertex_iterator vi, vend;
for(boost::tie(vi, vend) = boost::vertices(g_); vi != vend; ++vi, ++index)
indexProperty_[*vi] = index;
boost::property_map<Graph, boost::vertex_predecessor_t>::type prev;
try
{
// Consider using a persistent distance_map if it's slow
boost::astar_search(g_, start,
boost::bind(&LazyPRM::costHeuristic, this, _1, goal),
boost::predecessor_map(prev).
distance_compare(boost::bind(&base::OptimizationObjective::
isCostBetterThan, opt_.get(), _1, _2)).
distance_combine(boost::bind(&base::OptimizationObjective::
combineCosts, opt_.get(), _1, _2)).
distance_inf(opt_->infiniteCost()).
distance_zero(opt_->identityCost()).
visitor(AStarGoalVisitor<Vertex>(goal)));
}
catch (AStarFoundGoal&)
{
}
if (prev[goal] == goal)
throw Exception(name_, "Could not find solution path");
// First, get the solution states without copying them, and check them for validity.
// We do all the node validity checks for the vertices, as this may remove a larger
// part of the graph (compared to removing an edge).
std::vector<const base::State*> states(1, stateProperty_[goal]);
std::set<Vertex> milestonesToRemove;
for (Vertex pos = prev[goal]; prev[pos] != pos; pos = prev[pos])
{
const base::State *st = stateProperty_[pos];
unsigned int &vd = vertexValidityProperty_[pos];
if ((vd & VALIDITY_TRUE) == 0)
if (si_->isValid(st))
vd |= VALIDITY_TRUE;
if ((vd & VALIDITY_TRUE) == 0)
milestonesToRemove.insert(pos);
if (milestonesToRemove.empty())
states.push_back(st);
}
// We remove *all* invalid vertices. This is not entirely as described in the original LazyPRM
// paper, as the paper suggest removing the first vertex only, and then recomputing the
// shortest path. Howeve, the paper says the focus is on efficient vertex & edge removal,
// rather than collision checking, so this modification is in the spirit of the paper.
if (!milestonesToRemove.empty())
{
unsigned long int comp = vertexComponentProperty_[start];
// Remember the current neighbors.
std::set<Vertex> neighbors;
for (std::set<Vertex>::iterator it = milestonesToRemove.begin() ; it != milestonesToRemove.end() ; ++it)
{
boost::graph_traits<Graph>::adjacency_iterator nbh, last;
for (boost::tie(nbh, last) = boost::adjacent_vertices(*it, g_) ; nbh != last ; ++nbh)
if (milestonesToRemove.find(*nbh) == milestonesToRemove.end())
neighbors.insert(*nbh);
// Remove vertex from nearest neighbors data structure.
nn_->remove(*it);
// Free vertex state.
si_->freeState(stateProperty_[*it]);
// Remove all edges.
boost::clear_vertex(*it, g_);
// Remove the vertex.
boost::remove_vertex(*it, g_);
}
// Update the connected component ID for neighbors.
for (std::set<Vertex>::iterator it = neighbors.begin() ; it != neighbors.end() ; ++it)
{
if (comp == vertexComponentProperty_[*it])
{
unsigned long int newComponent = componentCount_++;
componentSize_[newComponent] = 0;
markComponent(*it, newComponent);
}
}
return base::PathPtr();
}
// start is checked for validity already
states.push_back(stateProperty_[start]);
// Check the edges too, if the vertices were valid. Remove the first invalid edge only.
std::vector<const base::State*>::const_iterator prevState = states.begin(), state = prevState + 1;
Vertex prevVertex = goal, pos = prev[goal];
do
{
Edge e = boost::lookup_edge(pos, prevVertex, g_).first;
unsigned int &evd = edgeValidityProperty_[e];
if ((evd & VALIDITY_TRUE) == 0)
{
if (si_->checkMotion(*state, *prevState))
evd |= VALIDITY_TRUE;
}
if ((evd & VALIDITY_TRUE) == 0)
{
boost::remove_edge(e, g_);
unsigned long int newComponent = componentCount_++;
componentSize_[newComponent] = 0;
markComponent(pos, newComponent);
return base::PathPtr();
}
prevState = state;
++state;
prevVertex = pos;
pos = prev[pos];
}
while (prevVertex != pos);
PathGeometric *p = new PathGeometric(si_);
for (std::vector<const base::State*>::const_reverse_iterator st = states.rbegin(); st != states.rend(); ++st)
p->append(*st);
return base::PathPtr(p);
}
ompl::base::PlannerStatus ompl::geometric::EST::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())
{
Motion *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 with %u states", getName().c_str(), tree_.size);
Motion *solution = NULL;
Motion *approxsol = NULL;
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 */
Motion *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 == NULL)
{
solution = approxsol;
approximate = true;
}
if (solution != NULL)
{
lastGoalMotion_ = solution;
/* construct the solution path */
std::vector<Motion*> mpath;
while (solution != NULL)
{
mpath.push_back(solution);
solution = solution->parent;
}
/* set the solution path */
PathGeometric *path = new PathGeometric(si_);
for (int i = mpath.size() - 1 ; i >= 0 ; --i)
path->append(mpath[i]->state);
pdef_->addSolutionPath(base::PathPtr(path), approximate, approxdif);
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);
}
bool ompl::geometric::LazyRRT::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())
{
Motion *motion = new Motion(si_);
si_->copyState(motion->state, st);
motion->valid = true;
nn_->add(motion);
}
if (nn_->size() == 0)
{
msg_.error("There are no valid initial states!");
return false;
}
if (!sampler_)
sampler_ = si_->allocStateSampler();
msg_.inform("Starting with %u states", nn_->size());
Motion *solution = NULL;
double distsol = -1.0;
Motion *rmotion = new Motion(si_);
base::State *rstate = rmotion->state;
base::State *xstate = si_->allocState();
RETRY:
while (ptc() == false)
{
/* sample random state (with goal biasing) */
if (goal_s && rng_.uniform01() < goalBias_ && goal_s->canSample())
goal_s->sampleGoal(rstate);
else
sampler_->sampleUniform(rstate);
/* find closest state in the tree */
Motion *nmotion = nn_->nearest(rmotion);
assert(nmotion != rmotion);
base::State *dstate = rstate;
/* find state to add */
double d = si_->distance(nmotion->state, rstate);
if (d > maxDistance_)
{
si_->getStateSpace()->interpolate(nmotion->state, rstate, maxDistance_ / d, xstate);
dstate = xstate;
}
/* create a motion */
Motion *motion = new Motion(si_);
si_->copyState(motion->state, dstate);
motion->parent = nmotion;
nmotion->children.push_back(motion);
nn_->add(motion);
double dist = 0.0;
if (goal->isSatisfied(motion->state, &dist))
{
distsol = dist;
solution = motion;
break;
}
}
bool solved = false;
if (solution != NULL)
{
/* construct the solution path */
std::vector<Motion*> mpath;
while (solution != NULL)
{
mpath.push_back(solution);
solution = solution->parent;
}
/* check the path */
for (int i = mpath.size() - 1 ; i >= 0 ; --i)
if (!mpath[i]->valid)
{
if (si_->checkMotion(mpath[i]->parent->state, mpath[i]->state))
mpath[i]->valid = true;
else
{
removeMotion(mpath[i]);
goto RETRY;
}
}
/* set the solution path */
PathGeometric *path = new PathGeometric(si_);
for (int i = mpath.size() - 1 ; i >= 0 ; --i)
path->append(mpath[i]->state);
goal->addSolutionPath(base::PathPtr(path), false, distsol);
solved = true;
}
si_->freeState(xstate);
si_->freeState(rstate);
delete rmotion;
msg_.inform("Created %u states", nn_->size());
return solved;
}
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();
while (const base::State *st = pis_.nextStart())
{
Motion *motion = new Motion(si_);
si_->copyState(motion->state, st);
motion->cost = opt_->identityCost();
nn_->add(motion);
}
if (nn_->size() == 0)
{
OMPL_ERROR("%s: There are no valid initial states!", getName().c_str());
return base::PlannerStatus::INVALID_START;
}
if (!sampler_)
sampler_ = si_->allocStateSampler();
OMPL_INFORM("%s: Starting planning with %u states already in datastructure", getName().c_str(), nn_->size());
Motion *solution = lastGoalMotion_;
// \TODO Make this variable unnecessary, or at least have it
// persist across solve runs
base::Cost bestCost = opt_->infiniteCost();
Motion *approximation = NULL;
double approximatedist = std::numeric_limits<double>::infinity();
bool sufficientlyShort = false;
Motion *rmotion = new Motion(si_);
base::State *rstate = rmotion->state;
base::State *xstate = si_->allocState();
// e+e/d. K-nearest RRT*
double k_rrg = boost::math::constants::e<double>() +
(boost::math::constants::e<double>() / (double)si_->getStateSpace()->getDimension());
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.v);
OMPL_INFORM("%s: Initial k-nearest value of %u", getName().c_str(), (unsigned int)std::ceil(k_rrg * log((double)(nn_->size()+1))));
// 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
sampler_->sampleUniform(rstate);
// find closest state in the tree
Motion *nmotion = nn_->nearest(rmotion);
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
++collisionChecks_;
if (si_->checkMotion(nmotion->state, dstate))
{
// create a motion
Motion *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 - k-nearest RRT*
unsigned int k = std::ceil(k_rrg * log((double)(nn_->size() + 1)));
nn_->nearestK(motion, k, 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)
++collisionChecks_;
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))
{
++collisionChecks_;
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;
}
}
}
// 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)
{
++collisionChecks_;
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)
{
for (size_t i = 0; i < goalMotions_.size(); ++i)
{
if (opt_->isCostBetterThan(goalMotions_[i]->cost, bestCost))
{
bestCost = goalMotions_[i]->cost;
bestCost_ = bestCost;
}
sufficientlyShort = opt_->isSatisfied(goalMotions_[i]->cost);
if (sufficientlyShort)
{
solution = goalMotions_[i];
break;
}
else if (!solution ||
opt_->isCostBetterThan(goalMotions_[i]->cost,solution->cost))
solution = goalMotions_[i];
}
}
// 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 == 0);
bool addedSolution = false;
if (approximate)
solution = approximation;
else
lastGoalMotion_ = solution;
if (solution != 0)
{
// construct the solution path
std::vector<Motion*> mpath;
while (solution != 0)
{
mpath.push_back(solution);
solution = solution->parent;
}
// set the solution path
PathGeometric *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, whether it is approximate (not reaching the goal), and the
// distance from the end of the path to the goal (-1 if satisfying the goal).
base::PlannerSolution psol(path, approximate, approximate ? approximatedist : -1.0, getName());
// Does the solution satisfy the optimization objective?
psol.optimized_ = 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.", getName().c_str(), statesGenerated, rewireTest, goalMotions_.size());
return base::PlannerStatus(addedSolution, approximate);
}
bool ompl::geometric::RRTstar::solve(const base::PlannerTerminationCondition &ptc)
{
checkValidity();
base::Goal *goal = pdef_->getGoal().get();
base::GoalSampleableRegion *goal_s = dynamic_cast<base::GoalSampleableRegion*>(goal);
parent_ = NULL;
if (!goal)
{
msg_.error("Goal undefined");
return false;
}
while (const base::State *st = pis_.nextStart())
{
Motion *motion = new Motion(si_);
si_->copyState(motion->state, st);
// TODO set this based on initial bearing
motion->set_bearing(initialBearing_);
nn_->add(motion);
}
if (nn_->size() == 0)
{
msg_.error("There are no valid initial states!");
return false;
}
if (!sampler_)
sampler_ = si_->allocStateSampler();
msg_.inform("Starting with %u states", nn_->size());
Motion *solution = NULL;
Motion *approximation = NULL;
double approximatedist = std::numeric_limits<double>::infinity();
bool sufficientlyShort = false;
Motion *rmotion = new Motion(si_);
base::State *rstate = rmotion->state;
base::State *xstate = si_->allocState();
std::vector<Motion*> solCheck;
std::vector<Motion*> nbh;
std::vector<double> dists;
std::vector<int> valid;
unsigned int rewireTest = 0;
while (ptc() == false)
{
bool validneighbor = false;
Motion *nmotion;
base::State *dstate;
//ADDED
parent_ = NULL;
//while (!validneighbor)
//{
if (ptc())
{
msg_.error("Loop failed to find proper states!");
return false;
}
// sample random state (with goal biasing)
if (goal_s && rng_.uniform01() < goalBias_ && goal_s->canSample())
goal_s->sampleGoal(rstate);
else
sampler_->sampleUniform(rstate);
// find closest state in the tree
nmotion = nn_->nearest(rmotion);
dstate = rstate;
// find state to add
double d = si_->distance(nmotion->state, rstate);
if (d > maxDistance_)
{
si_->getStateSpace()->interpolate(nmotion->state, rstate, maxDistance_ / d, xstate);
dstate = xstate;
}
validneighbor = check_bearing(nmotion, dstate);
//}
parent_ = nmotion;
if (si_->checkMotion(nmotion->state, dstate))
{
// create a motion
//ADDED
double distN = si_->distance(dstate, nmotion->state);
Motion *motion = new Motion(si_);
si_->copyState(motion->state, dstate);
motion->parent = nmotion;
motion->set_bearing();
motion->cost = nmotion->cost + distN;
// find nearby neighbors
double r = std::min(ballRadiusConst_ * (sqrt(log((double)(1 + nn_->size())) / ((double)(nn_->size())))),
ballRadiusMax_);
nn_->nearestR(motion, r, nbh);
rewireTest += nbh.size();
// cache for distance computations
dists.resize(nbh.size());
// cache for motion validity
valid.resize(nbh.size());
std::fill(valid.begin(), valid.end(), 0);
if(delayCC_)
{
// calculate all costs and distances
for (unsigned int i = 0 ; i < nbh.size() ; ++i)
nbh[i]->cost += si_->distance(nbh[i]->state, dstate);
// sort the nodes
std::sort(nbh.begin(), nbh.end(), compareMotion);
for (unsigned int i = 0 ; i < nbh.size() ; ++i)
{
dists[i] = si_->distance(nbh[i]->state, dstate);
nbh[i]->cost -= dists[i];
}
// collision check until a valid motion is found
for (unsigned int i = 0 ; i < nbh.size() ; ++i)
{
if (nbh[i] != nmotion)
{
double c = nbh[i]->cost + dists[i];
if (c < motion->cost)
{
//ADDED
parent_ = nbh[i];
if (si_->checkMotion(nbh[i]->state, dstate) /*&& check_bearing(nbh[i], motion->state)*/)
{
motion->cost = c;
motion->parent = nbh[i];
motion->set_bearing();
valid[i] = 1;
break;
}
else
valid[i] = -1;
}
}
else
{
valid[i] = 1;
dists[i] = distN;
break;
}
}
}
else
{
// find which one we connect the new state to
for (unsigned int i = 0 ; i < nbh.size() ; ++i)
{
if (nbh[i] != nmotion)
{
dists[i] = si_->distance(nbh[i]->state, dstate);
double c = nbh[i]->cost + dists[i];
if (c < motion->cost)
{
// ADDED
parent_ = nbh[i];
if (si_->checkMotion(nbh[i]->state, dstate) /*&& check_bearing(nbh[i], dstate)*/)
{
motion->cost = c;
motion->parent = nbh[i];
motion->set_bearing();
valid[i] = 1;
}
else
valid[i] = -1;
}
}
else
{
valid[i] = 1;
dists[i] = distN;
}
}
}
// add motion to the tree
nn_->add(motion);
motion->parent->children.push_back(motion);
solCheck.resize(1);
solCheck[0] = motion;
// rewire tree if needed
for (unsigned int i = 0 ; i < nbh.size() ; ++i)
if (nbh[i] != motion->parent)
{
double c = motion->cost + dists[i];
if ((c < nbh[i]->cost))
{
parent_ = motion;
bool v = valid[i] == 0 ? si_->checkMotion(nbh[i]->state, dstate) : valid[i] == 1;
if (v /*&& check_bearing(motion, nbh[i]->state)*/)
{
// Remove this node from its parent list
removeFromParent (nbh[i]);
double delta = c - nbh[i]->cost;
// Add this node to the new parent
nbh[i]->parent = motion;
nbh[i]->cost = c;
nbh[i]->set_bearing();
nbh[i]->parent->children.push_back(nbh[i]);
solCheck.push_back(nbh[i]);
// Update the costs of the node's children
updateChildCosts(nbh[i], delta);
}
}
}
// check if we found a solution
for (unsigned int i = 0 ; i < solCheck.size() ; ++i)
{
double dist = 0.0;
bool solved = goal->isSatisfied(solCheck[i]->state, &dist);
sufficientlyShort = solved ? goal->isPathLengthSatisfied(solCheck[i]->cost) : false;
if (solved)
{
if (sufficientlyShort)
{
solution = solCheck[i];
break;
}
else if (!solution || (solCheck[i]->cost < solution->cost))
{
solution = solCheck[i];
}
}
else if (!solution && dist < approximatedist)
{
approximation = solCheck[i];
approximatedist = dist;
}
}
}
// terminate if a sufficient solution is found
if (solution && sufficientlyShort)
break;
}
double solutionCost;
bool approximate = (solution == NULL);
bool addedSolution = false;
if (approximate)
{
solution = approximation;
solutionCost = approximatedist;
}
else
solutionCost = solution->cost;
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
PathGeometric *path = new PathGeometric(si_);
for (int i = mpath.size() - 1 ; i >= 0 ; --i)
path->append(mpath[i]->state);
goal->addSolutionPath(base::PathPtr(path), approximate, solutionCost);
addedSolution = true;
}
si_->freeState(xstate);
if (rmotion->state)
si_->freeState(rmotion->state);
delete rmotion;
msg_.inform("Created %u states. Checked %lu rewire options.", nn_->size(), rewireTest);
return addedSolution;
}