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>
86 OBJECTIVE_PATHCLEARANCE,
88 OBJECTIVE_THRESHOLDPATHLENGTH,
89 OBJECTIVE_WEIGHTEDCOMBO
93bool argParse(
int argc,
char** argv,
double *runTimePtr, optimalPlanner *plannerPtr, planningObjective *objectivePtr, std::string *outputFilePtr);
102 ValidityChecker(
const ob::SpaceInformationPtr& si) :
107 bool isValid(
const ob::State* state)
const override
118 const auto* state2D =
119 state->
as<ob::RealVectorStateSpace::StateType>();
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;
131ob::OptimizationObjectivePtr getPathLengthObjective(
const ob::SpaceInformationPtr& si);
133ob::OptimizationObjectivePtr getThresholdPathLengthObj(
const ob::SpaceInformationPtr& si);
135ob::OptimizationObjectivePtr getClearanceObjective(
const ob::SpaceInformationPtr& si);
137ob::OptimizationObjectivePtr getBalancedObjective1(
const ob::SpaceInformationPtr& si);
139ob::OptimizationObjectivePtr getBalancedObjective2(
const ob::SpaceInformationPtr& si);
141ob::OptimizationObjectivePtr getPathLengthObjWithCostToGo(
const ob::SpaceInformationPtr& si);
143ob::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();
196ob::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();
219void 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));
239 start->as<ob::RealVectorStateSpace::StateType>()->values[0] = 0.0;
240 start->as<ob::RealVectorStateSpace::StateType>()->values[1] = 0.0;
245 goal->as<ob::RealVectorStateSpace::StateType>()->values[0] = 1.0;
246 goal->as<ob::RealVectorStateSpace::StateType>()->values[1] = 1.0;
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;
293int 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);
318ob::OptimizationObjectivePtr getPathLengthObjective(
const ob::SpaceInformationPtr& si)
320 return std::make_shared<ob::PathLengthOptimizationObjective>(si);
326ob::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()));
367ob::OptimizationObjectivePtr getClearanceObjective(
const ob::SpaceInformationPtr& si)
369 return std::make_shared<ClearanceObjective>(si);
384ob::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);
398ob::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;
409ob::OptimizationObjectivePtr getPathLengthObjWithCostToGo(
const ob::SpaceInformationPtr& si)
411 auto obj(std::make_shared<ob::PathLengthOptimizationObjective>(si));
417bool 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.
Abstract definition for a class checking the validity of states. The implementation of this class mus...
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...
Cost goalRegionCostToGo(const State *state, const Goal *goal)
For use when the cost-to-go of a state under the optimization objective is equivalent to the goal reg...
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()