bool ompl::geometric::PathSimplifier::collapseCloseVertices(PathGeometric &path, unsigned int maxSteps, unsigned int maxEmptySteps)
{
if (path.getStateCount() < 3)
return false;
if (maxSteps == 0)
maxSteps = path.getStateCount();
if (maxEmptySteps == 0)
maxEmptySteps = path.getStateCount();
const base::SpaceInformationPtr &si = path.getSpaceInformation();
std::vector<base::State*> &states = path.getStates();
// compute pair-wise distances in path (construct only half the matrix)
std::map<std::pair<const base::State*, const base::State*>, double> distances;
for (unsigned int i = 0 ; i < states.size() ; ++i)
for (unsigned int j = i + 2 ; j < states.size() ; ++j)
distances[std::make_pair(states[i], states[j])] = si->distance(states[i], states[j]);
bool result = false;
unsigned int nochange = 0;
for (unsigned int s = 0 ; s < maxSteps && nochange < maxEmptySteps ; ++s, ++nochange)
{
// find closest pair of points
double minDist = std::numeric_limits<double>::infinity();
int p1 = -1;
int p2 = -1;
for (unsigned int i = 0 ; i < states.size() ; ++i)
for (unsigned int j = i + 2 ; j < states.size() ; ++j)
{
double d = distances[std::make_pair(states[i], states[j])];
if (d < minDist)
{
minDist = d;
p1 = i;
p2 = j;
}
}
if (p1 >= 0 && p2 >= 0)
{
if (si->checkMotion(states[p1], states[p2]))
{
if (freeStates_)
for (int i = p1 + 1 ; i < p2 ; ++i)
si->freeState(states[i]);
states.erase(states.begin() + p1 + 1, states.begin() + p2);
result = true;
nochange = 0;
}
else
distances[std::make_pair(states[p1], states[p2])] = std::numeric_limits<double>::infinity();
}
else
break;
}
return result;
}
/* Based on COMP450 2010 project of Yun Yu and Linda Hill (Rice University) */
void ompl::geometric::PathSimplifier::smoothBSpline(PathGeometric &path, unsigned int maxSteps, double minChange)
{
if (path.getStateCount() < 3)
return;
const base::SpaceInformationPtr &si = path.getSpaceInformation();
std::vector<base::State*> &states = path.getStates();
base::State *temp1 = si->allocState();
base::State *temp2 = si->allocState();
for (unsigned int s = 0 ; s < maxSteps ; ++s)
{
path.subdivide();
unsigned int i = 2, u = 0, n1 = states.size() - 1;
while (i < n1)
{
if (si->isValid(states[i - 1]))
{
si->getStateSpace()->interpolate(states[i - 1], states[i], 0.5, temp1);
si->getStateSpace()->interpolate(states[i], states[i + 1], 0.5, temp2);
si->getStateSpace()->interpolate(temp1, temp2, 0.5, temp1);
if (si->checkMotion(states[i - 1], temp1) && si->checkMotion(temp1, states[i + 1]))
{
if (si->distance(states[i], temp1) > minChange)
{
si->copyState(states[i], temp1);
++u;
}
}
}
i += 2;
}
if (u == 0)
break;
}
si->freeState(temp1);
si->freeState(temp2);
}
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;
}
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;
}
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;
}
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::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;
}
bool ompl::geometric::PathSimplifier::shortcutPath(PathGeometric &path, unsigned int maxSteps, unsigned int maxEmptySteps, double rangeRatio, double snapToVertex)
{
if (path.getStateCount() < 3)
return false;
if (maxSteps == 0)
maxSteps = path.getStateCount();
if (maxEmptySteps == 0)
maxEmptySteps = path.getStateCount();
const base::SpaceInformationPtr &si = path.getSpaceInformation();
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 *temp0 = si->allocState();
base::State *temp1 = si->allocState();
bool result = false;
unsigned int nochange = 0;
// Attempt shortcutting maxSteps times or when no improvement is found after
// maxEmptySteps attempts, whichever comes first
for (unsigned int i = 0 ; i < maxSteps && nochange < maxEmptySteps ; ++i, ++nochange)
{
// Sample a random point anywhere along the path
base::State *s0 = nullptr;
int index0 = -1;
double t0 = 0.0;
double p0 = rng_.uniformReal(0.0, dists.back()); // sample a random point (p0) along the path
std::vector<double>::iterator pit = std::lower_bound(dists.begin(), dists.end(), p0); // find the NEXT waypoint after the random point
int pos0 = pit == dists.end() ? dists.size() - 1 : pit - dists.begin(); // get the index of the NEXT waypoint after the point
if (pos0 == 0 || dists[pos0] - p0 < threshold) // snap to the NEXT waypoint
index0 = pos0;
else
{
while (pos0 > 0 && p0 < dists[pos0])
--pos0;
if (p0 - dists[pos0] < threshold) // snap to the PREVIOUS waypoint
index0 = pos0;
}
// Sample a random point within rd distance of the previously sampled point
base::State *s1 = nullptr;
int index1 = -1;
double t1 = 0.0;
double p1 = rng_.uniformReal(std::max(0.0, p0 - rd), std::min(p0 + rd, dists.back())); // sample a random point (p1) near p0
pit = std::lower_bound(dists.begin(), dists.end(), p1); // find the NEXT waypoint after the random point
int pos1 = pit == dists.end() ? dists.size() - 1 : pit - dists.begin(); // get the index of the NEXT waypoint after the point
if (pos1 == 0 || dists[pos1] - p1 < threshold) // snap to the NEXT waypoint
index1 = pos1;
else
{
while (pos1 > 0 && p1 < dists[pos1])
--pos1;
if (p1 - dists[pos1] < threshold) // snap to the PREVIOUS waypoint
index1 = pos1;
}
// Don't waste time on points that are on the same path segment
if (pos0 == pos1 || index0 == pos1 || index1 == pos0 ||
pos0 + 1 == index1 || pos1 + 1 == index0 ||
(index0 >=0 && index1 >= 0 && abs(index0 - index1) < 2))
continue;
// Get the state pointer for p0
if (index0 >= 0)
s0 = states[index0];
else
{
t0 = (p0 - dists[pos0]) / (dists[pos0 + 1] - dists[pos0]);
si->getStateSpace()->interpolate(states[pos0], states[pos0 + 1], t0, temp0);
s0 = temp0;
}
// Get the state pointer for p1
if (index1 >= 0)
s1 = states[index1];
else
{
t1 = (p1 - dists[pos1]) / (dists[pos1 + 1] - dists[pos1]);
si->getStateSpace()->interpolate(states[pos1], states[pos1 + 1], t1, temp1);
s1 = temp1;
}
// Check for validity between s0 and s1
if (si->checkMotion(s0, s1))
{
if (pos0 > pos1)
{
std::swap(pos0, pos1);
std::swap(index0, index1);
std::swap(s0, s1);
std::swap(t0, t1);
}
// Modify the path with the new, shorter result
if (index0 < 0 && index1 < 0)
{
if (pos0 + 1 == pos1)
{
si->copyState(states[pos1], s0);
states.insert(states.begin() + pos1 + 1, si->cloneState(s1));
}
else
{
if (freeStates_)
for (int j = pos0 + 2 ; j < pos1 ; ++j)
si->freeState(states[j]);
si->copyState(states[pos0 + 1], s0);
si->copyState(states[pos1], s1);
states.erase(states.begin() + pos0 + 2, states.begin() + pos1);
}
}
else
if (index0 >= 0 && index1 >= 0)
{
if (freeStates_)
for (int j = index0 + 1 ; j < index1 ; ++j)
si->freeState(states[j]);
states.erase(states.begin() + index0 + 1, states.begin() + index1);
}
else
if (index0 < 0 && index1 >= 0)
{
if (freeStates_)
for (int j = pos0 + 2 ; j < index1 ; ++j)
si->freeState(states[j]);
si->copyState(states[pos0 + 1], s0);
states.erase(states.begin() + pos0 + 2, states.begin() + index1);
}
else
if (index0 >= 0 && index1 < 0)
{
if (freeStates_)
for (int j = index0 + 1 ; j < pos1 ; ++j)
si->freeState(states[j]);
si->copyState(states[pos1], s1);
states.erase(states.begin() + index0 + 1, states.begin() + pos1);
}
// fix the helper variables
dists.resize(states.size(), 0.0);
for (unsigned int j = pos0 + 1 ; j < dists.size() ; ++j)
dists[j] = dists[j - 1] + si->distance(states[j-1], states[j]);
threshold = dists.back() * snapToVertex;
rd = rangeRatio * dists.back();
result = true;
nochange = 0;
}
}
si->freeState(temp1);
si->freeState(temp0);
return result;
}
bool ompl::geometric::PathSimplifier::reduceVertices(PathGeometric &path, unsigned int maxSteps, unsigned int maxEmptySteps, double rangeRatio)
{
if (path.getStateCount() < 3)
return false;
if (maxSteps == 0)
maxSteps = path.getStateCount();
if (maxEmptySteps == 0)
maxEmptySteps = path.getStateCount();
bool result = false;
unsigned int nochange = 0;
const base::SpaceInformationPtr &si = path.getSpaceInformation();
std::vector<base::State*> &states = path.getStates();
if (si->checkMotion(states.front(), states.back()))
{
if (freeStates_)
for (std::size_t i = 2 ; i < states.size() ; ++i)
si->freeState(states[i-1]);
std::vector<base::State*> newStates(2);
newStates[0] = states.front();
newStates[1] = states.back();
states.swap(newStates);
result = true;
}
else
for (unsigned int i = 0 ; i < maxSteps && nochange < maxEmptySteps ; ++i, ++nochange)
{
int count = states.size();
int maxN = count - 1;
int range = 1 + (int)(floor(0.5 + (double)count * rangeRatio));
int p1 = rng_.uniformInt(0, maxN);
int p2 = rng_.uniformInt(std::max(p1 - range, 0), std::min(maxN, p1 + range));
if (abs(p1 - p2) < 2)
{
if (p1 < maxN - 1)
p2 = p1 + 2;
else
if (p1 > 1)
p2 = p1 - 2;
else
continue;
}
if (p1 > p2)
std::swap(p1, p2);
if (si->checkMotion(states[p1], states[p2]))
{
if (freeStates_)
for (int j = p1 + 1 ; j < p2 ; ++j)
si->freeState(states[j]);
states.erase(states.begin() + p1 + 1, states.begin() + p2);
nochange = 0;
result = true;
}
}
return result;
}