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>();
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition of a scoped state.
Defines optimization objectives where path cost can be represented as a path integral over a cost fun...
Cost stateCost(const State *s) const override
Returns a cost with a value of 1.
ompl::base::State StateType
Define the type of state allocated by this space.
Abstract definition for a class checking the validity of states. The implementation of this class mus...
virtual bool isValid(const State *state) const =0
Return true if the state state is valid. Usually, this means at least collision checking....
virtual double clearance(const State *) const
Report the distance to the nearest invalid state when starting from state. If the distance is negativ...
StateValidityChecker(SpaceInformation *si)
Constructor.
Definition of an abstract state.
const T * as() const
Cast this instance to a desired type.
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
This namespace contains sampling based planning routines shared by both planning under geometric cons...
This namespace contains code that is specific to planning under geometric constraints.
void setLogLevel(LogLevel level)
Set the minimum level of logging data to output. Messages with lower logging levels will not be recor...
A class to store the exit status of Planner::solve()