Beispiel #1
0
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");
}
Beispiel #2
0
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));
}
Beispiel #4
0
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;
}
Beispiel #5
0
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));
}