demos/RigidBodyPlanningWithControls.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 from math import sin, cos
00040 from functools import partial
00041 try:
00042     from ompl import util as ou
00043     from ompl import base as ob
00044     from ompl import control as oc
00045     from ompl import geometric as og
00046 except:
00047     # if the ompl module is not in the PYTHONPATH assume it is installed in a
00048     # subdirectory of the parent directory called "py-bindings."
00049     from os.path import abspath, dirname, join
00050     import sys
00051     sys.path.insert(0, join(dirname(dirname(abspath(__file__))),'py-bindings'))
00052     from ompl import util as ou
00053     from ompl import base as ob
00054     from ompl import control as oc
00055     from ompl import geometric as og
00056 
00057 ## @cond IGNORE
00058 # a decomposition is only needed for SyclopRRT and SyclopEST
00059 class MyDecomposition(oc.GridDecomposition):
00060     def __init__(self, length, bounds):
00061         super(MyDecomposition, self).__init__(length, 2, bounds)
00062     def project(self, s, coord):
00063         coord[0] = s.getX()
00064         coord[1] = s.getY()
00065     def sampleFullState(self, sampler, coord, s):
00066         sampler.sampleUniform(s)
00067         s.setXY(coord[0], coord[1])
00068 ## @endcond
00069 
00070 def isStateValid(spaceInformation, state):
00071     # perform collision checking or check if other constraints are
00072     # satisfied
00073     return spaceInformation.satisfiesBounds(state)
00074 
00075 def propagate(start, control, duration, state):
00076     state.setX( start.getX() + control[0] * duration * cos(start.getYaw()) )
00077     state.setY( start.getY() + control[0] * duration * sin(start.getYaw()) )
00078     state.setYaw(start.getYaw() + control[1] * duration)
00079 
00080 def plan():
00081     # construct the state space we are planning in
00082     space = ob.SE2StateSpace()
00083 
00084     # set the bounds for the R^2 part of SE(2)
00085     bounds = ob.RealVectorBounds(2)
00086     bounds.setLow(-1)
00087     bounds.setHigh(1)
00088     space.setBounds(bounds)
00089 
00090     # create a control space
00091     cspace = oc.RealVectorControlSpace(space, 2)
00092 
00093     # set the bounds for the control space
00094     cbounds = ob.RealVectorBounds(2)
00095     cbounds.setLow(-.3)
00096     cbounds.setHigh(.3)
00097     cspace.setBounds(cbounds)
00098 
00099     # define a simple setup class
00100     ss = oc.SimpleSetup(cspace)
00101     ss.setStateValidityChecker(ob.StateValidityCheckerFn(partial(isStateValid, ss.getSpaceInformation())))
00102     ss.setStatePropagator(oc.StatePropagatorFn(propagate))
00103 
00104     # create a start state
00105     start = ob.State(space)
00106     start().setX(-0.5);
00107     start().setY(0.0);
00108     start().setYaw(0.0);
00109 
00110     # create a goal state
00111     goal = ob.State(space);
00112     goal().setX(0.0);
00113     goal().setY(0.5);
00114     goal().setYaw(0.0);
00115 
00116     # set the start and goal states
00117     ss.setStartAndGoalStates(start, goal, 0.05)
00118 
00119     # (optionally) set planner
00120     si = ss.getSpaceInformation()
00121     #planner = oc.RRT(si)
00122     #planner = oc.EST(si)
00123     #planner = oc.KPIECE1(si) # this is the default
00124     # SyclopEST and SyclopRRT require a decomposition to guide the search
00125     decomp = MyDecomposition(32, bounds)
00126     planner = oc.SyclopEST(si, decomp)
00127     #planner = oc.SyclopRRT(si, decomp)
00128     ss.setPlanner(planner)
00129     # (optionally) set propagation step size
00130     si.setPropagationStepSize(.1)
00131 
00132     # attempt to solve the problem
00133     solved = ss.solve(20.0)
00134 
00135     if solved:
00136         # print the path to screen
00137         print("Found solution:\n%s" % ss.getSolutionPath().printAsMatrix())
00138 
00139 if __name__ == "__main__":
00140     plan()
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines