RandomWalkPlanner.py
1 #!/usr/bin/env python
2 
3 ######################################################################
4 # Software License Agreement (BSD License)
5 #
6 # Copyright (c) 2010, Rice University
7 # All rights reserved.
8 #
9 # Redistribution and use in source and binary forms, with or without
10 # modification, are permitted provided that the following conditions
11 # are met:
12 #
13 # * Redistributions of source code must retain the above copyright
14 # notice, this list of conditions and the following disclaimer.
15 # * Redistributions in binary form must reproduce the above
16 # copyright notice, this list of conditions and the following
17 # disclaimer in the documentation and/or other materials provided
18 # with the distribution.
19 # * Neither the name of the Rice University nor the names of its
20 # contributors may be used to endorse or promote products derived
21 # from this software without specific prior written permission.
22 #
23 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27 # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34 # POSSIBILITY OF SUCH DAMAGE.
35 ######################################################################
36 
37 # Author: Mark Moll
38 
39 try:
40  from ompl import util as ou
41  from ompl import base as ob
42  from ompl import geometric as og
43 except:
44  # if the ompl module is not in the PYTHONPATH assume it is installed in a
45  # subdirectory of the parent directory called "py-bindings."
46  from os.path import abspath, dirname, join
47  import sys
48  sys.path.insert(0, join(dirname(dirname(abspath(__file__))),'py-bindings'))
49  from ompl import util as ou
50  from ompl import base as ob
51  from ompl import geometric as og
52 from random import choice
53 
54 ## @cond IGNORE
55 
57  def __init__(self, si):
58  super(RandomWalkPlanner, self).__init__(si, "RandomWalkPlanner")
59  self.states_ = []
60  self.sampler_ = si.allocStateSampler()
61 
62  def solve(self, ptc):
63  pdef = self.getProblemDefinition()
64  goal = pdef.getGoal()
65  si = self.getSpaceInformation()
66  pi = self.getPlannerInputStates()
67  st = pi.nextStart()
68  while st:
69  self.states_.append(st)
70  st = pi.nextStart()
71  solution = None
72  approxsol = 0
73  approxdif = 1e6
74  while not ptc():
75  rstate = si.allocState()
76  # pick a random state in the state space
77  self.sampler_.sampleUniform(rstate)
78  # check motion
79  if si.checkMotion(self.states_[-1], rstate):
80  self.states_.append(rstate)
81  sat = goal.isSatisfied(rstate)
82  dist = goal.distanceGoal(rstate)
83  if sat:
84  approxdif = dist
85  solution = len(self.states_)
86  break
87  if dist < approxdif:
88  approxdif = dist
89  approxsol = len(self.states_)
90  solved = False
91  approximate = False
92  if not solution:
93  solution = approxsol
94  approximate = True
95  if solution:
96  path = og.PathGeometric(si)
97  for s in self.states_[:solution]:
98  path.append(s)
99  pdef.addSolutionPath(path)
100  solved = True
101  return ob.PlannerStatus(solved, approximate)
102 
103  def clear(self):
104  super(RandomWalkPlanner, self).clear()
105  self.states_ = []
106 
107 ## @endcond
108 
109 def isStateValid(state):
110  x = state[0]
111  y = state[1]
112  z = state[2]
113  return x*x + y*y + z*z > .8
114 
115 def plan():
116  # create an R^3 state space
117  space = ob.RealVectorStateSpace(3)
118  # set lower and upper bounds
119  bounds = ob.RealVectorBounds(3)
120  bounds.setLow(-1)
121  bounds.setHigh(1)
122  space.setBounds(bounds)
123  # create a simple setup object
124  ss = og.SimpleSetup(space)
125  ss.setStateValidityChecker(ob.StateValidityCheckerFn(isStateValid))
126  start = ob.State(space)
127  start()[0] = -1.
128  start()[1] = -1.
129  start()[2] = -1.
130  goal = ob.State(space)
131  goal()[0] = 1.
132  goal()[1] = 1.
133  goal()[2] = 1.
134  ss.setStartAndGoalStates(start, goal, .05)
135  # set the planner
136  planner = RandomWalkPlanner(ss.getSpaceInformation())
137  ss.setPlanner(planner)
138 
139  result = ss.solve(10.0)
140  if result:
141  if result.getStatus() == ob.PlannerStatus.APPROXIMATE_SOLUTION:
142  print("Solution is approximate")
143  # try to shorten the path
144  ss.simplifySolution()
145  # print the simplified path
146  print(ss.getSolutionPath())
147 
148 
149 if __name__ == "__main__":
150  plan()
boost::function< bool(const State *)> StateValidityCheckerFn
If no state validity checking class is specified (StateValidityChecker), a boost function can be spec...
Create the set of classes typically needed to solve a geometric problem.
Definition: SimpleSetup.h:65
Base class for a planner.
Definition: Planner.h:232
A class to store the exit status of Planner::solve()
Definition: PlannerStatus.h:48
A state space representing Rn. The distance function is the L2 norm.
Definition of an abstract state.
Definition: State.h:50
The lower and upper bounds for an Rn space.
Definition of a geometric path.
Definition: PathGeometric.h:60