RandomWalkPlanner.py
1 #!/usr/bin/env python
2 
3 
36 
37 # Author: Mark Moll
38 
39 try:
40  from ompl import base as ob
41  from ompl import geometric as og
42 except ImportError:
43  # if the ompl module is not in the PYTHONPATH assume it is installed in a
44  # subdirectory of the parent directory called "py-bindings."
45  from os.path import abspath, dirname, join
46  import sys
47  sys.path.insert(0, join(dirname(dirname(abspath(__file__))), "py-bindings"))
48  from ompl import base as ob
49  from ompl import geometric as og
50 
51 
52 
54  def __init__(self, si):
55  super(RandomWalkPlanner, self).__init__(si, "RandomWalkPlanner")
56  self.states_ = []
57  self.sampler_ = si.allocStateSampler()
58 
59  def solve(self, ptc):
60  pdef = self.getProblemDefinition()
61  goal = pdef.getGoal()
62  si = self.getSpaceInformation()
63  pi = self.getPlannerInputStates()
64  st = pi.nextStart()
65  while st:
66  self.states_.append(st)
67  st = pi.nextStart()
68  solution = None
69  approxsol = 0
70  approxdif = 1e6
71  while not ptc():
72  rstate = si.allocState()
73  # pick a random state in the state space
74  self.sampler_.sampleUniform(rstate)
75  # check motion
76  if si.checkMotion(self.states_[-1], rstate):
77  self.states_.append(rstate)
78  sat = goal.isSatisfied(rstate)
79  dist = goal.distanceGoal(rstate)
80  if sat:
81  approxdif = dist
82  solution = len(self.states_)
83  break
84  if dist < approxdif:
85  approxdif = dist
86  approxsol = len(self.states_)
87  solved = False
88  approximate = False
89  if not solution:
90  solution = approxsol
91  approximate = True
92  if solution:
93  path = og.PathGeometric(si)
94  for s in self.states_[:solution]:
95  path.append(s)
96  pdef.addSolutionPath(path)
97  solved = True
98  return ob.PlannerStatus(solved, approximate)
99 
100  def clear(self):
101  super(RandomWalkPlanner, self).clear()
102  self.states_ = []
103 
104 
105 
106 def isStateValid(state):
107  x = state[0]
108  y = state[1]
109  z = state[2]
110  return x*x + y*y + z*z > .8
111 
112 def plan():
113  # create an R^3 state space
114  space = ob.RealVectorStateSpace(3)
115  # set lower and upper bounds
116  bounds = ob.RealVectorBounds(3)
117  bounds.setLow(-1)
118  bounds.setHigh(1)
119  space.setBounds(bounds)
120  # create a simple setup object
121  ss = og.SimpleSetup(space)
122  ss.setStateValidityChecker(ob.StateValidityCheckerFn(isStateValid))
123  start = ob.State(space)
124  start()[0] = -1.
125  start()[1] = -1.
126  start()[2] = -1.
127  goal = ob.State(space)
128  goal()[0] = 1.
129  goal()[1] = 1.
130  goal()[2] = 1.
131  ss.setStartAndGoalStates(start, goal, .05)
132  # set the planner
133  planner = RandomWalkPlanner(ss.getSpaceInformation())
134  ss.setPlanner(planner)
135 
136  result = ss.solve(10.0)
137  if result:
138  if result.getStatus() == ob.PlannerStatus.APPROXIMATE_SOLUTION:
139  print("Solution is approximate")
140  # try to shorten the path
141  ss.simplifySolution()
142  # print the simplified path
143  print(ss.getSolutionPath())
144 
145 
146 if __name__ == "__main__":
147  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:223
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...