ompl::geometric::SPARS::SPARS(const base::SpaceInformationPtr &si) :
base::Planner(si, "SPARS"),
geomPath_(si),
stateProperty_(boost::get(vertex_state_t(), g_)),
sparseStateProperty_(boost::get(vertex_state_t(), s_)),
sparseColorProperty_(boost::get(vertex_color_t(), s_)),
representativesProperty_(boost::get(vertex_representative_t(), g_)),
nonInterfaceListsProperty_(boost::get(vertex_list_t(), s_)),
interfaceListsProperty_(boost::get(vertex_interface_list_t(), s_)),
weightProperty_(boost::get(boost::edge_weight, g_)),
sparseDJSets_(boost::get(boost::vertex_rank, s_),
boost::get(boost::vertex_predecessor, s_)),
consecutiveFailures_(0),
iterations_(0),
stretchFactor_(3.),
maxFailures_(1000),
addedSolution_(false),
denseDeltaFraction_(.001),
sparseDeltaFraction_(.25),
denseDelta_(0.),
sparseDelta_(0.)
{
specs_.recognizedGoal = base::GOAL_SAMPLEABLE_REGION;
specs_.approximateSolutions = false;
specs_.optimizingPaths = false;
psimp_.reset(new PathSimplifier(si_));
psimp_->freeStates(false);
Planner::declareParam<double>("stretch_factor", this, &SPARS::setStretchFactor, &SPARS::getStretchFactor, "1.1:0.1:3.0");
Planner::declareParam<double>("sparse_delta_fraction", this, &SPARS::setSparseDeltaFraction, &SPARS::getSparseDeltaFraction, "0.0:0.01:1.0");
Planner::declareParam<double>("dense_delta_fraction", this, &SPARS::setDenseDeltaFraction, &SPARS::getDenseDeltaFraction, "0.0:0.0001:0.1");
Planner::declareParam<unsigned int>("max_failures", this, &SPARS::setMaxFailures, &SPARS::getMaxFailures, "100:10:3000");
}
ompl::geometric::SPARS::SPARS(const base::SpaceInformationPtr &si)
: base::Planner(si, "SPARS")
, geomPath_(si)
, stateProperty_(boost::get(vertex_state_t(), g_))
, sparseStateProperty_(boost::get(vertex_state_t(), s_))
, sparseColorProperty_(boost::get(vertex_color_t(), s_))
, representativesProperty_(boost::get(vertex_representative_t(), g_))
, nonInterfaceListsProperty_(boost::get(vertex_list_t(), s_))
, interfaceListsProperty_(boost::get(vertex_interface_list_t(), s_))
, weightProperty_(boost::get(boost::edge_weight, g_))
, sparseDJSets_(boost::get(boost::vertex_rank, s_), boost::get(boost::vertex_predecessor, s_))
, consecutiveFailures_(0)
, stretchFactor_(3.)
, maxFailures_(1000)
, addedSolution_(false)
, denseDeltaFraction_(.001)
, sparseDeltaFraction_(.25)
, denseDelta_(0.)
, sparseDelta_(0.)
, iterations_(0)
, bestCost_(std::numeric_limits<double>::quiet_NaN())
{
specs_.recognizedGoal = base::GOAL_SAMPLEABLE_REGION;
specs_.approximateSolutions = false;
specs_.optimizingPaths = true;
specs_.multithreaded = true;
psimp_ = std::make_shared<PathSimplifier>(si_);
psimp_->freeStates(false);
Planner::declareParam<double>("stretch_factor", this, &SPARS::setStretchFactor, &SPARS::getStretchFactor, "1.1:0.1:"
"3.0");
Planner::declareParam<double>("sparse_delta_fraction", this, &SPARS::setSparseDeltaFraction,
&SPARS::getSparseDeltaFraction, "0.0:0.01:1.0");
Planner::declareParam<double>("dense_delta_fraction", this, &SPARS::setDenseDeltaFraction,
&SPARS::getDenseDeltaFraction, "0.0:0.0001:0.1");
Planner::declareParam<unsigned int>("max_failures", this, &SPARS::setMaxFailures, &SPARS::getMaxFailures, "100:10:"
"3000");
addPlannerProgressProperty("iterations INTEGER", [this]
{
return getIterationCount();
});
addPlannerProgressProperty("best cost REAL", [this]
{
return getBestCost();
});
}
ompl::geometric::LazyPRM::LazyPRM(const base::SpaceInformationPtr &si, bool starStrategy) :
base::Planner(si, "LazyPRM"),
starStrategy_(starStrategy),
userSetConnectionStrategy_(false),
maxDistance_(0.0),
indexProperty_(boost::get(boost::vertex_index_t(), g_)),
stateProperty_(boost::get(vertex_state_t(), g_)),
weightProperty_(boost::get(boost::edge_weight, g_)),
vertexComponentProperty_(boost::get(vertex_component_t(), g_)),
vertexValidityProperty_(boost::get(vertex_flags_t(), g_)),
edgeValidityProperty_(boost::get(edge_flags_t(), g_)),
componentCount_(0),
bestCost_(std::numeric_limits<double>::quiet_NaN()),
iterations_(0)
{
specs_.recognizedGoal = base::GOAL_SAMPLEABLE_REGION;
specs_.approximateSolutions = false;
specs_.optimizingPaths = true;
Planner::declareParam<double>("range", this, &LazyPRM::setRange, &LazyPRM::getRange, "0.:1.:10000.");
if (!starStrategy_)
Planner::declareParam<unsigned int>("max_nearest_neighbors", this, &LazyPRM::setMaxNearestNeighbors, std::string("8:1000"));
addPlannerProgressProperty("iterations INTEGER",
boost::bind(&LazyPRM::getIterationCount, this));
addPlannerProgressProperty("best cost REAL",
boost::bind(&LazyPRM::getBestCost, this));
addPlannerProgressProperty("milestone count INTEGER",
boost::bind(&LazyPRM::getMilestoneCountString, this));
addPlannerProgressProperty("edge count INTEGER",
boost::bind(&LazyPRM::getEdgeCountString, this));
}
ompl::geometric::PathHybridization::PathHybridization(base::SpaceInformationPtr si)
: si_(std::move(si)), stateProperty_(boost::get(vertex_state_t(), g_)), name_("PathHybridization")
{
root_ = boost::add_vertex(g_);
stateProperty_[root_] = nullptr;
goal_ = boost::add_vertex(g_);
stateProperty_[goal_] = nullptr;
}
ompl::geometric::SPARStwo::SPARStwo(const base::SpaceInformationPtr &si) :
base::Planner(si, "SPARStwo"),
stretchFactor_(3.),
sparseDeltaFraction_(.25),
denseDeltaFraction_(.001),
maxFailures_(5000),
nearSamplePoints_((2*si_->getStateDimension())),
stateProperty_(boost::get(vertex_state_t(), g_)),
weightProperty_(boost::get(boost::edge_weight, g_)),
colorProperty_(boost::get(vertex_color_t(), g_)),
interfaceDataProperty_(boost::get(vertex_interface_data_t(), g_)),
disjointSets_(boost::get(boost::vertex_rank, g_),
boost::get(boost::vertex_predecessor, g_)),
addedSolution_(false),
consecutiveFailures_(0),
sparseDelta_(0.),
denseDelta_(0.),
iterations_(0),
bestCost_(std::numeric_limits<double>::quiet_NaN())
{
specs_.recognizedGoal = base::GOAL_SAMPLEABLE_REGION;
specs_.approximateSolutions = false;
specs_.optimizingPaths = true;
specs_.multithreaded = true;
psimp_.reset(new PathSimplifier(si_));
Planner::declareParam<double>("stretch_factor", this, &SPARStwo::setStretchFactor, &SPARStwo::getStretchFactor, "1.1:0.1:3.0");
Planner::declareParam<double>("sparse_delta_fraction", this, &SPARStwo::setSparseDeltaFraction, &SPARStwo::getSparseDeltaFraction, "0.0:0.01:1.0");
Planner::declareParam<double>("dense_delta_fraction", this, &SPARStwo::setDenseDeltaFraction, &SPARStwo::getDenseDeltaFraction, "0.0:0.0001:0.1");
Planner::declareParam<unsigned int>("max_failures", this, &SPARStwo::setMaxFailures, &SPARStwo::getMaxFailures, "100:10:3000");
addPlannerProgressProperty("iterations INTEGER",
boost::bind(&SPARStwo::getIterationCount, this));
addPlannerProgressProperty("best cost REAL",
boost::bind(&SPARStwo::getBestCost, this));
}
