ompl::geometric::RRTstar::RRTstar(const base::SpaceInformationPtr &si) : base::Planner(si, "RRTstar")
{
specs_.approximateSolutions = true;
specs_.optimizingPaths = true;
goalBias_ = 0.05;
maxDistance_ = 0.0;
delayCC_ = true;
lastGoalMotion_ = NULL;
iterations_ = 0;
collisionChecks_ = 0;
bestCost_ = base::Cost(std::numeric_limits<double>::quiet_NaN());
Planner::declareParam<double>("range", this, &RRTstar::setRange, &RRTstar::getRange, "0.:1.:10000.");
Planner::declareParam<double>("goal_bias", this, &RRTstar::setGoalBias, &RRTstar::getGoalBias, "0.:.05:1.");
Planner::declareParam<bool>("delay_collision_checking", this, &RRTstar::setDelayCC, &RRTstar::getDelayCC, "0,1");
addPlannerProgressProperty("iterations INTEGER",
boost::bind(&RRTstar::getIterationCount, this));
addPlannerProgressProperty("collision checks INTEGER",
boost::bind(&RRTstar::getCollisionCheckCount, this));
addPlannerProgressProperty("best cost REAL",
boost::bind(&RRTstar::getBestCost, this));
}
ompl::geometric::RRTsharp::RRTsharp(const base::SpaceInformationPtr &si) :
base::Planner(si, "RRTsharp"),
goalBias_(0.05),
maxDistance_(0.0),
delayCC_(true),
lastGoalMotion_(NULL),
prune_(false),
pruneStatesThreshold_(0.95),
iterations_(0),
bestCost_(std::numeric_limits<double>::quiet_NaN())
{
specs_.approximateSolutions = true;
specs_.optimizingPaths = true;
specs_.canReportIntermediateSolutions = true;
Planner::declareParam<double>("range", this, &RRTsharp::setRange, &RRTsharp::getRange, "0.:1.:10000.");
Planner::declareParam<double>("goal_bias", this, &RRTsharp::setGoalBias, &RRTsharp::getGoalBias, "0.:.05:1.");
Planner::declareParam<bool>("delay_collision_checking", this, &RRTsharp::setDelayCC, &RRTsharp::getDelayCC, "0,1");
Planner::declareParam<bool>("prune", this, &RRTsharp::setPrune, &RRTsharp::getPrune, "0,1");
Planner::declareParam<double>("prune_states_threshold", this, &RRTsharp::setPruneStatesImprovementThreshold, &RRTsharp::getPruneStatesImprovementThreshold, "0.:.01:1.");
addPlannerProgressProperty("iterations INTEGER",
boost::bind(&RRTsharp::getIterationCount, this));
addPlannerProgressProperty("best cost REAL",
boost::bind(&RRTsharp::getBestCost, this));
}
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::RRTXstatic::RRTXstatic(const base::SpaceInformationPtr &si)
: base::Planner(si, "RRTXstatic")
, mc_(opt_, pdef_)
, q_(mc_)
{
specs_.approximateSolutions = true;
specs_.optimizingPaths = true;
specs_.canReportIntermediateSolutions = true;
Planner::declareParam<double>("range", this, &RRTXstatic::setRange, &RRTXstatic::getRange, "0.:1.:10000.");
Planner::declareParam<double>("goal_bias", this, &RRTXstatic::setGoalBias, &RRTXstatic::getGoalBias, "0.:.05:1.");
Planner::declareParam<double>("epsilon", this, &RRTXstatic::setEpsilon, &RRTXstatic::getEpsilon, "0.:.01:10.");
Planner::declareParam<double>("rewire_factor", this, &RRTXstatic::setRewireFactor, &RRTXstatic::getRewireFactor,
"1.0:0.01:2."
"0");
Planner::declareParam<bool>("use_k_nearest", this, &RRTXstatic::setKNearest, &RRTXstatic::getKNearest, "0,1");
Planner::declareParam<bool>("update_children", this, &RRTXstatic::setUpdateChildren, &RRTXstatic::getUpdateChildren,
"0,1");
Planner::declareParam<int>("rejection_variant", this, &RRTXstatic::setVariant, &RRTXstatic::getVariant, "0:3");
Planner::declareParam<double>("rejection_variant_alpha", this, &RRTXstatic::setAlpha, &RRTXstatic::getAlpha, "0.:"
"1.");
Planner::declareParam<bool>("informed_sampling", this, &RRTXstatic::setInformedSampling,
&RRTXstatic::getInformedSampling, "0,"
"1");
Planner::declareParam<bool>("sample_rejection", this, &RRTXstatic::setSampleRejection,
&RRTXstatic::getSampleRejection, "0,1");
Planner::declareParam<bool>("number_sampling_attempts", this, &RRTXstatic::setNumSamplingAttempts,
&RRTXstatic::getNumSamplingAttempts, "10:10:100000");
addPlannerProgressProperty("iterations INTEGER", [this] { return numIterationsProperty(); });
addPlannerProgressProperty("motions INTEGER", [this] { return numMotionsProperty(); });
addPlannerProgressProperty("best cost REAL", [this] { return bestCostProperty(); });
}
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::RRTstar::RRTstar(const base::SpaceInformationPtr &si) :
base::Planner(si, "RRTstar"),
goalBias_(0.05),
maxDistance_(0.0),
useKNearest_(true),
rewireFactor_(1.1),
k_rrg_(0u),
r_rrg_(0.0),
delayCC_(true),
lastGoalMotion_(nullptr),
useTreePruning_(false),
pruneThreshold_(0.05),
usePrunedMeasure_(false),
useInformedSampling_(false),
useRejectionSampling_(false),
useNewStateRejection_(false),
useAdmissibleCostToCome_(true),
numSampleAttempts_ (100u),
bestCost_(std::numeric_limits<double>::quiet_NaN()),
prunedCost_(std::numeric_limits<double>::quiet_NaN()),
prunedMeasure_(0.0),
iterations_(0u)
{
specs_.approximateSolutions = true;
specs_.optimizingPaths = true;
specs_.canReportIntermediateSolutions = true;
Planner::declareParam<double>("range", this, &RRTstar::setRange, &RRTstar::getRange, "0.:1.:10000.");
Planner::declareParam<double>("goal_bias", this, &RRTstar::setGoalBias, &RRTstar::getGoalBias, "0.:.05:1.");
Planner::declareParam<double>("rewire_factor", this, &RRTstar::setRewireFactor, &RRTstar::getRewireFactor, "1.0:0.01:2.0");
Planner::declareParam<bool>("use_k_nearest", this, &RRTstar::setKNearest, &RRTstar::getKNearest, "0,1");
Planner::declareParam<bool>("delay_collision_checking", this, &RRTstar::setDelayCC, &RRTstar::getDelayCC, "0,1");
Planner::declareParam<bool>("tree_pruning", this, &RRTstar::setTreePruning, &RRTstar::getTreePruning, "0,1");
Planner::declareParam<double>("prune_threshold", this, &RRTstar::setPruneThreshold, &RRTstar::getPruneThreshold, "0.:.01:1.");
Planner::declareParam<bool>("pruned_measure", this, &RRTstar::setPrunedMeasure, &RRTstar::getPrunedMeasure, "0,1");
Planner::declareParam<bool>("informed_sampling", this, &RRTstar::setInformedSampling, &RRTstar::getInformedSampling, "0,1");
Planner::declareParam<bool>("sample_rejection", this, &RRTstar::setSampleRejection, &RRTstar::getSampleRejection, "0,1");
Planner::declareParam<bool>("new_state_rejection", this, &RRTstar::setNewStateRejection, &RRTstar::getNewStateRejection, "0,1");
Planner::declareParam<bool>("use_admissible_heuristic", this, &RRTstar::setAdmissibleCostToCome, &RRTstar::getAdmissibleCostToCome, "0,1");
Planner::declareParam<bool>("focus_search", this, &RRTstar::setFocusSearch, &RRTstar::getFocusSearch, "0,1");
Planner::declareParam<bool>("number_sampling_attempts", this, &RRTstar::setNumSamplingAttempts, &RRTstar::getNumSamplingAttempts, "10:10:100000");
addPlannerProgressProperty("iterations INTEGER",
std::bind(&RRTstar::numIterationsProperty, this));
addPlannerProgressProperty("best cost REAL",
std::bind(&RRTstar::bestCostProperty, this));
}
ompl::geometric::LazyLBTRRT::LazyLBTRRT(const base::SpaceInformationPtr &si)
: base::Planner(si, "LazyLBTRRT")
{
specs_.approximateSolutions = true;
specs_.directed = true;
Planner::declareParam<double>("range", this, &LazyLBTRRT::setRange, &LazyLBTRRT::getRange, "0.:1.:10000.");
Planner::declareParam<double>("goal_bias", this, &LazyLBTRRT::setGoalBias, &LazyLBTRRT::getGoalBias, "0.:.05:1.");
Planner::declareParam<double>("epsilon", this, &LazyLBTRRT::setApproximationFactor,
&LazyLBTRRT::getApproximationFactor, "0.:.1:10.");
addPlannerProgressProperty("iterations INTEGER", [this]
{
return getIterationCount();
});
addPlannerProgressProperty("best cost REAL", [this]
{
return getBestCost();
});
}
ompl::geometric::LBTRRT::LBTRRT(const base::SpaceInformationPtr &si) :
base::Planner(si, "LBTRRT"),
goalBias_(0.05),
maxDistance_(0.0),
epsilon_(0.4),
lastGoalMotion_(nullptr),
iterations_(0)
{
specs_.approximateSolutions = true;
specs_.directed = true;
Planner::declareParam<double>("range", this, &LBTRRT::setRange, &LBTRRT::getRange, "0.:1.:10000.");
Planner::declareParam<double>("goal_bias", this, &LBTRRT::setGoalBias, &LBTRRT::getGoalBias, "0.:.05:1.");
Planner::declareParam<double>("epsilon", this, &LBTRRT::setApproximationFactor, &LBTRRT::getApproximationFactor, "0.:.1:10.");
addPlannerProgressProperty("iterations INTEGER",
std::bind(&LBTRRT::getIterationCount, this));
addPlannerProgressProperty("best cost REAL",
std::bind(&LBTRRT::getBestCost, this));
}
ompl::geometric::CForest::CForest(const base::SpaceInformationPtr &si) : base::Planner(si, "CForest")
{
specs_.optimizingPaths = true;
specs_.multithreaded = true;
numPathsShared_ = 0;
numStatesShared_ = 0;
focusSearch_ = true;
numThreads_ = std::max(std::thread::hardware_concurrency(), 2u);
Planner::declareParam<bool>("focus_search", this, &CForest::setFocusSearch, &CForest::getFocusSearch, "0,1");
Planner::declareParam<unsigned int>("num_threads", this, &CForest::setNumThreads, &CForest::getNumThreads, "0:64");
addPlannerProgressProperty("best cost REAL",
std::bind(&CForest::getBestCost, this));
addPlannerProgressProperty("shared paths INTEGER",
std::bind(&CForest::getNumPathsShared, this));
addPlannerProgressProperty("shared states INTEGER",
std::bind(&CForest::getNumStatesShared, this));
}
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));
}
