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 base as ob 00041 from ompl import geometric as og 00042 except: 00043 # if the ompl module is not in the PYTHONPATH assume it is installed in a 00044 # subdirectory of the parent directory called "py-bindings." 00045 from os.path import basename, abspath, dirname, join 00046 import sys 00047 sys.path.insert(0, join(dirname(dirname(abspath(__file__))),'py-bindings')) 00048 from ompl import base as ob 00049 from ompl import geometric as og 00050 from time import sleep 00051 from math import fabs 00052 00053 ## @cond IGNORE 00054 00055 # This is a problem-specific sampler that automatically generates valid 00056 # states; it doesn't need to call SpaceInformation::isValid. This is an 00057 # example of constrained sampling. If you can explicitly describe the set valid 00058 # states and can draw samples from it, then this is typically much more 00059 # efficient than generating random samples from the entire state space and 00060 # checking for validity. 00061 class MyValidStateSampler(ob.ValidStateSampler): 00062 def __init__(si): 00063 super(MyValidStateSampler, self).__init__(si) 00064 self.name_ = "my sampler" 00065 self.rng_ = RNG() 00066 00067 # Generate a sample in the valid part of the R^3 state space. 00068 # Valid states satisfy the following constraints: 00069 # -1<= x,y,z <=1 00070 # if .25 <= z <= .5, then |x|>.8 and |y|>.8 00071 def sample(state): 00072 z = self.rng_.uniformReal(-1,1) 00073 00074 if z>.25 and z<.5: 00075 x = self.rng_.uniformReal(0,1.8) 00076 y = self.rng_.uniformReal(0,.2) 00077 i = self.rng_.uniformInt(0,3) 00078 if i==0: 00079 state[0]=x-1 00080 state[1]=y-1 00081 elif i==1: 00082 state[0]=x-.8 00083 state[1]=y+.8 00084 elif i==2: 00085 state[0]=y-1 00086 state[1]=x-1 00087 elif i==3: 00088 state[0]=y+.8 00089 state[1]=x-.8 00090 else: 00091 state[0] = self.rng_.uniformReal(-1,1) 00092 state[1] = self.rng_.uniformReal(-1,1) 00093 state[2] = z 00094 return True 00095 00096 ## @endcond 00097 00098 # This function is needed, even when we can write a sampler like the one 00099 # above, because we need to check path segments for validity 00100 def isStateValid(spaceInformation, state): 00101 # Let's pretend that the validity check is computationally relatively 00102 # expensive to emphasize the benefit of explicitly generating valid 00103 # samples 00104 sleep(.001) 00105 # Valid states satisfy the following constraints: 00106 # -1<= x,y,z <=1 00107 # if .25 <= z <= .5, then |x|>.8 and |y|>.8 00108 return not (fabs(state[0]<.8) and fabs(state[1]<.8) and 00109 state[2]>.25 and state[2]<.5) 00110 00111 # return an obstacle-based sampler 00112 def allocOBValidStateSampler(si): 00113 # we can perform any additional setup / configuration of a sampler here, 00114 # but there is nothing to tweak in case of the ObstacleBasedValidStateSampler. 00115 return ob.ValidStateSamplerPtr(ob.ObstacleBasedValidStateSampler(si)) 00116 00117 # return an instance of my sampler 00118 def allocMyValidStateSampler(si): 00119 return ob.ValidStateSamplerPtr(MyValidStateSampler(si)) 00120 00121 def plan(samplerIndex): 00122 # construct the state space we are planning in 00123 space = ob.RealVectorStateSpace(3) 00124 00125 # set the bounds 00126 bounds = ob.RealVectorBounds(3) 00127 bounds.setLow(-1) 00128 bounds.setHigh(1) 00129 space.setBounds(bounds) 00130 00131 # define a simple setup class 00132 ss = og.SimpleSetup(space) 00133 00134 # set state validity checking for this space 00135 ss.setStateValidityChecker(isStateValid) 00136 00137 # create a start state 00138 start = ob.State(space) 00139 start[0] = 0 00140 start[1] = 0 00141 start[2] = 0 00142 00143 # create a goal state 00144 goal = ob.State(space) 00145 goal[0] = 0 00146 goal[1] = 0 00147 goal[2] = 1 00148 00149 # set the start and goal states; 00150 ss.setStartAndGoalStates(start, goal) 00151 00152 # set sampler (optional; the default is uniform sampling) 00153 if samplerIndex==1: 00154 # use obstacle-based sampling 00155 ss.getSpaceInformation().setValidStateSamplerAllocator(allocOBValidStateSampler) 00156 elif samplerIndex==2: 00157 # use my sampler 00158 ss.getSpaceInformation().setValidStateSamplerAllocator(allocMyValidStateSampler) 00159 00160 # set the planner (optional) 00161 # create a planner for the defined space 00162 planner = og.RRTConnect(ss.getSpaceInformation()) 00163 ss.setPlanner(planner) 00164 00165 # attempt to solve the problem within ten seconds of planning time 00166 solved = ss.solve(10.0) 00167 if (solved): 00168 print "Found solution:" 00169 # print the path to screen 00170 ss.simplifySolution() 00171 print ss.getSolutionPath() 00172 else: 00173 print "No solution found" 00174 00175 00176 if __name__ == '__main__': 00177 print "Using default uniform sampler:" 00178 plan(0) 00179 print "\nUsing obstacle-based sampler:" 00180 plan(1) 00181 print "\nUsing my sampler:" 00182 plan(2)