All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator
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 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)