All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends
StateSampling.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 basename, 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 ## @cond IGNORE
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 ## @endcond
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)