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 
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.");
214  return ob::OptimizationObjectivePtr();
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  std::numeric_limits<double>::min()));
362  }
363 };
364 
367 ob::OptimizationObjectivePtr getClearanceObjective(const ob::SpaceInformationPtr& si)
368 {
369  return std::make_shared<ClearanceObjective>(si);
370 }
371 
384 ob::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 
398 ob::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 
409 ob::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 
417 bool 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 }
virtual double clearance(const State *) const
Report the distance to the nearest invalid state when starting from state. If the distance is negativ...
This namespace contains code that is specific to planning under geometric constraints.
Definition: GeneticSearch.h:80
Definition of an abstract state.
Definition: State.h:114
This namespace contains sampling based planning routines shared by both planning under geometric cons...
Defines optimization objectives where path cost can be represented as a path integral over a cost fun...
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition: Cost.h:112
const T * as() const
Cast this instance to a desired type.
Definition: State.h:162
ompl::base::State StateType
Define the type of state allocated by this space.
Definition: StateSpace.h:142
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
StateValidityChecker(SpaceInformation *si)
Constructor.
A class to store the exit status of Planner::solve()
Cost stateCost(const State *s) const override
Returns a cost with a value of 1.
virtual bool isValid(const State *state) const =0
Return true if the state state is valid. Usually, this means at least collision checking....
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Definition: Console.h:64
Definition of a scoped state.
Definition: ScopedState.h:121
Abstract definition for a class checking the validity of states. The implementation of this class mus...