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 }
virtual int locateRegion(const base::State *s) const
Return the id of the region that this state falls in.
Definition of an abstract state.
Definition: State.h:114
int gridCellToRid(const std::vector< int > &cell) const
Return the region id corresponding to the (discrete) grid cell coordinates.
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 double distanceHeuristic(int r1, int r2) const
An admissible and consistent distance heuristic between two regions. Manhattan distance on grid.
int coordToRegion(const std::vector< double > &coord) const
Return the region id of the given coordinate in the decomposition.
virtual int getDimension() const
Return the dimension of this HiLoDecomposition.
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...
The exception type for ompl.
Definition: Exception.h:79
bool hasDiagonalEdges() const
Return true if the decomposition has diagonal edges.