37 #include <ompl/base/SpaceInformation.h>
38 #include <ompl/base/objectives/PathLengthOptimizationObjective.h>
39 #include <ompl/base/objectives/StateCostIntegralObjective.h>
40 #include <ompl/base/objectives/MaximizeMinClearanceObjective.h>
41 #include <ompl/base/spaces/RealVectorStateSpace.h>
46 #include <ompl/geometric/planners/informedtrees/BITstar.h>
47 #include <ompl/geometric/planners/cforest/CForest.h>
48 #include <ompl/geometric/planners/fmt/FMT.h>
49 #include <ompl/geometric/planners/fmt/BFMT.h>
50 #include <ompl/geometric/planners/prm/PRMstar.h>
51 #include <ompl/geometric/planners/rrt/InformedRRTstar.h>
52 #include <ompl/geometric/planners/rrt/RRTstar.h>
53 #include <ompl/geometric/planners/rrt/SORRTstar.h>
57 #include <boost/program_options.hpp>
59 #include <boost/algorithm/string.hpp>
84 enum planningObjective
86 OBJECTIVE_PATHCLEARANCE,
88 OBJECTIVE_THRESHOLDPATHLENGTH,
89 OBJECTIVE_WEIGHTEDCOMBO
93 bool argParse(
int argc,
char** argv,
double *runTimePtr, optimalPlanner *plannerPtr, planningObjective *objectivePtr, std::string *outputFilePtr);
102 ValidityChecker(
const ob::SpaceInformationPtr& si) :
118 const auto* state2D =
122 double x = state2D->values[0];
123 double y = state2D->values[1];
127 return sqrt((x-0.5)*(x-0.5) + (y-0.5)*(y-0.5)) - 0.25;
131 ob::OptimizationObjectivePtr getPathLengthObjective(
const ob::SpaceInformationPtr& si);
133 ob::OptimizationObjectivePtr getThresholdPathLengthObj(
const ob::SpaceInformationPtr& si);
135 ob::OptimizationObjectivePtr getClearanceObjective(
const ob::SpaceInformationPtr& si);
137 ob::OptimizationObjectivePtr getBalancedObjective1(
const ob::SpaceInformationPtr& si);
139 ob::OptimizationObjectivePtr getBalancedObjective2(
const ob::SpaceInformationPtr& si);
141 ob::OptimizationObjectivePtr getPathLengthObjWithCostToGo(
const ob::SpaceInformationPtr& si);
143 ob::PlannerPtr allocatePlanner(ob::SpaceInformationPtr si, optimalPlanner plannerType)
147 case PLANNER_BFMTSTAR:
149 return std::make_shared<og::BFMT>(si);
152 case PLANNER_BITSTAR:
154 return std::make_shared<og::BITstar>(si);
157 case PLANNER_CFOREST:
159 return std::make_shared<og::CForest>(si);
162 case PLANNER_FMTSTAR:
164 return std::make_shared<og::FMT>(si);
167 case PLANNER_INF_RRTSTAR:
169 return std::make_shared<og::InformedRRTstar>(si);
172 case PLANNER_PRMSTAR:
174 return std::make_shared<og::PRMstar>(si);
177 case PLANNER_RRTSTAR:
179 return std::make_shared<og::RRTstar>(si);
182 case PLANNER_SORRTSTAR:
184 return std::make_shared<og::SORRTstar>(si);
189 OMPL_ERROR(
"Planner-type enum is not implemented in allocation function.");
190 return ob::PlannerPtr();
196 ob::OptimizationObjectivePtr allocateObjective(
const ob::SpaceInformationPtr& si, planningObjective objectiveType)
198 switch (objectiveType)
200 case OBJECTIVE_PATHCLEARANCE:
201 return getClearanceObjective(si);
203 case OBJECTIVE_PATHLENGTH:
204 return getPathLengthObjective(si);
206 case OBJECTIVE_THRESHOLDPATHLENGTH:
207 return getThresholdPathLengthObj(si);
209 case OBJECTIVE_WEIGHTEDCOMBO:
210 return getBalancedObjective1(si);
213 OMPL_ERROR(
"Optimization-objective enum is not implemented in allocation function.");
214 return ob::OptimizationObjectivePtr();
219 void plan(
double runTime, optimalPlanner plannerType, planningObjective objectiveType,
const std::string& outputFile)
223 auto space(std::make_shared<ob::RealVectorStateSpace>(2));
226 space->setBounds(0.0, 1.0);
229 auto si(std::make_shared<ob::SpaceInformation>(space));
232 si->setStateValidityChecker(std::make_shared<ValidityChecker>(si));
249 auto pdef(std::make_shared<ob::ProblemDefinition>(si));
252 pdef->setStartAndGoalStates(start, goal);
256 pdef->setOptimizationObjective(allocateObjective(si, objectiveType));
260 ob::PlannerPtr optimizingPlanner = allocatePlanner(si, plannerType);
263 optimizingPlanner->setProblemDefinition(pdef);
264 optimizingPlanner->setup();
273 << optimizingPlanner->getName()
274 <<
" found a solution of length "
275 << pdef->getSolutionPath()->length()
276 <<
" with an optimization objective value of "
277 << pdef->getSolutionPath()->cost(pdef->getOptimizationObjective()) << std::endl;
281 if (!outputFile.empty())
283 std::ofstream outFile(outputFile.c_str());
284 std::static_pointer_cast<og::PathGeometric>(pdef->getSolutionPath())->
285 printAsMatrix(outFile);
290 std::cout <<
"No solution found." << std::endl;
293 int main(
int argc,
char** argv)
297 optimalPlanner plannerType;
298 planningObjective objectiveType;
299 std::string outputFile;
302 if (argParse(argc, argv, &runTime, &plannerType, &objectiveType, &outputFile))
305 plan(runTime, plannerType, objectiveType, outputFile);
318 ob::OptimizationObjectivePtr getPathLengthObjective(
const ob::SpaceInformationPtr& si)
320 return std::make_shared<ob::PathLengthOptimizationObjective>(si);
326 ob::OptimizationObjectivePtr getThresholdPathLengthObj(
const ob::SpaceInformationPtr& si)
328 auto obj(std::make_shared<ob::PathLengthOptimizationObjective>(si));
329 obj->setCostThreshold(
ob::Cost(1.51));
348 ClearanceObjective(
const ob::SpaceInformationPtr& si) :
349 ob::StateCostIntegralObjective(si, true)
360 return ob::Cost(1 / (si_->getStateValidityChecker()->clearance(s) +
361 std::numeric_limits<double>::min()));
367 ob::OptimizationObjectivePtr getClearanceObjective(
const ob::SpaceInformationPtr& si)
369 return std::make_shared<ClearanceObjective>(si);
384 ob::OptimizationObjectivePtr getBalancedObjective1(
const ob::SpaceInformationPtr& si)
386 auto lengthObj(std::make_shared<ob::PathLengthOptimizationObjective>(si));
387 auto clearObj(std::make_shared<ClearanceObjective>(si));
388 auto opt(std::make_shared<ob::MultiOptimizationObjective>(si));
389 opt->addObjective(lengthObj, 10.0);
390 opt->addObjective(clearObj, 1.0);
392 return ob::OptimizationObjectivePtr(opt);
398 ob::OptimizationObjectivePtr getBalancedObjective2(
const ob::SpaceInformationPtr& si)
400 auto lengthObj(std::make_shared<ob::PathLengthOptimizationObjective>(si));
401 auto clearObj(std::make_shared<ClearanceObjective>(si));
403 return 10.0*lengthObj + clearObj;
409 ob::OptimizationObjectivePtr getPathLengthObjWithCostToGo(
const ob::SpaceInformationPtr& si)
411 auto obj(std::make_shared<ob::PathLengthOptimizationObjective>(si));
412 obj->setCostToGoHeuristic(&ob::goalRegionCostToGo);
417 bool argParse(
int argc,
char** argv,
double* runTimePtr, optimalPlanner *plannerPtr, planningObjective *objectivePtr, std::string *outputFilePtr)
419 namespace bpo = boost::program_options;
422 bpo::options_description desc(
"Allowed options");
424 (
"help,h",
"produce help message")
425 (
"runtime,t", bpo::value<double>()->default_value(1.0),
"(Optional) Specify the runtime in seconds. Defaults to 1 and must be greater than 0.")
426 (
"planner,p", bpo::value<std::string>()->default_value(
"RRTstar"),
"(Optional) Specify the optimal planner to use, defaults to RRTstar if not given. Valid options are BFMTstar, BITstar, CForest, FMTstar, InformedRRTstar, PRMstar, RRTstar, and SORRTstar.")
427 (
"objective,o", bpo::value<std::string>()->default_value(
"PathLength"),
"(Optional) Specify the optimization objective, defaults to PathLength if not given. Valid options are PathClearance, PathLength, ThresholdPathLength, and WeightedLengthAndClearanceCombo.")
428 (
"file,f", bpo::value<std::string>()->default_value(
""),
"(Optional) Specify an output path for the found solution path.")
429 (
"info,i", bpo::value<unsigned int>()->default_value(0u),
"(Optional) Set the OMPL log level. 0 for WARN, 1 for INFO, 2 for DEBUG. Defaults to WARN.");
430 bpo::variables_map vm;
431 bpo::store(bpo::parse_command_line(argc, argv, desc), vm);
435 if (vm.count(
"help") != 0u)
437 std::cout << desc << std::endl;
442 unsigned int logLevel = vm[
"info"].as<
unsigned int>();
449 else if (logLevel == 1u)
453 else if (logLevel == 2u)
459 std::cout <<
"Invalid log-level integer." << std::endl << std::endl << desc << std::endl;
464 *runTimePtr = vm[
"runtime"].as<
double>();
467 if (*runTimePtr <= 0.0)
469 std::cout <<
"Invalid runtime." << std::endl << std::endl << desc << std::endl;
474 std::string plannerStr = vm[
"planner"].as<std::string>();
477 if (boost::iequals(
"BFMTstar", plannerStr))
479 *plannerPtr = PLANNER_BFMTSTAR;
481 else if (boost::iequals(
"BITstar", plannerStr))
483 *plannerPtr = PLANNER_BITSTAR;
485 else if (boost::iequals(
"CForest", plannerStr))
487 *plannerPtr = PLANNER_CFOREST;
489 else if (boost::iequals(
"FMTstar", plannerStr))
491 *plannerPtr = PLANNER_FMTSTAR;
493 else if (boost::iequals(
"InformedRRTstar", plannerStr))
495 *plannerPtr = PLANNER_INF_RRTSTAR;
497 else if (boost::iequals(
"PRMstar", plannerStr))
499 *plannerPtr = PLANNER_PRMSTAR;
501 else if (boost::iequals(
"RRTstar", plannerStr))
503 *plannerPtr = PLANNER_RRTSTAR;
505 else if (boost::iequals(
"SORRTstar", plannerStr))
507 *plannerPtr = PLANNER_SORRTSTAR;
511 std::cout <<
"Invalid planner string." << std::endl << std::endl << desc << std::endl;
516 std::string objectiveStr = vm[
"objective"].as<std::string>();
519 if (boost::iequals(
"PathClearance", objectiveStr))
521 *objectivePtr = OBJECTIVE_PATHCLEARANCE;
523 else if (boost::iequals(
"PathLength", objectiveStr))
525 *objectivePtr = OBJECTIVE_PATHLENGTH;
527 else if (boost::iequals(
"ThresholdPathLength", objectiveStr))
529 *objectivePtr = OBJECTIVE_THRESHOLDPATHLENGTH;
531 else if (boost::iequals(
"WeightedLengthAndClearanceCombo", objectiveStr))
533 *objectivePtr = OBJECTIVE_WEIGHTEDCOMBO;
537 std::cout <<
"Invalid objective string." << std::endl << std::endl << desc << std::endl;
542 *outputFilePtr = vm[
"file"].as<std::string>();