demos/StateSampling.py
00001 #!/usr/bin/env python 00002 00003 ###################################################################### 00004 # Software License Agreement (BSD License) 00005 # 00006 # Copyright (c) 2010, Rice University 00007 # All rights reserved. 00008 # 00009 # Redistribution and use in source and binary forms, with or without 00010 # modification, are permitted provided that the following conditions 00011 # are met: 00012 # 00013 # * Redistributions of source code must retain the above copyright 00014 # notice, this list of conditions and the following disclaimer. 00015 # * Redistributions in binary form must reproduce the above 00016 # copyright notice, this list of conditions and the following 00017 # disclaimer in the documentation and/or other materials provided 00018 # with the distribution. 00019 # * Neither the name of the Rice University nor the names of its 00020 # contributors may be used to endorse or promote products derived 00021 # from this software without specific prior written permission. 00022 # 00023 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00024 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00025 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00026 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00027 # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00028 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00029 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00030 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00031 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00032 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00033 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00034 # POSSIBILITY OF SUCH DAMAGE. 00035 ###################################################################### 00036 00037 # Author: Mark Moll 00038 00039 try: 00040 from ompl import util as ou 00041 from ompl import base as ob 00042 from ompl import geometric as og 00043 except: 00044 # if the ompl module is not in the PYTHONPATH assume it is installed in a 00045 # subdirectory of the parent directory called "py-bindings." 00046 from os.path import abspath, dirname, join 00047 import sys 00048 sys.path.insert(0, join(dirname(dirname(abspath(__file__))),'py-bindings')) 00049 from ompl import util as ou 00050 from ompl import base as ob 00051 from ompl import geometric as og 00052 from time import sleep 00053 from math import fabs 00054 00055 ## @cond IGNORE 00056 00057 # This is a problem-specific sampler that automatically generates valid 00058 # states; it doesn't need to call SpaceInformation::isValid. This is an 00059 # example of constrained sampling. If you can explicitly describe the set valid 00060 # states and can draw samples from it, then this is typically much more 00061 # efficient than generating random samples from the entire state space and 00062 # checking for validity. 00063 class MyValidStateSampler(ob.ValidStateSampler): 00064 def __init__(self, si): 00065 super(MyValidStateSampler, self).__init__(si) 00066 self.name_ = "my sampler" 00067 self.rng_ = ou.RNG() 00068 00069 # Generate a sample in the valid part of the R^3 state space. 00070 # Valid states satisfy the following constraints: 00071 # -1<= x,y,z <=1 00072 # if .25 <= z <= .5, then |x|>.8 and |y|>.8 00073 def sample(self, state): 00074 z = self.rng_.uniformReal(-1,1) 00075 00076 if z>.25 and z<.5: 00077 x = self.rng_.uniformReal(0,1.8) 00078 y = self.rng_.uniformReal(0,.2) 00079 i = self.rng_.uniformInt(0,3) 00080 if i==0: 00081 state[0]=x-1 00082 state[1]=y-1 00083 elif i==1: 00084 state[0]=x-.8 00085 state[1]=y+.8 00086 elif i==2: 00087 state[0]=y-1 00088 state[1]=x-1 00089 elif i==3: 00090 state[0]=y+.8 00091 state[1]=x-.8 00092 else: 00093 state[0] = self.rng_.uniformReal(-1,1) 00094 state[1] = self.rng_.uniformReal(-1,1) 00095 state[2] = z 00096 return True 00097 00098 ## @endcond 00099 00100 # This function is needed, even when we can write a sampler like the one 00101 # above, because we need to check path segments for validity 00102 def isStateValid(state): 00103 # Let's pretend that the validity check is computationally relatively 00104 # expensive to emphasize the benefit of explicitly generating valid 00105 # samples 00106 sleep(.001) 00107 # Valid states satisfy the following constraints: 00108 # -1<= x,y,z <=1 00109 # if .25 <= z <= .5, then |x|>.8 and |y|>.8 00110 return not (fabs(state[0]<.8) and fabs(state[1]<.8) and 00111 state[2]>.25 and state[2]<.5) 00112 00113 # return an obstacle-based sampler 00114 def allocOBValidStateSampler(si): 00115 # we can perform any additional setup / configuration of a sampler here, 00116 # but there is nothing to tweak in case of the ObstacleBasedValidStateSampler. 00117 return ob.ObstacleBasedValidStateSampler(si) 00118 00119 # return an instance of my sampler 00120 def allocMyValidStateSampler(si): 00121 return MyValidStateSampler(si) 00122 00123 def plan(samplerIndex): 00124 # construct the state space we are planning in 00125 space = ob.RealVectorStateSpace(3) 00126 00127 # set the bounds 00128 bounds = ob.RealVectorBounds(3) 00129 bounds.setLow(-1) 00130 bounds.setHigh(1) 00131 space.setBounds(bounds) 00132 00133 # define a simple setup class 00134 ss = og.SimpleSetup(space) 00135 00136 # set state validity checking for this space 00137 ss.setStateValidityChecker(ob.StateValidityCheckerFn(isStateValid)) 00138 00139 # create a start state 00140 start = ob.State(space) 00141 start[0] = 0 00142 start[1] = 0 00143 start[2] = 0 00144 00145 # create a goal state 00146 goal = ob.State(space) 00147 goal[0] = 0 00148 goal[1] = 0 00149 goal[2] = 1 00150 00151 # set the start and goal states; 00152 ss.setStartAndGoalStates(start, goal) 00153 00154 # set sampler (optional; the default is uniform sampling) 00155 si = ss.getSpaceInformation() 00156 if samplerIndex==1: 00157 # use obstacle-based sampling 00158 si.setValidStateSamplerAllocator(ob.ValidStateSamplerAllocator(allocOBValidStateSampler)) 00159 elif samplerIndex==2: 00160 # use my sampler 00161 si.setValidStateSamplerAllocator(ob.ValidStateSamplerAllocator(allocMyValidStateSampler)) 00162 00163 # create a planner for the defined space 00164 planner = og.PRM(si) 00165 ss.setPlanner(planner) 00166 00167 # attempt to solve the problem within ten seconds of planning time 00168 solved = ss.solve(10.0) 00169 if (solved): 00170 print("Found solution:") 00171 # print the path to screen 00172 print(ss.getSolutionPath()) 00173 else: 00174 print("No solution found") 00175 00176 00177 if __name__ == '__main__': 00178 print("Using default uniform sampler:") 00179 plan(0) 00180 print("\nUsing obstacle-based sampler:") 00181 plan(1) 00182 print("\nUsing my sampler:") 00183 plan(2)