void ompl::base::GoalLazySamples::goalSamplingThread()
{
if (!si_->isSetup())
{
OMPL_DEBUG("Waiting for space information to be set up before the sampling thread can begin computation...");
// wait for everything to be set up before performing computation
while (!terminateSamplingThread_ && !si_->isSetup())
std::this_thread::sleep_for(time::seconds(0.01));
}
unsigned int prevsa = samplingAttempts_;
if (!terminateSamplingThread_ && samplerFunc_)
{
OMPL_DEBUG("Beginning sampling thread computation");
ScopedState<> s(si_);
while (!terminateSamplingThread_ && samplerFunc_(this, s.get()))
{
++samplingAttempts_;
if (si_->satisfiesBounds(s.get()) && si_->isValid(s.get()))
addStateIfDifferent(s.get(), minDist_);
}
}
else
OMPL_WARN("Goal sampling thread never did any work.%s",
samplerFunc_ ? (si_->isSetup() ? "" : " Space information not set up.") : " No sampling function "
"set.");
terminateSamplingThread_ = true;
OMPL_DEBUG("Stopped goal sampling thread after %u sampling attempts", samplingAttempts_ - prevsa);
}
ompl::base::PlannerStatus ompl::tools::OptimizePlan::solve(double solveTime, unsigned int maxSol, unsigned int nthreads)
{
time::point end = time::now() + time::seconds(solveTime);
unsigned int nt = std::min(nthreads, (unsigned int)planners_.size());
OMPL_DEBUG("Using %u threads", nt);
base::PlannerStatus result;
unsigned int np = 0;
const base::ProblemDefinitionPtr &pdef = getProblemDefinition();
pp_.clearHybridizationPaths();
while (time::now() < end)
{
pp_.clearPlanners();
for (unsigned int i = 0; i < nt; ++i)
{
planners_[np]->clear();
pp_.addPlanner(planners_[np]);
np = (np + 1) % planners_.size();
}
base::PlannerStatus localResult = pp_.solve(std::max(time::seconds(end - time::now()), 0.0), true);
if (localResult)
{
if (result != base::PlannerStatus::EXACT_SOLUTION)
result = localResult;
if (!pdef->hasOptimizationObjective())
{
OMPL_DEBUG("Terminating early since there is no optimization objective specified");
break;
}
base::Cost obj_cost = pdef->getSolutionPath()->cost(pdef->getOptimizationObjective());
if (pdef->getOptimizationObjective()->isSatisfied(obj_cost))
{
OMPL_DEBUG("Terminating early since solution path satisfies the optimization objective");
break;
}
if (pdef->getSolutionCount() >= maxSol)
{
OMPL_DEBUG("Terminating early since %u solutions were generated", maxSol);
break;
}
}
}
// if we have more time, and we have a geometric path, we try to simplify it
if (time::now() < end && result)
{
geometric::PathGeometric *p = dynamic_cast<geometric::PathGeometric *>(pdef->getSolutionPath().get());
if (p)
{
geometric::PathSimplifier ps(getProblemDefinition()->getSpaceInformation());
ps.simplify(*p, std::max(time::seconds(end - time::now()), 0.0));
}
}
return result;
}
bool ompl::base::ProblemDefinition::fixInvalidInputState(State *state, double dist, bool start, unsigned int attempts)
{
bool result = false;
bool b = si_->satisfiesBounds(state);
bool v = false;
if (b)
{
v = si_->isValid(state);
if (!v)
OMPL_DEBUG("%s state is not valid", start ? "Start" : "Goal");
}
else
OMPL_DEBUG("%s state is not within space bounds", start ? "Start" : "Goal");
if (!b || !v)
{
std::stringstream ss;
si_->printState(state, ss);
ss << " within distance " << dist;
OMPL_DEBUG("Attempting to fix %s state %s", start ? "start" : "goal", ss.str().c_str());
State *temp = si_->allocState();
if (si_->searchValidNearby(temp, state, dist, attempts))
{
si_->copyState(state, temp);
result = true;
}
else
OMPL_WARN("Unable to fix %s state", start ? "start" : "goal");
si_->freeState(temp);
}
return result;
}
void ompl::tools::LightningDB::getAllPlannerDatas(std::vector<ompl::base::PlannerDataPtr> &plannerDatas) const
{
OMPL_DEBUG("LightningDB: getAllPlannerDatas");
// Convert the NN tree to a vector
nn_->list(plannerDatas);
OMPL_DEBUG("Number of paths found: %d", plannerDatas.size());
}
void ompl::geometric::CForest::solve(base::Planner *planner, const base::PlannerTerminationCondition &ptc)
{
OMPL_DEBUG("Starting %s", planner->getName().c_str());
time::point start = time::now();
if (planner->solve(ptc))
{
double duration = time::seconds(time::now() - start);
OMPL_DEBUG("Solution found by %s in %lf seconds", planner->getName().c_str(), duration);
}
}
static void nearCallback(void *data, dGeomID o1, dGeomID o2)
{
// if a collision has not already been detected
if (!reinterpret_cast<CallbackParam *>(data)->collision)
{
dBodyID b1 = dGeomGetBody(o1);
dBodyID b2 = dGeomGetBody(o2);
if ((b1 != nullptr) && (b2 != nullptr) && (dAreConnectedExcluding(b1, b2, dJointTypeContact) != 0))
return;
dContact contact[1]; // one contact is sufficient
int numc = dCollide(o1, o2, 1, &contact[0].geom, sizeof(dContact));
// check if there is really a collision
if (numc != 0)
{
// check if the collision is allowed
bool valid = reinterpret_cast<CallbackParam *>(data)->env->isValidCollision(o1, o2, contact[0]);
reinterpret_cast<CallbackParam *>(data)->collision = !valid;
if (reinterpret_cast<CallbackParam *>(data)->env->verboseContacts_)
{
OMPL_DEBUG("%s contact between %s and %s", (valid ? "Valid" : "Invalid"),
reinterpret_cast<CallbackParam *>(data)->env->getGeomName(o1).c_str(),
reinterpret_cast<CallbackParam *>(data)->env->getGeomName(o2).c_str());
}
}
}
}
void ompl::geometric::FMT::assureGoalIsSampled(const ompl::base::GoalSampleableRegion *goal)
{
// Ensure that there is at least one node near each goal
while (const base::State *goalState = pis_.nextGoal())
{
Motion *gMotion = new Motion(si_);
si_->copyState(gMotion->getState(), goalState);
std::vector<Motion*> nearGoal;
nn_->nearestR(gMotion, goal->getThreshold(), nearGoal);
// If there is no node in the goal region, insert one
if (nearGoal.empty())
{
OMPL_DEBUG("No state inside goal region");
if (si_->getStateValidityChecker()->isValid(gMotion->getState()))
{
nn_->add(gMotion);
goalState_ = gMotion->getState();
}
else
{
si_->freeState(gMotion->getState());
delete gMotion;
}
}
else // There is already a sample in the goal region
{
goalState_ = nearGoal[0]->getState();
si_->freeState(gMotion->getState());
delete gMotion;
}
} // For each goal
}
const ompl::base::State *ompl::base::PlannerInputStates::nextStart()
{
if (pdef_ == nullptr || si_ == nullptr)
{
std::string error = "Missing space information or problem definition";
if (planner_ != nullptr)
throw Exception(planner_->getName(), error);
else
throw Exception(error);
}
while (addedStartStates_ < pdef_->getStartStateCount())
{
const base::State *st = pdef_->getStartState(addedStartStates_);
addedStartStates_++;
bool bounds = si_->satisfiesBounds(st);
bool valid = bounds ? si_->isValid(st) : false;
if (bounds && valid)
return st;
OMPL_WARN("%s: Skipping invalid start state (invalid %s)",
planner_ ? planner_->getName().c_str() : "PlannerInputStates", bounds ? "state" : "bounds");
std::stringstream ss;
si_->printState(st, ss);
OMPL_DEBUG("%s: Discarded start state %s", planner_ ? planner_->getName().c_str() : "PlannerInputStates",
ss.str().c_str());
}
return nullptr;
}
bool ompl::control::KPIECE1::selectMotion(Motion* &smotion, Grid::Cell* &scell)
{
scell = rng_.uniform01() < std::max(selectBorderFraction_, tree_.grid.fracExternal()) ?
tree_.grid.topExternal() : tree_.grid.topInternal();
// We are running on finite precision, so our update scheme will end up
// with 0 values for the score. This is where we fix the problem
if (scell->data->score < std::numeric_limits<double>::epsilon())
{
OMPL_DEBUG("Numerical precision limit reached. Resetting costs.");
std::vector<CellData*> content;
content.reserve(tree_.grid.size());
tree_.grid.getContent(content);
for (std::vector<CellData*>::iterator it = content.begin() ; it != content.end() ; ++it)
(*it)->score += 1.0 + log((double)((*it)->iteration));
tree_.grid.updateAll();
}
if (scell && !scell->data->motions.empty())
{
scell->data->selections++;
smotion = scell->data->motions[rng_.halfNormalInt(0, scell->data->motions.size() - 1)];
return true;
}
else
return false;
}
void ompl::base::GoalLazySamples::goalSamplingThread()
{
{
/* Wait for startSampling() to finish assignment
* samplingThread_ */
std::lock_guard<std::mutex> slock(lock_);
}
if (!si_->isSetup()) // this looks racy
{
OMPL_DEBUG("Waiting for space information to be set up before the sampling thread can begin computation...");
// wait for everything to be set up before performing computation
while (!terminateSamplingThread_ && !si_->isSetup())
std::this_thread::sleep_for(time::seconds(0.01));
}
unsigned int prevsa = samplingAttempts_;
if (isSampling() && samplerFunc_)
{
OMPL_DEBUG("Beginning sampling thread computation");
ScopedState<> s(si_);
while (isSampling() && samplerFunc_(this, s.get()))
{
++samplingAttempts_;
if (si_->satisfiesBounds(s.get()) && si_->isValid(s.get()))
{
OMPL_DEBUG("Adding goal state");
addStateIfDifferent(s.get(), minDist_);
}
else
{
OMPL_DEBUG("Invalid goal candidate");
}
}
}
else
OMPL_WARN("Goal sampling thread never did any work.%s",
samplerFunc_ ? (si_->isSetup() ? "" : " Space information not set up.") : " No sampling function "
"set.");
{
std::lock_guard<std::mutex> slock(lock_);
terminateSamplingThread_ = true;
}
OMPL_DEBUG("Stopped goal sampling thread after %u sampling attempts", samplingAttempts_ - prevsa);
}
void ompl::base::GoalLazySamples::startSampling()
{
if (samplingThread_ == nullptr)
{
OMPL_DEBUG("Starting goal sampling thread");
terminateSamplingThread_ = false;
samplingThread_ = new std::thread(&GoalLazySamples::goalSamplingThread, this);
}
}
void configurePlannerRange(double &range, const std::string &context)
{
if (range < std::numeric_limits<double>::epsilon())
{
base::SpaceInformationPtr si = wsi_.lock();
if (si)
{
range = si->getMaximumExtent() * magic::MAX_MOTION_LENGTH_AS_SPACE_EXTENT_FRACTION;
OMPL_DEBUG("%sPlanner range detected to be %lf", context.c_str(), range);
}
else
OMPL_ERROR("%sUnable to detect planner range. SpaceInformation instance has expired.", context.c_str());
}
}
void ompl::base::GoalLazySamples::stopSampling()
{
if (isSampling())
{
OMPL_DEBUG("Attempting to stop goal sampling thread...");
terminateSamplingThread_ = true;
samplingThread_->join();
delete samplingThread_;
samplingThread_ = nullptr;
}
else if (samplingThread_)
{ // join a finished thread
samplingThread_->join();
delete samplingThread_;
samplingThread_ = nullptr;
}
}
void ompl::geometric::PathSimplifier::simplify(PathGeometric &path, const base::PlannerTerminationCondition &ptc)
{
if (path.getStateCount() < 3)
return;
// try a randomized step of connecting vertices
bool tryMore = false;
if (ptc == false)
tryMore = reduceVertices(path);
// try to collapse close-by vertices
if (ptc == false)
collapseCloseVertices(path);
// try to reduce verices some more, if there is any point in doing so
int times = 0;
while (tryMore && ptc == false && ++times <= 5)
tryMore = reduceVertices(path);
// if the space is metric, we can do some additional smoothing
if(si_->getStateSpace()->isMetricSpace())
{
bool tryMore = true;
unsigned int times = 0;
do
{
bool shortcut = shortcutPath(path); // split path segments, not just vertices
bool better_goal = gsr_ ? findBetterGoal(path, ptc) : false; // Try to connect the path to a closer goal
tryMore = shortcut || better_goal;
} while(ptc == false && tryMore && ++times <= 5);
// smooth the path with BSpline interpolation
if(ptc == false)
smoothBSpline(path, 3, path.length()/100.0);
// we always run this if the metric-space algorithms were run. In non-metric spaces this does not work.
const std::pair<bool, bool> &p = path.checkAndRepair(magic::MAX_VALID_SAMPLE_ATTEMPTS);
if (!p.second)
OMPL_WARN("Solution path may slightly touch on an invalid region of the state space");
else
if (!p.first)
OMPL_DEBUG("The solution path was slightly touching on an invalid region of the state space, but it was successfully fixed.");
}
}
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();
}
void ompl::geometric::BiTRRT::setup()
{
Planner::setup();
tools::SelfConfig sc(si_, getName());
// Configuring the range of the planner
if (maxDistance_ < std::numeric_limits<double>::epsilon())
{
sc.configurePlannerRange(maxDistance_);
maxDistance_ *= magic::COST_MAX_MOTION_LENGTH_AS_SPACE_EXTENT_FRACTION;
}
// Configuring nearest neighbors structures for the planning trees
if (!tStart_)
tStart_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Motion*>(this));
if (!tGoal_)
tGoal_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Motion*>(this));
tStart_->setDistanceFunction(boost::bind(&BiTRRT::distanceFunction, this, _1, _2));
tGoal_->setDistanceFunction(boost::bind(&BiTRRT::distanceFunction, this, _1, _2));
// Setup the optimization objective, if it isn't specified
if (!pdef_ || !pdef_->hasOptimizationObjective())
{
OMPL_INFORM("%s: No optimization objective specified. Defaulting to mechanical work minimization.", getName().c_str());
opt_.reset(new base::MechanicalWorkOptimizationObjective(si_));
}
else
opt_ = pdef_->getOptimizationObjective();
// 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_);
}
// initialize TRRT specific variables
temp_ = initTemperature_;
nonfrontierCount_ = 1;
frontierCount_ = 1; // init to 1 to prevent division by zero error
bestCost_ = worstCost_ = opt_->identityCost();
connectionRange_ = 10.0 * si_->getStateSpace()->getLongestValidSegmentLength();
}
void ompl::base::GoalLazySamples::stopSampling()
{
/* Set termination flag */
{
std::lock_guard<std::mutex> slock(lock_);
if (!terminateSamplingThread_)
{
OMPL_DEBUG("Attempting to stop goal sampling thread...");
terminateSamplingThread_ = true;
}
}
/* Join thread */
if (samplingThread_ != nullptr)
{
samplingThread_->join();
delete samplingThread_;
samplingThread_ = nullptr;
}
}
void ompl::geometric::LightningRetrieveRepair::setup()
{
Planner::setup();
// Setup repair planner (for use by the rrPlanner)
// Note: does not use the same pdef as the main planner in this class
if (!repairPlanner_)
{
// Set the repair planner
repairPlanner_.reset(new ompl::geometric::RRTConnect(si_));
OMPL_DEBUG("LightningRetrieveRepair: No repairing planner specified. Using default: %s", repairPlanner_->getName().c_str() );
}
// Setup the problem definition for the repair planner
repairProblemDef_->setOptimizationObjective(pdef_->getOptimizationObjective()); // copy primary problem def
// Setup repair planner
repairPlanner_->setProblemDefinition(repairProblemDef_);
if (!repairPlanner_->isSetup())
repairPlanner_->setup();
}
void nearCallback(void *data, dGeomID o1, dGeomID o2)
{
dBodyID b1 = dGeomGetBody(o1);
dBodyID b2 = dGeomGetBody(o2);
if (b1 && b2 && dAreConnectedExcluding(b1, b2, dJointTypeContact)) return;
CallbackParam *cp = reinterpret_cast<CallbackParam*>(data);
const unsigned int maxContacts = cp->env->getMaxContacts(o1, o2);
if (maxContacts <= 0) return;
dContact *contact = new dContact[maxContacts];
for (unsigned int i = 0; i < maxContacts; ++i)
cp->env->setupContact(o1, o2, contact[i]);
if (int numc = dCollide(o1, o2, maxContacts, &contact[0].geom, sizeof(dContact)))
{
for (int i = 0; i < numc; ++i)
{
dJointID c = dJointCreateContact(cp->env->world_, cp->env->contactGroup_, contact + i);
dJointAttach(c, b1, b2);
bool valid = cp->env->isValidCollision(o1, o2, contact[i]);
if (!valid)
cp->collision = true;
if (cp->env->verboseContacts_)
{
OMPL_DEBUG("%s contact between %s and %s", (valid ? "Valid" : "Invalid"),
cp->env->getGeomName(o1).c_str(), cp->env->getGeomName(o1).c_str());
}
}
}
delete[] contact;
}
bool ompl::geometric::LightningRetrieveRepair::findBestPath(const base::State *startState, const base::State *goalState, ompl::base::PlannerDataPtr &chosenPath)
{
OMPL_INFORM("LightningRetrieveRepair: Found %d similar paths. Filtering", nearestPaths_.size());
// Filter down to just 1 chosen path
ompl::base::PlannerDataPtr bestPath = nearestPaths_.front();
std::size_t bestPathScore = std::numeric_limits<std::size_t>::max();
// Track which path has the shortest distance
std::vector<double> distances(nearestPaths_.size(), 0);
std::vector<bool> isReversed(nearestPaths_.size());
assert(isReversed.size() == nearestPaths_.size());
for (std::size_t pathID = 0; pathID < nearestPaths_.size(); ++pathID)
{
const ompl::base::PlannerDataPtr ¤tPath = nearestPaths_[pathID];
// Error check
if (currentPath->numVertices() < 2) // needs at least a start and a goal
{
OMPL_ERROR("A path was recalled that somehow has less than 2 vertices, which shouldn't happen");
return false;
}
const ompl::base::State *pathStartState = currentPath->getVertex(0).getState();
const ompl::base::State *pathGoalState = currentPath->getVertex(currentPath->numVertices()-1).getState();
double regularDistance = si_->distance(startState,pathStartState) + si_->distance(goalState,pathGoalState);
double reversedDistance = si_->distance(startState,pathGoalState) + si_->distance(goalState,pathStartState);
// Check if path is reversed from normal [start->goal] direction and cache the distance
if ( regularDistance > reversedDistance )
{
// The distance between starts and goals is less when in reverse
isReversed[pathID] = true;
distances[pathID] = reversedDistance;
// We won't actually flip it until later to save memory operations and not alter our NN tree in the LightningDB
}
else
{
isReversed[pathID] = false;
distances[pathID] = regularDistance;
}
std::size_t pathScore = 0; // the score
// Check the validity between our start location and the path's start
// TODO: this might bias the score to be worse for the little connecting segment
if (!isReversed[pathID])
pathScore += checkMotionScore(startState, pathStartState);
else
pathScore += checkMotionScore(startState, pathGoalState);
// Score current path for validity
std::size_t invalidStates = 0;
for (std::size_t vertex_id = 0; vertex_id < currentPath->numVertices(); ++vertex_id)
{
// Check if the sampled points are valid
if (!si_->isValid( currentPath->getVertex(vertex_id).getState()))
{
invalidStates++;
}
}
// Track separate for debugging
pathScore += invalidStates;
// Check the validity between our goal location and the path's goal
// TODO: this might bias the score to be worse for the little connecting segment
if (!isReversed[pathID])
pathScore += checkMotionScore( goalState, pathGoalState );
else
pathScore += checkMotionScore( goalState, pathStartState );
// Factor in the distance between start/goal and our new start/goal
OMPL_INFORM("LightningRetrieveRepair: Path %d | %d verticies | %d invalid | score %d | reversed: %s | distance: %f",
int(pathID), currentPath->numVertices(), invalidStates, pathScore,
isReversed[pathID] ? "true" : "false", distances[pathID]);
// Check if we have a perfect score (0) and this is the shortest path (the first one)
if (pathID == 0 && pathScore == 0)
{
OMPL_DEBUG("LightningRetrieveRepair: --> The shortest path (path 0) has a perfect score (0), ending filtering early.");
bestPathScore = pathScore;
bestPath = currentPath;
nearestPathsChosenID_ = pathID;
break; // end the for loop
}
// Check if this is the best score we've seen so far
if (pathScore < bestPathScore)
{
OMPL_DEBUG("LightningRetrieveRepair: --> This path is the best we've seen so far. Previous best: %d", bestPathScore);
bestPathScore = pathScore;
bestPath = currentPath;
nearestPathsChosenID_ = pathID;
}
// if the best score is the same as a previous one we've seen,
// choose the one that has the shortest connecting component
else if (pathScore == bestPathScore && distances[nearestPathsChosenID_] > distances[pathID])
{
// This new path is a shorter distance
OMPL_DEBUG("LightningRetrieveRepair: --> This path is as good as the best we've seen so far, but its path is shorter. Previous best score: %d from index %d",
bestPathScore, nearestPathsChosenID_);
bestPathScore = pathScore;
bestPath = currentPath;
nearestPathsChosenID_ = pathID;
}
else
OMPL_DEBUG("LightningRetrieveRepair: --> Not best. Best score: %d from index %d", bestPathScore, nearestPathsChosenID_);
}
// Check if we have a solution
if (!bestPath)
{
OMPL_ERROR("LightningRetrieveRepair: No best path found from k filtered paths");
return false;
}
else if(!bestPath->numVertices() || bestPath->numVertices() == 1)
{
OMPL_ERROR("LightningRetrieveRepair: Only %d verticies found in PlannerData loaded from file. This is a bug.", bestPath->numVertices());
return false;
}
// Reverse the path if necessary. We allocate memory for this so that we don't alter the database
if (isReversed[nearestPathsChosenID_])
{
OMPL_DEBUG("LightningRetrieveRepair: Reversing planner data verticies count %d", bestPath->numVertices());
ompl::base::PlannerDataPtr newPath(new ompl::base::PlannerData(si_));
for (std::size_t i = bestPath->numVertices(); i > 0; --i) // size_t can't go negative so subtract 1 instead
{
newPath->addVertex( bestPath->getVertex(i-1) );
}
// Set result
chosenPath = newPath;
}
else
{
// Set result
chosenPath = bestPath;
}
OMPL_DEBUG("LightningRetrieveRepair: Done Filtering\n");
return true;
}
bool ompl::geometric::LightningRetrieveRepair::repairPath(const base::PlannerTerminationCondition &ptc,
ompl::geometric::PathGeometric &primaryPath)
{
// \todo: we should reuse our collision checking from the previous step to make this faster
OMPL_INFORM("LightningRetrieveRepair: Repairing path");
// Error check
if (primaryPath.getStateCount() < 2)
{
OMPL_ERROR("LightningRetrieveRepair: Cannot repair a path with less than 2 states");
return false;
}
// Loop through every pair of states and make sure path is valid.
// If not, replan between those states
for (std::size_t toID = 1; toID < primaryPath.getStateCount(); ++toID)
{
std::size_t fromID = toID - 1; // this is our last known valid state
ompl::base::State *fromState = primaryPath.getState(fromID);
ompl::base::State *toState = primaryPath.getState(toID);
// Check if our planner is out of time
if (ptc == true)
{
OMPL_DEBUG("LightningRetrieveRepair: Repair path function interrupted because termination condition is true.");
return false;
}
// Check path between states
if (!si_->checkMotion(fromState, toState))
{
// Path between (from, to) states not valid, but perhaps to STATE is
// Search until next valid STATE is found in existing path
std::size_t subsearchID = toID;
ompl::base::State *new_to;
OMPL_DEBUG("LightningRetrieveRepair: Searching for next valid state, because state %d to %d was not valid out %d total states",
fromID,toID,primaryPath.getStateCount());
while (subsearchID < primaryPath.getStateCount())
{
new_to = primaryPath.getState(subsearchID);
if (si_->isValid(new_to))
{
OMPL_DEBUG("LightningRetrieveRepair: State %d was found to valid, we can now repair between states", subsearchID);
// This future state is valid, we can stop searching
toID = subsearchID;
toState = new_to;
break;
}
++subsearchID; // keep searching for a new state to plan to
}
// Check if we ever found a next state that is valid
if (subsearchID >= primaryPath.getStateCount())
{
// We never found a valid state to plan to, instead we reached the goal state and it too wasn't valid. This is bad.
// I think this is a bug.
OMPL_ERROR("LightningRetrieveRepair: No state was found valid in the remainder of the path. Invalid goal state. This should not happen.");
return false;
}
// Plan between our two valid states
PathGeometric newPathSegment(si_);
// Not valid motion, replan
OMPL_DEBUG("LightningRetrieveRepair: Planning from %d to %d", fromID, toID);
if (!replan(fromState, toState, newPathSegment, ptc))
{
OMPL_INFORM("LightningRetrieveRepair: Unable to repair path between state %d and %d", fromID, toID);
return false;
}
// TODO make sure not approximate solution
// Reference to the path
std::vector<base::State*> &primaryPathStates = primaryPath.getStates();
// Remove all invalid states between (fromID, toID) - not including those states themselves
while (fromID != toID - 1)
{
OMPL_INFORM("LightningRetrieveRepair: Deleting state %d", fromID + 1);
primaryPathStates.erase(primaryPathStates.begin() + fromID + 1);
--toID; // because vector has shrunk
OMPL_INFORM("LightningRetrieveRepair: toID is now %d", toID);
}
// Insert new path segment into current path
OMPL_DEBUG("LightningRetrieveRepair: Inserting new %d states into old path. Previous length: %d",
newPathSegment.getStateCount()-2, primaryPathStates.size());
// Note: skip first and last states because they should be same as our start and goal state, same as `fromID` and `toID`
for (std::size_t i = 1; i < newPathSegment.getStateCount() - 1; ++i)
{
std::size_t insertLocation = toID + i - 1;
OMPL_DEBUG("LightningRetrieveRepair: Inserting newPathSegment state %d into old path at position %d",
i, insertLocation);
primaryPathStates.insert(primaryPathStates.begin() + insertLocation,
si_->cloneState(newPathSegment.getStates()[i]) );
}
OMPL_DEBUG("LightningRetrieveRepair: Inserted new states into old path. New length: %d", primaryPathStates.size());
// Set the toID to jump over the newly inserted states to the next unchecked state. Subtract 2 because we ignore start and goal
toID = toID + newPathSegment.getStateCount() - 2;
OMPL_DEBUG("LightningRetrieveRepair: Continuing searching at state %d", toID);
}
}
OMPL_INFORM("LightningRetrieveRepair: Done repairing");
return true;
}
const ompl::base::State *ompl::base::PlannerInputStates::nextGoal(const PlannerTerminationCondition &ptc)
{
if (pdef_ == nullptr || si_ == nullptr)
{
std::string error = "Missing space information or problem definition";
if (planner_ != nullptr)
throw Exception(planner_->getName(), error);
else
throw Exception(error);
}
const GoalSampleableRegion *goal =
pdef_->getGoal()->hasType(GOAL_SAMPLEABLE_REGION) ? pdef_->getGoal()->as<GoalSampleableRegion>() : nullptr;
if (goal != nullptr)
{
time::point start_wait;
bool first = true;
bool attempt = true;
while (attempt)
{
attempt = false;
if (sampledGoalsCount_ < goal->maxSampleCount() && goal->canSample())
{
if (tempState_ == nullptr)
tempState_ = si_->allocState();
do
{
goal->sampleGoal(tempState_);
sampledGoalsCount_++;
bool bounds = si_->satisfiesBounds(tempState_);
bool valid = bounds ? si_->isValid(tempState_) : false;
if (bounds && valid)
{
if (!first) // if we waited, show how long
{
OMPL_DEBUG("%s: Waited %lf seconds for the first goal sample.",
planner_ ? planner_->getName().c_str() : "PlannerInputStates",
time::seconds(time::now() - start_wait));
}
return tempState_;
}
OMPL_WARN("%s: Skipping invalid goal state (invalid %s)",
planner_ ? planner_->getName().c_str() : "PlannerInputStates",
bounds ? "state" : "bounds");
std::stringstream ss;
si_->printState(tempState_, ss);
OMPL_DEBUG("%s: Discarded goal state:\n%s",
planner_ ? planner_->getName().c_str() : "PlannerInputStates", ss.str().c_str());
} while (!ptc && sampledGoalsCount_ < goal->maxSampleCount() && goal->canSample());
}
if (goal->couldSample() && !ptc)
{
if (first)
{
first = false;
start_wait = time::now();
OMPL_DEBUG("%s: Waiting for goal region samples ...",
planner_ ? planner_->getName().c_str() : "PlannerInputStates");
}
std::this_thread::sleep_for(time::seconds(0.01));
attempt = !ptc;
}
}
}
return nullptr;
}
ompl::base::PlannerStatus ompl::geometric::FMT::solve(const base::PlannerTerminationCondition &ptc)
{
if (lastGoalMotion_) {
OMPL_INFORM("solve() called before clear(); returning previous solution");
traceSolutionPathThroughTree(lastGoalMotion_);
OMPL_DEBUG("Final path cost: %f", lastGoalMotion_->getCost().value());
return base::PlannerStatus(true, false);
}
else if (Open_.size() > 0)
{
OMPL_INFORM("solve() called before clear(); no previous solution so starting afresh");
clear();
}
checkValidity();
base::GoalSampleableRegion *goal = dynamic_cast<base::GoalSampleableRegion*>(pdef_->getGoal().get());
Motion *initMotion = nullptr;
if (!goal)
{
OMPL_ERROR("%s: Unknown type of goal", getName().c_str());
return base::PlannerStatus::UNRECOGNIZED_GOAL_TYPE;
}
// Add start states to V (nn_) and Open
while (const base::State *st = pis_.nextStart())
{
initMotion = new Motion(si_);
si_->copyState(initMotion->getState(), st);
Open_.insert(initMotion);
initMotion->setSetType(Motion::SET_OPEN);
initMotion->setCost(opt_->initialCost(initMotion->getState()));
nn_->add(initMotion); // V <-- {x_init}
}
if (!initMotion)
{
OMPL_ERROR("Start state undefined");
return base::PlannerStatus::INVALID_START;
}
// Sample N free states in the configuration space
if (!sampler_)
sampler_ = si_->allocStateSampler();
sampleFree(ptc);
assureGoalIsSampled(goal);
OMPL_INFORM("%s: Starting planning with %u states already in datastructure", getName().c_str(), nn_->size());
// Calculate the nearest neighbor search radius
/// \todo Create a PRM-like connection strategy
if (nearestK_)
{
NNk_ = std::ceil(std::pow(2.0 * radiusMultiplier_, (double)si_->getStateDimension()) *
(boost::math::constants::e<double>() / (double)si_->getStateDimension()) *
log((double)nn_->size()));
OMPL_DEBUG("Using nearest-neighbors k of %d", NNk_);
}
else
{
NNr_ = calculateRadius(si_->getStateDimension(), nn_->size());
OMPL_DEBUG("Using radius of %f", NNr_);
}
// Execute the planner, and return early if the planner returns a failure
bool plannerSuccess = false;
bool successfulExpansion = false;
Motion *z = initMotion; // z <-- xinit
saveNeighborhood(z);
while (!ptc)
{
if ((plannerSuccess = goal->isSatisfied(z->getState())))
break;
successfulExpansion = expandTreeFromNode(&z);
if (!extendedFMT_ && !successfulExpansion)
break;
else if (extendedFMT_ && !successfulExpansion)
{
//Apply RRT*-like connections: sample and connect samples to tree
std::vector<Motion*> nbh;
std::vector<base::Cost> costs;
std::vector<base::Cost> incCosts;
std::vector<std::size_t> sortedCostIndices;
// our functor for sorting nearest neighbors
CostIndexCompare compareFn(costs, *opt_);
Motion *m = new Motion(si_);
while (!ptc && Open_.empty())
{
sampler_->sampleUniform(m->getState());
if (!si_->isValid(m->getState()))
continue;
if (nearestK_)
nn_->nearestK(m, NNk_, nbh);
else
nn_->nearestR(m, NNr_, nbh);
// Get neighbours in the tree.
std::vector<Motion*> yNear;
yNear.reserve(nbh.size());
for (std::size_t j = 0; j < nbh.size(); ++j)
{
if (nbh[j]->getSetType() == Motion::SET_CLOSED)
{
if (nearestK_)
{
// Only include neighbors that are mutually k-nearest
// Relies on NN datastructure returning k-nearest in sorted order
const base::Cost connCost = opt_->motionCost(nbh[j]->getState(), m->getState());
const base::Cost worstCost = opt_->motionCost(neighborhoods_[nbh[j]].back()->getState(), nbh[j]->getState());
if (opt_->isCostBetterThan(worstCost, connCost))
continue;
else
yNear.push_back(nbh[j]);
}
else
yNear.push_back(nbh[j]);
}
}
// Sample again if the new sample does not connect to the tree.
if (yNear.empty())
continue;
// 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() < yNear.size())
{
costs.resize(yNear.size());
incCosts.resize(yNear.size());
sortedCostIndices.resize(yNear.size());
}
// 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
//
// calculate all costs and distances
for (std::size_t i = 0 ; i < yNear.size(); ++i)
{
incCosts[i] = opt_->motionCost(yNear[i]->getState(), m->getState());
costs[i] = opt_->combineCosts(yNear[i]->getCost(), 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 < yNear.size(); ++i)
sortedCostIndices[i] = i;
std::sort(sortedCostIndices.begin(), sortedCostIndices.begin() + yNear.size(),
compareFn);
// collision check until a valid motion is found
for (std::vector<std::size_t>::const_iterator i = sortedCostIndices.begin();
i != sortedCostIndices.begin() + yNear.size();
++i)
{
if (si_->checkMotion(yNear[*i]->getState(), m->getState()))
{
m->setParent(yNear[*i]);
yNear[*i]->getChildren().push_back(m);
const base::Cost incCost = opt_->motionCost(yNear[*i]->getState(), m->getState());
m->setCost(opt_->combineCosts(yNear[*i]->getCost(), incCost));
m->setHeuristicCost(opt_->motionCostHeuristic(m->getState(), goalState_));
m->setSetType(Motion::SET_OPEN);
nn_->add(m);
saveNeighborhood(m);
updateNeighborhood(m,nbh);
Open_.insert(m);
z = m;
break;
}
}
} // while (!ptc && Open_.empty())
} // else if (extendedFMT_ && !successfulExpansion)
} // While not at goal
if (plannerSuccess)
{
// Return the path to z, since by definition of planner success, z is in the goal region
lastGoalMotion_ = z;
traceSolutionPathThroughTree(lastGoalMotion_);
OMPL_DEBUG("Final path cost: %f", lastGoalMotion_->getCost().value());
return base::PlannerStatus(true, false);
} // if plannerSuccess
else
{
// Planner terminated without accomplishing goal
return base::PlannerStatus(false, false);
}
}
void ompl::geometric::TRRTConnect::setup() {
Planner::setup();
tools::SelfConfig sc(si_,getName());
//Set optimization objective
if (pdef_->hasOptimizationObjective()) {
opt_ = pdef_->getOptimizationObjective();
} else {
opt_.reset(new base::MechanicalWorkOptimizationObjective(si_));
OMPL_INFORM("%s: No optimization objective specified.",getName().c_str());
OMPL_INFORM("%s: Defaulting to optimizing path length.",getName().c_str());
}
//Set maximum distance a new node can be from its nearest neighbor
if (maxDistance_ < std::numeric_limits<double>::epsilon()) {
sc.configurePlannerRange(maxDistance_);
maxDistance_ *= magic::COST_MAX_MOTION_LENGTH_AS_SPACE_EXTENT_FRACTION;
}
//Set the threshold to decide if a new node is a frontier node
if (frontierThreshold_ < std::numeric_limits<double>::epsilon()) {
frontierThreshold_ = 0.01*si_->getMaximumExtent();
OMPL_DEBUG("%s: Frontier threshold detected to be %lf",
getName().c_str(),frontierThreshold_);
}
//Autoconfigure the K constant
if (kConstant_ < std::numeric_limits<double>::epsilon()) {
//Find the average cost of states by sampling
kConstant_ = opt_->averageStateCost(magic::TEST_STATE_COUNT).value();
}
//Create the nearest neighbor function the first time setup is run
#if OMPL_VERSION_VALUE < 1001000 // 1.1.0
if (!tStart_) tStart_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Motion*>
(si_->getStateSpace()));
if (!tGoal_) tGoal_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Motion*>
(si_->getStateSpace()));
#else
if (!tStart_) tStart_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Motion*>(this));
if (!tGoal_) tGoal_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Motion*>(this));
#endif
//Set the distance function
tStart_->setDistanceFunction(std::bind(&TRRTConnect::distanceFunction,this,std::placeholders::_1,std::placeholders::_2));
tGoal_->setDistanceFunction(std::bind(&TRRTConnect::distanceFunction,this,std::placeholders::_1,std::placeholders::_2));
//Setup TRRTConnect specific variables
clearTree(tStart_);
clearTree(tGoal_);
if (tStart_.compareFn_) delete tStart_.compareFn_;
tStart_.compareFn_ = new CostIndexCompare(tStart_.costs_,*opt_);
if (tGoal_.compareFn_) delete tGoal_.compareFn_;
tGoal_.compareFn_ = new CostIndexCompare(tGoal_.costs_,*opt_);
k_rrg_ = boost::math::constants::e<double>()*
double(si_->getStateSpace()->getDimension()+1)/
double(si_->getStateSpace()->getDimension());//e+e/d
}