40 from ompl
import base
as ob
41 from ompl
import geometric
as og
45 from os.path
import abspath, dirname, join
47 sys.path.insert(0, join(dirname(dirname(abspath(__file__))),
'py-bindings'))
48 from ompl
import util
as ou
49 from ompl
import base
as ob
50 from ompl
import geometric
as og
52 def isStateValid(state):
56 return state.getX() < .6
58 def planWithSimpleSetup():
66 space.setBounds(bounds)
84 ss.setStartAndGoalStates(start, goal)
88 solved = ss.solve(1.0)
94 print(ss.getSolutionPath())
104 space.setBounds(bounds)
118 pdef.setStartAndGoalStates(start, goal)
122 planner.setProblemDefinition(pdef)
130 solved = planner.solve(1.0)
135 path = pdef.getSolutionPath()
136 print(
"Found solution:\n%s" % path)
138 print(
"No solution found")
141 if __name__ ==
"__main__":
142 planWithSimpleSetup()
boost::function< bool(const State *)> StateValidityCheckerFn
If no state validity checking class is specified (StateValidityChecker), a boost function can be spec...
Create the set of classes typically needed to solve a geometric problem.
A state space representing SE(2)
Definition of an abstract state.
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.