void ompl::control::SpaceInformation::setup()
{
base::SpaceInformation::setup();
if (!statePropagator_)
throw Exception("State propagator not defined");
if (minSteps_ > maxSteps_)
throw Exception("The minimum number of steps cannot be larger than the maximum number of steps");
if (minSteps_ == 0 && maxSteps_ == 0)
{
minSteps_ = 1;
maxSteps_ = 10;
OMPL_WARN("Assuming propagation will always have between %d and %d steps", minSteps_, maxSteps_);
}
if (minSteps_ < 1)
throw Exception("The minimum number of steps must be at least 1");
if (stepSize_ < std::numeric_limits<double>::epsilon())
{
stepSize_ = getStateValidityCheckingResolution() * getMaximumExtent();
if (stepSize_ < std::numeric_limits<double>::epsilon())
throw Exception("The propagation step size must be larger than 0");
OMPL_WARN("The propagation step size is assumed to be %f", stepSize_);
}
controlSpace_->setup();
if (controlSpace_->getDimension() <= 0)
throw Exception("The dimension of the control space we plan in must be > 0");
}
void ompl::control::SST::setup()
{
base::Planner::setup();
if (!nn_)
nn_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Motion*>(this));
nn_->setDistanceFunction(boost::bind(&SST::distanceFunction, this, _1, _2));
if (!witnesses_)
witnesses_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Motion*>(this));
witnesses_->setDistanceFunction(boost::bind(&SST::distanceFunction, this, _1, _2));
if (pdef_)
{
if (pdef_->hasOptimizationObjective())
{
opt_ = pdef_->getOptimizationObjective();
if (dynamic_cast<base::MaximizeMinClearanceObjective*>(opt_.get()) || dynamic_cast<base::MinimaxObjective*>(opt_.get()))
OMPL_WARN("%s: Asymptotic near-optimality has only been proven with Lipschitz continuous cost functions w.r.t. state and control. This optimization objective will result in undefined behavior", getName().c_str());
}
else
{
OMPL_WARN("%s: No optimization object set. Using path length", getName().c_str());
opt_.reset(new base::PathLengthOptimizationObjective(si_));
pdef_->setOptimizationObjective(opt_);
}
}
prevSolutionCost_ = opt_->infiniteCost();
}
bool ompl::tools::LightningDB::load(const std::string &fileName)
{
// Error checking
if (fileName.empty())
{
OMPL_ERROR("Empty filename passed to save function");
return false;
}
if ( !boost::filesystem::exists( fileName ) )
{
OMPL_WARN("Database file does not exist: %s", fileName.c_str());
return false;
}
// Load database from file, track loading time
time::point start = time::now();
OMPL_INFORM("Loading database from file: %s", fileName.c_str());
// Open a binary input stream
std::ifstream iStream(fileName.c_str(), std::ios::binary);
// Get the total number of paths saved
double numPaths = 0;
iStream >> numPaths;
// Check that the number of paths makes sense
if (numPaths < 0 || numPaths > std::numeric_limits<double>::max())
{
OMPL_WARN("Number of paths to load %d is a bad value", numPaths);
return false;
}
// Start loading all the PlannerDatas
for (std::size_t i = 0; i < numPaths; ++i)
{
// Create a new planner data instance
ompl::base::PlannerDataPtr plannerData(new ompl::base::PlannerData(si_));
// Note: the StateStorage class checks if the states match for us
plannerDataStorage_.load(iStream, *plannerData.get());
// Add to nearest neighbor tree
nn_->add(plannerData);
}
// Close file
iStream.close();
double loadTime = time::seconds(time::now() - start);
OMPL_INFORM("Loaded database from file in %f sec with %d paths", loadTime, nn_->size());
return true;
}
void ompl::geometric::CForest::addPlannerInstanceInternal(const base::PlannerPtr &planner)
{
if (!planner->getSpecs().canReportIntermediateSolutions)
OMPL_WARN("%s cannot report intermediate solutions, not added as CForest planner.", planner->getName().c_str());
else
{
planner->setProblemDefinition(pdef_);
if (planner->params().hasParam("focus_search"))
planner->params()["focus_search"] = focusSearch_;
else
OMPL_WARN("%s does not appear to support search focusing.", planner->getName().c_str());
planners_.push_back(planner);
}
}
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);
}
//The base InformedStateSampler class:
InformedStateSampler::InformedStateSampler(const StateSpace* space, const ProblemDefinitionPtr probDefn, const Cost* bestCost)
: StateSampler(space),
probDefn_(probDefn),
bestCostPtr_(bestCost)
{
//Sanity check the problem.
//Check that we were given a valid pointer
if (bestCostPtr_ == false)
{
throw Exception ("InformedStateSampler: The cost pointer must be valid at construction.");
}
//No else
//Check that there is an optimization objective
if (probDefn_->hasOptimizationObjective() == false)
{
throw Exception ("InformedStateSampler: An optimization objective must be specified at construction.");
}
//No else
//Make sure we have at least one start and warn if we have more than one
if (probDefn_->getStartStateCount() == 0u)
{
throw Exception ("InformedStateSampler: At least one start state must be specified at construction.");
}
else if (probDefn_->getStartStateCount() > 1u)
{
OMPL_WARN("InformedStateSampler: More than 1 start state present. Informed samplers will only use the first.");
}
//No else
//Store the optimization objective for later ease.
opt_ = probDefn_->getOptimizationObjective();
}
ompl::base::PlannerStatus ompl::geometric::CForest::solve(const base::PlannerTerminationCondition &ptc)
{
using solveFunctionType = void (ompl::geometric::CForest::*)(base::Planner *, const base::PlannerTerminationCondition &);
checkValidity();
time::point start = time::now();
std::vector<std::thread*> threads(planners_.size());
const base::ReportIntermediateSolutionFn prevSolutionCallback = getProblemDefinition()->getIntermediateSolutionCallback();
if (prevSolutionCallback)
OMPL_WARN("Cannot use previously set intermediate solution callback with %s", getName().c_str());
pdef_->setIntermediateSolutionCallback(std::bind(&CForest::newSolutionFound, this,
std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
bestCost_ = opt_->infiniteCost();
// run each planner in its own thread, with the same ptc.
for (std::size_t i = 0 ; i < threads.size() ; ++i)
threads[i] = new std::thread(std::bind((solveFunctionType)&CForest::solve, this, planners_[i].get(), ptc));
for (auto & thread : threads)
{
thread->join();
delete thread;
}
// restore callback
getProblemDefinition()->setIntermediateSolutionCallback(prevSolutionCallback);
OMPL_INFORM("Solution found in %f seconds", time::seconds(time::now() - start));
return base::PlannerStatus(pdef_->hasSolution(), pdef_->hasApproximateSolution());
}
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::tools::ThunderDB::addPath(ompl::geometric::PathGeometric &solutionPath, double &insertionTime)
{
// Error check
if (!spars_)
{
OMPL_ERROR("SPARSdb planner has not been passed into the ThunderDB yet");
insertionTime = 0;
return false;
}
// Prevent inserting into database
if (!saving_enabled_)
{
OMPL_WARN("ThunderDB: Saving is disabled so not adding path");
return false;
}
bool result;
double seconds = 120; // 10; // a large number, should never need to use this
ompl::base::PlannerTerminationCondition ptc = ompl::base::timedPlannerTerminationCondition(seconds, 0.1);
// Benchmark runtime
time::point startTime = time::now();
{
result = spars_->addPathToRoadmap(ptc, solutionPath);
}
insertionTime = time::seconds(time::now() - startTime);
OMPL_INFORM("SPARSdb now has %d states", spars_->getNumVertices());
// Record this new addition
numPathsInserted_++;
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::geometric::SPARStwo::setup()
{
Planner::setup();
if (!nn_)
nn_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Vertex>(si_->getStateSpace()));
nn_->setDistanceFunction(boost::bind(&SPARStwo::distanceFunction, this, _1, _2));
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_.reset(new base::PathLengthOptimizationObjective(si_));
}
else
{
OMPL_INFORM("%s: problem definition is not set, deferring setup completion...", getName().c_str());
setup_ = false;
}
}
void ompl::geometric::RRTstar::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*>(si_->getStateSpace()));
nn_->setDistanceFunction(boost::bind(&RRTstar::distanceFunction, this, _1, _2));
// 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_.reset(new base::PathLengthOptimizationObjective(si_));
}
}
else
{
OMPL_INFORM("%s: problem definition is not set, deferring setup completion...", getName().c_str());
setup_ = false;
}
}
ompl::geometric::PathSimplifier::PathSimplifier(const base::SpaceInformationPtr &si, const base::GoalPtr& goal) : si_(si), freeStates_(true)
{
if (goal)
{
gsr_ = std::dynamic_pointer_cast<base::GoalSampleableRegion>(goal);
if (!gsr_)
OMPL_WARN("%s: Goal could not be cast to GoalSampleableRegion. Goal simplification will not be performed.", __FUNCTION__);
}
}
// The default rejection-sampling class:
RejectionInfSampler::RejectionInfSampler(const ProblemDefinitionPtr& probDefn, unsigned int maxNumberCalls)
: InformedSampler(probDefn, maxNumberCalls)
{
// Create the basic sampler
baseSampler_ = InformedSampler::space_->allocDefaultStateSampler();
// Warn if a cost-to-go heuristic is not defined
if (InformedSampler::opt_->hasCostToGoHeuristic() == false)
{
OMPL_WARN("RejectionInfSampler: The optimization objective does not have a cost-to-go heuristic defined. Informed sampling will likely have little to no effect.");
}
// No else
}
std::vector<ompl::control::ProductGraph::State *>
ompl::control::LTLPlanner::getHighLevelPath(const std::vector<base::State *> &path, ProductGraph::State *start) const
{
std::vector<ProductGraph::State *> hlPath(path.size());
hlPath[0] = (start != nullptr ? start : ltlsi_->getProdGraphState(path[0]));
for (unsigned int i = 1; i < path.size(); ++i)
{
hlPath[i] = ltlsi_->getProdGraphState(path[i]);
if (!hlPath[i]->isValid())
OMPL_WARN("High-level path fails automata");
}
return hlPath;
}
void ompl::base::Planner::setup()
{
if (!si_->isSetup())
{
OMPL_INFORM("%s: Space information setup was not yet called. Calling now.", getName().c_str());
si_->setup();
}
if (setup_)
OMPL_WARN("%s: Planner setup called multiple times", getName().c_str());
else
setup_ = true;
}
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::RRTstar::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(std::bind(&RRTstar::distanceFunction, this, std::placeholders::_1, std::placeholders::_2));
// 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_.reset(new base::PathLengthOptimizationObjective(si_));
// Store the new objective in the problem def'n
pdef_->setOptimizationObjective(opt_);
}
}
else
{
OMPL_INFORM("%s: problem definition is not set, deferring setup completion...", getName().c_str());
setup_ = false;
}
// Get the measure of the entire space:
prunedMeasure_ = si_->getSpaceMeasure();
// Calculate some constants:
calculateRewiringLowerBounds();
// Set the bestCost_ and prunedCost_ as infinite
bestCost_ = opt_->infiniteCost();
prunedCost_ = opt_->infiniteCost();
}
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();
}
//The default rejection-sampling class:
RejectionInfSampler::RejectionInfSampler(const StateSpace* space, const ProblemDefinitionPtr probDefn, const Cost* bestCost)
: InformedStateSampler(space, probDefn, bestCost)
{
//Create the basic sampler
baseSampler_ = StateSampler::space_->allocDefaultStateSampler();
//Set it's seed to the same as mine
baseSampler_->setLocalSeed( this->getLocalSeed() );
//Warn if a cost-to-go heuristic is not defined
if (InformedStateSampler::opt_->hasCostToGoHeuristic() == false)
{
OMPL_WARN("RejectionInfSampler: The optimization objective does not have a cost-to-go heuristic defined. Informed sampling will likely have little to no effect.");
}
//No else
}
ompl::base::ProjectionMatrix::Matrix ompl::base::ProjectionMatrix::ComputeRandom(const unsigned int from,
const unsigned int to,
const std::vector<double> &scale)
{
namespace nu = boost::numeric::ublas;
RNG rng;
Matrix projection(to, from);
for (unsigned int j = 0; j < from; ++j)
{
if (scale.size() == from && fabs(scale[j]) < std::numeric_limits<double>::epsilon())
nu::column(projection, j) = nu::zero_vector<double>(to);
else
for (unsigned int i = 0; i < to; ++i)
projection(i, j) = rng.gaussian01();
}
for (unsigned int i = 0; i < to; ++i)
{
nu::matrix_row<Matrix> row(projection, i);
for (unsigned int j = 0; j < i; ++j)
{
nu::matrix_row<Matrix> prevRow(projection, j);
// subtract projection
row -= inner_prod(row, prevRow) * prevRow;
}
// normalize
row /= norm_2(row);
}
assert(scale.size() == from || scale.size() == 0);
if (scale.size() == from)
{
unsigned int z = 0;
for (unsigned int i = 0; i < from; ++i)
{
if (fabs(scale[i]) < std::numeric_limits<double>::epsilon())
z++;
else
nu::column(projection, i) /= scale[i];
}
if (z == from)
OMPL_WARN("Computed projection matrix is all 0s");
}
return projection;
}
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::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);
}
ompl::base::OptimizationObjectivePtr ompl::app::getOptimizationObjective(
const base::SpaceInformationPtr &si, const std::string &objective, double threshold)
{
base::OptimizationObjectivePtr obj;
if (objective == std::string("length"))
obj.reset(new base::PathLengthOptimizationObjective(si));
else if (objective == std::string("max min clearance"))
obj.reset(new base::MaximizeMinClearanceObjective(si));
else if (objective == std::string("mechanical work"))
obj.reset(new base::MechanicalWorkOptimizationObjective(si));
else
{
OMPL_WARN("ompl::app::getOptimizationObjective: unknown optimization objective called \"%s\"; using \"length\" instead", objective.c_str());
obj.reset(new base::PathLengthOptimizationObjective(si));
}
obj->setCostThreshold(base::Cost(threshold));
return obj;
}
void ompl::geometric::SimpleSetup::simplifySolution(const base::PlannerTerminationCondition &ptc)
{
if (pdef_)
{
const base::PathPtr &p = pdef_->getSolutionPath();
if (p)
{
time::point start = time::now();
auto &path = static_cast<PathGeometric &>(*p);
std::size_t numStates = path.getStateCount();
psk_->simplify(path, ptc);
simplifyTime_ = time::seconds(time::now() - start);
OMPL_INFORM("SimpleSetup: Path simplification took %f seconds and changed from %d to %d states",
simplifyTime_, numStates, path.getStateCount());
return;
}
}
OMPL_WARN("No solution to simplify");
}
void ompl::base::ProjectionEvaluator::inferCellSizes()
{
cellSizesWereInferred_ = true;
if (!hasBounds())
inferBounds();
unsigned int dim = getDimension();
cellSizes_.resize(dim);
for (unsigned int j = 0; j < dim; ++j)
{
cellSizes_[j] = (bounds_.high[j] - bounds_.low[j]) / magic::PROJECTION_DIMENSION_SPLITS;
if (cellSizes_[j] < std::numeric_limits<double>::epsilon())
{
cellSizes_[j] = 1.0;
OMPL_WARN("Inferred cell size for dimension %u of a projection for state space %s is 0. Setting arbitrary "
"value of 1 instead.",
j, space_->getName().c_str());
}
}
}
void ompl::base::SpaceInformation::setup()
{
if (!stateValidityChecker_)
{
stateValidityChecker_ = std::make_shared<AllValidStateValidityChecker>(this);
OMPL_WARN("State validity checker not set! No collision checking is performed");
}
if (!motionValidator_)
setDefaultMotionValidator();
stateSpace_->setup();
if (stateSpace_->getDimension() <= 0)
throw Exception("The dimension of the state space we plan in must be > 0");
params_.clear();
params_.include(stateSpace_->params());
setup_ = true;
}
ompl::base::PlannerStatus ompl::geometric::CForest::solve(const base::PlannerTerminationCondition &ptc)
{
checkValidity();
time::point start = time::now();
std::vector<std::thread *> threads(planners_.size());
const base::ReportIntermediateSolutionFn prevSolutionCallback =
getProblemDefinition()->getIntermediateSolutionCallback();
if (prevSolutionCallback)
OMPL_WARN("Cannot use previously set intermediate solution callback with %s", getName().c_str());
pdef_->setIntermediateSolutionCallback(
[this](const base::Planner *planner, const std::vector<const base::State *> &states, const base::Cost cost)
{
return newSolutionFound(planner, states, cost);
});
bestCost_ = opt_->infiniteCost();
// run each planner in its own thread, with the same ptc.
for (std::size_t i = 0; i < threads.size(); ++i)
{
base::Planner *planner = planners_[i].get();
threads[i] = new std::thread([this, planner, &ptc]
{
return solve(planner, ptc);
});
}
for (auto &thread : threads)
{
thread->join();
delete thread;
}
// restore callback
getProblemDefinition()->setIntermediateSolutionCallback(prevSolutionCallback);
OMPL_INFORM("Solution found in %f seconds", time::seconds(time::now() - start));
return base::PlannerStatus(pdef_->hasSolution(), pdef_->hasApproximateSolution());
}
void ompl::geometric::SimpleSetup::simplifySolution(double duration)
{
if (pdef_)
{
const base::PathPtr &p = pdef_->getSolutionPath();
if (p)
{
time::point start = time::now();
auto &path = static_cast<PathGeometric &>(*p);
std::size_t numStates = path.getStateCount();
if (duration < std::numeric_limits<double>::epsilon())
psk_->simplifyMax(static_cast<PathGeometric &>(*p));
else
psk_->simplify(static_cast<PathGeometric &>(*p), duration);
simplifyTime_ = time::seconds(time::now() - start);
OMPL_INFORM("SimpleSetup: Path simplification took %f seconds and changed from %d to %d states",
simplifyTime_, numStates, path.getStateCount());
return;
}
}
OMPL_WARN("No solution to simplify");
}
void ompl::geometric::FMT::setup()
{
if (pdef_)
{
/* Setup the 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_->hasOptimizationObjective())
opt_ = pdef_->getOptimizationObjective();
else
{
OMPL_INFORM("%s: No optimization objective specified. Defaulting to optimizing path length.", getName().c_str());
opt_.reset(new base::PathLengthOptimizationObjective(si_));
// Store the new objective in the problem def'n
pdef_->setOptimizationObjective(opt_);
}
Open_.getComparisonOperator().opt_ = opt_.get();
Open_.getComparisonOperator().heuristics_ = heuristics_;
if (!nn_)
nn_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Motion*>(this));
nn_->setDistanceFunction(std::bind(&FMT::distanceFunction, this,
std::placeholders::_1, std::placeholders::_2));
if (nearestK_ && !nn_->reportsSortedResults())
{
OMPL_WARN("%s: NearestNeighbors datastructure does not return sorted solutions. Nearest K strategy disabled.", getName().c_str());
nearestK_ = false;
}
}
else
{
OMPL_INFORM("%s: problem definition is not set, deferring setup completion...", getName().c_str());
setup_ = false;
}
}
