QuotientSpacePlanningRigidBody2D.py
1 #!/usr/bin/env python
2 
3 
36 
37 # Author: Mark Moll
38 
39 from os.path import abspath, dirname, join
40 import sys
41 try:
42  from ompl import util as ou
43  from ompl import base as ob
44  from ompl import geometric as og
45 except ImportError:
46  # if the ompl module is not in the PYTHONPATH assume it is installed in a
47  # subdirectory of the parent directory called "py-bindings."
48  sys.path.insert(0, join(dirname(dirname(dirname(abspath(__file__)))), 'py-bindings'))
49  from ompl import util as ou
50  from ompl import base as ob
51  from ompl import geometric as og
52 from math import sqrt, pi
53 
54 # Path Planning in SE2 = R2 \times SO2
55 # using quotient-spaces R2 and SE2
56 
57 def boxConstraint(x, y):
58  x = x - 0.5
59  y = y - 0.5
60  pos_cnstr = sqrt(x * x + y * y)
61  return pos_cnstr > 0.2
62 
63 def isStateValid_SE2(state):
64  return boxConstraint(state.getX(), state.getY()) and state.getYaw() < pi / 2.0
65 
66 def isStateValid_R2(state):
67  return boxConstraint(state[0], state[1])
68 
69 if __name__ == "__main__":
70  # Setup SE2
71  SE2 = ob.SE2StateSpace()
72  bounds = ob.RealVectorBounds(2)
73  bounds.setLow(0)
74  bounds.setHigh(1)
75  SE2.setBounds(bounds)
76  si_SE2 = ob.SpaceInformation(SE2)
77  si_SE2.setStateValidityChecker(ob.StateValidityCheckerFn(isStateValid_SE2))
78 
79  # Setup Quotient-Space R2
81  R2.setBounds(0, 1)
82  si_R2 = ob.SpaceInformation(R2)
83  si_R2.setStateValidityChecker(ob.StateValidityCheckerFn(isStateValid_R2))
84 
85  # Create vector of spaceinformationptr
86  si_vec = og.vectorSpaceInformation()
87  si_vec.append(si_R2)
88  si_vec.append(si_SE2)
89 
90  # Define Planning Problem
91  start_SE2 = ob.State(SE2)
92  goal_SE2 = ob.State(SE2)
93  start_SE2().setXY(0, 0)
94  start_SE2().setYaw(0)
95  goal_SE2().setXY(1, 1)
96  goal_SE2().setYaw(0)
97 
98  pdef = ob.ProblemDefinition(si_SE2)
99  pdef.setStartAndGoalStates(start_SE2, goal_SE2)
100 
101  # Setup Planner using vector of spaceinformationptr
102  planner = og.QRRT(si_vec)
103 
104  # Planner can be used as any other OMPL algorithm
105  planner.setProblemDefinition(pdef)
106  planner.setup()
107 
108  solved = planner.solve(1.0)
109 
110  if solved:
111  print('-' * 80)
112  print('Configuration-Space Path (SE2):')
113  print('-' * 80)
114  print(pdef.getSolutionPath())
115 
116  print('-' * 80)
117  print('Quotient-Space Path (R2):')
118  print('-' * 80)
119  print(planner.getProblemDefinition(0).getSolutionPath())
120 
121  nodes = planner.getFeasibleNodes()
122  print('-' * 80)
123  for (i, node) in enumerate(nodes):
124  print('QuotientSpace%d has %d nodes.' % (i, node))
The base class for space information. This contains all the information about the space planning is d...
Definition of a problem to be solved. This includes the start state(s) for the system and a goal spec...
Definition of an abstract state.
Definition: State.h:114
A state space representing Rn. The distance function is the L2 norm.
A state space representing SE(2)
std::function< bool(const State *)> StateValidityCheckerFn
If no state validity checking class is specified (StateValidityChecker), a std::function can be speci...
A sequence of multiple quotient-spaces The class MultiQuotient can be used with any planner which inh...
The lower and upper bounds for an Rn space.