Loading...
Searching...
No Matches
StateSampling.py
1#!/usr/bin/env python
2
3
36
37# Author: Mark Moll
38
39try:
40 from ompl import util as ou
41 from ompl import base as ob
42 from ompl import geometric as og
43except ImportError:
44 # if the ompl module is not in the PYTHONPATH assume it is installed in a
45 # subdirectory of the parent directory called "py-bindings."
46 from os.path import abspath, dirname, join
47 import sys
48 sys.path.insert(0, join(dirname(dirname(abspath(__file__))), 'py-bindings'))
49 from ompl import util as ou
50 from ompl import base as ob
51 from ompl import geometric as og
52from time import sleep
53from math import fabs
54
55
56
57# This is a problem-specific sampler that automatically generates valid
58# states; it doesn't need to call SpaceInformation::isValid. This is an
59# example of constrained sampling. If you can explicitly describe the set valid
60# states and can draw samples from it, then this is typically much more
61# efficient than generating random samples from the entire state space and
62# checking for validity.
63class MyValidStateSampler(ob.ValidStateSampler):
64 def __init__(self, si):
65 super(MyValidStateSampler, self).__init__(si)
66 self.name_ = "my sampler"
67 self.rng_ = ou.RNG()
68
69 # Generate a sample in the valid part of the R^3 state space.
70 # Valid states satisfy the following constraints:
71 # -1<= x,y,z <=1
72 # if .25 <= z <= .5, then |x|>.8 and |y|>.8
73 def sample(self, state):
74 z = self.rng_.uniformReal(-1, 1)
75
76 if z > .25 and z < .5:
77 x = self.rng_.uniformReal(0, 1.8)
78 y = self.rng_.uniformReal(0, .2)
79 i = self.rng_.uniformInt(0, 3)
80 if i == 0:
81 state[0] = x-1
82 state[1] = y-1
83 elif i == 1:
84 state[0] = x-.8
85 state[1] = y+.8
86 elif i == 2:
87 state[0] = y-1
88 state[1] = x-1
89 elif i == 3:
90 state[0] = y+.8
91 state[1] = x-.8
92 else:
93 state[0] = self.rng_.uniformReal(-1, 1)
94 state[1] = self.rng_.uniformReal(-1, 1)
95 state[2] = z
96 return True
97
98
99
100# This function is needed, even when we can write a sampler like the one
101# above, because we need to check path segments for validity
102def isStateValid(state):
103 # Let's pretend that the validity check is computationally relatively
104 # expensive to emphasize the benefit of explicitly generating valid
105 # samples
106 sleep(.001)
107 # Valid states satisfy the following constraints:
108 # -1<= x,y,z <=1
109 # if .25 <= z <= .5, then |x|>.8 and |y|>.8
110 return not (fabs(state[0] < .8) and fabs(state[1] < .8) and \
111 state[2] > .25 and state[2] < .5)
112
113# return an obstacle-based sampler
114def allocOBValidStateSampler(si):
115 # we can perform any additional setup / configuration of a sampler here,
116 # but there is nothing to tweak in case of the ObstacleBasedValidStateSampler.
118
119# return an instance of my sampler
120def allocMyValidStateSampler(si):
121 return MyValidStateSampler(si)
122
123def plan(samplerIndex):
124 # construct the state space we are planning in
125 space = ob.RealVectorStateSpace(3)
126
127 # set the bounds
128 bounds = ob.RealVectorBounds(3)
129 bounds.setLow(-1)
130 bounds.setHigh(1)
131 space.setBounds(bounds)
132
133 # define a simple setup class
134 ss = og.SimpleSetup(space)
135
136 # set state validity checking for this space
137 ss.setStateValidityChecker(ob.StateValidityCheckerFn(isStateValid))
138
139 # create a start state
140 start = ob.State(space)
141 start[0] = 0
142 start[1] = 0
143 start[2] = 0
144
145 # create a goal state
146 goal = ob.State(space)
147 goal[0] = 0
148 goal[1] = 0
149 goal[2] = 1
150
151 # set the start and goal states;
152 ss.setStartAndGoalStates(start, goal)
153
154 # set sampler (optional; the default is uniform sampling)
155 si = ss.getSpaceInformation()
156 if samplerIndex == 1:
157 # use obstacle-based sampling
158 si.setValidStateSamplerAllocator(ob.ValidStateSamplerAllocator(allocOBValidStateSampler))
159 elif samplerIndex == 2:
160 # use my sampler
161 si.setValidStateSamplerAllocator(ob.ValidStateSamplerAllocator(allocMyValidStateSampler))
162
163 # create a planner for the defined space
164 planner = og.PRM(si)
165 ss.setPlanner(planner)
166
167 # attempt to solve the problem within ten seconds of planning time
168 solved = ss.solve(10.0)
169 if solved:
170 print("Found solution:")
171 # print the path to screen
172 print(ss.getSolutionPath())
173 else:
174 print("No solution found")
175
176
177if __name__ == '__main__':
178 print("Using default uniform sampler:")
179 plan(0)
180 print("\nUsing obstacle-based sampler:")
181 plan(1)
182 print("\nUsing my sampler:")
183 plan(2)
Generate valid samples using obstacle based sampling. First sample an invalid state,...
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
Abstract definition of a state sampler.
Probabilistic RoadMap planner.
Definition PRM.h:81
Create the set of classes typically needed to solve a geometric problem.
Definition SimpleSetup.h:63
std::function< ValidStateSamplerPtr(const SpaceInformation *)> ValidStateSamplerAllocator
Definition of a function that can allocate a valid state sampler.
std::function< bool(const State *)> StateValidityCheckerFn
If no state validity checking class is specified (StateValidityChecker), a std::function can be speci...