demos/RandomWalkPlanner.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 random import choice
00053 
00054 ## @cond IGNORE
00055 
00056 class RandomWalkPlanner(ob.Planner):
00057     def __init__(self, si):
00058         super(RandomWalkPlanner, self).__init__(si, "RandomWalkPlanner")
00059         self.states_ = []
00060         self.sampler_ = si.allocStateSampler()
00061 
00062     def solve(self, ptc):
00063         pdef = self.getProblemDefinition()
00064         goal = pdef.getGoal()
00065         si = self.getSpaceInformation()
00066         pi = self.getPlannerInputStates()
00067         st = pi.nextStart()
00068         while st:
00069             self.states_.append(st)
00070             st = pi.nextStart()
00071         solution = None
00072         approxsol = 0
00073         approxdif = 1e6
00074         while not ptc():
00075             rstate = si.allocState()
00076             # pick a random state in the state space
00077             self.sampler_.sampleUniform(rstate)
00078             # check motion
00079             if si.checkMotion(self.states_[-1], rstate):
00080                 self.states_.append(rstate)
00081                 sat = goal.isSatisfied(rstate)
00082                 dist = goal.distanceGoal(rstate)
00083                 if sat:
00084                     approxdif = dist
00085                     solution = len(self.states_)
00086                     break
00087                 if dist < approxdif:
00088                     approxdif = dist
00089                     approxsol = len(self.states_)
00090         solved = False
00091         approximate = False
00092         if not solution:
00093             solution = approxsol
00094             approximate = True
00095         if solution:
00096             path = og.PathGeometric(si)
00097             for s in self.states_[:solution]:
00098                 path.append(s)
00099             pdef.addSolutionPath(path)
00100             solved = True
00101         return ob.PlannerStatus(solved, approximate)
00102 
00103     def clear(self):
00104         super(RandomWalkPlanner, self).clear()
00105         self.states_ = []
00106 
00107 ## @endcond
00108 
00109 def isStateValid(state):
00110     x = state[0]
00111     y = state[1]
00112     z = state[2]
00113     return x*x + y*y + z*z > .8
00114 
00115 def plan():
00116     # create an R^3 state space
00117     space = ob.RealVectorStateSpace(3)
00118     # set lower and upper bounds
00119     bounds = ob.RealVectorBounds(3)
00120     bounds.setLow(-1)
00121     bounds.setHigh(1)
00122     space.setBounds(bounds)
00123     # create a simple setup object
00124     ss = og.SimpleSetup(space)
00125     ss.setStateValidityChecker(ob.StateValidityCheckerFn(isStateValid))
00126     start = ob.State(space)
00127     start()[0] = -1.
00128     start()[1] = -1.
00129     start()[2] = -1.
00130     goal = ob.State(space)
00131     goal()[0] = 1.
00132     goal()[1] = 1.
00133     goal()[2] = 1.
00134     ss.setStartAndGoalStates(start, goal, .05)
00135     # set the planner
00136     planner = RandomWalkPlanner(ss.getSpaceInformation())
00137     ss.setPlanner(planner)
00138 
00139     result = ss.solve(10.0)
00140     if result:
00141         if result.getStatus() == ob.PlannerStatus.APPROXIMATE_SOLUTION:
00142             print("Solution is approximate")
00143         # try to shorten the path
00144         ss.simplifySolution()
00145         # print the simplified path
00146         print(ss.getSolutionPath())
00147 
00148 
00149 if __name__ == "__main__":
00150     plan()
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines