Loading...
Searching...
No Matches
RigidBodyPlanning.py
1#!/usr/bin/env python
2
3
36
37# Author: Mark Moll
38
39try:
40 from ompl import base as ob
41 from ompl import geometric as og
42except 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
51def 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
57def 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
96def 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
140if __name__ == "__main__":
141 planWithSimpleSetup()
142 print("")
143 planTheHardWay()
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 SE(2)
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
RRT-Connect (RRTConnect)
Definition RRTConnect.h:62
Create the set of classes typically needed to solve a geometric problem.
Definition SimpleSetup.h:63
std::function< bool(const State *)> StateValidityCheckerFn
If no state validity checking class is specified (StateValidityChecker), a std::function can be speci...