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 *runTime, 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 ob::RealVectorStateSpace::StateType* 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  else
311  {
312  // Return with error
313  return -1;
314  }
315 }
316 
321 ob::OptimizationObjectivePtr getPathLengthObjective(const ob::SpaceInformationPtr& si)
322 {
323  return std::make_shared<ob::PathLengthOptimizationObjective>(si);
324 }
325 
329 ob::OptimizationObjectivePtr getThresholdPathLengthObj(const ob::SpaceInformationPtr& si)
330 {
331  auto obj(std::make_shared<ob::PathLengthOptimizationObjective>(si));
332  obj->setCostThreshold(ob::Cost(1.51));
333  return obj;
334 }
335 
348 class ClearanceObjective : public ob::StateCostIntegralObjective
349 {
350 public:
351  ClearanceObjective(const ob::SpaceInformationPtr& si) :
352  ob::StateCostIntegralObjective(si, true)
353  {
354  }
355 
356  // Our requirement is to maximize path clearance from obstacles,
357  // but we want to represent the objective as a path cost
358  // minimization. Therefore, we set each state's cost to be the
359  // reciprocal of its clearance, so that as state clearance
360  // increases, the state cost decreases.
361  ob::Cost stateCost(const ob::State* s) const override
362  {
363  return ob::Cost(1 / si_->getStateValidityChecker()->clearance(s));
364  }
365 };
366 
369 ob::OptimizationObjectivePtr getClearanceObjective(const ob::SpaceInformationPtr& si)
370 {
371  return std::make_shared<ClearanceObjective>(si);
372 }
373 
386 ob::OptimizationObjectivePtr getBalancedObjective1(const ob::SpaceInformationPtr& si)
387 {
388  auto lengthObj(std::make_shared<ob::PathLengthOptimizationObjective>(si));
389  auto clearObj(std::make_shared<ClearanceObjective>(si));
390  auto opt(std::make_shared<ob::MultiOptimizationObjective>(si));
391  opt->addObjective(lengthObj, 10.0);
392  opt->addObjective(clearObj, 1.0);
393 
394  return ob::OptimizationObjectivePtr(opt);
395 }
396 
400 ob::OptimizationObjectivePtr getBalancedObjective2(const ob::SpaceInformationPtr& si)
401 {
402  auto lengthObj(std::make_shared<ob::PathLengthOptimizationObjective>(si));
403  auto clearObj(std::make_shared<ClearanceObjective>(si));
404 
405  return 10.0*lengthObj + clearObj;
406 }
407 
411 ob::OptimizationObjectivePtr getPathLengthObjWithCostToGo(const ob::SpaceInformationPtr& si)
412 {
413  auto obj(std::make_shared<ob::PathLengthOptimizationObjective>(si));
414  obj->setCostToGoHeuristic(&ob::goalRegionCostToGo);
415  return obj;
416 }
417 
419 bool argParse(int argc, char** argv, double* runTimePtr, optimalPlanner *plannerPtr, planningObjective *objectivePtr, std::string *outputFilePtr)
420 {
421  namespace bpo = boost::program_options;
422 
423  // Declare the supported options.
424  bpo::options_description desc("Allowed options");
425  desc.add_options()
426  ("help,h", "produce help message")
427  ("runtime,t", bpo::value<double>()->default_value(1.0), "(Optional) Specify the runtime in seconds. Defaults to 1 and must be greater than 0.")
428  ("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
429  ("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
430  ("file,f", bpo::value<std::string>()->default_value(""), "(Optional) Specify an output path for the found solution path.")
431  ("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.");
432  bpo::variables_map vm;
433  bpo::store(bpo::parse_command_line(argc, argv, desc), vm);
434  bpo::notify(vm);
435 
436  // Check if the help flag has been given:
437  if (vm.count("help"))
438  {
439  std::cout << desc << std::endl;
440  return false;
441  }
442 
443  // Set the log-level
444  unsigned int logLevel = vm["info"].as<unsigned int>();
445 
446  // Switch to setting the log level:
447  if (logLevel == 0u)
448  {
449  ompl::msg::setLogLevel(ompl::msg::LOG_WARN);
450  }
451  else if (logLevel == 1u)
452  {
453  ompl::msg::setLogLevel(ompl::msg::LOG_INFO);
454  }
455  else if (logLevel == 2u)
456  {
457  ompl::msg::setLogLevel(ompl::msg::LOG_DEBUG);
458  }
459  else
460  {
461  std::cout << "Invalid log-level integer." << std::endl << std::endl << desc << std::endl;
462  return false;
463  }
464 
465  // Get the runtime as a double
466  *runTimePtr = vm["runtime"].as<double>();
467 
468  // Sanity check
469  if (*runTimePtr <= 0.0)
470  {
471  std::cout << "Invalid runtime." << std::endl << std::endl << desc << std::endl;
472  return false;
473  }
474 
475  // Get the specified planner as a string
476  std::string plannerStr = vm["planner"].as<std::string>();
477 
478  // Map the string to the enum
479  if (boost::iequals("BFMTstar", plannerStr))
480  {
481  *plannerPtr = PLANNER_BFMTSTAR;
482  }
483  else if (boost::iequals("BITstar", plannerStr))
484  {
485  *plannerPtr = PLANNER_BITSTAR;
486  }
487  else if (boost::iequals("CForest", plannerStr))
488  {
489  *plannerPtr = PLANNER_CFOREST;
490  }
491  else if (boost::iequals("FMTstar", plannerStr))
492  {
493  *plannerPtr = PLANNER_FMTSTAR;
494  }
495  else if (boost::iequals("InformedRRTstar", plannerStr))
496  {
497  *plannerPtr = PLANNER_INF_RRTSTAR;
498  }
499  else if (boost::iequals("PRMstar", plannerStr))
500  {
501  *plannerPtr = PLANNER_PRMSTAR;
502  }
503  else if (boost::iequals("RRTstar", plannerStr))
504  {
505  *plannerPtr = PLANNER_RRTSTAR;
506  }
507  else if (boost::iequals("SORRTstar", plannerStr))
508  {
509  *plannerPtr = PLANNER_SORRTSTAR;
510  }
511  else
512  {
513  std::cout << "Invalid planner string." << std::endl << std::endl << desc << std::endl;
514  return false;
515  }
516 
517  // Get the specified objective as a string
518  std::string objectiveStr = vm["objective"].as<std::string>();
519 
520  // Map the string to the enum
521  if (boost::iequals("PathClearance", objectiveStr))
522  {
523  *objectivePtr = OBJECTIVE_PATHCLEARANCE;
524  }
525  else if (boost::iequals("PathLength", objectiveStr))
526  {
527  *objectivePtr = OBJECTIVE_PATHLENGTH;
528  }
529  else if (boost::iequals("ThresholdPathLength", objectiveStr))
530  {
531  *objectivePtr = OBJECTIVE_THRESHOLDPATHLENGTH;
532  }
533  else if (boost::iequals("WeightedLengthAndClearanceCombo", objectiveStr))
534  {
535  *objectivePtr = OBJECTIVE_WEIGHTEDCOMBO;
536  }
537  else
538  {
539  std::cout << "Invalid objective string." << std::endl << std::endl << desc << std::endl;
540  return false;
541  }
542 
543  // Get the output file string and store it in the return pointer
544  *outputFilePtr = vm["file"].as<std::string>();
545 
546  // Looks like we parsed the arguments successfully
547  return true;
548 }
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