/**
* @brief Initializer for 3D points (translation only)
*
* @param sampler ...
* @param limits ...
* @return void
*/
void PlannerOMPL::initialize(Sampler *sampler)
{
QList<QPair<float,float> > limits = sampler->getLimits();
qDebug() << __FUNCTION__ << limits;
assert(limits.size() == 3);
//Create state space as R3
ob::RealVectorStateSpace *space = new ob::RealVectorStateSpace();
space->addDimension("X", limits[0].first, limits[0].second);
space->addDimension("Y", limits[1].first, limits[1].second);
space->addDimension("Z", limits[2].first, limits[2].second);
// qDebug() << space->getBounds().low[0] << space->getBounds().high[0] ;
// qDebug() << space->getBounds().low[1] << space->getBounds().high[1] ;
// qDebug() << space->getBounds().low[2] << space->getBounds().high[2] ;
//Setup class
simpleSetUp.reset(new og::SimpleSetup(ob::StateSpacePtr(space)));
//set Sampler
simpleSetUp->getSpaceInformation()->setValidStateSamplerAllocator(allocOBValidStateSampler);
simpleSetUp->setStateValidityChecker(boost::bind(&Sampler::isStateValid, sampler, _1));
ob::ProblemDefinitionPtr pdef(new ob::ProblemDefinition(simpleSetUp->getSpaceInformation()));
pdef->setOptimizationObjective(getPathLengthObjective(simpleSetUp->getSpaceInformation()));
space->setup();
simpleSetUp->getSpaceInformation()->setStateValidityCheckingResolution(0.1);
//simpleSetUp->getSpaceInformation()->setStateValidityCheckingResolution(100 / space->getMaximumExtent());
//simpleSetUp->setPlanner(ob::PlannerPtr(new og::RRTConnect(simpleSetUp->getSpaceInformation())));
simpleSetUp->setPlanner(ob::PlannerPtr(new og::RRTstar(simpleSetUp->getSpaceInformation())));
simpleSetUp->getPlanner()->as<og::RRTstar>()->setProblemDefinition(pdef);
//simpleSetUp->getPlanner()->as<og::RRTConnect>()->setRange(2000);
simpleSetUp->getPlanner()->as<og::RRTConnect>()->setRange(0.5);
}
ob::OptimizationObjectivePtr allocateObjective(ob::SpaceInformationPtr si, planningObjective objectiveType)
{
switch (objectiveType)
{
case OBJECTIVE_PATHCLEARANCE:
return getClearanceObjective(si);
break;
case OBJECTIVE_PATHLENGTH:
return getPathLengthObjective(si);
break;
case OBJECTIVE_THRESHOLDPATHLENGTH:
return getThresholdPathLengthObj(si);
break;
case OBJECTIVE_WEIGHTEDCOMBO:
return getBalancedObjective1(si);
break;
default:
OMPL_ERROR("Optimization-objective enum is not implemented in allocation function.");
return ob::OptimizationObjectivePtr();
break;
}
}
void plan(int argc, char** argv)
{
// Construct the robot state space in which we're planning. We're
// planning in [0,1]x[0,1], a subset of R^2.
ob::StateSpacePtr space(new ob::RealVectorStateSpace(2));
// Set the bounds of space to be in [0,1].
space->as<ob::RealVectorStateSpace>()->setBounds(0.0, 1.0);
// Construct a space information instance for this state space
ob::SpaceInformationPtr si(new ob::SpaceInformation(space));
// Set the object used to check which states in the space are valid
si->setStateValidityChecker(ob::StateValidityCheckerPtr(new ValidityChecker(si)));
si->setup();
// Set our robot's starting state to be the bottom-left corner of
// the environment, or (0,0).
ob::ScopedState<> start(space);
start->as<ob::RealVectorStateSpace::StateType>()->values[0] = 0.0;
start->as<ob::RealVectorStateSpace::StateType>()->values[1] = 0.0;
// Set our robot's goal state to be the top-right corner of the
// environment, or (1,1).
ob::ScopedState<> goal(space);
goal->as<ob::RealVectorStateSpace::StateType>()->values[0] = 1.0;
goal->as<ob::RealVectorStateSpace::StateType>()->values[1] = 1.0;
// Create a problem instance
ob::ProblemDefinitionPtr pdef(new ob::ProblemDefinition(si));
// Set the start and goal states
pdef->setStartAndGoalStates(start, goal);
// Since we want to find an optimal plan, we need to define what
// is optimal with an OptimizationObjective structure. Un-comment
// exactly one of the following 6 lines to see some examples of
// optimization objectives.
pdef->setOptimizationObjective(getPathLengthObjective(si));
// pdef->setOptimizationObjective(getThresholdPathLengthObj(si));
// pdef->setOptimizationObjective(getClearanceObjective(si));
// pdef->setOptimizationObjective(getBalancedObjective1(si));
// pdef->setOptimizationObjective(getBalancedObjective2(si));
// pdef->setOptimizationObjective(getPathLengthObjWithCostToGo(si));
// Construct our optimal planner using the RRTstarAR algorithm.
ob::PlannerPtr optimizingPlanner(new og::RRTstarAR(si));
// Set the problem instance for our planner to solve
optimizingPlanner->setProblemDefinition(pdef);
optimizingPlanner->setup();
// attempt to solve the planning problem within one second of
// planning time
ob::PlannerStatus solved = optimizingPlanner->solve(1.0);
if (solved)
{
// Output the length of the path found
std::cout
<< "Found solution of path length "
<< pdef->getSolutionPath()->length()
<< " with an optimization objective value of "
<< pdef->getSolutionPath()->cost(pdef->getOptimizationObjective()) << std::endl;
// If a filename was specified, output the path as a matrix to
// that file for visualization
if (argc > 1)
{
std::ofstream outFile(argv[1]);
boost::static_pointer_cast<og::PathGeometric>(pdef->getSolutionPath())->
printAsMatrix(outFile);
outFile.close();
}
}
else
std::cout << "No solution found." << std::endl;
}
