37 #include "ompl/geometric/planners/xxl/XXLPlanarDecomposition.h"
38 #include "ompl/util/Exception.h"
42 ompl::geometric::XXLPlanarDecomposition::XXLPlanarDecomposition(
const base::RealVectorBounds &xyBounds,
43 const std::vector<int> &xySlices,
const int thetaSlices,
45 : diagonalEdges_(diagonalEdges), xyBounds_(xyBounds), xySlices_(xySlices), thetaSlices_(thetaSlices)
47 if (xySlices_.size() != 2)
53 if (thetaLow_ > thetaHigh_)
57 for (
size_t i = 0; i < xySlices_.size(); ++i)
60 throw ompl::Exception(
"%s: Number of xySlices must be positive", __func__);
61 numRegions_ *= xySlices_[i];
63 numRegions_ *= thetaSlices_;
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_);
71 xSize_ = dx_ / xySlices_[0];
72 ySize_ = dy_ / xySlices_[1];
73 thetaSize_ = dTheta_ / thetaSlices_;
76 if (xySlices_[0] > 1 || xySlices_[1] > 1)
81 assert(dimension_ >= 1 && dimension_ <= 3);
84 ompl::geometric::XXLPlanarDecomposition::XXLPlanarDecomposition(
const base::RealVectorBounds &xyBounds,
85 const std::vector<int> &xySlices,
const int thetaSlices,
86 double thetaLowerBound,
double thetaUpperBound,
89 , diagonalEdges_(diagonalEdges)
91 , thetaLow_(thetaLowerBound)
92 , thetaHigh_(thetaUpperBound)
94 , thetaSlices_(thetaSlices)
96 if (xySlices_.size() != 2)
97 throw Exception(
"%s: xySlices must have length 2", __func__);
99 throw Exception(
"%s: thetaSlices must be at least 1", __func__);
102 if (thetaLow_ > thetaHigh_)
103 throw Exception(
"%s: theta lower bound > theta upper bound");
106 for (
size_t i = 0; i < xySlices_.size(); ++i)
108 if (xySlices_[i] < 1)
109 throw Exception(
"%s: Number of xySlices must be positive", __func__);
110 numRegions_ *= xySlices_[i];
112 numRegions_ *= thetaSlices_;
115 dx_ = fabs(xyBounds.high[0] - xyBounds.low[0]);
116 dy_ = fabs(xyBounds.high[1] - xyBounds.low[1]);
117 dTheta_ = fabs(thetaHigh_ - thetaLow_);
120 xSize_ = dx_ / xySlices_[0];
121 ySize_ = dy_ / xySlices_[1];
122 thetaSize_ = dTheta_ / thetaSlices_;
125 if (xySlices_[0] > 1 || xySlices_[1] > 1)
127 if (thetaSlices_ > 1)
130 assert(dimension_ >= 1 && dimension_ <= 3);
133 ompl::geometric::XXLPlanarDecomposition::~XXLPlanarDecomposition()
149 std::vector<double> coord;
151 return coordToRegion(coord);
156 return coordToRegion(coord);
164 getDiagonalNeighbors(rid, neighbors);
166 getNonDiagonalNeighbors(rid, neighbors);
171 getDiagonalNeighbors(rid, neighborhood);
174 void ompl::geometric::XXLPlanarDecomposition::getNonDiagonalNeighbors(
int rid, std::vector<int> &neighbors)
const
176 std::vector<int> ridCell;
177 ridToGridCell(rid, ridCell);
179 std::vector<int> cell(ridCell.begin(), ridCell.end());
180 std::vector<int> workCell(ridCell.begin(), ridCell.end());
183 for (
size_t i = 0; i < 2; ++i)
186 if (xySlices_[i] == 1)
190 if (workCell[i] >= 0 && workCell[i] < xySlices_[i])
191 neighbors.push_back(gridCellToRid(workCell));
193 if (xySlices_[i] > 2)
196 if (workCell[i] >= 0 && workCell[i] < xySlices_[i])
197 neighbors.push_back(gridCellToRid(workCell));
198 workCell[i] = cell[i];
203 if (thetaSlices_ > 1)
207 workCell[2] += thetaSlices_;
208 else if (workCell[2] >= thetaSlices_)
209 workCell[2] -= thetaSlices_;
210 neighbors.push_back(gridCellToRid(workCell));
212 if (thetaSlices_ > 2)
216 workCell[2] += thetaSlices_;
217 else if (workCell[2] >= thetaSlices_)
218 workCell[2] -= thetaSlices_;
219 neighbors.push_back(gridCellToRid(workCell));
224 void ompl::geometric::XXLPlanarDecomposition::getDiagonalNeighbors(
int rid, std::vector<int> &neighbors)
const
226 std::vector<int> ridCell;
227 ridToGridCell(rid, ridCell);
229 std::vector<int> cell(ridCell.begin(), ridCell.end());
231 for (
int x = -1; x <= 1; ++x)
233 int tX = ridCell[0] + x;
234 if (tX >= 0 && tX < xySlices_[0])
239 for (
int y = -1; y <= 1; ++y)
241 int tY = ridCell[1] + y;
242 if (tY >= 0 && tY < xySlices_[1])
247 for (
int theta = -1; theta <= 1; ++theta)
250 if (thetaSlices_ == 1 && theta != 0)
254 if (thetaSlices_ <= 2 && theta > 0)
258 if (x == 0 && y == 0 && theta == 0)
261 int tTheta = ridCell[2] + theta;
263 tTheta += thetaSlices_;
264 else if (tTheta >= thetaSlices_)
265 tTheta -= thetaSlices_;
268 neighbors.push_back(gridCellToRid(cell));
276 return coordToRegion(&coord[0]);
282 std::vector<int> cell(3);
283 cell[0] = (coord[0] - xyBounds_.low[0]) / xSize_;
284 cell[1] = (coord[1] - xyBounds_.low[1]) / ySize_;
285 cell[2] = (coord[2] - thetaLow_) / thetaSize_;
287 return gridCellToRid(cell);
290 void ompl::geometric::XXLPlanarDecomposition::ridToGridCell(
int rid, std::vector<int> &cell)
const
293 cell[2] = rid / (xySlices_[0] * xySlices_[1]);
295 rid %= (xySlices_[0] * xySlices_[1]);
296 cell[1] = rid / xySlices_[0];
304 int region = cell[0];
305 region += cell[1] * xySlices_[0];
306 region += cell[2] * xySlices_[0] * xySlices_[1];
313 std::vector<int> c1, c2;
314 ridToGridCell(r1, c1);
315 ridToGridCell(r2, c2);
319 for (
size_t i = 0; i < 2; ++i)
320 dist += std::abs(c1[i] - c2[i]);
323 if (thetaSlices_ > 1)
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));
335 return diagonalEdges_;
339 void ompl::geometric::XXLPlanarDecomposition::sampleCoordinateFromRegion(
int r, std::vector<double> &coord)
const
342 sampleCoordinateFromRegion(r, &coord[0]);
345 void ompl::geometric::XXLPlanarDecomposition::sampleCoordinateFromRegion(
int r,
double *coord)
const
347 std::vector<int> cell;
348 ridToGridCell(r, cell);
351 double xlow = xyBounds_.low[0] + (cell[0] * xSize_);
352 coord[0] = rng_.uniformReal(xlow, xlow + xSize_);
355 double ylow = xyBounds_.low[1] + (cell[1] * ySize_);
356 coord[1] = rng_.uniformReal(ylow, ylow + ySize_);
359 double tlow = thetaLow_ + (cell[2] * thetaSize_);
360 coord[2] = rng_.uniformReal(tlow, tlow + thetaSize_);
The exception type for ompl.
Definition of an abstract state.
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.