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 
42 ompl::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 
84 ompl::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 
133 ompl::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 
154 int ompl::geometric::XXLPlanarDecomposition::locateRegion(const std::vector<double> &coord) const
155 {
156  return coordToRegion(coord);
157 }
158 
159 void 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 
169 void ompl::geometric::XXLPlanarDecomposition::getNeighborhood(int rid, std::vector<int> &neighborhood) const
170 {
171  getDiagonalNeighbors(rid, neighborhood);
172 }
173 
174 void 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 
224 void 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 
274 int ompl::geometric::XXLPlanarDecomposition::coordToRegion(const std::vector<double> &coord) const
275 {
276  return coordToRegion(&coord[0]);
277 }
278 
279 int ompl::geometric::XXLPlanarDecomposition::coordToRegion(const double *coord) const
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 
290 void 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 
302 int 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)
339 void ompl::geometric::XXLPlanarDecomposition::sampleCoordinateFromRegion(int r, std::vector<double> &coord) const
340 {
341  coord.resize(3);
342  sampleCoordinateFromRegion(r, &coord[0]);
343 }
344 
345 void 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.
bool hasDiagonalEdges() const
Return true if the decomposition has diagonal edges.