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 ImportError:
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 base as ob
49  from ompl import geometric as og
50 
51 def isStateValid(state):
52  # Some arbitrary condition on the state (note that thanks to
53  # dynamic type checking we can just call getX() and do not need
54  # to convert state to an SE2State.)
55  return state.getX() < .6
56 
57 def planWithSimpleSetup():
58  # create an SE2 state space
59  space = ob.SE2StateSpace()
60 
61  # set lower and upper bounds
62  bounds = ob.RealVectorBounds(2)
63  bounds.setLow(-1)
64  bounds.setHigh(1)
65  space.setBounds(bounds)
66 
67  # create a simple setup object
68  ss = og.SimpleSetup(space)
69  ss.setStateValidityChecker(ob.StateValidityCheckerFn(isStateValid))
70 
71  start = ob.State(space)
72  # we can pick a random start state...
73  start.random()
74  # ... or set specific values
75  start().setX(.5)
76 
77  goal = ob.State(space)
78  # we can pick a random goal state...
79  goal.random()
80  # ... or set specific values
81  goal().setX(-.5)
82 
83  ss.setStartAndGoalStates(start, goal)
84 
85  # this will automatically choose a default planner with
86  # default parameters
87  solved = ss.solve(1.0)
88 
89  if solved:
90  # try to shorten the path
91  ss.simplifySolution()
92  # print the simplified path
93  print(ss.getSolutionPath())
94 
95 
96 def planTheHardWay():
97  # create an SE2 state space
98  space = ob.SE2StateSpace()
99  # set lower and upper bounds
100  bounds = ob.RealVectorBounds(2)
101  bounds.setLow(-1)
102  bounds.setHigh(1)
103  space.setBounds(bounds)
104  # construct an instance of space information from this state space
105  si = ob.SpaceInformation(space)
106  # set state validity checking for this space
107  si.setStateValidityChecker(ob.StateValidityCheckerFn(isStateValid))
108  # create a random start state
109  start = ob.State(space)
110  start.random()
111  # create a random goal state
112  goal = ob.State(space)
113  goal.random()
114  # create a problem instance
115  pdef = ob.ProblemDefinition(si)
116  # set the start and goal states
117  pdef.setStartAndGoalStates(start, goal)
118  # create a planner for the defined space
119  planner = og.RRTConnect(si)
120  # set the problem we are trying to solve for the planner
121  planner.setProblemDefinition(pdef)
122  # perform setup steps for the planner
123  planner.setup()
124  # print the settings for this space
125  print(si.settings())
126  # print the problem settings
127  print(pdef)
128  # attempt to solve the problem within one second of planning time
129  solved = planner.solve(1.0)
130 
131  if solved:
132  # get the goal representation from the problem definition (not the same as the goal state)
133  # and inquire about the found path
134  path = pdef.getSolutionPath()
135  print("Found solution:\n%s" % path)
136  else:
137  print("No solution found")
138 
139 
140 if __name__ == "__main__":
141  planWithSimpleSetup()
142  print("")
143  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...