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