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 }
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:79
Definition of an abstract state.
Definition: State.h:113
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:111
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:120
Abstract definition for a class checking the validity of states. The implementation of this class mus...