demos/RigidBodyPlanning.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 abspath, dirname, join
00046     import sys
00047     sys.path.insert(0, join(dirname(dirname(abspath(__file__))),'py-bindings'))
00048     from ompl import util as ou
00049     from ompl import base as ob
00050     from ompl import geometric as og
00051 
00052 def isStateValid(state):
00053     # Some arbitrary condition on the state (note that thanks to
00054     # dynamic type checking we can just call getX() and do not need
00055     # to convert state to an SE2State.)
00056     return state.getX() < .6
00057 
00058 def planWithSimpleSetup():
00059     # create an SE2 state space
00060     space = ob.SE2StateSpace()
00061 
00062     # set lower and upper bounds
00063     bounds = ob.RealVectorBounds(2)
00064     bounds.setLow(-1)
00065     bounds.setHigh(1)
00066     space.setBounds(bounds)
00067 
00068     # create a simple setup object
00069     ss = og.SimpleSetup(space)
00070     ss.setStateValidityChecker(ob.StateValidityCheckerFn(isStateValid))
00071 
00072     start = ob.State(space)
00073     # we can pick a random start state...
00074     start.random()
00075     # ... or set specific values
00076     start().setX(.5)
00077 
00078     goal = ob.State(space)
00079     # we can pick a random goal state...
00080     goal.random()
00081     # ... or set specific values
00082     goal().setX(-.5)
00083 
00084     ss.setStartAndGoalStates(start, goal)
00085 
00086     # this will automatically choose a default planner with
00087     # default parameters
00088     solved = ss.solve(1.0)
00089 
00090     if solved:
00091         # try to shorten the path
00092         ss.simplifySolution()
00093         # print the simplified path
00094         print(ss.getSolutionPath())
00095 
00096 
00097 def planTheHardWay():
00098     # create an SE2 state space
00099     space = ob.SE2StateSpace()
00100     # set lower and upper bounds
00101     bounds = ob.RealVectorBounds(2)
00102     bounds.setLow(-1)
00103     bounds.setHigh(1)
00104     space.setBounds(bounds)
00105     # construct an instance of space information from this state space
00106     si = ob.SpaceInformation(space)
00107     # set state validity checking for this space
00108     si.setStateValidityChecker(ob.StateValidityCheckerFn(isStateValid))
00109     # create a random start state
00110     start = ob.State(space)
00111     start.random()
00112     # create a random goal state
00113     goal = ob.State(space)
00114     goal.random()
00115     # create a problem instance
00116     pdef = ob.ProblemDefinition(si)
00117     # set the start and goal states
00118     pdef.setStartAndGoalStates(start, goal)
00119     # create a planner for the defined space
00120     planner = og.RRTConnect(si)
00121     # set the problem we are trying to solve for the planner
00122     planner.setProblemDefinition(pdef)
00123     # perform setup steps for the planner
00124     planner.setup()
00125     # print the settings for this space
00126     print(si.settings())
00127     # print the problem settings
00128     print(pdef)
00129     # attempt to solve the problem within one second of planning time
00130     solved = planner.solve(1.0)
00131 
00132     if solved:
00133         # get the goal representation from the problem definition (not the same as the goal state)
00134         # and inquire about the found path
00135         path = pdef.getSolutionPath()
00136         print("Found solution:\n%s" % path)
00137     else:
00138         print("No solution found")
00139 
00140 
00141 if __name__ == "__main__":
00142     planWithSimpleSetup()
00143     print("")
00144     planTheHardWay()
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines