RandomWalkPlanner.py
1 #!/usr/bin/env python
2 
3 
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 
53 
54 
56  def __init__(self, si):
57  super(RandomWalkPlanner, self).__init__(si, "RandomWalkPlanner")
58  self.states_ = []
59  self.sampler_ = si.allocStateSampler()
60 
61  def solve(self, ptc):
62  pdef = self.getProblemDefinition()
63  goal = pdef.getGoal()
64  si = self.getSpaceInformation()
65  pi = self.getPlannerInputStates()
66  st = pi.nextStart()
67  while st:
68  self.states_.append(st)
69  st = pi.nextStart()
70  solution = None
71  approxsol = 0
72  approxdif = 1e6
73  while not ptc():
74  rstate = si.allocState()
75  # pick a random state in the state space
76  self.sampler_.sampleUniform(rstate)
77  # check motion
78  if si.checkMotion(self.states_[-1], rstate):
79  self.states_.append(rstate)
80  sat = goal.isSatisfied(rstate)
81  dist = goal.distanceGoal(rstate)
82  if sat:
83  approxdif = dist
84  solution = len(self.states_)
85  break
86  if dist < approxdif:
87  approxdif = dist
88  approxsol = len(self.states_)
89  solved = False
90  approximate = False
91  if not solution:
92  solution = approxsol
93  approximate = True
94  if solution:
95  path = og.PathGeometric(si)
96  for s in self.states_[:solution]:
97  path.append(s)
98  pdef.addSolutionPath(path)
99  solved = True
100  return ob.PlannerStatus(solved, approximate)
101 
102  def clear(self):
103  super(RandomWalkPlanner, self).clear()
104  self.states_ = []
105 
106 
107 
108 def isStateValid(state):
109  x = state[0]
110  y = state[1]
111  z = state[2]
112  return x*x + y*y + z*z > .8
113 
114 def plan():
115  # create an R^3 state space
116  space = ob.RealVectorStateSpace(3)
117  # set lower and upper bounds
118  bounds = ob.RealVectorBounds(3)
119  bounds.setLow(-1)
120  bounds.setHigh(1)
121  space.setBounds(bounds)
122  # create a simple setup object
123  ss = og.SimpleSetup(space)
124  ss.setStateValidityChecker(ob.StateValidityCheckerFn(isStateValid))
125  start = ob.State(space)
126  start()[0] = -1.
127  start()[1] = -1.
128  start()[2] = -1.
129  goal = ob.State(space)
130  goal()[0] = 1.
131  goal()[1] = 1.
132  goal()[2] = 1.
133  ss.setStartAndGoalStates(start, goal, .05)
134  # set the planner
135  planner = RandomWalkPlanner(ss.getSpaceInformation())
136  ss.setPlanner(planner)
137 
138  result = ss.solve(10.0)
139  if result:
140  if result.getStatus() == ob.PlannerStatus.APPROXIMATE_SOLUTION:
141  print("Solution is approximate")
142  # try to shorten the path
143  ss.simplifySolution()
144  # print the simplified path
145  print(ss.getSolutionPath())
146 
147 
148 if __name__ == "__main__":
149  plan()
Create the set of classes typically needed to solve a geometric problem.
Definition: SimpleSetup.h:63
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:49
The lower and upper bounds for an Rn space.
Definition of a geometric path.
Definition: PathGeometric.h:60
std::function< bool(const State *)> StateValidityCheckerFn
If no state validity checking class is specified (StateValidityChecker), a std::function can be speci...