Loading...
Searching...
No Matches
RandomWalkPlanner.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
51
52
54 def __init__(self, si):
55 super(RandomWalkPlanner, self).__init__(si, "RandomWalkPlanner")
56 self.states_ = []
57 self.sampler_ = si.allocStateSampler()
58
59 def solve(self, ptc):
60 pdef = self.getProblemDefinition()
61 goal = pdef.getGoal()
62 si = self.getSpaceInformation()
63 pi = self.getPlannerInputStates()
64 st = pi.nextStart()
65 while st:
66 self.states_.append(st)
67 st = pi.nextStart()
68 solution = None
69 approxsol = 0
70 approxdif = 1e6
71 while not ptc():
72 rstate = si.allocState()
73 # pick a random state in the state space
74 self.sampler_.sampleUniform(rstate)
75 # check motion
76 if si.checkMotion(self.states_[-1], rstate):
77 self.states_.append(rstate)
78 sat = goal.isSatisfied(rstate)
79 dist = goal.distanceGoal(rstate)
80 if sat:
81 approxdif = dist
82 solution = len(self.states_)
83 break
84 if dist < approxdif:
85 approxdif = dist
86 approxsol = len(self.states_)
87 solved = False
88 approximate = False
89 if not solution:
90 solution = approxsol
91 approximate = True
92 if solution:
93 path = og.PathGeometric(si)
94 for s in self.states_[:solution]:
95 path.append(s)
96 pdef.addSolutionPath(path)
97 solved = True
98 return ob.PlannerStatus(solved, approximate)
99
100 def clear(self):
101 super(RandomWalkPlanner, self).clear()
102 self.states_ = []
103
104
105
106def isStateValid(state):
107 x = state[0]
108 y = state[1]
109 z = state[2]
110 return x*x + y*y + z*z > .8
111
112def plan():
113 # create an R^3 state space
114 space = ob.RealVectorStateSpace(3)
115 # set lower and upper bounds
116 bounds = ob.RealVectorBounds(3)
117 bounds.setLow(-1)
118 bounds.setHigh(1)
119 space.setBounds(bounds)
120 # create a simple setup object
121 ss = og.SimpleSetup(space)
122 ss.setStateValidityChecker(ob.StateValidityCheckerFn(isStateValid))
123 start = ob.State(space)
124 start()[0] = -1.
125 start()[1] = -1.
126 start()[2] = -1.
127 goal = ob.State(space)
128 goal()[0] = 1.
129 goal()[1] = 1.
130 goal()[2] = 1.
131 ss.setStartAndGoalStates(start, goal, .05)
132 # set the planner
133 planner = RandomWalkPlanner(ss.getSpaceInformation())
134 ss.setPlanner(planner)
135
136 result = ss.solve(10.0)
137 if result:
138 if result.getStatus() == ob.PlannerStatus.APPROXIMATE_SOLUTION:
139 print("Solution is approximate")
140 # try to shorten the path
141 ss.simplifySolution()
142 # print the simplified path
143 print(ss.getSolutionPath())
144
145
146if __name__ == "__main__":
147 plan()
Base class for a planner.
Definition Planner.h:216
The lower and upper bounds for an Rn space.
A state space representing Rn. The distance function is the L2 norm.
Definition of an abstract state.
Definition State.h:50
Definition of a geometric path.
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...
A class to store the exit status of Planner::solve()