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/AITstar.h>
47#include <ompl/geometric/planners/informedtrees/BITstar.h>
48#include <ompl/geometric/planners/cforest/CForest.h>
49#include <ompl/geometric/planners/fmt/FMT.h>
50#include <ompl/geometric/planners/fmt/BFMT.h>
51#include <ompl/geometric/planners/prm/PRMstar.h>
52#include <ompl/geometric/planners/rrt/InformedRRTstar.h>
53#include <ompl/geometric/planners/rrt/RRTstar.h>
54#include <ompl/geometric/planners/rrt/SORRTstar.h>
58#include <boost/program_options.hpp>
60#include <boost/algorithm/string.hpp>
88 OBJECTIVE_PATHCLEARANCE,
90 OBJECTIVE_THRESHOLDPATHLENGTH,
91 OBJECTIVE_WEIGHTEDCOMBO
95bool argParse(
int argc,
char** argv,
double *runTimePtr, optimalPlanner *plannerPtr, planningObjective *objectivePtr, std::string *outputFilePtr);
104 ValidityChecker(
const ob::SpaceInformationPtr& si) :
109 bool isValid(
const ob::State* state)
const override
116 double clearance(
const ob::State* state)
const override
120 const auto* state2D =
124 double x = state2D->values[0];
125 double y = state2D->values[1];
129 return sqrt((x-0.5)*(x-0.5) + (y-0.5)*(y-0.5)) - 0.25;
133ob::OptimizationObjectivePtr getPathLengthObjective(
const ob::SpaceInformationPtr& si);
135ob::OptimizationObjectivePtr getThresholdPathLengthObj(
const ob::SpaceInformationPtr& si);
137ob::OptimizationObjectivePtr getClearanceObjective(
const ob::SpaceInformationPtr& si);
139ob::OptimizationObjectivePtr getBalancedObjective1(
const ob::SpaceInformationPtr& si);
141ob::OptimizationObjectivePtr getBalancedObjective2(
const ob::SpaceInformationPtr& si);
143ob::OptimizationObjectivePtr getPathLengthObjWithCostToGo(
const ob::SpaceInformationPtr& si);
145ob::PlannerPtr allocatePlanner(ob::SpaceInformationPtr si, optimalPlanner plannerType)
149 case PLANNER_AITSTAR:
151 return std::make_shared<og::AITstar>(si);
154 case PLANNER_BFMTSTAR:
156 return std::make_shared<og::BFMT>(si);
159 case PLANNER_BITSTAR:
161 return std::make_shared<og::BITstar>(si);
164 case PLANNER_CFOREST:
166 return std::make_shared<og::CForest>(si);
169 case PLANNER_FMTSTAR:
171 return std::make_shared<og::FMT>(si);
174 case PLANNER_INF_RRTSTAR:
176 return std::make_shared<og::InformedRRTstar>(si);
179 case PLANNER_PRMSTAR:
181 return std::make_shared<og::PRMstar>(si);
184 case PLANNER_RRTSTAR:
186 return std::make_shared<og::RRTstar>(si);
189 case PLANNER_SORRTSTAR:
191 return std::make_shared<og::SORRTstar>(si);
196 OMPL_ERROR(
"Planner-type enum is not implemented in allocation function.");
197 return ob::PlannerPtr();
203ob::OptimizationObjectivePtr allocateObjective(
const ob::SpaceInformationPtr& si, planningObjective objectiveType)
205 switch (objectiveType)
207 case OBJECTIVE_PATHCLEARANCE:
208 return getClearanceObjective(si);
210 case OBJECTIVE_PATHLENGTH:
211 return getPathLengthObjective(si);
213 case OBJECTIVE_THRESHOLDPATHLENGTH:
214 return getThresholdPathLengthObj(si);
216 case OBJECTIVE_WEIGHTEDCOMBO:
217 return getBalancedObjective1(si);
220 OMPL_ERROR(
"Optimization-objective enum is not implemented in allocation function.");
221 return ob::OptimizationObjectivePtr();
226void plan(
double runTime, optimalPlanner plannerType, planningObjective objectiveType,
const std::string& outputFile)
230 auto space(std::make_shared<ob::RealVectorStateSpace>(2));
233 space->setBounds(0.0, 1.0);
236 auto si(std::make_shared<ob::SpaceInformation>(space));
239 si->setStateValidityChecker(std::make_shared<ValidityChecker>(si));
256 auto pdef(std::make_shared<ob::ProblemDefinition>(si));
259 pdef->setStartAndGoalStates(start, goal);
263 pdef->setOptimizationObjective(allocateObjective(si, objectiveType));
267 ob::PlannerPtr optimizingPlanner = allocatePlanner(si, plannerType);
270 optimizingPlanner->setProblemDefinition(pdef);
271 optimizingPlanner->setup();
280 << optimizingPlanner->getName()
281 <<
" found a solution of length "
282 << pdef->getSolutionPath()->length()
283 <<
" with an optimization objective value of "
284 << pdef->getSolutionPath()->cost(pdef->getOptimizationObjective()) << std::endl;
288 if (!outputFile.empty())
290 std::ofstream outFile(outputFile.c_str());
291 std::static_pointer_cast<og::PathGeometric>(pdef->getSolutionPath())->
292 printAsMatrix(outFile);
297 std::cout <<
"No solution found." << std::endl;
300int main(
int argc,
char** argv)
304 optimalPlanner plannerType;
305 planningObjective objectiveType;
306 std::string outputFile;
309 if (argParse(argc, argv, &runTime, &plannerType, &objectiveType, &outputFile))
312 plan(runTime, plannerType, objectiveType, outputFile);
325ob::OptimizationObjectivePtr getPathLengthObjective(
const ob::SpaceInformationPtr& si)
327 return std::make_shared<ob::PathLengthOptimizationObjective>(si);
333ob::OptimizationObjectivePtr getThresholdPathLengthObj(
const ob::SpaceInformationPtr& si)
335 auto obj(std::make_shared<ob::PathLengthOptimizationObjective>(si));
336 obj->setCostThreshold(
ob::Cost(1.51));
355 ClearanceObjective(
const ob::SpaceInformationPtr& si) :
356 ob::StateCostIntegralObjective(si, true)
365 ob::Cost
stateCost(
const ob::State* s)
const override
367 return ob::Cost(1 / (si_->getStateValidityChecker()->clearance(s) +
368 std::numeric_limits<double>::min()));
374ob::OptimizationObjectivePtr getClearanceObjective(
const ob::SpaceInformationPtr& si)
376 return std::make_shared<ClearanceObjective>(si);
391ob::OptimizationObjectivePtr getBalancedObjective1(
const ob::SpaceInformationPtr& si)
393 auto lengthObj(std::make_shared<ob::PathLengthOptimizationObjective>(si));
394 auto clearObj(std::make_shared<ClearanceObjective>(si));
395 auto opt(std::make_shared<ob::MultiOptimizationObjective>(si));
396 opt->addObjective(lengthObj, 10.0);
397 opt->addObjective(clearObj, 1.0);
399 return ob::OptimizationObjectivePtr(opt);
405ob::OptimizationObjectivePtr getBalancedObjective2(
const ob::SpaceInformationPtr& si)
407 auto lengthObj(std::make_shared<ob::PathLengthOptimizationObjective>(si));
408 auto clearObj(std::make_shared<ClearanceObjective>(si));
410 return 10.0*lengthObj + clearObj;
416ob::OptimizationObjectivePtr getPathLengthObjWithCostToGo(
const ob::SpaceInformationPtr& si)
418 auto obj(std::make_shared<ob::PathLengthOptimizationObjective>(si));
424bool argParse(
int argc,
char** argv,
double* runTimePtr, optimalPlanner *plannerPtr, planningObjective *objectivePtr, std::string *outputFilePtr)
426 namespace bpo = boost::program_options;
429 bpo::options_description desc(
"Allowed options");
431 (
"help,h",
"produce help message")
432 (
"runtime,t", bpo::value<double>()->default_value(1.0),
"(Optional) Specify the runtime in seconds. Defaults to 1 and must be greater than 0.")
433 (
"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 AITstar, BFMTstar, BITstar, CForest, FMTstar, InformedRRTstar, PRMstar, RRTstar, and SORRTstar.")
434 (
"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.")
435 (
"file,f", bpo::value<std::string>()->default_value(
""),
"(Optional) Specify an output path for the found solution path.")
436 (
"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.");
437 bpo::variables_map vm;
438 bpo::store(bpo::parse_command_line(argc, argv, desc), vm);
442 if (vm.count(
"help") != 0u)
444 std::cout << desc << std::endl;
449 unsigned int logLevel = vm[
"info"].as<
unsigned int>();
456 else if (logLevel == 1u)
460 else if (logLevel == 2u)
466 std::cout <<
"Invalid log-level integer." << std::endl << std::endl << desc << std::endl;
471 *runTimePtr = vm[
"runtime"].as<
double>();
474 if (*runTimePtr <= 0.0)
476 std::cout <<
"Invalid runtime." << std::endl << std::endl << desc << std::endl;
481 std::string plannerStr = vm[
"planner"].as<std::string>();
484 if (boost::iequals(
"AITstar", plannerStr)) {
485 *plannerPtr = PLANNER_AITSTAR;
487 else if (boost::iequals(
"BFMTstar", plannerStr))
489 *plannerPtr = PLANNER_BFMTSTAR;
491 else if (boost::iequals(
"BITstar", plannerStr))
493 *plannerPtr = PLANNER_BITSTAR;
495 else if (boost::iequals(
"CForest", plannerStr))
497 *plannerPtr = PLANNER_CFOREST;
499 else if (boost::iequals(
"FMTstar", plannerStr))
501 *plannerPtr = PLANNER_FMTSTAR;
503 else if (boost::iequals(
"InformedRRTstar", plannerStr))
505 *plannerPtr = PLANNER_INF_RRTSTAR;
507 else if (boost::iequals(
"PRMstar", plannerStr))
509 *plannerPtr = PLANNER_PRMSTAR;
511 else if (boost::iequals(
"RRTstar", plannerStr))
513 *plannerPtr = PLANNER_RRTSTAR;
515 else if (boost::iequals(
"SORRTstar", plannerStr))
517 *plannerPtr = PLANNER_SORRTSTAR;
521 std::cout <<
"Invalid planner string." << std::endl << std::endl << desc << std::endl;
526 std::string objectiveStr = vm[
"objective"].as<std::string>();
529 if (boost::iequals(
"PathClearance", objectiveStr))
531 *objectivePtr = OBJECTIVE_PATHCLEARANCE;
533 else if (boost::iequals(
"PathLength", objectiveStr))
535 *objectivePtr = OBJECTIVE_PATHLENGTH;
537 else if (boost::iequals(
"ThresholdPathLength", objectiveStr))
539 *objectivePtr = OBJECTIVE_THRESHOLDPATHLENGTH;
541 else if (boost::iequals(
"WeightedLengthAndClearanceCombo", objectiveStr))
543 *objectivePtr = OBJECTIVE_WEIGHTEDCOMBO;
547 std::cout <<
"Invalid objective string." << std::endl << std::endl << desc << std::endl;
552 *outputFilePtr = vm[
"file"].as<std::string>();
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
The definition of a state in Rn
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 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.
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()