demos/Point2DPlanning.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: Ioan Sucan, 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 functools import partial
00053 
00054 class Plane2DEnvironment:
00055     def __init__(self, ppm_file):
00056         self.ppm_ = ou.PPM()
00057         self.ppm_.loadFile(ppm_file)
00058         space = ob.RealVectorStateSpace()
00059         space.addDimension(0.0, self.ppm_.getWidth())
00060         space.addDimension(0.0, self.ppm_.getHeight())
00061         self.maxWidth_ = self.ppm_.getWidth() - 1
00062         self.maxHeight_ = self.ppm_.getHeight() - 1
00063         self.ss_ = og.SimpleSetup(space)
00064 
00065         # set state validity checking for this space
00066         self.ss_.setStateValidityChecker(ob.StateValidityCheckerFn(
00067             partial(Plane2DEnvironment.isStateValid, self)))
00068         space.setup()
00069         self.ss_.getSpaceInformation().setStateValidityCheckingResolution(1.0 / space.getMaximumExtent())
00070         #      self.ss_.setPlanner(og.RRTConnect(self.ss_.getSpaceInformation()))
00071 
00072     def plan(self, start_row, start_col, goal_row, goal_col):
00073         if not self.ss_:
00074             return false
00075         start = ob.State(self.ss_.getStateSpace())
00076         start()[0] = start_row
00077         start()[1] = start_col
00078         goal = ob.State(self.ss_.getStateSpace())
00079         goal()[0] = goal_row
00080         goal()[1] = goal_col
00081         self.ss_.setStartAndGoalStates(start, goal)
00082         # generate a few solutions; all will be added to the goal
00083         for i in range(10):
00084             if self.ss_.getPlanner():
00085                 self.ss_.getPlanner().clear()
00086             self.ss_.solve()
00087         ns = self.ss_.getProblemDefinition().getSolutionCount()
00088         print("Found %d solutions" % ns)
00089         if self.ss_.haveSolutionPath():
00090             self.ss_.simplifySolution()
00091             p = self.ss_.getSolutionPath()
00092             ps = og.PathSimplifier(self.ss_.getSpaceInformation())
00093             ps.simplifyMax(p)
00094             ps.smoothBSpline(p)
00095             return True
00096         else:
00097             return False
00098 
00099     def recordSolution(self):
00100         if not self.ss_ or not self.ss_.haveSolutionPath():
00101             return
00102         p = self.ss_.getSolutionPath()
00103         p.interpolate()
00104         for i in range(p.getStateCount()):
00105             w = min(self.maxWidth_, int(p.getState(i)[0]))
00106             h = min(self.maxHeight_, int(p.getState(i)[1]))
00107             c = self.ppm_.getPixel(h, w)
00108             c.red = 255
00109             c.green = 0
00110             c.blue = 0
00111 
00112     def save(self, filename):
00113         if not self.ss_:
00114             return
00115         self.ppm_.saveFile(filename)
00116 
00117     def isStateValid(self, state):
00118         w = min(int(state[0]), self.maxWidth_)
00119         h = min(int(state[1]), self.maxHeight_)
00120 
00121         c = self.ppm_.getPixel(h, w)
00122         return c.red > 127 and c.green > 127 and c.blue > 127
00123 
00124 
00125 if __name__ == "__main__":
00126     fname = join(join(join(join(dirname(dirname(abspath(__file__))),
00127         'tests'), 'resources'), 'ppm'), 'floor.ppm')
00128     env = Plane2DEnvironment(fname)
00129 
00130     if env.plan(0, 0, 777, 1265):
00131         env.recordSolution()
00132         env.save("result_demo.ppm")
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines