39 from os.path
import abspath, dirname, join
42 from ompl
import util
as ou
43 from ompl
import base
as ob
44 from ompl
import geometric
as og
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
57 def boxConstraint(x, y):
60 pos_cnstr = sqrt(x * x + y * y)
61 return pos_cnstr > 0.2
63 def isStateValid_SE2(state):
64 return boxConstraint(state.getX(), state.getY())
and state.getYaw() < pi / 2.0
66 def isStateValid_R2(state):
67 return boxConstraint(state[0], state[1])
69 if __name__ ==
"__main__":
86 si_vec = og.vectorSpaceInformation()
93 start_SE2().setXY(0, 0)
95 goal_SE2().setXY(1, 1)
99 pdef.setStartAndGoalStates(start_SE2, goal_SE2)
105 planner.setProblemDefinition(pdef)
108 solved = planner.solve(1.0)
112 print(
'Configuration-Space Path (SE2):')
114 print(pdef.getSolutionPath())
117 print(
'Quotient-Space Path (R2):')
119 print(planner.getProblemDefinition(0).getSolutionPath())
121 nodes = planner.getFeasibleNodes()
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 of an abstract state.
A sequence of multiple quotient-spaces The class MultiQuotient can be used with any planner which inh...
std::function< bool(const State *)> StateValidityCheckerFn
If no state validity checking class is specified (StateValidityChecker), a std::function can be speci...