Loading...
Searching...
No Matches
QuotientSpacePlanningRigidBody2D.py
1#!/usr/bin/env python
2
3
36
37# Author: Mark Moll
38
39from os.path import abspath, dirname, join
40import sys
41try:
42 from ompl import util as ou
43 from ompl import base as ob
44 from ompl import geometric as og
45except 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
52from math import sqrt, pi
53
54# Path Planning in SE2 = R2 \times SO2
55# using quotient-spaces R2 and SE2
56
57def 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
63def isStateValid_SE2(state):
64 return boxConstraint(state.getX(), state.getY()) and state.getYaw() < pi / 2.0
65
66def isStateValid_R2(state):
67 return boxConstraint(state[0], state[1])
68
69if __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))
Definition of a problem to be solved. This includes the start state(s) for the system and a goal spec...
The lower and upper bounds for an Rn space.
A state space representing Rn. The distance function is the L2 norm.
A state space representing SE(2)
Definition: SE2StateSpace.h:50
The base class for space information. This contains all the information about the space planning is d...
Definition of an abstract state.
Definition: State.h:50
A sequence of multiple quotient-spaces The class MultiQuotient can be used with any planner which inh...
Definition: MultiQuotient.h:59
std::function< bool(const State *)> StateValidityCheckerFn
If no state validity checking class is specified (StateValidityChecker), a std::function can be speci...