Ejemplo n.º 1
0
/** \brief Set the goal for planning. This call is not
    needed if setStartAndGoalStates() has been called. */
void ompl::geometric::SimpleSetup::setGoal(const base::GoalPtr &goal)
{
    pdef_->setGoal(goal);

    if (goal && goal->hasType(base::GOAL_SAMPLEABLE_REGION))
        psk_ = std::make_shared<PathSimplifier>(si_, pdef_->getGoal());
    else
        psk_ = std::make_shared<PathSimplifier>(si_);
}
Ejemplo n.º 2
0
ompl::base::PlannerPtr ompl::tools::SelfConfig::getDefaultPlanner(const base::GoalPtr &goal)
{
    base::PlannerPtr planner;
    if (!goal)
        throw Exception("Unable to allocate default planner for unspecified goal definition");

    base::SpaceInformationPtr si(goal->getSpaceInformation());
    control::SpaceInformationPtr siC(boost::dynamic_pointer_cast<control::SpaceInformation, base::SpaceInformation>(si));
    if (siC) // kinodynamic planning
    {
        // if we have a default projection
        if (siC->getStateSpace()->hasDefaultProjection())
            planner = base::PlannerPtr(new control::KPIECE1(siC));
        // otherwise use a single-tree planner
        else
            planner = base::PlannerPtr(new control::RRT(siC));
    }
    // if we can sample the goal region, use a bi-directional planner
    else if (goal->hasType(base::GOAL_SAMPLEABLE_REGION))
    {
        // if we have a default projection
        if (goal->getSpaceInformation()->getStateSpace()->hasDefaultProjection())
            planner = base::PlannerPtr(new geometric::LBKPIECE1(goal->getSpaceInformation()));
        else
            planner = base::PlannerPtr(new geometric::RRTConnect(goal->getSpaceInformation()));
    }
    // otherwise use a single-tree planner
    else
    {
        // if we have a default projection
        if (goal->getSpaceInformation()->getStateSpace()->hasDefaultProjection())
            planner = base::PlannerPtr(new geometric::KPIECE1(goal->getSpaceInformation()));
        else
            planner = base::PlannerPtr(new geometric::RRT(goal->getSpaceInformation()));
    }

    if (!planner)
        throw Exception("Unable to allocate default planner");

    return planner;
}
Ejemplo n.º 3
0
ompl::base::PlannerPtr ompl::control::getDefaultPlanner(const base::GoalPtr &goal)
{
    base::PlannerPtr planner;
    if (!goal)
        throw Exception("Unable to allocate default planner for unspecified goal definition");

    SpaceInformationPtr si = boost::static_pointer_cast<SpaceInformation, base::SpaceInformation>(goal->getSpaceInformation());
    if (si->getStateSpace()->hasDefaultProjection())
        planner = base::PlannerPtr(new KPIECE1(si));
    else
        planner = base::PlannerPtr(new RRT(si));

    return planner;
}