ompl::base::Cost ompl::base::OptimizationObjective::getCost(const Path &path) const
{
// Cast path down to a PathGeometric
const geometric::PathGeometric *pathGeom = dynamic_cast<const geometric::PathGeometric*>(&path);
// Give up if this isn't a PathGeometric or if the path is empty.
if (!pathGeom)
{
OMPL_ERROR("Error: Cost computation is only implemented for paths of type PathGeometric.");
return this->identityCost();
}
else
{
std::size_t numStates = pathGeom->getStateCount();
if (numStates == 0)
{
OMPL_ERROR("Cannot compute cost of an empty path.");
return this->identityCost();
}
else
{
// Compute path cost by accumulating the cost along the path
Cost cost(this->identityCost());
for (std::size_t i = 1; i < numStates; ++i)
{
const State *s1 = pathGeom->getState(i-1);
const State *s2 = pathGeom->getState(i);
cost = this->combineCosts(cost, this->motionCost(s1, s2));
}
return cost;
}
}
}
bool ompl::app::RigidBodyGeometry::addEnvironmentMesh(const std::string &env)
{
assert(!env.empty());
std::size_t p = importerEnv_.size();
importerEnv_.resize(p + 1);
importerEnv_[p] = std::make_shared<Assimp::Importer>();
const aiScene* envScene = importerEnv_[p]->ReadFile(env.c_str(),
aiProcess_Triangulate |
aiProcess_JoinIdenticalVertices |
aiProcess_SortByPType |
aiProcess_OptimizeGraph |
aiProcess_OptimizeMeshes);
if (envScene != nullptr)
{
if (!envScene->HasMeshes())
{
OMPL_ERROR("There is no mesh specified in the indicated environment resource: %s", env.c_str());
importerEnv_.resize(p);
}
}
else
{
OMPL_ERROR("Unable to load environment scene: %s", env.c_str());
importerEnv_.resize(p);
}
if (p < importerEnv_.size())
{
computeGeometrySpecification();
return true;
}
return false;
}
void ompl::base::PlannerDataStorage::store(const PlannerData &pd, std::ostream &out)
{
const SpaceInformationPtr &si = pd.getSpaceInformation();
if (!out.good())
{
OMPL_ERROR("Failed to store PlannerData: output stream is invalid");
return;
}
if (!si)
{
OMPL_ERROR("Failed to store PlannerData: SpaceInformation is invalid");
return;
}
try
{
boost::archive::binary_oarchive oa(out);
// Writing the header
Header h;
h.marker = OMPL_PLANNER_DATA_ARCHIVE_MARKER;
h.vertex_count = pd.numVertices();
h.edge_count = pd.numEdges();
si->getStateSpace()->computeSignature(h.signature);
oa << h;
storeVertices(pd, oa);
storeEdges(pd, oa);
}
catch (boost::archive::archive_exception &ae)
{
OMPL_ERROR("Failed to store PlannerData: %s", ae.what());
}
}
bool ompl::base::ProblemDefinition::isTrivial(unsigned int *startIndex, double *distance) const
{
if (!goal_)
{
OMPL_ERROR("Goal undefined");
return false;
}
for (unsigned int i = 0 ; i < startStates_.size() ; ++i)
{
const State *start = startStates_[i];
if (start && si_->isValid(start) && si_->satisfiesBounds(start))
{
double dist;
if (goal_->isSatisfied(start, &dist))
{
if (startIndex)
*startIndex = i;
if (distance)
*distance = dist;
return true;
}
}
else
{
OMPL_ERROR("Initial state is in collision!");
}
}
return false;
}
void ompl::base::PlannerDataStorage::load(std::istream &in, PlannerData &pd)
{
pd.clear();
const SpaceInformationPtr &si = pd.getSpaceInformation();
if (!in.good())
{
OMPL_ERROR("Failed to load PlannerData: input stream is invalid");
return;
}
if (!si)
{
OMPL_ERROR("Failed to load PlannerData: SpaceInformation is invalid");
return;
}
// Loading the planner data:
try
{
boost::archive::binary_iarchive ia(in);
// Read the header
Header h;
ia >> h;
// Checking the archive marker
if (h.marker != OMPL_PLANNER_DATA_ARCHIVE_MARKER)
{
OMPL_ERROR("Failed to load PlannerData: PlannerData archive marker not found");
return;
}
// Verify that the state space is the same
std::vector<int> sig;
si->getStateSpace()->computeSignature(sig);
if (h.signature != sig)
{
OMPL_ERROR("Failed to load PlannerData: StateSpace signature mismatch");
return;
}
// File seems ok... loading vertices and edges
loadVertices(pd, h.vertex_count, ia);
loadEdges(pd, h.edge_count, ia);
}
catch (boost::archive::archive_exception &ae)
{
OMPL_ERROR("Failed to load PlannerData: %s", ae.what());
}
}
const ompl::base::StateValidityCheckerPtr& ompl::app::RigidBodyGeometry::allocStateValidityChecker(const base::SpaceInformationPtr &si, const GeometricStateExtractor &se, bool selfCollision)
{
if (validitySvc_)
return validitySvc_;
GeometrySpecification geom = getGeometrySpecification();
switch (ctype_)
{
#if OMPL_HAS_PQP
case PQP:
if (mtype_ == Motion_2D)
validitySvc_ = std::make_shared<PQPStateValidityChecker<Motion_2D>>(si, geom, se, selfCollision);
else
validitySvc_ = std::make_shared<PQPStateValidityChecker<Motion_3D>>(si, geom, se, selfCollision);
break;
#endif
case FCL:
if (mtype_ == Motion_2D)
validitySvc_ = std::make_shared<FCLStateValidityChecker<Motion_2D>>(si, geom, se, selfCollision);
else
validitySvc_ = std::make_shared<FCLStateValidityChecker<Motion_3D>>(si, geom, se, selfCollision);
break;
default:
OMPL_ERROR("Unexpected collision checker type (%d) encountered", ctype_);
};
return validitySvc_;
}
void ompl::base::SpaceInformation::printProperties(std::ostream &out) const
{
out << "Properties of the state space '" << stateSpace_->getName() << "'" << std::endl;
out << " - signature: ";
std::vector<int> sig;
stateSpace_->computeSignature(sig);
for (std::size_t i = 0 ; i < sig.size() ; ++i)
out << sig[i] << " ";
out << std::endl;
out << " - dimension: " << stateSpace_->getDimension() << std::endl;
out << " - extent: " << stateSpace_->getMaximumExtent() << std::endl;
if (isSetup())
{
bool result = true;
try
{
stateSpace_->sanityChecks();
}
catch(Exception &e)
{
result = false;
out << std::endl << " - SANITY CHECKS FOR STATE SPACE ***DID NOT PASS*** (" << e.what() << ")" << std::endl << std::endl;
OMPL_ERROR(e.what());
}
if (result)
out << " - sanity checks for state space passed" << std::endl;
out << " - probability of valid states: " << probabilityOfValidState(magic::TEST_STATE_COUNT) << std::endl;
out << " - average length of a valid motion: " << averageValidMotionLength(magic::TEST_STATE_COUNT) << std::endl;
double uniform, near, gaussian;
samplesPerSecond(uniform, near, gaussian, magic::TEST_STATE_COUNT);
out << " - average number of samples drawn per second: sampleUniform()=" << uniform << " sampleUniformNear()=" << near << " sampleGaussian()=" << gaussian << std::endl;
}
else
out << "Call setup() before to get more information" << std::endl;
}
Plane2DEnvironment(const char *ppm_file)
{
bool ok = false;
try
{
ppm_.loadFile(ppm_file);
ok = true;
}
catch(ompl::Exception &ex)
{
OMPL_ERROR("Unable to load %s.\n%s", ppm_file, ex.what());
}
if (ok)
{
ob::RealVectorStateSpace *space = new ob::RealVectorStateSpace();
space->addDimension(0.0, ppm_.getWidth());
space->addDimension(0.0, ppm_.getHeight());
maxWidth_ = ppm_.getWidth() - 1;
maxHeight_ = ppm_.getHeight() - 1;
lightning_.reset(new ot::Lightning(ob::StateSpacePtr(space)));
lightning_->setFilePath("lightning.db");
// set state validity checking for this space
lightning_->setStateValidityChecker(boost::bind(&Plane2DEnvironment::isStateValid, this, _1));
space->setup();
lightning_->getSpaceInformation()->setStateValidityCheckingResolution(1.0 / space->getMaximumExtent());
vss_ = lightning_->getSpaceInformation()->allocValidStateSampler();
}
}
void ompl::geometric::RRTstar::setSampleRejection(const bool reject)
{
if (static_cast<bool>(opt_) == true)
{
if (opt_->hasCostToGoHeuristic() == false)
{
OMPL_INFORM("%s: No cost-to-go heuristic set. Informed techniques will not work well.", getName().c_str());
}
}
// This option is mutually exclusive with setSampleRejection, assert that:
if (reject == true && useInformedSampling_ == true)
{
OMPL_ERROR("%s: InformedSampling and SampleRejection are mutually exclusive options.", getName().c_str());
}
// Check if we're changing the setting of rejection sampling. If we are, we will need to create a new sampler, which we only want to do if one is already allocated.
if (reject != useRejectionSampling_)
{
// Store the setting
useRejectionSampling_ = reject;
// If we currently have a sampler, we need to make a new one
if (sampler_ || infSampler_)
{
// Reset the samplers
sampler_.reset();
infSampler_.reset();
// Create the sampler
allocSampler();
}
}
}
ob::PlannerPtr allocatePlanner(ob::SpaceInformationPtr si, optimalPlanner plannerType)
{
switch (plannerType)
{
case PLANNER_BITSTAR:
return boost::make_shared<og::BITstar>(si);
break;
case PLANNER_CFOREST:
return boost::make_shared<og::CForest>(si);
break;
case PLANNER_FMTSTAR:
return boost::make_shared<og::FMT>(si);
break;
case PLANNER_PRMSTAR:
return boost::make_shared<og::PRMstar>(si);;
break;
case PLANNER_RRTSTAR:
return boost::make_shared<og::RRTstar>(si);;
break;
default:
OMPL_ERROR("Planner-type enum is not implemented in allocation function.");
return ob::PlannerPtr();
break;
}
}
Plane2DEnvironment(const char *ppm_file)
{
bool ok = false;
try
{
ppm_.loadFile(ppm_file);
ok = true;
}
catch(ompl::Exception &ex)
{
OMPL_ERROR("Unable to load %s.\n%s", ppm_file, ex.what());
}
if (ok)
{
auto *space = new ob::RealVectorStateSpace();
space->addDimension(0.0, ppm_.getWidth());
space->addDimension(0.0, ppm_.getHeight());
maxWidth_ = ppm_.getWidth() - 1;
maxHeight_ = ppm_.getHeight() - 1;
ss_.reset(new og::SimpleSetup(ob::StateSpacePtr(space)));
// set state validity checking for this space
ss_->setStateValidityChecker(std::bind(&Plane2DEnvironment::isStateValid, this, std::placeholders::_1));
space->setup();
ss_->getSpaceInformation()->setStateValidityCheckingResolution(1.0 / space->getMaximumExtent());
// ss_->setPlanner(ob::PlannerPtr(new og::RRTConnect(ss_->getSpaceInformation())));
}
}
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::control::World::operator[](unsigned int i) const
{
auto p = props_.find(i);
if (p == props_.end())
OMPL_ERROR("Proposition %u is not set in world", i);
return p->second;
}
double ompl::base::ProjectionEvaluator::getCellSizes(unsigned int dim) const
{
if (cellSizes_.size() > dim)
return cellSizes_[dim];
OMPL_ERROR("Dimension %u is not defined for projection evaluator", dim);
return 0.0;
}
Plane2DEnvironment(const char *ppm_file)
{
bool ok = false;
try
{
ppm_.loadFile(ppm_file);
ok = true;
}
catch(ompl::Exception &ex)
{
OMPL_ERROR("Unable to load %s.\n%s", ppm_file, ex.what());
}
if (ok)
{
auto space(std::make_shared<ob::RealVectorStateSpace>());
space->addDimension(0.0, ppm_.getWidth());
space->addDimension(0.0, ppm_.getHeight());
maxWidth_ = ppm_.getWidth() - 1;
maxHeight_ = ppm_.getHeight() - 1;
ss_ = std::make_shared<og::SimpleSetup>(space);
// set state validity checking for this space
ss_->setStateValidityChecker([this](const ob::State *state) { return isStateValid(state); });
space->setup();
ss_->getSpaceInformation()->setStateValidityCheckingResolution(1.0 / space->getMaximumExtent());
// ss_->setPlanner(std::make_shared<og::RRTConnect>(ss_->getSpaceInformation()));
}
}
bool ompl::tools::Lightning::saveIfChanged()
{
if (filePath_.empty())
{
OMPL_ERROR("No file path has been specified, unable to save experience DB");
return false;
}
return experienceDB_->saveIfChanged(filePath_);
}
void ompl::base::ProjectionEvaluator::setCellSizes(unsigned int dim, double cellSize)
{
if (cellSizes_.size() >= dim)
OMPL_ERROR("Dimension %u is not defined for projection evaluator", dim);
else
{
std::vector<double> c = cellSizes_;
c[dim] = cellSize;
setCellSizes(c);
}
}
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::tools::ThunderDB::getAllPlannerDatas(std::vector<ompl::base::PlannerDataPtr> &plannerDatas) const
{
if (!spars_)
{
OMPL_ERROR("SPARSdb planner has not been passed into the ThunderDB yet");
return;
}
auto data(std::make_shared<base::PlannerData>(si_));
spars_->getPlannerData(*data);
plannerDatas.push_back(data);
// OMPL_DEBUG("ThunderDB::getAllPlannerDatas: Number of planner databases found: %d", plannerDatas.size());
}
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::geometric::RRTstar::setInformedSampling(bool informedSampling)
{
if (static_cast<bool>(opt_) == true)
{
if (opt_->hasCostToGoHeuristic() == false)
{
OMPL_INFORM("%s: No cost-to-go heuristic set. Informed techniques will not work well.", getName().c_str());
}
}
// This option is mutually exclusive with setSampleRejection, assert that:
if (informedSampling == true && useRejectionSampling_ == true)
{
OMPL_ERROR("%s: InformedSampling and SampleRejection are mutually exclusive options.", getName().c_str());
}
// If we just disabled tree pruning, but we are using prunedMeasure, we need to disable that as it required myself
if (informedSampling == false && getPrunedMeasure() == true)
{
setPrunedMeasure(false);
}
// Check if we're changing the setting of informed sampling. If we are, we will need to create a new sampler, which we only want to do if one is already allocated.
if (informedSampling != useInformedSampling_)
{
//If we're disabled informedSampling, and prunedMeasure is enabled, we need to disable that
if (informedSampling == false && usePrunedMeasure_ == true)
{
setPrunedMeasure(false);
}
// Store the value
useInformedSampling_ = informedSampling;
// If we currently have a sampler, we need to make a new one
if (sampler_ || infSampler_)
{
// Reset the samplers
sampler_.reset();
infSampler_.reset();
// Create the sampler
allocSampler();
}
}
}
bool ompl::tools::LightningDB::save(const std::string &fileName)
{
// Error checking
if (fileName.empty())
{
OMPL_ERROR("Empty filename passed to save function");
return false;
}
// Save database from file, track saving time
time::point start = time::now();
OMPL_INFORM("Saving database to file: %s", fileName.c_str());
// Open a binary output stream
std::ofstream outStream(fileName.c_str(), std::ios::binary);
// Convert the NN tree to a vector
std::vector<ompl::base::PlannerDataPtr> plannerDatas;
nn_->list(plannerDatas);
// Write the number of paths we will be saving
double numPaths = plannerDatas.size();
outStream << numPaths;
// Start saving each planner data object
for (std::size_t i = 0; i < numPaths; ++i)
{
ompl::base::PlannerData &pd = *plannerDatas[i].get();
// Save a single planner data
plannerDataStorage_.store(pd, outStream);
}
// Close file
outStream.close();
// Benchmark
double loadTime = time::seconds(time::now() - start);
OMPL_INFORM("Saved database to file in %f sec with %d paths", loadTime, plannerDatas.size());
numUnsavedPaths_ = 0;
return true;
}
ob::PlannerPtr allocatePlanner(ob::SpaceInformationPtr si, optimalPlanner plannerType)
{
switch (plannerType)
{
case PLANNER_BITSTAR:
{
return std::make_shared<og::BITstar>(si);
break;
}
case PLANNER_CFOREST:
{
return std::make_shared<og::CForest>(si);
break;
}
case PLANNER_FMTSTAR:
{
return std::make_shared<og::FMT>(si);
break;
}
case PLANNER_INF_RRTSTAR:
{
return std::make_shared<og::InformedRRTstar>(si);
break;
}
case PLANNER_PRMSTAR:
{
return std::make_shared<og::PRMstar>(si);
break;
}
case PLANNER_RRTSTAR:
{
return std::make_shared<og::RRTstar>(si);
break;
}
default:
{
OMPL_ERROR("Planner-type enum is not implemented in allocation function.");
return ob::PlannerPtr(); // Address compiler warning re: no return value.
break;
}
}
}
void ompl::geometric::RRTstar::setPrunedMeasure(bool informedMeasure)
{
if (static_cast<bool>(opt_) == true)
{
if (opt_->hasCostToGoHeuristic() == false)
{
OMPL_INFORM("%s: No cost-to-go heuristic set. Informed techniques will not work well.", getName().c_str());
}
}
// This option only works with informed sampling
if (informedMeasure == true && (useInformedSampling_ == false || useTreePruning_ == false))
{
OMPL_ERROR("%s: InformedMeasure requires InformedSampling and TreePruning.", getName().c_str());
}
// Check if we're changed and update parameters if we have:
if (informedMeasure != usePrunedMeasure_)
{
// Store the setting
usePrunedMeasure_ = informedMeasure;
// Update the prunedMeasure_ appropriately, if it has been configured.
if (setup_ == true)
{
if (usePrunedMeasure_)
{
prunedMeasure_ = infSampler_->getInformedMeasure(prunedCost_);
}
else
{
prunedMeasure_ = si_->getSpaceMeasure();
}
}
// And either way, update the rewiring radius if necessary
if (useKNearest_ == false)
{
calculateRewiringLowerBounds();
}
}
}
void plan(KoulesSetup& ks, double maxTime, const std::string& outputFile)
{
if (ks.solve(maxTime))
{
std::ofstream out(outputFile.c_str());
oc::PathControl path(ks.getSolutionPath());
path.interpolate();
if (!path.check())
OMPL_ERROR("Path is invalid");
writeParams(out);
path.printAsMatrix(out);
if (!ks.haveExactSolutionPath())
OMPL_INFORM("Solution is approximate. Distance to actual goal is %g",
ks.getProblemDefinition()->getSolutionDifference());
OMPL_INFORM("Output saved in %s", outputFile.c_str());
}
#if 0
// Get the planner data, save the ship's (x,y) coordinates to one file and
// the edge information to another file. This can be used for debugging
// purposes; plotting the tree of states might give you some idea of
// a planner's strategy.
ob::PlannerData pd(ks.getSpaceInformation());
ks.getPlannerData(pd);
std::ofstream vertexFile((outputFile + "-vertices").c_str()), edgeFile((outputFile + "-edges").c_str());
double* coords;
unsigned numVerts = pd.numVertices();
std::vector<unsigned int> edgeList;
for (unsigned int i = 0; i < numVerts; ++i)
{
coords = pd.getVertex(i).getState()->as<KoulesStateSpace::StateType>()->values;
vertexFile << coords[0] << ' ' << coords[1] << '\n';
pd.getEdges(i, edgeList);
for (unsigned int j = 0; j < edgeList.size(); ++j)
edgeFile << i << ' ' << edgeList[j] << '\n';
}
#endif
}
void ompl::ProlateHyperspheroid::setTransverseDiameter(double transverseDiameter)
{
if (transverseDiameter < dataPtr_->minTransverseDiameter_)
{
OMPL_ERROR("%g < %g", transverseDiameter, dataPtr_->minTransverseDiameter_);
throw Exception("Transverse diameter cannot be less than the distance between the foci.");
}
// Store and update if changed
if (dataPtr_->transverseDiameter_ != transverseDiameter)
{
// Mark as out of date
dataPtr_->isTransformUpToDate_ = false;
// Store
dataPtr_->transverseDiameter_ = transverseDiameter;
// Update the transform
updateTransformation();
}
// No else, the diameter didn't change
}
ob::OptimizationObjectivePtr allocateObjective(ob::SpaceInformationPtr si, planningObjective objectiveType)
{
switch (objectiveType)
{
case OBJECTIVE_PATHCLEARANCE:
return getClearanceObjective(si);
break;
case OBJECTIVE_PATHLENGTH:
return getPathLengthObjective(si);
break;
case OBJECTIVE_THRESHOLDPATHLENGTH:
return getThresholdPathLengthObj(si);
break;
case OBJECTIVE_WEIGHTEDCOMBO:
return getBalancedObjective1(si);
break;
default:
OMPL_ERROR("Optimization-objective enum is not implemented in allocation function.");
return ob::OptimizationObjectivePtr();
break;
}
}
ompl::base::PlannerStatus ompl::control::EST::solve(const base::PlannerTerminationCondition &ptc)
{
checkValidity();
base::Goal *goal = pdef_->getGoal().get();
base::GoalSampleableRegion *goal_s = dynamic_cast<base::GoalSampleableRegion*>(goal);
// Initializing tree with start state(s)
while (const base::State *st = pis_.nextStart())
{
Motion *motion = new Motion(siC_);
si_->copyState(motion->state, st);
siC_->nullControl(motion->control);
addMotion(motion);
}
if (tree_.grid.size() == 0)
{
OMPL_ERROR("%s: There are no valid initial states!", getName().c_str());
return base::PlannerStatus::INVALID_START;
}
// Ensure that we have a state sampler AND a control sampler
if (!sampler_)
sampler_ = si_->allocValidStateSampler();
if (!controlSampler_)
controlSampler_ = siC_->allocDirectedControlSampler();
OMPL_INFORM("%s: Starting with %u states", getName().c_str(), tree_.size);
Motion *solution = NULL;
Motion *approxsol = NULL;
double approxdif = std::numeric_limits<double>::infinity();
Motion *rmotion = new Motion(siC_);
bool solved = false;
while (!ptc)
{
// Select a state to expand the tree from
Motion *existing = selectMotion();
assert (existing);
// sample a random state (with goal biasing) near the state selected for expansion
if (goal_s && rng_.uniform01() < goalBias_ && goal_s->canSample())
goal_s->sampleGoal(rmotion->state);
else
{
if (!sampler_->sampleNear(rmotion->state, existing->state, maxDistance_))
continue;
}
// Extend a motion toward the state we just sampled
unsigned int duration = controlSampler_->sampleTo(rmotion->control, existing->control,
existing->state, rmotion->state);
// If the system was propagated for a meaningful amount of time, save into the tree
if (duration >= siC_->getMinControlDuration())
{
// create a motion to the resulting state
Motion *motion = new Motion(siC_);
si_->copyState(motion->state, rmotion->state);
siC_->copyControl(motion->control, rmotion->control);
motion->steps = duration;
motion->parent = existing;
// save the state
addMotion(motion);
// Check if this state is the goal state, or improves the best solution so far
double dist = 0.0;
solved = goal->isSatisfied(motion->state, &dist);
if (solved)
{
approxdif = dist;
solution = motion;
break;
}
if (dist < approxdif)
{
approxdif = dist;
approxsol = motion;
}
}
}
bool approximate = false;
if (solution == NULL)
{
solution = approxsol;
approximate = true;
}
// Constructing the solution path
if (solution != NULL)
{
lastGoalMotion_ = solution;
std::vector<Motion*> mpath;
while (solution != NULL)
{
mpath.push_back(solution);
solution = solution->parent;
}
PathControl *path = new PathControl(si_);
for (int i = mpath.size() - 1 ; i >= 0 ; --i)
if (mpath[i]->parent)
path->append(mpath[i]->state, mpath[i]->control, mpath[i]->steps * siC_->getPropagationStepSize());
else
path->append(mpath[i]->state);
solved = true;
pdef_->addSolutionPath(base::PathPtr(path), approximate, approxdif);
}
// Cleaning up memory
if (rmotion->state)
si_->freeState(rmotion->state);
if (rmotion->control)
siC_->freeControl(rmotion->control);
delete rmotion;
OMPL_INFORM("%s: Created %u states in %u cells", getName().c_str(), tree_.size, tree_.grid.size());
return base::PlannerStatus(solved, approximate);
}
unsigned int ompl::geometric::PathHybridization::recordPath(const base::PathPtr &pp, bool matchAcrossGaps)
{
PathGeometric *p = dynamic_cast<PathGeometric*>(pp.get());
if (!p)
{
OMPL_ERROR("Path hybridization only works for geometric paths");
return 0;
}
if (p->getSpaceInformation() != si_)
{
OMPL_ERROR("Paths for hybridization must be from the same space information");
return 0;
}
// skip empty paths
if (p->getStateCount() == 0)
return 0;
PathInfo pi(pp);
// if this path was previously included in the hybridization, skip it
if (paths_.find(pi) != paths_.end())
return 0;
// the number of connection attempts
unsigned int nattempts = 0;
// start from virtual root
Vertex v0 = boost::add_vertex(g_);
stateProperty_[v0] = pi.states_[0];
pi.vertices_.push_back(v0);
// add all the vertices of the path, and the edges between them, to the HGraph
// also compute the path length for future use (just for computational savings)
const HGraph::edge_property_type prop0(0.0);
boost::add_edge(root_, v0, prop0, g_);
double length = 0.0;
for (std::size_t j = 1 ; j < pi.states_.size() ; ++j)
{
Vertex v1 = boost::add_vertex(g_);
stateProperty_[v1] = pi.states_[j];
double weight = si_->distance(pi.states_[j-1], pi.states_[j]);
const HGraph::edge_property_type properties(weight);
boost::add_edge(v0, v1, properties, g_);
length += weight;
pi.vertices_.push_back(v1);
v0 = v1;
}
// connect to virtual goal
boost::add_edge(v0, goal_, prop0, g_);
pi.length_ = length;
// find matches with previously added paths
for (std::set<PathInfo>::const_iterator it = paths_.begin() ; it != paths_.end() ; ++it)
{
const PathGeometric *q = static_cast<const PathGeometric*>(it->path_.get());
std::vector<int> indexP, indexQ;
matchPaths(*p, *q, (pi.length_ + it->length_) / (2.0 / magic::GAP_COST_FRACTION), indexP, indexQ);
if (matchAcrossGaps)
{
int lastP = -1;
int lastQ = -1;
int gapStartP = -1;
int gapStartQ = -1;
bool gapP = false;
bool gapQ = false;
for (std::size_t i = 0 ; i < indexP.size() ; ++i)
{
// a gap is found in p
if (indexP[i] < 0)
{
// remember this as the beginning of the gap, if needed
if (!gapP)
gapStartP = i;
// mark the fact we are now in a gap on p
gapP = true;
}
else
{
// check if a gap just ended;
// if it did, try to match the endpoint with the elements in q
if (gapP)
for (std::size_t j = gapStartP ; j < i ; ++j)
{
attemptNewEdge(pi, *it, indexP[i], indexQ[j]);
++nattempts;
}
// remember the last non-negative index in p
lastP = i;
gapP = false;
}
if (indexQ[i] < 0)
{
if (!gapQ)
gapStartQ = i;
gapQ = true;
}
else
{
if (gapQ)
for (std::size_t j = gapStartQ ; j < i ; ++j)
{
attemptNewEdge(pi, *it, indexP[j], indexQ[i]);
++nattempts;
}
lastQ = i;
gapQ = false;
}
// try to match corresponding index values and gep beginnings
if (lastP >= 0 && lastQ >= 0)
{
attemptNewEdge(pi, *it, indexP[lastP], indexQ[lastQ]);
++nattempts;
}
}
}
else
{
// attempt new edge only when states align
for (std::size_t i = 0 ; i < indexP.size() ; ++i)
if (indexP[i] >= 0 && indexQ[i] >= 0)
{
attemptNewEdge(pi, *it, indexP[i], indexQ[i]);
++nattempts;
}
}
}
// remember this path is part of the hybridization
paths_.insert(pi);
return nattempts;
}
ompl::base::PlannerStatus ompl::geometric::LBKPIECE1::solve(const base::PlannerTerminationCondition &ptc)
{
checkValidity();
base::GoalSampleableRegion *goal = dynamic_cast<base::GoalSampleableRegion*>(pdef_->getGoal().get());
if (!goal)
{
OMPL_ERROR("%s: Unknown type of goal", getName().c_str());
return base::PlannerStatus::UNRECOGNIZED_GOAL_TYPE;
}
Discretization<Motion>::Coord xcoord;
while (const base::State *st = pis_.nextStart())
{
Motion* motion = new Motion(si_);
si_->copyState(motion->state, st);
motion->root = st;
motion->valid = true;
projectionEvaluator_->computeCoordinates(motion->state, xcoord);
dStart_.addMotion(motion, xcoord);
}
if (dStart_.getMotionCount() == 0)
{
OMPL_ERROR("%s: Motion planning start tree could not be initialized!", getName().c_str());
return base::PlannerStatus::INVALID_START;
}
if (!goal->couldSample())
{
OMPL_ERROR("%s: Insufficient states in sampleable goal region", getName().c_str());
return base::PlannerStatus::INVALID_GOAL;
}
if (!sampler_)
sampler_ = si_->allocStateSampler();
OMPL_INFORM("%s: Starting with %d states", getName().c_str(), (int)(dStart_.getMotionCount() + dGoal_.getMotionCount()));
base::State *xstate = si_->allocState();
bool startTree = true;
bool solved = false;
while (ptc == false)
{
Discretization<Motion> &disc = startTree ? dStart_ : dGoal_;
startTree = !startTree;
Discretization<Motion> &otherDisc = startTree ? dStart_ : dGoal_;
disc.countIteration();
// if we have not sampled too many goals already
if (dGoal_.getMotionCount() == 0 || pis_.getSampledGoalsCount() < dGoal_.getMotionCount() / 2)
{
const base::State *st = dGoal_.getMotionCount() == 0 ? pis_.nextGoal(ptc) : pis_.nextGoal();
if (st)
{
Motion* motion = new Motion(si_);
si_->copyState(motion->state, st);
motion->root = motion->state;
motion->valid = true;
projectionEvaluator_->computeCoordinates(motion->state, xcoord);
dGoal_.addMotion(motion, xcoord);
}
if (dGoal_.getMotionCount() == 0)
{
OMPL_ERROR("%s: Unable to sample any valid states for goal tree", getName().c_str());
break;
}
}
Discretization<Motion>::Cell *ecell = NULL;
Motion *existing = NULL;
disc.selectMotion(existing, ecell);
assert(existing);
sampler_->sampleUniformNear(xstate, existing->state, maxDistance_);
/* create a motion */
Motion* motion = new Motion(si_);
si_->copyState(motion->state, xstate);
motion->parent = existing;
motion->root = existing->root;
existing->children.push_back(motion);
projectionEvaluator_->computeCoordinates(motion->state, xcoord);
disc.addMotion(motion, xcoord);
/* attempt to connect trees */
Discretization<Motion>::Cell *ocell = otherDisc.getGrid().getCell(xcoord);
if (ocell && !ocell->data->motions.empty())
{
Motion* connectOther = ocell->data->motions[rng_.uniformInt(0, ocell->data->motions.size() - 1)];
if (goal->isStartGoalPairValid(startTree ? connectOther->root : motion->root, startTree ? motion->root : connectOther->root))
{
Motion* connect = new Motion(si_);
si_->copyState(connect->state, connectOther->state);
connect->parent = motion;
connect->root = motion->root;
motion->children.push_back(connect);
projectionEvaluator_->computeCoordinates(connect->state, xcoord);
disc.addMotion(connect, xcoord);
if (isPathValid(disc, connect, xstate) && isPathValid(otherDisc, connectOther, xstate))
{
if (startTree)
connectionPoint_ = std::make_pair(connectOther->state, motion->state);
else
connectionPoint_ = std::make_pair(motion->state, connectOther->state);
/* extract the motions and put them in solution vector */
std::vector<Motion*> mpath1;
while (motion != NULL)
{
mpath1.push_back(motion);
motion = motion->parent;
}
std::vector<Motion*> mpath2;
while (connectOther != NULL)
{
mpath2.push_back(connectOther);
connectOther = connectOther->parent;
}
if (startTree)
mpath1.swap(mpath2);
PathGeometric *path = new PathGeometric(si_);
path->getStates().reserve(mpath1.size() + mpath2.size());
for (int i = mpath1.size() - 1 ; i >= 0 ; --i)
path->append(mpath1[i]->state);
for (unsigned int i = 0 ; i < mpath2.size() ; ++i)
path->append(mpath2[i]->state);
pdef_->addSolutionPath(base::PathPtr(path), false, 0.0);
solved = true;
break;
}
}
}
}
si_->freeState(xstate);
OMPL_INFORM("%s: Created %u (%u start + %u goal) states in %u cells (%u start (%u on boundary) + %u goal (%u on boundary))",
getName().c_str(),
dStart_.getMotionCount() + dGoal_.getMotionCount(), dStart_.getMotionCount(), dGoal_.getMotionCount(),
dStart_.getCellCount() + dGoal_.getCellCount(), dStart_.getCellCount(), dStart_.getGrid().countExternal(),
dGoal_.getCellCount(), dGoal_.getGrid().countExternal());
return solved ? base::PlannerStatus::EXACT_SOLUTION : base::PlannerStatus::TIMEOUT;
}
