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/bitstar/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);
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;
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.");
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.");
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());
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);
320 return std::make_shared<ob::PathLengthOptimizationObjective>(si);
328 auto obj(std::make_shared<ob::PathLengthOptimizationObjective>(si));
329 obj->setCostThreshold(
ob::Cost(1.51));
349 ob::StateCostIntegralObjective(si, true)
360 return ob::Cost(1 / si_->getStateValidityChecker()->clearance(s));
368 return std::make_shared<ClearanceObjective>(si);
385 auto lengthObj(std::make_shared<ob::PathLengthOptimizationObjective>(si));
386 auto clearObj(std::make_shared<ClearanceObjective>(si));
387 auto opt(std::make_shared<ob::MultiOptimizationObjective>(si));
388 opt->addObjective(lengthObj, 10.0);
389 opt->addObjective(clearObj, 1.0);
399 auto lengthObj(std::make_shared<ob::PathLengthOptimizationObjective>(si));
400 auto clearObj(std::make_shared<ClearanceObjective>(si));
402 return 10.0*lengthObj + clearObj;
410 auto obj(std::make_shared<ob::PathLengthOptimizationObjective>(si));
411 obj->setCostToGoHeuristic(&ob::goalRegionCostToGo);
416 bool argParse(
int argc,
char** argv,
double* runTimePtr, optimalPlanner *plannerPtr, planningObjective *objectivePtr, std::string *outputFilePtr)
418 namespace bpo = boost::program_options;
421 bpo::options_description desc(
"Allowed options");
423 (
"help,h",
"produce help message")
424 (
"runtime,t", bpo::value<double>()->default_value(1.0),
"(Optional) Specify the runtime in seconds. Defaults to 1 and must be greater than 0.")
425 (
"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.")
426 (
"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.")
427 (
"file,f", bpo::value<std::string>()->default_value(
""),
"(Optional) Specify an output path for the found solution path.")
428 (
"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.");
429 bpo::variables_map vm;
430 bpo::store(bpo::parse_command_line(argc, argv, desc), vm);
434 if (vm.count(
"help") != 0u)
436 std::cout << desc << std::endl;
441 unsigned int logLevel = vm[
"info"].as<
unsigned int>();
448 else if (logLevel == 1u)
452 else if (logLevel == 2u)
458 std::cout <<
"Invalid log-level integer." << std::endl << std::endl << desc << std::endl;
463 *runTimePtr = vm[
"runtime"].as<
double>();
466 if (*runTimePtr <= 0.0)
468 std::cout <<
"Invalid runtime." << std::endl << std::endl << desc << std::endl;
473 std::string plannerStr = vm[
"planner"].as<std::string>();
476 if (boost::iequals(
"BFMTstar", plannerStr))
478 *plannerPtr = PLANNER_BFMTSTAR;
480 else if (boost::iequals(
"BITstar", plannerStr))
482 *plannerPtr = PLANNER_BITSTAR;
484 else if (boost::iequals(
"CForest", plannerStr))
486 *plannerPtr = PLANNER_CFOREST;
488 else if (boost::iequals(
"FMTstar", plannerStr))
490 *plannerPtr = PLANNER_FMTSTAR;
492 else if (boost::iequals(
"InformedRRTstar", plannerStr))
494 *plannerPtr = PLANNER_INF_RRTSTAR;
496 else if (boost::iequals(
"PRMstar", plannerStr))
498 *plannerPtr = PLANNER_PRMSTAR;
500 else if (boost::iequals(
"RRTstar", plannerStr))
502 *plannerPtr = PLANNER_RRTSTAR;
504 else if (boost::iequals(
"SORRTstar", plannerStr))
506 *plannerPtr = PLANNER_SORRTSTAR;
510 std::cout <<
"Invalid planner string." << std::endl << std::endl << desc << std::endl;
515 std::string objectiveStr = vm[
"objective"].as<std::string>();
518 if (boost::iequals(
"PathClearance", objectiveStr))
520 *objectivePtr = OBJECTIVE_PATHCLEARANCE;
522 else if (boost::iequals(
"PathLength", objectiveStr))
524 *objectivePtr = OBJECTIVE_PATHLENGTH;
526 else if (boost::iequals(
"ThresholdPathLength", objectiveStr))
528 *objectivePtr = OBJECTIVE_THRESHOLDPATHLENGTH;
530 else if (boost::iequals(
"WeightedLengthAndClearanceCombo", objectiveStr))
532 *objectivePtr = OBJECTIVE_WEIGHTEDCOMBO;
536 std::cout <<
"Invalid objective string." << std::endl << std::endl << desc << std::endl;
541 *outputFilePtr = vm[
"file"].as<std::string>();
Definition of a scoped state.
virtual double clearance(const State *) const
Report the distance to the nearest invalid state when starting from state. If the distance is negativ...
A shared pointer wrapper for ompl::base::Planner.
const T * as() const
Cast this instance to a desired type.
ompl::base::State StateType
Define the type of state allocated by this space.
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Defines optimization objectives where path cost can be represented as a path integral over a cost fun...
void setLogLevel(LogLevel level)
Set the minimum level of logging data to output. Messages with lower logging levels will not be recor...
Abstract definition for a class checking the validity of states. The implementation of this class mus...
StateValidityChecker(SpaceInformation *si)
Constructor.
A class to store the exit status of Planner::solve()
Definition of an abstract state.
This namespace contains sampling based planning routines shared by both planning under geometric cons...
Cost stateCost(const State *s) const override
Returns a cost with a value of 1.
virtual bool isValid(const State *state) const =0
Return true if the state state is valid. Usually, this means at least collision checking. If it is possible that ompl::base::StateSpace::interpolate() or ompl::control::ControlSpace::propagate() return states that are outside of bounds, this function should also make a call to ompl::base::SpaceInformation::satisfiesBounds().
A shared pointer wrapper for ompl::base::OptimizationObjective.
Definition of a geometric path.
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
This namespace contains code that is specific to planning under geometric constraints.