Loading...
Searching...
No Matches
GridDecomposition.cpp
1/*********************************************************************
2* Software License Agreement (BSD License)
3*
4* Copyright (c) 2011, 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: Matt Maly */
36
37#include "ompl/control/planners/syclop/GridDecomposition.h"
38
39namespace
40{
42 static int calcNumGridCells(int len, int dim);
43}
44
46 : Decomposition(dim, b), length_(len), cellVolume_(b.getVolume()), numGridCells_(calcNumGridCells(len, dim))
47{
48 double lenInv = 1.0 / len;
49 for (int i = 0; i < dim; ++i)
50 cellVolume_ *= lenInv;
51}
52
53void ompl::control::GridDecomposition::getNeighbors(int rid, std::vector<int> &neighbors) const
54{
55 // We efficiently compute neighbors for dim = 1, 2, or 3; for higher dimensions we use a general approach.
56 if (dimension_ == 1)
57 {
58 if (rid > 0)
59 neighbors.push_back(rid - 1);
60 if (rid < length_ - 1)
61 neighbors.push_back(rid + 1);
62 }
63 else if (dimension_ == 2)
64 {
65 static const int offset[] = {-1, -1, 0, -1, +1, -1, -1, 0, +1, 0, -1, +1, 0, +1, +1, +1};
66 std::vector<int> coord(2);
67 regionToGridCoord(rid, coord);
68 std::vector<int> nc(2);
69 for (std::size_t i = 0; i < 16; i += 2)
70 {
71 nc[0] = coord[0] + offset[i];
72 nc[1] = coord[1] + offset[i + 1];
73 if (nc[0] >= 0 && (int)nc[0] < length_ && nc[1] >= 0 && (int)nc[1] < length_)
74 neighbors.push_back(nc[0] * length_ + nc[1]);
75 }
76 }
77 else if (dimension_ == 3)
78 {
79 static const int offset[] = {-1, 0, 0, +1, 0, 0, 0, -1, 0, 0, +1, 0, -1, -1, 0, -1, +1, 0, +1, -1,
80 0, +1, +1, 0, -1, 0, -1, +1, 0, -1, 0, -1, -1, 0, +1, -1, -1, -1, -1, -1,
81 +1, -1, +1, -1, -1, +1, +1, -1, -1, 0, +1, +1, 0, +1, 0, -1, +1, 0, +1, +1,
82 -1, -1, +1, -1, +1, +1, +1, -1, +1, +1, +1, +1, 0, 0, -1, 0, 0, +1};
83 std::vector<int> coord(3);
84 regionToGridCoord(rid, coord);
85 std::vector<int> nc(3);
86 for (int i = 0; i < 78; i += 3)
87 {
88 nc[0] = coord[0] + offset[i];
89 nc[1] = coord[1] + offset[i + 1];
90 nc[2] = coord[2] + offset[i + 2];
91 if (nc[0] >= 0 && (int)nc[0] < length_ && nc[1] >= 0 && (int)nc[1] < length_ && nc[2] >= 0 &&
92 (int)nc[2] < length_)
93 neighbors.push_back(nc[0] * length_ * length_ + nc[1] * length_ + nc[2]);
94 }
95 }
96 else
97 {
98 computeGridNeighbors(rid, neighbors);
99 }
100}
101
103{
104 std::vector<double> coord(dimension_);
105 project(s, coord);
106 return coordToRegion(coord);
107}
108
109void ompl::control::GridDecomposition::sampleFromRegion(int rid, RNG &rng, std::vector<double> &coord) const
110{
111 coord.resize(dimension_);
112 const base::RealVectorBounds &regionBounds(getRegionBounds(rid));
113 for (int i = 0; i < dimension_; ++i)
114 coord[i] = rng.uniformReal(regionBounds.low[i], regionBounds.high[i]);
115}
116
117void ompl::control::GridDecomposition::computeGridNeighbors(int rid, std::vector<int> &neighbors) const
118{
119 std::vector<int> candidate(dimension_, -1);
120 std::vector<int> coord;
121 regionToGridCoord(rid, coord);
122
123 computeGridNeighborsSub(coord, neighbors, 0, candidate);
124}
125
127 std::vector<int> &neighbors, int dim,
128 std::vector<int> &candidate) const
129{
130 // Stopping condition for recursive method.
131 if (dim == dimension_)
132 {
133 // Make sure we don't push back ourselves as a neighbor
134 bool same = true;
135 for (unsigned int i = 0; i < coord.size() && same; ++i)
136 same = (coord[i] == candidate[i]);
137
138 if (!same)
139 {
140 neighbors.push_back(gridCoordToRegion(candidate));
141 }
142 }
143 else
144 {
145 // Check neighbor in the cell preceding this one in this dimension
146 if (coord[dim] >= 1)
147 {
148 candidate[dim] = coord[dim] - 1;
149 computeGridNeighborsSub(coord, neighbors, dim + 1, candidate);
150 }
151
152 // Make sure to include the same coordinate, for neighbors "above", "below", "in front of", "behind", etcetera.
153 candidate[dim] = coord[dim];
154 computeGridNeighborsSub(coord, neighbors, dim + 1, candidate);
155
156 // Check neighbor in the cell after this one in this dimension
157 if (coord[dim] + 1 < length_)
158 {
159 candidate[dim] = coord[dim] + 1;
160 computeGridNeighborsSub(coord, neighbors, dim + 1, candidate);
161 }
162 }
163}
164
165void ompl::control::GridDecomposition::regionToGridCoord(int rid, std::vector<int> &coord) const
166{
167 coord.resize(dimension_);
168 for (int i = dimension_ - 1; i >= 0; --i)
169 {
170 int remainder = rid % length_;
171 coord[i] = remainder;
172 rid /= length_;
173 }
174}
175
176int ompl::control::GridDecomposition::gridCoordToRegion(const std::vector<int> &coord) const
177{
178 int region = 0;
179 for (unsigned int i = 0; i < coord.size(); i++)
180 {
181 // Computing length_^(dimension of coord -1)
182 int multiplicand = 1;
183 for (unsigned int j = 1; j < coord.size() - i; j++)
184 multiplicand *= length_;
185
186 region += (coord[i] * multiplicand);
187 }
188 return region;
189}
190
191int ompl::control::GridDecomposition::coordToRegion(const std::vector<double> &coord) const
192{
193 int region = 0;
194 int factor = 1;
195 int index;
196 for (int i = dimension_ - 1; i >= 0; --i)
197 {
198 index = (int)(length_ * (coord[i] - bounds_.low[i]) / (bounds_.high[i] - bounds_.low[i]));
199
200 // There is an edge case when the coordinate lies exactly on the upper bound where
201 // the region index will be out of bounds. Ensure index lies within [0, length_)
202 if (index >= length_)
203 index = length_ - 1;
204
205 region += factor * index;
206 factor *= length_;
207 }
208 return region;
209}
210
211void ompl::control::GridDecomposition::coordToGridCoord(const std::vector<double> &coord,
212 std::vector<int> &gridCoord) const
213{
214 gridCoord.resize(dimension_);
215 for (int i = 0; i < dimension_; ++i)
216 {
217 gridCoord[i] = (int)(length_ * (coord[i] - bounds_.low[i]) / (bounds_.high[i] - bounds_.low[i]));
218
219 // There is an edge case when the coordinate lies exactly on the upper bound where
220 // the region index will be out of bounds. Ensure index lies within [0, length_)
221 if (gridCoord[i] >= length_)
222 gridCoord[i] = length_ - 1;
223 }
224}
225
227{
228 if (regToBounds_.count(rid) > 0)
229 return *regToBounds_[rid];
230 auto regionBounds(std::make_shared<ompl::base::RealVectorBounds>(dimension_));
231 std::vector<int> rc(dimension_);
232 regionToGridCoord(rid, rc);
233 for (int i = 0; i < dimension_; ++i)
234 {
235 const double length = (bounds_.high[i] - bounds_.low[i]) / length_;
236 regionBounds->low[i] = bounds_.low[i] + length * rc[i];
237 regionBounds->high[i] = regionBounds->low[i] + length;
238 }
239 regToBounds_[rid] = regionBounds;
240 return *regToBounds_[rid];
241}
242
243namespace
244{
245 int calcNumGridCells(int len, int dim)
246 {
247 int numCells = 1;
248 for (int i = 0; i < dim; ++i)
249 numCells *= len;
250 return numCells;
251 }
252}
Random number generation. An instance of this class cannot be used by multiple threads at once (membe...
double uniformReal(double lower_bound, double upper_bound)
Generate a random real within given bounds: [lower_bound, upper_bound)
The lower and upper bounds for an Rn space.
std::vector< double > high
Upper bound.
std::vector< double > low
Lower bound.
Definition of an abstract state.
Definition State.h:50
A Decomposition is a partition of a bounded Euclidean space into a fixed number of regions which are ...
void computeGridNeighbors(int rid, std::vector< int > &neighbors) const
Computes the neighbors of the given region in a n-dimensional grid.
void computeGridNeighborsSub(const std::vector< int > &coord, std::vector< int > &neighbors, int dim, std::vector< int > &candidate) const
int locateRegion(const base::State *s) const override
Returns the index of the region containing a given State. Most often, this is obtained by first calli...
int gridCoordToRegion(const std::vector< int > &coord) const
Converts the given grid coordinate to its corresponding region ID.
int coordToRegion(const std::vector< double > &coord) const
Converts a decomposition space coordinate to the ID of the region that contains iit.
void regionToGridCoord(int rid, std::vector< int > &coord) const
Converts a given region to a coordinate in the grid.
void sampleFromRegion(int rid, RNG &rng, std::vector< double > &coord) const override
Samples a projected coordinate from a given region.
void getNeighbors(int rid, std::vector< int > &neighbors) const override
Stores a given region's neighbors into a given vector.
GridDecomposition(int len, int dim, const base::RealVectorBounds &b)
Constructor. Creates a GridDecomposition as a hypercube with a given dimension, side length,...
virtual const base::RealVectorBounds & getRegionBounds(int rid) const
Helper method to return the bounds of a given region.
void coordToGridCoord(const std::vector< double > &coord, std::vector< int > &gridCoord) const
Converts a decomposition space coordinate to a grid coordinate.