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()