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 
67 namespace ob = ompl::base;
68 namespace og = ompl::geometric;
70 
71 // An enum of supported optimal planners, alphabetical order
72 enum 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
86 enum planningObjective
87 {
88  OBJECTIVE_PATHCLEARANCE,
89  OBJECTIVE_PATHLENGTH,
90  OBJECTIVE_THRESHOLDPATHLENGTH,
91  OBJECTIVE_WEIGHTEDCOMBO
92 };
93 
94 // Parse the command-line arguments
95 bool 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".
101 class ValidityChecker : public ob::StateValidityChecker
102 {
103 public:
104  ValidityChecker(const ob::SpaceInformationPtr& si) :
105  ob::StateValidityChecker(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 =
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 
133 ob::OptimizationObjectivePtr getPathLengthObjective(const ob::SpaceInformationPtr& si);
134 
135 ob::OptimizationObjectivePtr getThresholdPathLengthObj(const ob::SpaceInformationPtr& si);
136 
137 ob::OptimizationObjectivePtr getClearanceObjective(const ob::SpaceInformationPtr& si);
138 
139 ob::OptimizationObjectivePtr getBalancedObjective1(const ob::SpaceInformationPtr& si);
140 
141 ob::OptimizationObjectivePtr getBalancedObjective2(const ob::SpaceInformationPtr& si);
142 
143 ob::OptimizationObjectivePtr getPathLengthObjWithCostToGo(const ob::SpaceInformationPtr& si);
144 
145 ob::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 
203 ob::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 
226 void 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 
300 int 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 
325 ob::OptimizationObjectivePtr getPathLengthObjective(const ob::SpaceInformationPtr& si)
326 {
327  return std::make_shared<ob::PathLengthOptimizationObjective>(si);
328 }
329 
333 ob::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 
352 class ClearanceObjective : public ob::StateCostIntegralObjective
353 {
354 public:
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 
374 ob::OptimizationObjectivePtr getClearanceObjective(const ob::SpaceInformationPtr& si)
375 {
376  return std::make_shared<ClearanceObjective>(si);
377 }
378 
391 ob::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 
405 ob::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 
416 ob::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 
424 bool 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.
ompl::base::State StateType
Define the type of state allocated by this space.
Definition: StateSpace.h:78
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...
This namespace contains code that is specific to planning under geometric constraints.
Definition: GeneticSearch.h:48
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()
Definition: PlannerStatus.h:49