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.
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()