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/bitstar/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 
66 namespace ob = ompl::base;
67 namespace og = ompl::geometric;
69 
70 // An enum of supported optimal planners, alphabetical order
71 enum 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
84 enum planningObjective
85 {
86  OBJECTIVE_PATHCLEARANCE,
87  OBJECTIVE_PATHLENGTH,
88  OBJECTIVE_THRESHOLDPATHLENGTH,
89  OBJECTIVE_WEIGHTEDCOMBO
90 };
91 
92 // Parse the command-line arguments
93 bool 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".
99 class ValidityChecker : public ob::StateValidityChecker
100 {
101 public:
102  ValidityChecker(const ob::SpaceInformationPtr& si) :
103  ob::StateValidityChecker(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 =
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 
131 ob::OptimizationObjectivePtr getPathLengthObjective(const ob::SpaceInformationPtr& si);
132 
133 ob::OptimizationObjectivePtr getThresholdPathLengthObj(const ob::SpaceInformationPtr& si);
134 
135 ob::OptimizationObjectivePtr getClearanceObjective(const ob::SpaceInformationPtr& si);
136 
137 ob::OptimizationObjectivePtr getBalancedObjective1(const ob::SpaceInformationPtr& si);
138 
139 ob::OptimizationObjectivePtr getBalancedObjective2(const ob::SpaceInformationPtr& si);
140 
141 ob::OptimizationObjectivePtr getPathLengthObjWithCostToGo(const ob::SpaceInformationPtr& si);
142 
143 ob::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 
196 ob::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.");
215  break;
216  }
217 }
218 
219 void 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 
293 int 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 
318 ob::OptimizationObjectivePtr getPathLengthObjective(const ob::SpaceInformationPtr& si)
319 {
320  return std::make_shared<ob::PathLengthOptimizationObjective>(si);
321 }
322 
326 ob::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 
345 class ClearanceObjective : public ob::StateCostIntegralObjective
346 {
347 public:
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  }
362 };
363 
366 ob::OptimizationObjectivePtr getClearanceObjective(const ob::SpaceInformationPtr& si)
367 {
368  return std::make_shared<ClearanceObjective>(si);
369 }
370 
383 ob::OptimizationObjectivePtr getBalancedObjective1(const ob::SpaceInformationPtr& si)
384 {
385  auto lengthObj(std::make_shared<ob::PathLengthOptimizationObjective>(si));
386  auto clearObj(std::make_shared<ClearanceObjective>(si));
387  auto opt(std::make_shared<ob::MultiOptimizationObjective>(si));
388  opt->addObjective(lengthObj, 10.0);
389  opt->addObjective(clearObj, 1.0);
390 
391  return ob::OptimizationObjectivePtr(opt);
392 }
393 
397 ob::OptimizationObjectivePtr getBalancedObjective2(const ob::SpaceInformationPtr& si)
398 {
399  auto lengthObj(std::make_shared<ob::PathLengthOptimizationObjective>(si));
400  auto clearObj(std::make_shared<ClearanceObjective>(si));
401 
402  return 10.0*lengthObj + clearObj;
403 }
404 
408 ob::OptimizationObjectivePtr getPathLengthObjWithCostToGo(const ob::SpaceInformationPtr& si)
409 {
410  auto obj(std::make_shared<ob::PathLengthOptimizationObjective>(si));
411  obj->setCostToGoHeuristic(&ob::goalRegionCostToGo);
412  return obj;
413 }
414 
416 bool argParse(int argc, char** argv, double* runTimePtr, optimalPlanner *plannerPtr, planningObjective *objectivePtr, std::string *outputFilePtr)
417 {
418  namespace bpo = boost::program_options;
419 
420  // Declare the supported options.
421  bpo::options_description desc("Allowed options");
422  desc.add_options()
423  ("help,h", "produce help message")
424  ("runtime,t", bpo::value<double>()->default_value(1.0), "(Optional) Specify the runtime in seconds. Defaults to 1 and must be greater than 0.")
425  ("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
426  ("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
427  ("file,f", bpo::value<std::string>()->default_value(""), "(Optional) Specify an output path for the found solution path.")
428  ("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.");
429  bpo::variables_map vm;
430  bpo::store(bpo::parse_command_line(argc, argv, desc), vm);
431  bpo::notify(vm);
432 
433  // Check if the help flag has been given:
434  if (vm.count("help") != 0u)
435  {
436  std::cout << desc << std::endl;
437  return false;
438  }
439 
440  // Set the log-level
441  unsigned int logLevel = vm["info"].as<unsigned int>();
442 
443  // Switch to setting the log level:
444  if (logLevel == 0u)
445  {
446  ompl::msg::setLogLevel(ompl::msg::LOG_WARN);
447  }
448  else if (logLevel == 1u)
449  {
450  ompl::msg::setLogLevel(ompl::msg::LOG_INFO);
451  }
452  else if (logLevel == 2u)
453  {
454  ompl::msg::setLogLevel(ompl::msg::LOG_DEBUG);
455  }
456  else
457  {
458  std::cout << "Invalid log-level integer." << std::endl << std::endl << desc << std::endl;
459  return false;
460  }
461 
462  // Get the runtime as a double
463  *runTimePtr = vm["runtime"].as<double>();
464 
465  // Sanity check
466  if (*runTimePtr <= 0.0)
467  {
468  std::cout << "Invalid runtime." << std::endl << std::endl << desc << std::endl;
469  return false;
470  }
471 
472  // Get the specified planner as a string
473  std::string plannerStr = vm["planner"].as<std::string>();
474 
475  // Map the string to the enum
476  if (boost::iequals("BFMTstar", plannerStr))
477  {
478  *plannerPtr = PLANNER_BFMTSTAR;
479  }
480  else if (boost::iequals("BITstar", plannerStr))
481  {
482  *plannerPtr = PLANNER_BITSTAR;
483  }
484  else if (boost::iequals("CForest", plannerStr))
485  {
486  *plannerPtr = PLANNER_CFOREST;
487  }
488  else if (boost::iequals("FMTstar", plannerStr))
489  {
490  *plannerPtr = PLANNER_FMTSTAR;
491  }
492  else if (boost::iequals("InformedRRTstar", plannerStr))
493  {
494  *plannerPtr = PLANNER_INF_RRTSTAR;
495  }
496  else if (boost::iequals("PRMstar", plannerStr))
497  {
498  *plannerPtr = PLANNER_PRMSTAR;
499  }
500  else if (boost::iequals("RRTstar", plannerStr))
501  {
502  *plannerPtr = PLANNER_RRTSTAR;
503  }
504  else if (boost::iequals("SORRTstar", plannerStr))
505  {
506  *plannerPtr = PLANNER_SORRTSTAR;
507  }
508  else
509  {
510  std::cout << "Invalid planner string." << std::endl << std::endl << desc << std::endl;
511  return false;
512  }
513 
514  // Get the specified objective as a string
515  std::string objectiveStr = vm["objective"].as<std::string>();
516 
517  // Map the string to the enum
518  if (boost::iequals("PathClearance", objectiveStr))
519  {
520  *objectivePtr = OBJECTIVE_PATHCLEARANCE;
521  }
522  else if (boost::iequals("PathLength", objectiveStr))
523  {
524  *objectivePtr = OBJECTIVE_PATHLENGTH;
525  }
526  else if (boost::iequals("ThresholdPathLength", objectiveStr))
527  {
528  *objectivePtr = OBJECTIVE_THRESHOLDPATHLENGTH;
529  }
530  else if (boost::iequals("WeightedLengthAndClearanceCombo", objectiveStr))
531  {
532  *objectivePtr = OBJECTIVE_WEIGHTEDCOMBO;
533  }
534  else
535  {
536  std::cout << "Invalid objective string." << std::endl << std::endl << desc << std::endl;
537  return false;
538  }
539 
540  // Get the output file string and store it in the return pointer
541  *outputFilePtr = vm["file"].as<std::string>();
542 
543  // Looks like we parsed the arguments successfully
544  return true;
545 }
Definition of a scoped state.
Definition: ScopedState.h:56
virtual double clearance(const State *) const
Report the distance to the nearest invalid state when starting from state. If the distance is negativ...
A shared pointer wrapper for ompl::base::Planner.
const T * as() const
Cast this instance to a desired type.
Definition: State.h:66
ompl::base::State StateType
Define the type of state allocated by this space.
Definition: StateSpace.h:78
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Definition: Console.h:64
Defines optimization objectives where path cost can be represented as a path integral over a cost fun...
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
Abstract definition for a class checking the validity of states. The implementation of this class mus...
StateValidityChecker(SpaceInformation *si)
Constructor.
A class to store the exit status of Planner::solve()
Definition: PlannerStatus.h:48
A shared pointer wrapper for ompl::base::SpaceInformation.
Definition of an abstract state.
Definition: State.h:49
This namespace contains sampling based planning routines shared by both planning under geometric cons...
Definition: Cost.h:44
virtual bool isValid(const State *state) const =0
Return true if the state state is valid. Usually, this means at least collision checking. If it is possible that ompl::base::StateSpace::interpolate() or ompl::control::ControlSpace::propagate() return states that are outside of bounds, this function should also make a call to ompl::base::SpaceInformation::satisfiesBounds().
A shared pointer wrapper for ompl::base::OptimizationObjective.
Definition of a geometric path.
Definition: PathGeometric.h:60
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition: Cost.h:47
This namespace contains code that is specific to planning under geometric constraints.
Definition: GeneticSearch.h:47