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_)));
}
