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:
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, then sample a valid state. Then, interpolate from the invalid state to the valid state, returning the first valid state encountered.
Create the set of classes typically needed to solve a geometric problem.
Definition: SimpleSetup.h:63
Abstract definition of a state sampler.
A state space representing Rn. The distance function is the L2 norm.
std::function< ValidStateSamplerPtr(const SpaceInformation *)> ValidStateSamplerAllocator
Definition of a function that can allocate a valid state sampler.
Definition of an abstract state.
Definition: State.h:49
The lower and upper bounds for an Rn space.
Probabilistic RoadMap planner.
Definition: PRM.h:80
std::function< bool(const State *)> StateValidityCheckerFn
If no state validity checking class is specified (StateValidityChecker), a std::function can be speci...