Loading...
Searching...
No Matches
XXLPlanarDecomposition.cpp
1/*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2015, Rice University
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 *
11 * * Redistributions of source code must retain the above copyright
12 * notice, this list of conditions and the following disclaimer.
13 * * Redistributions in binary form must reproduce the above
14 * copyright notice, this list of conditions and the following
15 * disclaimer in the documentation and/or other materials provided
16 * with the distribution.
17 * * Neither the name of the Rice University nor the names of its
18 * contributors may be used to endorse or promote products derived
19 * from this software without specific prior written permission.
20 *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 * POSSIBILITY OF SUCH DAMAGE.
33 *********************************************************************/
34
35/* Author: Ryan Luna */
36
37#include "ompl/geometric/planners/xxl/XXLPlanarDecomposition.h"
38#include "ompl/util/Exception.h"
39#include "ompl/util/Console.h"
40#include <limits>
41
42ompl::geometric::XXLPlanarDecomposition::XXLPlanarDecomposition(const base::RealVectorBounds &xyBounds,
43 const std::vector<int> &xySlices, const int thetaSlices,
44 bool diagonalEdges)
45 : diagonalEdges_(diagonalEdges), xyBounds_(xyBounds), xySlices_(xySlices), thetaSlices_(thetaSlices)
46{
47 if (xySlices_.size() != 2)
48 throw ompl::Exception("%s: xySlices must have length 2", __func__);
49 if (thetaSlices_ < 1)
50 throw ompl::Exception("%s: thetaSlices must be at least 1", __func__);
51 xyBounds_.check();
52
53 if (thetaLow_ > thetaHigh_)
54 throw ompl::Exception("%s: theta lower bound > theta upper bound");
55
56 numRegions_ = 1;
57 for (size_t i = 0; i < xySlices_.size(); ++i)
58 {
59 if (xySlices_[i] < 1)
60 throw ompl::Exception("%s: Number of xySlices must be positive", __func__);
61 numRegions_ *= xySlices_[i];
62 }
63 numRegions_ *= thetaSlices_;
64
65 // region volume will be the position part only...
66 dx_ = std::abs(xyBounds.high[0] - xyBounds.low[0]);
67 dy_ = std::abs(xyBounds.high[1] - xyBounds.low[1]);
68 dTheta_ = std::abs(thetaHigh_ - thetaLow_);
69
70 // The size of each grid cell in the XYZ dimensions
71 xSize_ = dx_ / xySlices_[0];
72 ySize_ = dy_ / xySlices_[1];
73 thetaSize_ = dTheta_ / thetaSlices_;
74
75 dimension_ = 1;
76 if (xySlices_[0] > 1 || xySlices_[1] > 1)
77 dimension_++;
78 if (thetaSlices_ > 1)
79 dimension_++;
80
81 assert(dimension_ >= 1 && dimension_ <= 3);
82}
83
84ompl::geometric::XXLPlanarDecomposition::XXLPlanarDecomposition(const base::RealVectorBounds &xyBounds,
85 const std::vector<int> &xySlices, const int thetaSlices,
86 double thetaLowerBound, double thetaUpperBound,
87 bool diagonalEdges)
88 : XXLDecomposition()
89 , diagonalEdges_(diagonalEdges)
90 , xyBounds_(xyBounds)
91 , thetaLow_(thetaLowerBound)
92 , thetaHigh_(thetaUpperBound)
93 , xySlices_(xySlices)
94 , thetaSlices_(thetaSlices)
95{
96 if (xySlices_.size() != 2)
97 throw Exception("%s: xySlices must have length 2", __func__);
98 if (thetaSlices_ < 1)
99 throw Exception("%s: thetaSlices must be at least 1", __func__);
100 xyBounds_.check();
101
102 if (thetaLow_ > thetaHigh_)
103 throw Exception("%s: theta lower bound > theta upper bound");
104
105 numRegions_ = 1;
106 for (size_t i = 0; i < xySlices_.size(); ++i)
107 {
108 if (xySlices_[i] < 1)
109 throw Exception("%s: Number of xySlices must be positive", __func__);
110 numRegions_ *= xySlices_[i];
111 }
112 numRegions_ *= thetaSlices_;
113
114 // region volume will be the position part only...
115 dx_ = fabs(xyBounds.high[0] - xyBounds.low[0]);
116 dy_ = fabs(xyBounds.high[1] - xyBounds.low[1]);
117 dTheta_ = fabs(thetaHigh_ - thetaLow_);
118
119 // The size of each grid cell in the XYZ dimensions
120 xSize_ = dx_ / xySlices_[0];
121 ySize_ = dy_ / xySlices_[1];
122 thetaSize_ = dTheta_ / thetaSlices_;
123
124 dimension_ = 1;
125 if (xySlices_[0] > 1 || xySlices_[1] > 1)
126 dimension_++;
127 if (thetaSlices_ > 1)
128 dimension_++;
129
130 assert(dimension_ >= 1 && dimension_ <= 3);
131}
132
133ompl::geometric::XXLPlanarDecomposition::~XXLPlanarDecomposition()
134{
135}
136
138{
139 return numRegions_;
140}
141
143{
144 return dimension_;
145}
146
148{
149 std::vector<double> coord;
150 project(s, coord);
151 return coordToRegion(coord);
152}
153
154int ompl::geometric::XXLPlanarDecomposition::locateRegion(const std::vector<double> &coord) const
155{
156 return coordToRegion(coord);
157}
158
159void ompl::geometric::XXLPlanarDecomposition::getNeighbors(int rid, std::vector<int> &neighbors) const
160{
161 // up, down, left, right for position dimensions
162 // same for orientation, but we must handle the wrap around case carefully
163 if (diagonalEdges_)
164 getDiagonalNeighbors(rid, neighbors);
165 else
166 getNonDiagonalNeighbors(rid, neighbors);
167}
168
169void ompl::geometric::XXLPlanarDecomposition::getNeighborhood(int rid, std::vector<int> &neighborhood) const
170{
171 getDiagonalNeighbors(rid, neighborhood);
172}
173
174void ompl::geometric::XXLPlanarDecomposition::getNonDiagonalNeighbors(int rid, std::vector<int> &neighbors) const
175{
176 std::vector<int> ridCell;
177 ridToGridCell(rid, ridCell);
178
179 std::vector<int> cell(ridCell.begin(), ridCell.end());
180 std::vector<int> workCell(ridCell.begin(), ridCell.end());
181
182 // xy
183 for (size_t i = 0; i < 2; ++i)
184 {
185 // There are no neighbors in this dimension
186 if (xySlices_[i] == 1)
187 continue;
188
189 workCell[i] -= 1;
190 if (workCell[i] >= 0 && workCell[i] < xySlices_[i])
191 neighbors.push_back(gridCellToRid(workCell));
192
193 if (xySlices_[i] > 2)
194 {
195 workCell[i] += 2;
196 if (workCell[i] >= 0 && workCell[i] < xySlices_[i])
197 neighbors.push_back(gridCellToRid(workCell));
198 workCell[i] = cell[i];
199 }
200 }
201
202 // theta
203 if (thetaSlices_ > 1)
204 {
205 workCell[2] -= 1;
206 if (workCell[2] < 0)
207 workCell[2] += thetaSlices_;
208 else if (workCell[2] >= thetaSlices_)
209 workCell[2] -= thetaSlices_;
210 neighbors.push_back(gridCellToRid(workCell));
211
212 if (thetaSlices_ > 2)
213 {
214 workCell[2] += 2;
215 if (workCell[2] < 0)
216 workCell[2] += thetaSlices_;
217 else if (workCell[2] >= thetaSlices_)
218 workCell[2] -= thetaSlices_;
219 neighbors.push_back(gridCellToRid(workCell));
220 }
221 }
222}
223
224void ompl::geometric::XXLPlanarDecomposition::getDiagonalNeighbors(int rid, std::vector<int> &neighbors) const
225{
226 std::vector<int> ridCell;
227 ridToGridCell(rid, ridCell);
228
229 std::vector<int> cell(ridCell.begin(), ridCell.end());
230
231 for (int x = -1; x <= 1; ++x)
232 {
233 int tX = ridCell[0] + x;
234 if (tX >= 0 && tX < xySlices_[0])
235 cell[0] = tX;
236 else
237 continue;
238
239 for (int y = -1; y <= 1; ++y)
240 {
241 int tY = ridCell[1] + y;
242 if (tY >= 0 && tY < xySlices_[1])
243 cell[1] = tY;
244 else
245 continue;
246
247 for (int theta = -1; theta <= 1; ++theta)
248 {
249 // No additional neighbors in this dimension
250 if (thetaSlices_ == 1 && theta != 0)
251 continue;
252
253 // Do not add duplicate neighbors on the wrap around
254 if (thetaSlices_ <= 2 && theta > 0)
255 continue;
256
257 // don't add ourself as a neighbor
258 if (x == 0 && y == 0 && theta == 0)
259 continue;
260
261 int tTheta = ridCell[2] + theta;
262 if (tTheta < 0)
263 tTheta += thetaSlices_;
264 else if (tTheta >= thetaSlices_)
265 tTheta -= thetaSlices_;
266 cell[2] = tTheta;
267
268 neighbors.push_back(gridCellToRid(cell));
269 }
270 }
271 }
272}
273
274int ompl::geometric::XXLPlanarDecomposition::coordToRegion(const std::vector<double> &coord) const
275{
276 return coordToRegion(&coord[0]);
277}
278
280{
281 // must perform computation about origin
282 std::vector<int> cell(3);
283 cell[0] = (coord[0] - xyBounds_.low[0]) / xSize_; // x
284 cell[1] = (coord[1] - xyBounds_.low[1]) / ySize_; // y
285 cell[2] = (coord[2] - thetaLow_) / thetaSize_; // theta
286
287 return gridCellToRid(cell);
288}
289
290void ompl::geometric::XXLPlanarDecomposition::ridToGridCell(int rid, std::vector<int> &cell) const
291{
292 cell.resize(3);
293 cell[2] = rid / (xySlices_[0] * xySlices_[1]);
294
295 rid %= (xySlices_[0] * xySlices_[1]);
296 cell[1] = rid / xySlices_[0];
297
298 rid %= xySlices_[0]; // mod should not be necessary
299 cell[0] = rid;
300}
301
302int ompl::geometric::XXLPlanarDecomposition::gridCellToRid(const std::vector<int> &cell) const
303{
304 int region = cell[0];
305 region += cell[1] * xySlices_[0];
306 region += cell[2] * xySlices_[0] * xySlices_[1];
307
308 return region;
309}
310
312{
313 std::vector<int> c1, c2;
314 ridToGridCell(r1, c1);
315 ridToGridCell(r2, c2);
316
317 // manhattan distance for everything
318 double dist = 0.0;
319 for (size_t i = 0; i < 2; ++i)
320 dist += std::abs(c1[i] - c2[i]);
321
322 // theta wraps around
323 if (thetaSlices_ > 1)
324 {
325 int min = std::min(c1[2], c2[2]);
326 int max = std::max(c1[2], c2[2]);
327 dist += std::min(std::abs(c1[2] - c2[2]), std::abs((min + thetaSlices_) - max));
328 }
329
330 return dist;
331}
332
334{
335 return diagonalEdges_;
336}
337
338// Sample a point in the SE(2) decomposition (position and orientation)
339void ompl::geometric::XXLPlanarDecomposition::sampleCoordinateFromRegion(int r, std::vector<double> &coord) const
340{
341 coord.resize(3);
342 sampleCoordinateFromRegion(r, &coord[0]);
343}
344
345void ompl::geometric::XXLPlanarDecomposition::sampleCoordinateFromRegion(int r, double *coord) const
346{
347 std::vector<int> cell;
348 ridToGridCell(r, cell);
349
350 // x
351 double xlow = xyBounds_.low[0] + (cell[0] * xSize_);
352 coord[0] = rng_.uniformReal(xlow, xlow + xSize_);
353
354 // y
355 double ylow = xyBounds_.low[1] + (cell[1] * ySize_);
356 coord[1] = rng_.uniformReal(ylow, ylow + ySize_);
357
358 // theta
359 double tlow = thetaLow_ + (cell[2] * thetaSize_);
360 coord[2] = rng_.uniformReal(tlow, tlow + thetaSize_);
361}
The exception type for ompl.
Definition Exception.h:47
Definition of an abstract state.
Definition State.h:50
virtual int getDimension() const
Return the dimension of this HiLoDecomposition.
int coordToRegion(const std::vector< double > &coord) const
Return the region id of the given coordinate in the decomposition.
virtual double distanceHeuristic(int r1, int r2) const
An admissible and consistent distance heuristic between two regions. Manhattan distance on grid.
virtual int locateRegion(const base::State *s) const
Return the id of the region that this state falls in.
virtual int getNumRegions() const
Return the total number of regions in this decomposition.
virtual void getNeighbors(int rid, std::vector< int > &neighbors) const
Stores the given region's neighbors into a given vector.
virtual void getNeighborhood(int rid, std::vector< int > &neighborhood) const
Stores the given region's neighbors into the vector. This returns the 8-connected grid neighbors of t...
int gridCellToRid(const std::vector< int > &cell) const
Return the region id corresponding to the (discrete) grid cell coordinates.
virtual void project(const base::State *s, std::vector< double > &coord, int layer=0) const =0
Project the given State into the XXLDecomposition.
bool hasDiagonalEdges() const
Return true if the decomposition has diagonal edges.