Beispiel #1
0
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));
}
Beispiel #2
0
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));
}
Beispiel #4
0
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(); });
}
Beispiel #5
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();
                               });
}
Beispiel #6
0
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));
}
Beispiel #7
0
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();
                               });
}
Beispiel #8
0
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));
}
Beispiel #9
0
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));
}
Beispiel #10
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));
}