XXLPositionDecomposition.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 <stack>
38 #include "ompl/geometric/planners/xxl/XXLPositionDecomposition.h"
39 #include "ompl/util/Exception.h"
40 
41 ompl::geometric::XXLPositionDecomposition::XXLPositionDecomposition(const base::RealVectorBounds &bounds,
42  const std::vector<int> &slices, bool diagonalEdges)
43  : bounds_(bounds), slices_(slices), diagonalEdges_(diagonalEdges)
44 {
45  bounds_.check();
46 
47  if (slices_.size() == 0)
48  throw ompl::Exception("There must be at least one dimension specified in slices");
49  if (bounds_.low.size() != slices_.size())
50  throw ompl::Exception("Total slice count must be equal to the number of dimensions");
51 
52  numRegions_ = slices_[0];
53  for (size_t i = 1; i < slices_.size(); ++i)
54  numRegions_ *= slices_[i];
55 
56  cellSizes_.resize(slices_.size());
57  for (size_t i = 0; i < slices_.size(); ++i)
58  {
59  double extent = fabs(bounds_.high[i] - bounds_.low[i]);
60  cellSizes_[i] = extent / slices_[i];
61  }
62 }
63 
64 ompl::geometric::XXLPositionDecomposition::~XXLPositionDecomposition()
65 {
66 }
67 
69 {
70  return numRegions_;
71 }
72 
74 {
75  return (int)slices_.size();
76 }
77 
79 {
80  std::vector<double> coord;
81  project(s, coord);
82  return coordToRegion(coord);
83 }
84 
85 int ompl::geometric::XXLPositionDecomposition::locateRegion(const std::vector<double> &coord) const
86 {
87  return coordToRegion(coord);
88 }
89 
90 void ompl::geometric::XXLPositionDecomposition::getNeighbors(int rid, std::vector<int> &neighbors) const
91 {
92  if (diagonalEdges_)
93  getDiagonalNeighbors(rid, neighbors);
94  else
95  getNonDiagonalNeighbors(rid, neighbors);
96 }
97 
98 void ompl::geometric::XXLPositionDecomposition::getNeighborhood(int rid, std::vector<int> &neighborhood) const
99 {
100  getDiagonalNeighbors(rid, neighborhood);
101 }
102 
104 {
105  std::vector<int> c1, c2;
106  ridToGridCell(r1, c1);
107  ridToGridCell(r2, c2);
108 
109  // manhattan distance for everything
110  double dist = 0.0;
111  for (size_t i = 0; i < 2; ++i)
112  dist += abs(c1[i] - c2[i]);
113 
114  // should do Chebyshev for diagonal edges...
115 
116  return dist;
117 }
118 
119 void ompl::geometric::XXLPositionDecomposition::ridToGridCell(int rid, std::vector<int> &cell) const
120 {
121  cell.resize(slices_.size());
122 
123  int lowerDimensionalRegions = numRegions_ / slices_.back();
124  for (int i = slices_.size() - 1; i >= 0; --i)
125  {
126  cell[i] = rid / lowerDimensionalRegions;
127 
128  rid %= lowerDimensionalRegions;
129  if (i > 0)
130  lowerDimensionalRegions /= slices_[i - 1];
131  }
132 }
133 
134 int ompl::geometric::XXLPositionDecomposition::gridCellToRid(const std::vector<int> &cell) const
135 {
136  int region = cell[0];
137  int lowerDimensionalRegions = slices_[0];
138  for (size_t i = 1; i < cell.size(); ++i)
139  {
140  region += (cell[i] * lowerDimensionalRegions);
141  lowerDimensionalRegions *= slices_[i - 1];
142  }
143  return region;
144 }
145 
146 int ompl::geometric::XXLPositionDecomposition::coordToRegion(const std::vector<double> &coord) const
147 {
148  return coordToRegion(&coord[0]);
149 }
150 
152 {
153  std::vector<int> cell(slices_.size());
154  for (size_t i = 0; i < slices_.size(); ++i)
155  cell[i] = (coord[i] - bounds_.low[i]) / cellSizes_[i];
156  return gridCellToRid(cell);
157 }
158 
160 {
161  return diagonalEdges_;
162 }
163 
164 void ompl::geometric::XXLPositionDecomposition::getNonDiagonalNeighbors(int rid, std::vector<int> &neighbors) const
165 {
166  std::vector<int> cell;
167  ridToGridCell(rid, cell);
168  std::vector<int> workCell(cell.begin(), cell.end());
169 
170  for (size_t i = 0; i < slices_.size(); ++i)
171  {
172  if (slices_[i] == 1) // no neighbors in this dimension
173  continue;
174 
175  workCell[i] -= 1;
176  if (workCell[i] >= 0 && workCell[i] < slices_[i])
177  neighbors.push_back(gridCellToRid(workCell));
178 
179  if (slices_[i] > 2)
180  {
181  workCell[i] += 2;
182  if (workCell[i] >= 0 && workCell[i] < slices_[i])
183  neighbors.push_back(gridCellToRid(workCell));
184  }
185  workCell[i] = cell[i];
186  }
187 }
188 
189 void ompl::geometric::XXLPositionDecomposition::getDiagonalNeighbors(int rid, std::vector<int> &neighbors) const
190 {
191  std::vector<int> ridCell;
192  ridToGridCell(rid, ridCell);
193 
194  std::stack<std::pair<int, std::vector<int>>> stack;
195  stack.push(std::make_pair(0, ridCell));
196 
197  while (!stack.empty())
198  {
199  std::pair<int, std::vector<int>> entry = stack.top();
200  stack.pop();
201 
202  if (entry.first == (int)slices_.size())
203  {
204  // make sure we don't add ourself as a neighbor
205  bool same = true;
206  for (size_t i = 0; i < entry.second.size() && same; ++i)
207  if (entry.second[i] != ridCell[i])
208  same = false;
209 
210  if (!same)
211  neighbors.push_back(gridCellToRid(entry.second));
212  }
213  else
214  {
215  int i = entry.first;
216 
217  entry.second[i] -= 1;
218  if (entry.second[i] >= 0) // don't go out of bounds
219  stack.push(std::make_pair(i + 1, entry.second));
220 
221  entry.second[i] += 1;
222  stack.push(std::make_pair(i + 1, entry.second));
223 
224  entry.second[i] += 1;
225  if (entry.second[i] < slices_[i])
226  stack.push(std::make_pair(i + 1, entry.second));
227  }
228  }
229 }
virtual int getNumRegions() const
Returns the number of regions in this XXLDecomposition.
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...
Definition of an abstract state.
Definition: State.h:113
virtual double distanceHeuristic(int r1, int r2) const
An admissible and consistent distance heuristic between two regions. Manhattan distance on grid.
bool hasDiagonalEdges() const
Return true if the decomposition has diagonal edges.
virtual int locateRegion(const base::State *s) const
Return the id of the region that this state falls in.
int coordToRegion(const std::vector< double > &coord) const
Return the region id of the given coordinate in the decomposition.
int gridCellToRid(const std::vector< int > &cell) const
Return the region id corresponding to the (discrete) grid cell coordinates.
virtual void getNeighbors(int rid, std::vector< int > &neighbors) const
Stores the given region's neighbors into a given vector.
virtual int getDimension() const
Return the dimension of this HiLoDecomposition.
The exception type for ompl.
Definition: Exception.h:78