QuotientSpacePlanningRigidBody2D.py

#!/usr/bin/env python
 
 
 
# Author: Mark Moll
 
from os.path import abspath, dirname, join
import sys
try:
    from ompl import util as ou
    from ompl import base as ob
    from ompl import geometric as og
except ImportError:
    # if the ompl module is not in the PYTHONPATH assume it is installed in a
    # subdirectory of the parent directory called "py-bindings."
    sys.path.insert(0, join(dirname(dirname(dirname(abspath(__file__)))), 'py-bindings'))
    from ompl import util as ou
    from ompl import base as ob
    from ompl import geometric as og
from math import sqrt, pi
 
# Path Planning in SE2 = R2 \times SO2
# using quotient-spaces R2 and SE2
 
def boxConstraint(x, y):
    x = x - 0.5
    y = y - 0.5
    pos_cnstr = sqrt(x * x + y * y)
    return pos_cnstr > 0.2
 
def isStateValid_SE2(state):
    return boxConstraint(state.getX(), state.getY()) and state.getYaw() < pi / 2.0
 
def isStateValid_R2(state):
    return boxConstraint(state[0], state[1])
 
if __name__ == "__main__":
    # Setup SE2
    SE2 = ob.SE2StateSpace()
    bounds = ob.RealVectorBounds(2)
    bounds.setLow(0)
    bounds.setHigh(1)
    SE2.setBounds(bounds)
    si_SE2 = ob.SpaceInformation(SE2)
    si_SE2.setStateValidityChecker(ob.StateValidityCheckerFn(isStateValid_SE2))
 
    # Setup Quotient-Space R2
    R2 = ob.RealVectorStateSpace(2)
    R2.setBounds(0, 1)
    si_R2 = ob.SpaceInformation(R2)
    si_R2.setStateValidityChecker(ob.StateValidityCheckerFn(isStateValid_R2))
 
    # Create vector of spaceinformationptr
    si_vec = og.vectorSpaceInformation()
    si_vec.append(si_R2)
    si_vec.append(si_SE2)
 
    # Define Planning Problem
    start_SE2 = ob.State(SE2)
    goal_SE2 = ob.State(SE2)
    start_SE2().setXY(0, 0)
    start_SE2().setYaw(0)
    goal_SE2().setXY(1, 1)
    goal_SE2().setYaw(0)
 
    pdef = ob.ProblemDefinition(si_SE2)
    pdef.setStartAndGoalStates(start_SE2, goal_SE2)
 
    # Setup Planner using vector of spaceinformationptr
    planner = og.QRRT(si_vec)
 
    # Planner can be used as any other OMPL algorithm
    planner.setProblemDefinition(pdef)
    planner.setup()
 
    solved = planner.solve(1.0)
 
    if solved:
        print('-' * 80)
        print('Configuration-Space Path (SE2):')
        print('-' * 80)
        print(pdef.getSolutionPath())
 
        print('-' * 80)
        print('Quotient-Space Path (R2):')
        print('-' * 80)
        print(planner.getProblemDefinition(0).getSolutionPath())
 
        nodes = planner.getFeasibleNodes()
        print('-' * 80)
        for (i, node) in enumerate(nodes):
            print('QuotientSpace%d has %d nodes.' % (i, node))