void DensoSetup::initialize(RobotBasePtr probot) { const ob::StateSpacePtr &sSpace = getStateSpace(); sSpace->setup(); ob::ScopedState<> start(sSpace); ob::ScopedState<> goal(sSpace); std::vector<double> startVec(sSpace->getDimension(), 0.); std::vector<double> goalVec(sSpace->getDimension(), 0.); unsigned int ndof = si_->getStateSpace()->as<DensoStateSpace>()-> _probot->GetActiveDOF(); for (unsigned int i = 0; i < ndof; i++) goalVec[i] = 1.0; if (_useRealScene) { startVec[0] = -1.576; startVec[1] = 1.374; startVec[2] = 0.0; startVec[3] = 0.0; startVec[4] = -1.36751533; startVec[5] = 1.615; goalVec[0] = 1.55; goalVec[1] = 1.35; goalVec[2] = 0.1; goalVec[3] = -000057; goalVec[4] = -1.40754; goalVec[5] = 1.6; } sSpace->copyFromReals(start.get(), startVec); sSpace->copyFromReals(goal.get(), goalVec); /* set start, goal, and threshold*/ setStartAndGoalStates(start, goal, threshold); /* set propagation step size and min-max propagation steps*/ si_->setPropagationStepSize(propagationStepSize); si_->setMinMaxControlDuration(propagationMinSteps, propagationMaxSteps); /* initialize a new KPIECE planner */ setPlanner(ob::PlannerPtr(new oc::KPIECE1(si_))); /* set default parameters */ getPlanner()->as<oc::KPIECE1>()->setMaxCloseSamplesCount(maxCloseSamplesCount); getPlanner()->as<oc::KPIECE1>()->setGoalBias(goalBias); /* set a state validity checker */ setStateValidityChecker(ob::StateValidityCheckerPtr (new DensoStateValidityChecker(si_))); si_->setStateValidityCheckingResolution(0.05); //5% /* set a state propagator */ setStatePropagator(oc::StatePropagatorPtr(new DensoStatePropagator(si_))); }
trajectory::InterpolatedPtr planOMPL( const statespace::StateSpace::State* _start, const statespace::StateSpace::State* _goal, statespace::ConstStateSpacePtr _stateSpace, statespace::InterpolatorPtr _interpolator, distance::DistanceMetricPtr _dmetric, constraint::SampleablePtr _sampler, constraint::TestablePtr _validityConstraint, constraint::TestablePtr _boundsConstraint, constraint::ProjectablePtr _boundsProjector, double _maxPlanTime, double _maxDistanceBtwValidityChecks) { // Create a SpaceInformation. This function will ensure state space matching auto si = getSpaceInformation( _stateSpace, _interpolator, std::move(_dmetric), std::move(_sampler), std::move(_validityConstraint), std::move(_boundsConstraint), std::move(_boundsProjector), _maxDistanceBtwValidityChecks); // Start and states auto pdef = ompl_make_shared<::ompl::base::ProblemDefinition>(si); auto sspace = ompl_static_pointer_cast<GeometricStateSpace>(si->getStateSpace()); auto start = sspace->allocState(_start); auto goal = sspace->allocState(_goal); // ProblemDefinition clones states and keeps them internally pdef->setStartAndGoalStates(start, goal); sspace->freeState(start); sspace->freeState(goal); auto planner = ompl_make_shared<PlannerType>(si); return planOMPL( planner, pdef, std::move(_stateSpace), std::move(_interpolator), _maxPlanTime); }
void SuperBotSetup::initialize(RobotBasePtr probot) { const ob::StateSpacePtr &sSpace = getStateSpace(); sSpace->setup(); ob::ScopedState<> start(sSpace); ob::ScopedState<> goal(sSpace); std::vector<double> startVec(sSpace->getDimension(), 0.); std::vector<double> goalVec(sSpace->getDimension(), 0.); unsigned int ndof = si_->getStateSpace()->as<SuperBotStateSpace>()->_probot->GetActiveDOF(); for (unsigned int i = 0; i < ndof; i++) goalVec[i] = 1.0; sSpace->copyFromReals(start.get(), startVec); sSpace->copyFromReals(goal.get(), goalVec); // set start, goal, and threshold setStartAndGoalStates(start, goal, threshold); // set propagation step size and min-max propagation steps si_->setPropagationStepSize(propagationStepSize); si_->setMinMaxControlDuration(propagationMinSteps, propagationMaxSteps); // initialize a new KPIECE planner setPlanner(ob::PlannerPtr(new oc::KPIECE1(si_))); // set default parameters getPlanner()->as<oc::KPIECE1>()->setMaxCloseSamplesCount(maxCloseSamplesCount); getPlanner()->as<oc::KPIECE1>()->setGoalBias(goalBias); // set a state validity checker setStateValidityChecker(ob::StateValidityCheckerPtr(new SuperBotStateValidityChecker(si_))); si_->setStateValidityCheckingResolution(0.05); //5% // set a state propagator setStatePropagator(oc::StatePropagatorPtr(new SuperBotStatePropagator(si_))); }