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/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>
55
56
57// For boost program options
58#include <boost/program_options.hpp>
59// For string comparison (boost::iequals)
60#include <boost/algorithm/string.hpp>
61// For std::make_shared
62#include <memory>
63
64#include <fstream>
65
66
67namespace ob = ompl::base;
68namespace og = ompl::geometric;
70
71// An enum of supported optimal planners, alphabetical order
72enum optimalPlanner
73{
74 PLANNER_AITSTAR,
75 PLANNER_BFMTSTAR,
76 PLANNER_BITSTAR,
77 PLANNER_CFOREST,
78 PLANNER_FMTSTAR,
79 PLANNER_INF_RRTSTAR,
80 PLANNER_PRMSTAR,
81 PLANNER_RRTSTAR,
82 PLANNER_SORRTSTAR,
83};
84
85// An enum of the supported optimization objectives, alphabetical order
86enum planningObjective
87{
88 OBJECTIVE_PATHCLEARANCE,
89 OBJECTIVE_PATHLENGTH,
90 OBJECTIVE_THRESHOLDPATHLENGTH,
91 OBJECTIVE_WEIGHTEDCOMBO
92};
93
94// Parse the command-line arguments
95bool argParse(int argc, char** argv, double *runTimePtr, optimalPlanner *plannerPtr, planningObjective *objectivePtr, std::string *outputFilePtr);
96
97// Our "collision checker". For this demo, our robot's state space
98// lies in [0,1]x[0,1], with a circular obstacle of radius 0.25
99// centered at (0.5,0.5). Any states lying in this circular region are
100// considered "in collision".
101class ValidityChecker : public ob::StateValidityChecker
102{
103public:
104 ValidityChecker(const ob::SpaceInformationPtr& si) :
106
107 // Returns whether the given state's position overlaps the
108 // circular obstacle
109 bool isValid(const ob::State* state) const override
110 {
111 return this->clearance(state) > 0.0;
112 }
113
114 // Returns the distance from the given state's position to the
115 // boundary of the circular obstacle.
116 double clearance(const ob::State* state) const override
117 {
118 // We know we're working with a RealVectorStateSpace in this
119 // example, so we downcast state into the specific type.
120 const auto* state2D =
121 state->as<ob::RealVectorStateSpace::StateType>();
122
123 // Extract the robot's (x,y) position from its state
124 double x = state2D->values[0];
125 double y = state2D->values[1];
126
127 // Distance formula between two points, offset by the circle's
128 // radius
129 return sqrt((x-0.5)*(x-0.5) + (y-0.5)*(y-0.5)) - 0.25;
130 }
131};
132
133ob::OptimizationObjectivePtr getPathLengthObjective(const ob::SpaceInformationPtr& si);
134
135ob::OptimizationObjectivePtr getThresholdPathLengthObj(const ob::SpaceInformationPtr& si);
136
137ob::OptimizationObjectivePtr getClearanceObjective(const ob::SpaceInformationPtr& si);
138
139ob::OptimizationObjectivePtr getBalancedObjective1(const ob::SpaceInformationPtr& si);
140
141ob::OptimizationObjectivePtr getBalancedObjective2(const ob::SpaceInformationPtr& si);
142
143ob::OptimizationObjectivePtr getPathLengthObjWithCostToGo(const ob::SpaceInformationPtr& si);
144
145ob::PlannerPtr allocatePlanner(ob::SpaceInformationPtr si, optimalPlanner plannerType)
146{
147 switch (plannerType)
148 {
149 case PLANNER_AITSTAR:
150 {
151 return std::make_shared<og::AITstar>(si);
152 break;
153 }
154 case PLANNER_BFMTSTAR:
155 {
156 return std::make_shared<og::BFMT>(si);
157 break;
158 }
159 case PLANNER_BITSTAR:
160 {
161 return std::make_shared<og::BITstar>(si);
162 break;
163 }
164 case PLANNER_CFOREST:
165 {
166 return std::make_shared<og::CForest>(si);
167 break;
168 }
169 case PLANNER_FMTSTAR:
170 {
171 return std::make_shared<og::FMT>(si);
172 break;
173 }
174 case PLANNER_INF_RRTSTAR:
175 {
176 return std::make_shared<og::InformedRRTstar>(si);
177 break;
178 }
179 case PLANNER_PRMSTAR:
180 {
181 return std::make_shared<og::PRMstar>(si);
182 break;
183 }
184 case PLANNER_RRTSTAR:
185 {
186 return std::make_shared<og::RRTstar>(si);
187 break;
188 }
189 case PLANNER_SORRTSTAR:
190 {
191 return std::make_shared<og::SORRTstar>(si);
192 break;
193 }
194 default:
195 {
196 OMPL_ERROR("Planner-type enum is not implemented in allocation function.");
197 return ob::PlannerPtr(); // Address compiler warning re: no return value.
198 break;
199 }
200 }
201}
202
203ob::OptimizationObjectivePtr allocateObjective(const ob::SpaceInformationPtr& si, planningObjective objectiveType)
204{
205 switch (objectiveType)
206 {
207 case OBJECTIVE_PATHCLEARANCE:
208 return getClearanceObjective(si);
209 break;
210 case OBJECTIVE_PATHLENGTH:
211 return getPathLengthObjective(si);
212 break;
213 case OBJECTIVE_THRESHOLDPATHLENGTH:
214 return getThresholdPathLengthObj(si);
215 break;
216 case OBJECTIVE_WEIGHTEDCOMBO:
217 return getBalancedObjective1(si);
218 break;
219 default:
220 OMPL_ERROR("Optimization-objective enum is not implemented in allocation function.");
221 return ob::OptimizationObjectivePtr();
222 break;
223 }
224}
225
226void plan(double runTime, optimalPlanner plannerType, planningObjective objectiveType, const std::string& outputFile)
227{
228 // Construct the robot state space in which we're planning. We're
229 // planning in [0,1]x[0,1], a subset of R^2.
230 auto space(std::make_shared<ob::RealVectorStateSpace>(2));
231
232 // Set the bounds of space to be in [0,1].
233 space->setBounds(0.0, 1.0);
234
235 // Construct a space information instance for this state space
236 auto si(std::make_shared<ob::SpaceInformation>(space));
237
238 // Set the object used to check which states in the space are valid
239 si->setStateValidityChecker(std::make_shared<ValidityChecker>(si));
240
241 si->setup();
242
243 // Set our robot's starting state to be the bottom-left corner of
244 // the environment, or (0,0).
245 ob::ScopedState<> start(space);
246 start->as<ob::RealVectorStateSpace::StateType>()->values[0] = 0.0;
247 start->as<ob::RealVectorStateSpace::StateType>()->values[1] = 0.0;
248
249 // Set our robot's goal state to be the top-right corner of the
250 // environment, or (1,1).
251 ob::ScopedState<> goal(space);
252 goal->as<ob::RealVectorStateSpace::StateType>()->values[0] = 1.0;
253 goal->as<ob::RealVectorStateSpace::StateType>()->values[1] = 1.0;
254
255 // Create a problem instance
256 auto pdef(std::make_shared<ob::ProblemDefinition>(si));
257
258 // Set the start and goal states
259 pdef->setStartAndGoalStates(start, goal);
260
261 // Create the optimization objective specified by our command-line argument.
262 // This helper function is simply a switch statement.
263 pdef->setOptimizationObjective(allocateObjective(si, objectiveType));
264
265 // Construct the optimal planner specified by our command line argument.
266 // This helper function is simply a switch statement.
267 ob::PlannerPtr optimizingPlanner = allocatePlanner(si, plannerType);
268
269 // Set the problem instance for our planner to solve
270 optimizingPlanner->setProblemDefinition(pdef);
271 optimizingPlanner->setup();
272
273 // attempt to solve the planning problem in the given runtime
274 ob::PlannerStatus solved = optimizingPlanner->solve(runTime);
275
276 if (solved)
277 {
278 // Output the length of the path found
279 std::cout
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;
285
286 // If a filename was specified, output the path as a matrix to
287 // that file for visualization
288 if (!outputFile.empty())
289 {
290 std::ofstream outFile(outputFile.c_str());
291 std::static_pointer_cast<og::PathGeometric>(pdef->getSolutionPath())->
292 printAsMatrix(outFile);
293 outFile.close();
294 }
295 }
296 else
297 std::cout << "No solution found." << std::endl;
298}
299
300int main(int argc, char** argv)
301{
302 // The parsed arguments
303 double runTime;
304 optimalPlanner plannerType;
305 planningObjective objectiveType;
306 std::string outputFile;
307
308 // Parse the arguments, returns true if successful, false otherwise
309 if (argParse(argc, argv, &runTime, &plannerType, &objectiveType, &outputFile))
310 {
311 // Plan
312 plan(runTime, plannerType, objectiveType, outputFile);
313
314 // Return with success
315 return 0;
316 }
317 // Return with error
318 return -1;
319}
320
325ob::OptimizationObjectivePtr getPathLengthObjective(const ob::SpaceInformationPtr& si)
326{
327 return std::make_shared<ob::PathLengthOptimizationObjective>(si);
328}
329
333ob::OptimizationObjectivePtr getThresholdPathLengthObj(const ob::SpaceInformationPtr& si)
334{
335 auto obj(std::make_shared<ob::PathLengthOptimizationObjective>(si));
336 obj->setCostThreshold(ob::Cost(1.51));
337 return obj;
338}
339
352class ClearanceObjective : public ob::StateCostIntegralObjective
353{
354public:
355 ClearanceObjective(const ob::SpaceInformationPtr& si) :
356 ob::StateCostIntegralObjective(si, true)
357 {
358 }
359
360 // Our requirement is to maximize path clearance from obstacles,
361 // but we want to represent the objective as a path cost
362 // minimization. Therefore, we set each state's cost to be the
363 // reciprocal of its clearance, so that as state clearance
364 // increases, the state cost decreases.
365 ob::Cost stateCost(const ob::State* s) const override
366 {
367 return ob::Cost(1 / (si_->getStateValidityChecker()->clearance(s) +
368 std::numeric_limits<double>::min()));
369 }
370};
371
374ob::OptimizationObjectivePtr getClearanceObjective(const ob::SpaceInformationPtr& si)
375{
376 return std::make_shared<ClearanceObjective>(si);
377}
378
391ob::OptimizationObjectivePtr getBalancedObjective1(const ob::SpaceInformationPtr& si)
392{
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);
398
399 return ob::OptimizationObjectivePtr(opt);
400}
401
405ob::OptimizationObjectivePtr getBalancedObjective2(const ob::SpaceInformationPtr& si)
406{
407 auto lengthObj(std::make_shared<ob::PathLengthOptimizationObjective>(si));
408 auto clearObj(std::make_shared<ClearanceObjective>(si));
409
410 return 10.0*lengthObj + clearObj;
411}
412
416ob::OptimizationObjectivePtr getPathLengthObjWithCostToGo(const ob::SpaceInformationPtr& si)
417{
418 auto obj(std::make_shared<ob::PathLengthOptimizationObjective>(si));
419 obj->setCostToGoHeuristic(&ob::goalRegionCostToGo);
420 return obj;
421}
422
424bool argParse(int argc, char** argv, double* runTimePtr, optimalPlanner *plannerPtr, planningObjective *objectivePtr, std::string *outputFilePtr)
425{
426 namespace bpo = boost::program_options;
427
428 // Declare the supported options.
429 bpo::options_description desc("Allowed options");
430 desc.add_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.") //Alphabetical order
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.") //Alphabetical order
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);
439 bpo::notify(vm);
440
441 // Check if the help flag has been given:
442 if (vm.count("help") != 0u)
443 {
444 std::cout << desc << std::endl;
445 return false;
446 }
447
448 // Set the log-level
449 unsigned int logLevel = vm["info"].as<unsigned int>();
450
451 // Switch to setting the log level:
452 if (logLevel == 0u)
453 {
454 ompl::msg::setLogLevel(ompl::msg::LOG_WARN);
455 }
456 else if (logLevel == 1u)
457 {
458 ompl::msg::setLogLevel(ompl::msg::LOG_INFO);
459 }
460 else if (logLevel == 2u)
461 {
462 ompl::msg::setLogLevel(ompl::msg::LOG_DEBUG);
463 }
464 else
465 {
466 std::cout << "Invalid log-level integer." << std::endl << std::endl << desc << std::endl;
467 return false;
468 }
469
470 // Get the runtime as a double
471 *runTimePtr = vm["runtime"].as<double>();
472
473 // Sanity check
474 if (*runTimePtr <= 0.0)
475 {
476 std::cout << "Invalid runtime." << std::endl << std::endl << desc << std::endl;
477 return false;
478 }
479
480 // Get the specified planner as a string
481 std::string plannerStr = vm["planner"].as<std::string>();
482
483 // Map the string to the enum
484 if (boost::iequals("AITstar", plannerStr)) {
485 *plannerPtr = PLANNER_AITSTAR;
486 }
487 else if (boost::iequals("BFMTstar", plannerStr))
488 {
489 *plannerPtr = PLANNER_BFMTSTAR;
490 }
491 else if (boost::iequals("BITstar", plannerStr))
492 {
493 *plannerPtr = PLANNER_BITSTAR;
494 }
495 else if (boost::iequals("CForest", plannerStr))
496 {
497 *plannerPtr = PLANNER_CFOREST;
498 }
499 else if (boost::iequals("FMTstar", plannerStr))
500 {
501 *plannerPtr = PLANNER_FMTSTAR;
502 }
503 else if (boost::iequals("InformedRRTstar", plannerStr))
504 {
505 *plannerPtr = PLANNER_INF_RRTSTAR;
506 }
507 else if (boost::iequals("PRMstar", plannerStr))
508 {
509 *plannerPtr = PLANNER_PRMSTAR;
510 }
511 else if (boost::iequals("RRTstar", plannerStr))
512 {
513 *plannerPtr = PLANNER_RRTSTAR;
514 }
515 else if (boost::iequals("SORRTstar", plannerStr))
516 {
517 *plannerPtr = PLANNER_SORRTSTAR;
518 }
519 else
520 {
521 std::cout << "Invalid planner string." << std::endl << std::endl << desc << std::endl;
522 return false;
523 }
524
525 // Get the specified objective as a string
526 std::string objectiveStr = vm["objective"].as<std::string>();
527
528 // Map the string to the enum
529 if (boost::iequals("PathClearance", objectiveStr))
530 {
531 *objectivePtr = OBJECTIVE_PATHCLEARANCE;
532 }
533 else if (boost::iequals("PathLength", objectiveStr))
534 {
535 *objectivePtr = OBJECTIVE_PATHLENGTH;
536 }
537 else if (boost::iequals("ThresholdPathLength", objectiveStr))
538 {
539 *objectivePtr = OBJECTIVE_THRESHOLDPATHLENGTH;
540 }
541 else if (boost::iequals("WeightedLengthAndClearanceCombo", objectiveStr))
542 {
543 *objectivePtr = OBJECTIVE_WEIGHTEDCOMBO;
544 }
545 else
546 {
547 std::cout << "Invalid objective string." << std::endl << std::endl << desc << std::endl;
548 return false;
549 }
550
551 // Get the output file string and store it in the return pointer
552 *outputFilePtr = vm["file"].as<std::string>();
553
554 // Looks like we parsed the arguments successfully
555 return true;
556}
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.
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()