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