Loading...
Searching...
No Matches
OptimalPlanning.cpp
1/*********************************************************************
2* Software License Agreement (BSD License)
3*
4* Copyright (c) 2011, Rice University
5* All rights reserved.
6*
7* Redistribution and use in source and binary forms, with or without
8* modification, are permitted provided that the following conditions
9* are met:
10*
11* * Redistributions of source code must retain the above copyright
12* notice, this list of conditions and the following disclaimer.
13* * Redistributions in binary form must reproduce the above
14* copyright notice, this list of conditions and the following
15* disclaimer in the documentation and/or other materials provided
16* with the distribution.
17* * Neither the name of the Rice University nor the names of its
18* contributors may be used to endorse or promote products derived
19* from this software without specific prior written permission.
20*
21* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32* POSSIBILITY OF SUCH DAMAGE.
33*********************************************************************/
34
35/* Author: Luis G. Torres, Jonathan Gammell */
36
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>
42// For ompl::msg::setLogLevel
43#include "ompl/util/Console.h"
44
45// The supported optimal planners, in alphabetical order
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>
54
55
56// For boost program options
57#include <boost/program_options.hpp>
58// For string comparison (boost::iequals)
59#include <boost/algorithm/string.hpp>
60// For std::make_shared
61#include <memory>
62
63#include <fstream>
64
65
66namespace ob = ompl::base;
67namespace og = ompl::geometric;
69
70// An enum of supported optimal planners, alphabetical order
71enum optimalPlanner
72{
73 PLANNER_BFMTSTAR,
74 PLANNER_BITSTAR,
75 PLANNER_CFOREST,
76 PLANNER_FMTSTAR,
77 PLANNER_INF_RRTSTAR,
78 PLANNER_PRMSTAR,
79 PLANNER_RRTSTAR,
80 PLANNER_SORRTSTAR,
81};
82
83// An enum of the supported optimization objectives, alphabetical order
84enum planningObjective
85{
86 OBJECTIVE_PATHCLEARANCE,
87 OBJECTIVE_PATHLENGTH,
88 OBJECTIVE_THRESHOLDPATHLENGTH,
89 OBJECTIVE_WEIGHTEDCOMBO
90};
91
92// Parse the command-line arguments
93bool argParse(int argc, char** argv, double *runTimePtr, optimalPlanner *plannerPtr, planningObjective *objectivePtr, std::string *outputFilePtr);
94
95// Our "collision checker". For this demo, our robot's state space
96// lies in [0,1]x[0,1], with a circular obstacle of radius 0.25
97// centered at (0.5,0.5). Any states lying in this circular region are
98// considered "in collision".
99class ValidityChecker : public ob::StateValidityChecker
100{
101public:
102 ValidityChecker(const ob::SpaceInformationPtr& si) :
104
105 // Returns whether the given state's position overlaps the
106 // circular obstacle
107 bool isValid(const ob::State* state) const override
108 {
109 return this->clearance(state) > 0.0;
110 }
111
112 // Returns the distance from the given state's position to the
113 // boundary of the circular obstacle.
114 double clearance(const ob::State* state) const override
115 {
116 // We know we're working with a RealVectorStateSpace in this
117 // example, so we downcast state into the specific type.
118 const auto* state2D =
119 state->as<ob::RealVectorStateSpace::StateType>();
120
121 // Extract the robot's (x,y) position from its state
122 double x = state2D->values[0];
123 double y = state2D->values[1];
124
125 // Distance formula between two points, offset by the circle's
126 // radius
127 return sqrt((x-0.5)*(x-0.5) + (y-0.5)*(y-0.5)) - 0.25;
128 }
129};
130
131ob::OptimizationObjectivePtr getPathLengthObjective(const ob::SpaceInformationPtr& si);
132
133ob::OptimizationObjectivePtr getThresholdPathLengthObj(const ob::SpaceInformationPtr& si);
134
135ob::OptimizationObjectivePtr getClearanceObjective(const ob::SpaceInformationPtr& si);
136
137ob::OptimizationObjectivePtr getBalancedObjective1(const ob::SpaceInformationPtr& si);
138
139ob::OptimizationObjectivePtr getBalancedObjective2(const ob::SpaceInformationPtr& si);
140
141ob::OptimizationObjectivePtr getPathLengthObjWithCostToGo(const ob::SpaceInformationPtr& si);
142
143ob::PlannerPtr allocatePlanner(ob::SpaceInformationPtr si, optimalPlanner plannerType)
144{
145 switch (plannerType)
146 {
147 case PLANNER_BFMTSTAR:
148 {
149 return std::make_shared<og::BFMT>(si);
150 break;
151 }
152 case PLANNER_BITSTAR:
153 {
154 return std::make_shared<og::BITstar>(si);
155 break;
156 }
157 case PLANNER_CFOREST:
158 {
159 return std::make_shared<og::CForest>(si);
160 break;
161 }
162 case PLANNER_FMTSTAR:
163 {
164 return std::make_shared<og::FMT>(si);
165 break;
166 }
167 case PLANNER_INF_RRTSTAR:
168 {
169 return std::make_shared<og::InformedRRTstar>(si);
170 break;
171 }
172 case PLANNER_PRMSTAR:
173 {
174 return std::make_shared<og::PRMstar>(si);
175 break;
176 }
177 case PLANNER_RRTSTAR:
178 {
179 return std::make_shared<og::RRTstar>(si);
180 break;
181 }
182 case PLANNER_SORRTSTAR:
183 {
184 return std::make_shared<og::SORRTstar>(si);
185 break;
186 }
187 default:
188 {
189 OMPL_ERROR("Planner-type enum is not implemented in allocation function.");
190 return ob::PlannerPtr(); // Address compiler warning re: no return value.
191 break;
192 }
193 }
194}
195
196ob::OptimizationObjectivePtr allocateObjective(const ob::SpaceInformationPtr& si, planningObjective objectiveType)
197{
198 switch (objectiveType)
199 {
200 case OBJECTIVE_PATHCLEARANCE:
201 return getClearanceObjective(si);
202 break;
203 case OBJECTIVE_PATHLENGTH:
204 return getPathLengthObjective(si);
205 break;
206 case OBJECTIVE_THRESHOLDPATHLENGTH:
207 return getThresholdPathLengthObj(si);
208 break;
209 case OBJECTIVE_WEIGHTEDCOMBO:
210 return getBalancedObjective1(si);
211 break;
212 default:
213 OMPL_ERROR("Optimization-objective enum is not implemented in allocation function.");
214 return ob::OptimizationObjectivePtr();
215 break;
216 }
217}
218
219void plan(double runTime, optimalPlanner plannerType, planningObjective objectiveType, const std::string& outputFile)
220{
221 // Construct the robot state space in which we're planning. We're
222 // planning in [0,1]x[0,1], a subset of R^2.
223 auto space(std::make_shared<ob::RealVectorStateSpace>(2));
224
225 // Set the bounds of space to be in [0,1].
226 space->setBounds(0.0, 1.0);
227
228 // Construct a space information instance for this state space
229 auto si(std::make_shared<ob::SpaceInformation>(space));
230
231 // Set the object used to check which states in the space are valid
232 si->setStateValidityChecker(std::make_shared<ValidityChecker>(si));
233
234 si->setup();
235
236 // Set our robot's starting state to be the bottom-left corner of
237 // the environment, or (0,0).
238 ob::ScopedState<> start(space);
239 start->as<ob::RealVectorStateSpace::StateType>()->values[0] = 0.0;
240 start->as<ob::RealVectorStateSpace::StateType>()->values[1] = 0.0;
241
242 // Set our robot's goal state to be the top-right corner of the
243 // environment, or (1,1).
244 ob::ScopedState<> goal(space);
245 goal->as<ob::RealVectorStateSpace::StateType>()->values[0] = 1.0;
246 goal->as<ob::RealVectorStateSpace::StateType>()->values[1] = 1.0;
247
248 // Create a problem instance
249 auto pdef(std::make_shared<ob::ProblemDefinition>(si));
250
251 // Set the start and goal states
252 pdef->setStartAndGoalStates(start, goal);
253
254 // Create the optimization objective specified by our command-line argument.
255 // This helper function is simply a switch statement.
256 pdef->setOptimizationObjective(allocateObjective(si, objectiveType));
257
258 // Construct the optimal planner specified by our command line argument.
259 // This helper function is simply a switch statement.
260 ob::PlannerPtr optimizingPlanner = allocatePlanner(si, plannerType);
261
262 // Set the problem instance for our planner to solve
263 optimizingPlanner->setProblemDefinition(pdef);
264 optimizingPlanner->setup();
265
266 // attempt to solve the planning problem in the given runtime
267 ob::PlannerStatus solved = optimizingPlanner->solve(runTime);
268
269 if (solved)
270 {
271 // Output the length of the path found
272 std::cout
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;
278
279 // If a filename was specified, output the path as a matrix to
280 // that file for visualization
281 if (!outputFile.empty())
282 {
283 std::ofstream outFile(outputFile.c_str());
284 std::static_pointer_cast<og::PathGeometric>(pdef->getSolutionPath())->
285 printAsMatrix(outFile);
286 outFile.close();
287 }
288 }
289 else
290 std::cout << "No solution found." << std::endl;
291}
292
293int main(int argc, char** argv)
294{
295 // The parsed arguments
296 double runTime;
297 optimalPlanner plannerType;
298 planningObjective objectiveType;
299 std::string outputFile;
300
301 // Parse the arguments, returns true if successful, false otherwise
302 if (argParse(argc, argv, &runTime, &plannerType, &objectiveType, &outputFile))
303 {
304 // Plan
305 plan(runTime, plannerType, objectiveType, outputFile);
306
307 // Return with success
308 return 0;
309 }
310 // Return with error
311 return -1;
312}
313
318ob::OptimizationObjectivePtr getPathLengthObjective(const ob::SpaceInformationPtr& si)
319{
320 return std::make_shared<ob::PathLengthOptimizationObjective>(si);
321}
322
326ob::OptimizationObjectivePtr getThresholdPathLengthObj(const ob::SpaceInformationPtr& si)
327{
328 auto obj(std::make_shared<ob::PathLengthOptimizationObjective>(si));
329 obj->setCostThreshold(ob::Cost(1.51));
330 return obj;
331}
332
345class ClearanceObjective : public ob::StateCostIntegralObjective
346{
347public:
348 ClearanceObjective(const ob::SpaceInformationPtr& si) :
349 ob::StateCostIntegralObjective(si, true)
350 {
351 }
352
353 // Our requirement is to maximize path clearance from obstacles,
354 // but we want to represent the objective as a path cost
355 // minimization. Therefore, we set each state's cost to be the
356 // reciprocal of its clearance, so that as state clearance
357 // increases, the state cost decreases.
358 ob::Cost stateCost(const ob::State* s) const override
359 {
360 return ob::Cost(1 / (si_->getStateValidityChecker()->clearance(s) +
361 std::numeric_limits<double>::min()));
362 }
363};
364
367ob::OptimizationObjectivePtr getClearanceObjective(const ob::SpaceInformationPtr& si)
368{
369 return std::make_shared<ClearanceObjective>(si);
370}
371
384ob::OptimizationObjectivePtr getBalancedObjective1(const ob::SpaceInformationPtr& si)
385{
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);
391
392 return ob::OptimizationObjectivePtr(opt);
393}
394
398ob::OptimizationObjectivePtr getBalancedObjective2(const ob::SpaceInformationPtr& si)
399{
400 auto lengthObj(std::make_shared<ob::PathLengthOptimizationObjective>(si));
401 auto clearObj(std::make_shared<ClearanceObjective>(si));
402
403 return 10.0*lengthObj + clearObj;
404}
405
409ob::OptimizationObjectivePtr getPathLengthObjWithCostToGo(const ob::SpaceInformationPtr& si)
410{
411 auto obj(std::make_shared<ob::PathLengthOptimizationObjective>(si));
412 obj->setCostToGoHeuristic(&ob::goalRegionCostToGo);
413 return obj;
414}
415
417bool argParse(int argc, char** argv, double* runTimePtr, optimalPlanner *plannerPtr, planningObjective *objectivePtr, std::string *outputFilePtr)
418{
419 namespace bpo = boost::program_options;
420
421 // Declare the supported options.
422 bpo::options_description desc("Allowed options");
423 desc.add_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.") //Alphabetical order
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.") //Alphabetical order
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);
432 bpo::notify(vm);
433
434 // Check if the help flag has been given:
435 if (vm.count("help") != 0u)
436 {
437 std::cout << desc << std::endl;
438 return false;
439 }
440
441 // Set the log-level
442 unsigned int logLevel = vm["info"].as<unsigned int>();
443
444 // Switch to setting the log level:
445 if (logLevel == 0u)
446 {
447 ompl::msg::setLogLevel(ompl::msg::LOG_WARN);
448 }
449 else if (logLevel == 1u)
450 {
451 ompl::msg::setLogLevel(ompl::msg::LOG_INFO);
452 }
453 else if (logLevel == 2u)
454 {
455 ompl::msg::setLogLevel(ompl::msg::LOG_DEBUG);
456 }
457 else
458 {
459 std::cout << "Invalid log-level integer." << std::endl << std::endl << desc << std::endl;
460 return false;
461 }
462
463 // Get the runtime as a double
464 *runTimePtr = vm["runtime"].as<double>();
465
466 // Sanity check
467 if (*runTimePtr <= 0.0)
468 {
469 std::cout << "Invalid runtime." << std::endl << std::endl << desc << std::endl;
470 return false;
471 }
472
473 // Get the specified planner as a string
474 std::string plannerStr = vm["planner"].as<std::string>();
475
476 // Map the string to the enum
477 if (boost::iequals("BFMTstar", plannerStr))
478 {
479 *plannerPtr = PLANNER_BFMTSTAR;
480 }
481 else if (boost::iequals("BITstar", plannerStr))
482 {
483 *plannerPtr = PLANNER_BITSTAR;
484 }
485 else if (boost::iequals("CForest", plannerStr))
486 {
487 *plannerPtr = PLANNER_CFOREST;
488 }
489 else if (boost::iequals("FMTstar", plannerStr))
490 {
491 *plannerPtr = PLANNER_FMTSTAR;
492 }
493 else if (boost::iequals("InformedRRTstar", plannerStr))
494 {
495 *plannerPtr = PLANNER_INF_RRTSTAR;
496 }
497 else if (boost::iequals("PRMstar", plannerStr))
498 {
499 *plannerPtr = PLANNER_PRMSTAR;
500 }
501 else if (boost::iequals("RRTstar", plannerStr))
502 {
503 *plannerPtr = PLANNER_RRTSTAR;
504 }
505 else if (boost::iequals("SORRTstar", plannerStr))
506 {
507 *plannerPtr = PLANNER_SORRTSTAR;
508 }
509 else
510 {
511 std::cout << "Invalid planner string." << std::endl << std::endl << desc << std::endl;
512 return false;
513 }
514
515 // Get the specified objective as a string
516 std::string objectiveStr = vm["objective"].as<std::string>();
517
518 // Map the string to the enum
519 if (boost::iequals("PathClearance", objectiveStr))
520 {
521 *objectivePtr = OBJECTIVE_PATHCLEARANCE;
522 }
523 else if (boost::iequals("PathLength", objectiveStr))
524 {
525 *objectivePtr = OBJECTIVE_PATHLENGTH;
526 }
527 else if (boost::iequals("ThresholdPathLength", objectiveStr))
528 {
529 *objectivePtr = OBJECTIVE_THRESHOLDPATHLENGTH;
530 }
531 else if (boost::iequals("WeightedLengthAndClearanceCombo", objectiveStr))
532 {
533 *objectivePtr = OBJECTIVE_WEIGHTEDCOMBO;
534 }
535 else
536 {
537 std::cout << "Invalid objective string." << std::endl << std::endl << desc << std::endl;
538 return false;
539 }
540
541 // Get the output file string and store it in the return pointer
542 *outputFilePtr = vm["file"].as<std::string>();
543
544 // Looks like we parsed the arguments successfully
545 return true;
546}
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition: Cost.h:48
Definition of a scoped state.
Definition: ScopedState.h:57
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 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.
Definition: State.h:50
const T * as() const
Cast this instance to a desired type.
Definition: State.h:66
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Definition: Console.h:64
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.
Definition: GeneticSearch.h:48
void setLogLevel(LogLevel level)
Set the minimum level of logging data to output. Messages with lower logging levels will not be recor...
Definition: Console.cpp:136
A class to store the exit status of Planner::solve()
Definition: PlannerStatus.h:49