StateSampling.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 ImportError:
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 time import sleep
53 from math import fabs
54
55
56
57 # This is a problem-specific sampler that automatically generates valid
58 # states; it doesn't need to call SpaceInformation::isValid. This is an
59 # example of constrained sampling. If you can explicitly describe the set valid
60 # states and can draw samples from it, then this is typically much more
61 # efficient than generating random samples from the entire state space and
62 # checking for validity.
63 class MyValidStateSampler(ob.ValidStateSampler):
64  def __init__(self, si):
65  super(MyValidStateSampler, self).__init__(si)
66  self.name_ = "my sampler"
67  self.rng_ = ou.RNG()
68
69  # Generate a sample in the valid part of the R^3 state space.
70  # Valid states satisfy the following constraints:
71  # -1<= x,y,z <=1
72  # if .25 <= z <= .5, then |x|>.8 and |y|>.8
73  def sample(self, state):
74  z = self.rng_.uniformReal(-1, 1)
75
76  if z > .25 and z < .5:
77  x = self.rng_.uniformReal(0, 1.8)
78  y = self.rng_.uniformReal(0, .2)
79  i = self.rng_.uniformInt(0, 3)
80  if i == 0:
81  state[0] = x-1
82  state[1] = y-1
83  elif i == 1:
84  state[0] = x-.8
85  state[1] = y+.8
86  elif i == 2:
87  state[0] = y-1
88  state[1] = x-1
89  elif i == 3:
90  state[0] = y+.8
91  state[1] = x-.8
92  else:
93  state[0] = self.rng_.uniformReal(-1, 1)
94  state[1] = self.rng_.uniformReal(-1, 1)
95  state[2] = z
96  return True
97
98
99
100 # This function is needed, even when we can write a sampler like the one
101 # above, because we need to check path segments for validity
102 def isStateValid(state):
103  # Let's pretend that the validity check is computationally relatively
104  # expensive to emphasize the benefit of explicitly generating valid
105  # samples
106  sleep(.001)
107  # Valid states satisfy the following constraints:
108  # -1<= x,y,z <=1
109  # if .25 <= z <= .5, then |x|>.8 and |y|>.8
110  return not (fabs(state[0] < .8) and fabs(state[1] < .8) and \
111  state[2] > .25 and state[2] < .5)
112
113 # return an obstacle-based sampler
114 def allocOBValidStateSampler(si):
115  # we can perform any additional setup / configuration of a sampler here,
116  # but there is nothing to tweak in case of the ObstacleBasedValidStateSampler.
118
119 # return an instance of my sampler
120 def allocMyValidStateSampler(si):
121  return MyValidStateSampler(si)
122
123 def plan(samplerIndex):
124  # construct the state space we are planning in
125  space = ob.RealVectorStateSpace(3)
126
127  # set the bounds
128  bounds = ob.RealVectorBounds(3)
129  bounds.setLow(-1)
130  bounds.setHigh(1)
131  space.setBounds(bounds)
132
133  # define a simple setup class
134  ss = og.SimpleSetup(space)
135
136  # set state validity checking for this space
137  ss.setStateValidityChecker(ob.StateValidityCheckerFn(isStateValid))
138
139  # create a start state
140  start = ob.State(space)
141  start[0] = 0
142  start[1] = 0
143  start[2] = 0
144
145  # create a goal state
146  goal = ob.State(space)
147  goal[0] = 0
148  goal[1] = 0
149  goal[2] = 1
150
151  # set the start and goal states;
152  ss.setStartAndGoalStates(start, goal)
153
154  # set sampler (optional; the default is uniform sampling)
155  si = ss.getSpaceInformation()
156  if samplerIndex == 1:
157  # use obstacle-based sampling
158  si.setValidStateSamplerAllocator(ob.ValidStateSamplerAllocator(allocOBValidStateSampler))
159  elif samplerIndex == 2:
160  # use my sampler
161  si.setValidStateSamplerAllocator(ob.ValidStateSamplerAllocator(allocMyValidStateSampler))
162
163  # create a planner for the defined space
164  planner = og.PRM(si)
165  ss.setPlanner(planner)
166
167  # attempt to solve the problem within ten seconds of planning time
168  solved = ss.solve(10.0)
169  if solved:
170  print("Found solution:")
171  # print the path to screen
172  print(ss.getSolutionPath())
173  else:
174  print("No solution found")
175
176
177 if __name__ == '__main__':
178  print("Using default uniform sampler:")
179  plan(0)
180  print("\nUsing obstacle-based sampler:")
181  plan(1)
182  print("\nUsing my sampler:")
183  plan(2)
Generate valid samples using obstacle based sampling. First sample an invalid state,...
The lower and upper bounds for an Rn space.
A state space representing Rn. The distance function is the L2 norm.
Definition of an abstract state.
Definition: State.h:50
Abstract definition of a state sampler.