void ompl::control::RRT::setup()
{
base::Planner::setup();
if (!nn_)
nn_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Motion *>(this));
nn_->setDistanceFunction([this](const Motion *a, const Motion *b)
{
return distanceFunction(a, b);
});
}
void ompl::geometric::RRTConnect::setup()
{
Planner::setup();
tools::SelfConfig sc(si_, getName());
sc.configurePlannerRange(maxDistance_);
if (!tStart_)
tStart_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Motion *>(this));
if (!tGoal_)
tGoal_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Motion *>(this));
tStart_->setDistanceFunction([this](const Motion *a, const Motion *b)
{
return distanceFunction(a, b);
});
tGoal_->setDistanceFunction([this](const Motion *a, const Motion *b)
{
return distanceFunction(a, b);
});
}
double ompl::geometric::LBTRRT::lazilyUpdateApxParent(Motion *child, Motion *parent)
{
double dist = distanceFunction(parent, child);
removeFromParentApx(child);
double deltaApx = parent->costApx_ + dist - child->costApx_;
child->parentApx_ = parent;
parent->childrenApx_.push_back(child);
child->costApx_ = parent->costApx_ + dist;
return deltaApx;
}
void ompl::geometric::LazyLBTRRT::setup()
{
Planner::setup();
tools::SelfConfig sc(si_, getName());
sc.configurePlannerRange(maxDistance_);
if (!nn_)
nn_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Motion *>(this));
nn_->setDistanceFunction([this](const Motion *a, const Motion *b)
{
return distanceFunction(a, b);
});
}
void ompl::geometric::SPARS::setup()
{
Planner::setup();
if (!nn_)
nn_.reset(tools::SelfConfig::getDefaultNearestNeighbors<DenseVertex>(this));
nn_->setDistanceFunction([this](const DenseVertex a, const DenseVertex b)
{
return distanceFunction(a, b);
});
if (!snn_)
snn_.reset(tools::SelfConfig::getDefaultNearestNeighbors<SparseVertex>(this));
snn_->setDistanceFunction([this](const SparseVertex a, const SparseVertex b)
{
return sparseDistanceFunction(a, b);
});
if (!connectionStrategy_)
connectionStrategy_ = KStarStrategy<DenseVertex>(
[this]
{
return milestoneCount();
},
nn_, si_->getStateDimension());
double maxExt = si_->getMaximumExtent();
sparseDelta_ = sparseDeltaFraction_ * maxExt;
denseDelta_ = denseDeltaFraction_ * maxExt;
// Setup optimization objective
//
// If no optimization objective was specified, then default to
// optimizing path length as computed by the distance() function
// in the state space.
if (pdef_)
{
if (pdef_->hasOptimizationObjective())
{
opt_ = pdef_->getOptimizationObjective();
if (!dynamic_cast<base::PathLengthOptimizationObjective *>(opt_.get()))
OMPL_WARN("%s: Asymptotic optimality has only been proven with path length optimizaton; convergence "
"for other optimizaton objectives is not guaranteed.",
getName().c_str());
}
else
opt_ = std::make_shared<base::PathLengthOptimizationObjective>(si_);
}
else
{
OMPL_INFORM("%s: problem definition is not set, deferring setup completion...", getName().c_str());
setup_ = false;
}
}
void ompl::geometric::RRTXstatic::setup()
{
Planner::setup();
tools::SelfConfig sc(si_, getName());
sc.configurePlannerRange(maxDistance_);
if (!si_->getStateSpace()->hasSymmetricDistance() || !si_->getStateSpace()->hasSymmetricInterpolate())
{
OMPL_WARN("%s requires a state space with symmetric distance and symmetric interpolation.", getName().c_str());
}
if (!nn_)
nn_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Motion *>(this));
nn_->setDistanceFunction([this](const Motion *a, const Motion *b) { return distanceFunction(a, b); });
// Setup optimization objective
//
// If no optimization objective was specified, then default to
// optimizing path length as computed by the distance() function
// in the state space.
if (pdef_)
{
if (pdef_->hasOptimizationObjective())
opt_ = pdef_->getOptimizationObjective();
else
{
OMPL_INFORM("%s: No optimization objective specified. Defaulting to optimizing path length for the allowed "
"planning time.",
getName().c_str());
opt_ = std::make_shared<base::PathLengthOptimizationObjective>(si_);
// Store the new objective in the problem def'n
pdef_->setOptimizationObjective(opt_);
}
mc_ = MotionCompare(opt_, pdef_);
q_ = BinaryHeap<Motion *, MotionCompare>(mc_);
}
else
{
OMPL_INFORM("%s: problem definition is not set, deferring setup completion...", getName().c_str());
setup_ = false;
}
// Calculate some constants:
calculateRewiringLowerBounds();
// Set the bestCost_ and prunedCost_ as infinite
bestCost_ = opt_->infiniteCost();
}
void ompl::control::SyclopRRT::setup()
{
Syclop::setup();
sampler_ = si_->allocStateSampler();
controlSampler_ = siC_->allocDirectedControlSampler();
lastGoalMotion_ = nullptr;
// Create a default GNAT nearest neighbors structure if the user doesn't want
// the default regionalNN check from the discretization
if (!nn_ && !regionalNN_)
{
nn_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Motion *>(this));
nn_->setDistanceFunction([this](Motion *a, const Motion *b)
{
return distanceFunction(a, b);
});
}
}
Vector3 AIElementGroupSwarm::rule2(AIElement* b)
{
//LogManager::getSingleton().logMessage("Swarm step.");
Vector3 res=Vector3::ZERO;
int i;
for (i=0; i!=nodes.size(); i++)
{
if (nodes[i]!=b)
{
Vector3 dist = nodes[i]->getPosition() - b->getPosition();
float force = distanceFunction(dist.length());
res-=(nodes[i]->getPosition() - b->getPosition())*(force);
}
}
return res/mParams.RULE2_BYPASS;
}
void ompl::geometric::TRRT::setup()
{
Planner::setup();
tools::SelfConfig selfConfig(si_, getName());
if (!pdef_ || !pdef_->hasOptimizationObjective())
{
OMPL_INFORM("%s: No optimization objective specified. Defaulting to mechanical work minimization.",
getName().c_str());
opt_ = std::make_shared<base::MechanicalWorkOptimizationObjective>(si_);
}
else
opt_ = pdef_->getOptimizationObjective();
// Set maximum distance a new node can be from its nearest neighbor
if (maxDistance_ < std::numeric_limits<double>::epsilon())
{
selfConfig.configurePlannerRange(maxDistance_);
maxDistance_ *= magic::COST_MAX_MOTION_LENGTH_AS_SPACE_EXTENT_FRACTION;
}
// Set the threshold that decides if a new node is a frontier node or non-frontier node
if (frontierThreshold_ < std::numeric_limits<double>::epsilon())
{
frontierThreshold_ = si_->getMaximumExtent() * 0.01;
OMPL_DEBUG("%s: Frontier threshold detected to be %lf", getName().c_str(), frontierThreshold_);
}
// Create the nearest neighbor function the first time setup is run
if (!nearestNeighbors_)
nearestNeighbors_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Motion *>(this));
// Set the distance function
nearestNeighbors_->setDistanceFunction([this](const Motion *a, const Motion *b)
{
return distanceFunction(a, b);
});
// Setup TRRT specific variables ---------------------------------------------------------
temp_ = initTemperature_;
nonfrontierCount_ = 1;
frontierCount_ = 1; // init to 1 to prevent division by zero error
bestCost_ = worstCost_ = opt_->identityCost();
}
ompl::control::SST::Witness* ompl::control::SST::findClosestWitness(ompl::control::SST::Motion *node)
{
if(witnesses_->size() > 0)
{
Witness* closest = static_cast<Witness*>(witnesses_->nearest(node));
if (distanceFunction(closest,node) > pruningRadius_)
{
closest = new Witness(siC_);
closest->linkRep(node);
si_->copyState(closest->state_, node->state_);
witnesses_->add(closest);
}
return closest;
}
else
{
Witness* closest = new Witness(siC_);
closest->linkRep(node);
si_->copyState(closest->state_, node->state_);
witnesses_->add(closest);
return closest;
}
}
ompl::base::PlannerStatus ompl::geometric::LazyLBTRRT::solve(const base::PlannerTerminationCondition &ptc)
{
checkValidity();
// update goal and check validity
base::Goal *goal = pdef_->getGoal().get();
auto *goal_s = dynamic_cast<base::GoalSampleableRegion *>(goal);
if (goal == nullptr)
{
OMPL_ERROR("%s: Goal undefined", getName().c_str());
return base::PlannerStatus::INVALID_GOAL;
}
while (const base::State *st = pis_.nextStart())
{
startMotion_ = createMotion(goal_s, st);
break;
}
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());
bool solved = false;
auto *rmotion = new Motion(si_);
base::State *xstate = si_->allocState();
goalMotion_ = createGoalMotion(goal_s);
CostEstimatorLb costEstimatorLb(goal, idToMotionMap_);
LPAstarLb_ = new LPAstarLb(startMotion_->id_, goalMotion_->id_, graphLb_, costEstimatorLb); // rooted at source
CostEstimatorApx costEstimatorApx(this);
LPAstarApx_ = new LPAstarApx(goalMotion_->id_, startMotion_->id_, graphApx_, costEstimatorApx); // rooted at target
double approxdif = std::numeric_limits<double>::infinity();
// e+e/d. K-nearest RRT*
double k_rrg = boost::math::constants::e<double>() +
boost::math::constants::e<double>() / (double)si_->getStateSpace()->getDimension();
////////////////////////////////////////////
// step (1) - RRT
////////////////////////////////////////////
bestCost_ = std::numeric_limits<double>::infinity();
rrt(ptc, goal_s, xstate, rmotion, approxdif);
if (!ptc())
{
solved = true;
////////////////////////////////////////////
// step (2) - Lazy construction of G_lb
////////////////////////////////////////////
idToMotionMap_.push_back(goalMotion_);
int k = getK(idToMotionMap_.size(), k_rrg);
std::vector<Motion *> nnVec;
nnVec.reserve(k);
BOOST_FOREACH (Motion *motion, idToMotionMap_)
{
nn_->nearestK(motion, k, nnVec);
BOOST_FOREACH (Motion *neighbor, nnVec)
if (neighbor->id_ != motion->id_ && !edgeExistsLb(motion, neighbor))
addEdgeLb(motion, neighbor, distanceFunction(motion, neighbor));
}
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;
}
void ompl::control::SyclopRRT::selectAndExtend(Region ®ion, std::vector<Motion *> &newMotions)
{
auto *rmotion = new Motion(siC_);
base::StateSamplerPtr sampler(si_->allocStateSampler());
std::vector<double> coord(decomp_->getDimension());
decomp_->sampleFromRegion(region.index, rng_, coord);
decomp_->sampleFullState(sampler, coord, rmotion->state);
Motion *nmotion;
if (regionalNN_)
{
/* Instead of querying the nearest neighbors datastructure over the entire tree of motions,
* here we perform a linear search over all motions in the selected region and its neighbors. */
std::vector<int> searchRegions;
decomp_->getNeighbors(region.index, searchRegions);
searchRegions.push_back(region.index);
std::vector<Motion *> motions;
for (const auto &i : searchRegions)
{
const std::vector<Motion *> ®ionMotions = getRegionFromIndex(i).motions;
motions.insert(motions.end(), regionMotions.begin(), regionMotions.end());
}
std::vector<Motion *>::const_iterator i = motions.begin();
nmotion = *i;
double minDistance = distanceFunction(rmotion, nmotion);
++i;
while (i != motions.end())
{
Motion *m = *i;
const double dist = distanceFunction(rmotion, m);
if (dist < minDistance)
{
nmotion = m;
minDistance = dist;
}
++i;
}
}
else
{
assert(nn_);
nmotion = nn_->nearest(rmotion);
}
unsigned int duration =
controlSampler_->sampleTo(rmotion->control, nmotion->control, nmotion->state, rmotion->state);
if (duration >= siC_->getMinControlDuration())
{
rmotion->steps = duration;
rmotion->parent = nmotion;
newMotions.push_back(rmotion);
if (nn_)
nn_->add(rmotion);
lastGoalMotion_ = rmotion;
}
else
{
si_->freeState(rmotion->state);
siC_->freeControl(rmotion->control);
delete rmotion;
}
}
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);
}
ompl::base::PlannerStatus ompl::geometric::RRTXstatic::solve(const base::PlannerTerminationCondition &ptc)
{
checkValidity();
base::Goal *goal = pdef_->getGoal().get();
auto *goal_s = dynamic_cast<base::GoalSampleableRegion *>(goal);
// Check if there are more starts
if (pis_.haveMoreStartStates() == true)
{
// There are, add them
while (const base::State *st = pis_.nextStart())
{
auto *motion = new Motion(si_);
si_->copyState(motion->state, st);
motion->cost = opt_->identityCost();
nn_->add(motion);
}
// And assure that, if we're using an informed sampler, it's reset
infSampler_.reset();
}
// No else
if (nn_->size() == 0)
{
OMPL_ERROR("%s: There are no valid initial states!", getName().c_str());
return base::PlannerStatus::INVALID_START;
}
// Allocate a sampler if necessary
if (!sampler_ && !infSampler_)
{
allocSampler();
}
OMPL_INFORM("%s: Starting planning with %u states already in datastructure", getName().c_str(), nn_->size());
if (!si_->getStateSpace()->isMetricSpace())
OMPL_WARN("%s: The state space (%s) is not metric and as a result the optimization objective may not satisfy "
"the triangle inequality. "
"You may need to disable rejection.",
getName().c_str(), si_->getStateSpace()->getName().c_str());
const base::ReportIntermediateSolutionFn intermediateSolutionCallback = pdef_->getIntermediateSolutionCallback();
Motion *solution = lastGoalMotion_;
Motion *approximation = nullptr;
double approximatedist = std::numeric_limits<double>::infinity();
bool sufficientlyShort = false;
auto *rmotion = new Motion(si_);
base::State *rstate = rmotion->state;
base::State *xstate = si_->allocState();
base::State *dstate;
Motion *motion;
Motion *nmotion;
Motion *nb;
Motion *min;
Motion *c;
bool feas;
unsigned int rewireTest = 0;
unsigned int statesGenerated = 0;
base::Cost incCost, cost;
if (solution)
OMPL_INFORM("%s: Starting planning with existing solution of cost %.5f", getName().c_str(),
solution->cost.value());
if (useKNearest_)
OMPL_INFORM("%s: Initial k-nearest value of %u", getName().c_str(),
(unsigned int)std::ceil(k_rrt_ * log((double)(nn_->size() + 1u))));
else
OMPL_INFORM(
"%s: Initial rewiring radius of %.2f", getName().c_str(),
std::min(maxDistance_, r_rrt_ * std::pow(log((double)(nn_->size() + 1u)) / ((double)(nn_->size() + 1u)),
1 / (double)(si_->getStateDimension()))));
while (ptc == false)
{
iterations_++;
// Computes the RRG values for this iteration (number or radius of neighbors)
calculateRRG();
// sample random state (with goal biasing)
// Goal samples are only sampled until maxSampleCount() goals are in the tree, to prohibit duplicate goal
// states.
if (goal_s && goalMotions_.size() < goal_s->maxSampleCount() && rng_.uniform01() < goalBias_ &&
goal_s->canSample())
goal_s->sampleGoal(rstate);
else
{
// Attempt to generate a sample, if we fail (e.g., too many rejection attempts), skip the remainder of this
// loop and return to try again
if (!sampleUniform(rstate))
continue;
}
// find closest state in the tree
nmotion = nn_->nearest(rmotion);
if (intermediateSolutionCallback && si_->equalStates(nmotion->state, rstate))
continue;
dstate = rstate;
// find state to add to the tree
double d = si_->distance(nmotion->state, rstate);
if (d > maxDistance_)
{
si_->getStateSpace()->interpolate(nmotion->state, rstate, maxDistance_ / d, xstate);
dstate = xstate;
}
// Check if the motion between the nearest state and the state to add is valid
if (si_->checkMotion(nmotion->state, dstate))
{
// create a motion
motion = new Motion(si_);
si_->copyState(motion->state, dstate);
motion->parent = nmotion;
incCost = opt_->motionCost(nmotion->state, motion->state);
motion->cost = opt_->combineCosts(nmotion->cost, incCost);
// Find nearby neighbors of the new motion
getNeighbors(motion);
// find which one we connect the new state to
for (auto it = motion->nbh.begin(); it != motion->nbh.end();)
{
nb = it->first;
feas = it->second;
// Compute cost using nb as a parent
incCost = opt_->motionCost(nb->state, motion->state);
cost = opt_->combineCosts(nb->cost, incCost);
if (opt_->isCostBetterThan(cost, motion->cost))
{
// Check range and feasibility
if ((!useKNearest_ || distanceFunction(motion, nb) < maxDistance_) &&
si_->checkMotion(nb->state, motion->state))
{
// mark than the motino has been checked as valid
it->second = true;
motion->cost = cost;
motion->parent = nb;
++it;
}
else
{
// Remove unfeasible neighbor from the list of neighbors
it = motion->nbh.erase(it);
}
}
else
{
++it;
}
}
// Check if the vertex should included
if (!includeVertex(motion))
{
si_->freeState(motion->state);
delete motion;
continue;
}
// Update neighbor motions neighbor datastructure
for (auto it = motion->nbh.begin(); it != motion->nbh.end(); ++it)
{
it->first->nbh.emplace_back(motion, it->second);
}
// add motion to the tree
++statesGenerated;
nn_->add(motion);
if (updateChildren_)
motion->parent->children.push_back(motion);
// add the new motion to the queue to propagate the changes
updateQueue(motion);
bool checkForSolution = false;
// Add the new motion to the goalMotion_ list, if it satisfies the goal
double distanceFromGoal;
if (goal->isSatisfied(motion->state, &distanceFromGoal))
{
goalMotions_.push_back(motion);
checkForSolution = true;
}
// Process the elements in the queue and rewire the tree until epsilon-optimality
while (!q_.empty())
{
// Get element to update
min = q_.top()->data;
// Remove element from the queue and NULL the handle so that we know it's not in the queue anymore
q_.pop();
min->handle = nullptr;
// Stop cost propagation if it is not in the relevant region
if (opt_->isCostBetterThan(bestCost_, mc_.costPlusHeuristic(min)))
break;
// Try min as a parent to optimize each neighbor
for (auto it = min->nbh.begin(); it != min->nbh.end();)
{
nb = it->first;
feas = it->second;
// Neighbor culling: removes neighbors farther than the neighbor radius
if ((!useKNearest_ || min->nbh.size() > rrg_k_) && distanceFunction(min, nb) > rrg_r_)
{
it = min->nbh.erase(it);
continue;
}
// Calculate cost of nb using min as a parent
incCost = opt_->motionCost(min->state, nb->state);
cost = opt_->combineCosts(min->cost, incCost);
// If cost improvement is better than epsilon
if (opt_->isCostBetterThan(opt_->combineCosts(cost, epsilonCost_), nb->cost))
{
if (nb->parent != min)
{
// changing parent, check feasibility
if (!feas)
{
feas = si_->checkMotion(nb->state, min->state);
if (!feas)
{
// Remove unfeasible neighbor from the list of neighbors
it = min->nbh.erase(it);
continue;
}
else
{
// mark than the motino has been checked as valid
it->second = true;
}
}
if (updateChildren_)
{
// Remove this node from its parent list
removeFromParent(nb);
// add it as a children of min
min->children.push_back(nb);
}
// Add this node to the new parent
nb->parent = min;
++rewireTest;
}
nb->cost = cost;
// Add to the queue for more improvements
updateQueue(nb);
checkForSolution = true;
}
++it;
}
if (updateChildren_)
{
// Propagatino of the cost to the children
for (auto it = min->children.begin(), end = min->children.end(); it != end; ++it)
{
c = *it;
incCost = opt_->motionCost(min->state, c->state);
cost = opt_->combineCosts(min->cost, incCost);
c->cost = cost;
// Add to the queue for more improvements
updateQueue(c);
checkForSolution = true;
}
}
}
// empty q and reset handles
while (!q_.empty())
{
q_.top()->data->handle = nullptr;
q_.pop();
}
q_.clear();
// Checking for solution or iterative improvement
if (checkForSolution)
{
bool updatedSolution = false;
for (auto &goalMotion : goalMotions_)
{
if (opt_->isCostBetterThan(goalMotion->cost, bestCost_))
{
if (opt_->isFinite(bestCost_) == false)
{
OMPL_INFORM("%s: Found an initial solution with a cost of %.2f in %u iterations (%u "
"vertices in the graph)",
getName().c_str(), goalMotion->cost.value(), iterations_, nn_->size());
}
bestCost_ = goalMotion->cost;
updatedSolution = true;
}
sufficientlyShort = opt_->isSatisfied(goalMotion->cost);
if (sufficientlyShort)
{
solution = goalMotion;
break;
}
else if (!solution || opt_->isCostBetterThan(goalMotion->cost, solution->cost))
{
solution = goalMotion;
updatedSolution = true;
}
}
if (updatedSolution)
{
if (intermediateSolutionCallback)
{
std::vector<const base::State *> spath;
Motion *intermediate_solution =
solution->parent; // Do not include goal state to simplify code.
// Push back until we find the start, but not the start itself
while (intermediate_solution->parent != nullptr)
{
spath.push_back(intermediate_solution->state);
intermediate_solution = intermediate_solution->parent;
}
intermediateSolutionCallback(this, spath, bestCost_);
}
}
}
// Checking for approximate solution (closest state found to the goal)
if (goalMotions_.size() == 0 && distanceFromGoal < approximatedist)
{
approximation = motion;
approximatedist = distanceFromGoal;
}
}
// terminate if a sufficient solution is found
if (solution && sufficientlyShort)
break;
}
bool approximate = (solution == nullptr);
bool addedSolution = false;
if (approximate)
solution = approximation;
else
lastGoalMotion_ = solution;
if (solution != nullptr)
{
ptc.terminate();
// construct the solution path
std::vector<Motion *> mpath;
while (solution != nullptr)
{
mpath.push_back(solution);
solution = solution->parent;
}
// set the solution path
auto path = std::make_shared<PathGeometric>(si_);
for (int i = mpath.size() - 1; i >= 0; --i)
path->append(mpath[i]->state);
// Add the solution path.
base::PlannerSolution psol(path);
psol.setPlannerName(getName());
if (approximate)
psol.setApproximate(approximatedist);
// Does the solution satisfy the optimization objective?
psol.setOptimized(opt_, bestCost_, sufficientlyShort);
pdef_->addSolutionPath(psol);
addedSolution = true;
}
si_->freeState(xstate);
if (rmotion->state)
si_->freeState(rmotion->state);
delete rmotion;
OMPL_INFORM("%s: Created %u new states. Checked %u rewire options. %u goal states in tree. Final solution cost "
"%.3f",
getName().c_str(), statesGenerated, rewireTest, goalMotions_.size(), bestCost_.value());
return {addedSolution, approximate};
}
