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 */
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 #include <ompl/geometric/planners/rrt/RRTstar.h>
43 
44 #include <fstream>
45 
46 namespace ob = ompl::base;
47 namespace og = ompl::geometric;
49 // Our "collision checker". For this demo, our robot's state space
50 // lies in [0,1]x[0,1], with a circular obstacle of radius 0.25
51 // centered at (0.5,0.5). Any states lying in this circular region are
52 // considered "in collision".
53 class ValidityChecker : public ob::StateValidityChecker
54 {
55 public:
56  ValidityChecker(const ob::SpaceInformationPtr& si) :
57  ob::StateValidityChecker(si) {}
58 
59  // Returns whether the given state's position overlaps the
60  // circular obstacle
61  bool isValid(const ob::State* state) const
62  {
63  return this->clearance(state) > 0.0;
64  }
65 
66  // Returns the distance from the given state's position to the
67  // boundary of the circular obstacle.
68  double clearance(const ob::State* state) const
69  {
70  // We know we're working with a RealVectorStateSpace in this
71  // example, so we downcast state into the specific type.
74 
75  // Extract the robot's (x,y) position from its state
76  double x = state2D->values[0];
77  double y = state2D->values[1];
78 
79  // Distance formula between two points, offset by the circle's
80  // radius
81  return sqrt((x-0.5)*(x-0.5) + (y-0.5)*(y-0.5)) - 0.25;
82  }
83 };
84 
85 ob::OptimizationObjectivePtr getPathLengthObjective(const ob::SpaceInformationPtr& si);
86 
87 ob::OptimizationObjectivePtr getThresholdPathLengthObj(const ob::SpaceInformationPtr& si);
88 
89 ob::OptimizationObjectivePtr getClearanceObjective(const ob::SpaceInformationPtr& si);
90 
91 ob::OptimizationObjectivePtr getBalancedObjective1(const ob::SpaceInformationPtr& si);
92 
93 ob::OptimizationObjectivePtr getBalancedObjective2(const ob::SpaceInformationPtr& si);
94 
95 ob::OptimizationObjectivePtr getPathLengthObjWithCostToGo(const ob::SpaceInformationPtr& si);
96 
97 void plan(int argc, char** argv)
98 {
99  // Construct the robot state space in which we're planning. We're
100  // planning in [0,1]x[0,1], a subset of R^2.
102 
103  // Set the bounds of space to be in [0,1].
104  space->as<ob::RealVectorStateSpace>()->setBounds(0.0, 1.0);
105 
106  // Construct a space information instance for this state space
108 
109  // Set the object used to check which states in the space are valid
110  si->setStateValidityChecker(ob::StateValidityCheckerPtr(new ValidityChecker(si)));
111 
112  si->setup();
113 
114  // Set our robot's starting state to be the bottom-left corner of
115  // the environment, or (0,0).
116  ob::ScopedState<> start(space);
117  start->as<ob::RealVectorStateSpace::StateType>()->values[0] = 0.0;
118  start->as<ob::RealVectorStateSpace::StateType>()->values[1] = 0.0;
119 
120  // Set our robot's goal state to be the top-right corner of the
121  // environment, or (1,1).
122  ob::ScopedState<> goal(space);
123  goal->as<ob::RealVectorStateSpace::StateType>()->values[0] = 1.0;
124  goal->as<ob::RealVectorStateSpace::StateType>()->values[1] = 1.0;
125 
126  // Create a problem instance
128 
129  // Set the start and goal states
130  pdef->setStartAndGoalStates(start, goal);
131 
132  // Since we want to find an optimal plan, we need to define what
133  // is optimal with an OptimizationObjective structure. Un-comment
134  // exactly one of the following 6 lines to see some examples of
135  // optimization objectives.
136  pdef->setOptimizationObjective(getPathLengthObjective(si));
137  // pdef->setOptimizationObjective(getThresholdPathLengthObj(si));
138  // pdef->setOptimizationObjective(getClearanceObjective(si));
139  // pdef->setOptimizationObjective(getBalancedObjective1(si));
140  // pdef->setOptimizationObjective(getBalancedObjective2(si));
141  // pdef->setOptimizationObjective(getPathLengthObjWithCostToGo(si));
142 
143  // Construct our optimal planner using the RRTstar algorithm.
144  ob::PlannerPtr optimizingPlanner(new og::RRTstar(si));
145 
146  // Set the problem instance for our planner to solve
147  optimizingPlanner->setProblemDefinition(pdef);
148  optimizingPlanner->setup();
149 
150  // attempt to solve the planning problem within one second of
151  // planning time
152  ob::PlannerStatus solved = optimizingPlanner->solve(1.0);
153 
154  if (solved)
155  {
156  // Output the length of the path found
157  std::cout
158  << "Found solution of path length "
159  << pdef->getSolutionPath()->length() << std::endl;
160 
161  // If a filename was specified, output the path as a matrix to
162  // that file for visualization
163  if (argc > 1)
164  {
165  std::ofstream outFile(argv[1]);
166  boost::static_pointer_cast<og::PathGeometric>(pdef->getSolutionPath())->
167  printAsMatrix(outFile);
168  outFile.close();
169  }
170  }
171  else
172  std::cout << "No solution found." << std::endl;
173 }
174 
175 int main(int argc, char** argv)
176 {
177  plan(argc, argv);
178 
179  return 0;
180 }
181 
186 ob::OptimizationObjectivePtr getPathLengthObjective(const ob::SpaceInformationPtr& si)
187 {
189 }
190 
194 ob::OptimizationObjectivePtr getThresholdPathLengthObj(const ob::SpaceInformationPtr& si)
195 {
197  obj->setCostThreshold(ob::Cost(1.51));
198  return obj;
199 }
200 
213 class ClearanceObjective : public ob::StateCostIntegralObjective
214 {
215 public:
216  ClearanceObjective(const ob::SpaceInformationPtr& si) :
217  ob::StateCostIntegralObjective(si, true)
218  {
219  }
220 
221  // Our requirement is to maximize path clearance from obstacles,
222  // but we want to represent the objective as a path cost
223  // minimization. Therefore, we set each state's cost to be the
224  // reciprocal of its clearance, so that as state clearance
225  // increases, the state cost decreases.
226  ob::Cost stateCost(const ob::State* s) const
227  {
228  return ob::Cost(1 / si_->getStateValidityChecker()->clearance(s));
229  }
230 };
231 
234 ob::OptimizationObjectivePtr getClearanceObjective(const ob::SpaceInformationPtr& si)
235 {
236  return ob::OptimizationObjectivePtr(new ClearanceObjective(si));
237 }
238 
251 ob::OptimizationObjectivePtr getBalancedObjective1(const ob::SpaceInformationPtr& si)
252 {
254  ob::OptimizationObjectivePtr clearObj(new ClearanceObjective(si));
255 
257  opt->addObjective(lengthObj, 10.0);
258  opt->addObjective(clearObj, 1.0);
259 
260  return ob::OptimizationObjectivePtr(opt);
261 }
262 
266 ob::OptimizationObjectivePtr getBalancedObjective2(const ob::SpaceInformationPtr& si)
267 {
269  ob::OptimizationObjectivePtr clearObj(new ClearanceObjective(si));
270 
271  return 10.0*lengthObj + clearObj;
272 }
273 
277 ob::OptimizationObjectivePtr getPathLengthObjWithCostToGo(const ob::SpaceInformationPtr& si)
278 {
280  obj->setCostToGoHeuristic(&ob::goalRegionCostToGo);
281  return obj;
282 }
Optimal Rapidly-exploring Random Trees.
Definition: RRTstar.h:76
A boost shared pointer wrapper for ompl::base::ProblemDefinition.
This class allows for the definition of multiobjective optimal planning problems. Objectives are adde...
Definition of a scoped state.
Definition: ScopedState.h:56
A boost shared pointer wrapper for ompl::base::StateSpace.
void addObjective(const OptimizationObjectivePtr &objective, double weight)
Adds a new objective for this multiobjective. A weight must also be specified for specifying importan...
virtual double clearance(const State *) const
Report the distance to the nearest invalid state when starting from state. If the distance is negativ...
State StateType
Define the type of state allocated by this space.
Definition: StateSpace.h:78
A boost shared pointer wrapper for ompl::base::StateValidityChecker.
A boost shared pointer wrapper for ompl::base::Planner.
const T * as() const
Cast this instance to a desired type.
Definition: State.h:74
Defines optimization objectives where path cost can be represented as a path integral over a cost fun...
Abstract definition for a class checking the validity of states. The implementation of this class mus...
A class to store the exit status of Planner::solve()
Definition: PlannerStatus.h:48
A boost shared pointer wrapper for ompl::base::SpaceInformation.
A state space representing Rn. The distance function is the L2 norm.
The base class for space information. This contains all the information about the space planning is d...
An optimization objective which corresponds to optimizing path length.
Definition of an abstract state.
Definition: State.h:50
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().
Definition of a problem to be solved. This includes the start state(s) for the system and a goal spec...
A boost 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:48