RigidBodyPlanning.py
1 #!/usr/bin/env python
2 
3 
36 
37 # Author: Mark Moll
38 
39 try:
40  from ompl import base as ob
41  from ompl import geometric as og
42 except:
43  # if the ompl module is not in the PYTHONPATH assume it is installed in a
44  # subdirectory of the parent directory called "py-bindings."
45  from os.path import abspath, dirname, join
46  import sys
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
51 
52 def isStateValid(state):
53  # Some arbitrary condition on the state (note that thanks to
54  # dynamic type checking we can just call getX() and do not need
55  # to convert state to an SE2State.)
56  return state.getX() < .6
57 
58 def planWithSimpleSetup():
59  # create an SE2 state space
60  space = ob.SE2StateSpace()
61 
62  # set lower and upper bounds
63  bounds = ob.RealVectorBounds(2)
64  bounds.setLow(-1)
65  bounds.setHigh(1)
66  space.setBounds(bounds)
67 
68  # create a simple setup object
69  ss = og.SimpleSetup(space)
70  ss.setStateValidityChecker(ob.StateValidityCheckerFn(isStateValid))
71 
72  start = ob.State(space)
73  # we can pick a random start state...
74  start.random()
75  # ... or set specific values
76  start().setX(.5)
77 
78  goal = ob.State(space)
79  # we can pick a random goal state...
80  goal.random()
81  # ... or set specific values
82  goal().setX(-.5)
83 
84  ss.setStartAndGoalStates(start, goal)
85 
86  # this will automatically choose a default planner with
87  # default parameters
88  solved = ss.solve(1.0)
89 
90  if solved:
91  # try to shorten the path
92  ss.simplifySolution()
93  # print the simplified path
94  print(ss.getSolutionPath())
95 
96 
97 def planTheHardWay():
98  # create an SE2 state space
99  space = ob.SE2StateSpace()
100  # set lower and upper bounds
101  bounds = ob.RealVectorBounds(2)
102  bounds.setLow(-1)
103  bounds.setHigh(1)
104  space.setBounds(bounds)
105  # construct an instance of space information from this state space
106  si = ob.SpaceInformation(space)
107  # set state validity checking for this space
108  si.setStateValidityChecker(ob.StateValidityCheckerFn(isStateValid))
109  # create a random start state
110  start = ob.State(space)
111  start.random()
112  # create a random goal state
113  goal = ob.State(space)
114  goal.random()
115  # create a problem instance
116  pdef = ob.ProblemDefinition(si)
117  # set the start and goal states
118  pdef.setStartAndGoalStates(start, goal)
119  # create a planner for the defined space
120  planner = og.RRTConnect(si)
121  # set the problem we are trying to solve for the planner
122  planner.setProblemDefinition(pdef)
123  # perform setup steps for the planner
124  planner.setup()
125  # print the settings for this space
126  print(si.settings())
127  # print the problem settings
128  print(pdef)
129  # attempt to solve the problem within one second of planning time
130  solved = planner.solve(1.0)
131 
132  if solved:
133  # get the goal representation from the problem definition (not the same as the goal state)
134  # and inquire about the found path
135  path = pdef.getSolutionPath()
136  print("Found solution:\n%s" % path)
137  else:
138  print("No solution found")
139 
140 
141 if __name__ == "__main__":
142  planWithSimpleSetup()
143  print("")
144  planTheHardWay()
Create the set of classes typically needed to solve a geometric problem.
Definition: SimpleSetup.h:63
A state space representing SE(2)
Definition: SE2StateSpace.h:49
RRT-Connect (RRTConnect)
Definition: RRTConnect.h:61
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:49
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.
std::function< bool(const State *)> StateValidityCheckerFn
If no state validity checking class is specified (StateValidityChecker), a std::function can be speci...