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
41ompl::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
64ompl::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
85int ompl::geometric::XXLPositionDecomposition::locateRegion(const std::vector<double> &coord) const
86{
87 return coordToRegion(coord);
88}
89
90void 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
98void 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
119void 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
134int 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
146int 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
164void 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
189void 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}
The exception type for ompl.
Definition: Exception.h:47
Definition of an abstract state.
Definition: State.h:50
virtual void getNeighbors(int rid, std::vector< int > &neighbors) const
Stores the given region's neighbors into a given vector.
bool hasDiagonalEdges() const
Return true if the decomposition has diagonal edges.
int gridCellToRid(const std::vector< int > &cell) const
Return the region id corresponding to the (discrete) grid cell coordinates.
virtual int getDimension() const
Return the dimension of this HiLoDecomposition.
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 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...
virtual int getNumRegions() const
Returns the number of regions in this XXLDecomposition.
int coordToRegion(const std::vector< double > &coord) const
Return the region id of the given coordinate in the decomposition.