ompl::control::OpenDEStateValidityChecker::OpenDEStateValidityChecker(const SpaceInformationPtr &si) : base::StateValidityChecker(si) { if (!dynamic_cast<OpenDEStateSpace *>(si->getStateSpace().get())) throw Exception("Cannot create state validity checking for OpenDE without OpenDE state space"); osm_ = si->getStateSpace()->as<OpenDEStateSpace>(); }
ompl::control::OpenDEStatePropagator::OpenDEStatePropagator(const SpaceInformationPtr &si) : StatePropagator(si) { if (OpenDEStateSpace *oss = dynamic_cast<OpenDEStateSpace*>(si->getStateSpace().get())) env_ = oss->getEnvironment(); else throw Exception("OpenDE State Space needed for OpenDEStatePropagator"); }
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; }