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 
39 namespace
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 
53 void 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[] = {
66  -1, -1,
67  0, -1,
68  +1, -1,
69  -1, 0,
70  +1, 0,
71  -1, +1,
72  0, +1,
73  +1, +1
74  };
75  std::vector<int> coord(2);
76  regionToGridCoord(rid, coord);
77  std::vector<int> nc(2);
78  for (std::size_t i = 0; i < 16; i += 2)
79  {
80  nc[0] = coord[0] + offset[i];
81  nc[1] = coord[1] + offset[i+1];
82  if (nc[0] >= 0 && (int) nc[0] < length_ && nc[1] >= 0
83  && (int) nc[1] < length_)
84  neighbors.push_back(nc[0]*length_ + nc[1]);
85  }
86  }
87  else if (dimension_ == 3)
88  {
89  static const int offset[] = {
90  -1, 0, 0,
91  +1, 0, 0,
92  0, -1, 0,
93  0, +1, 0,
94  -1, -1, 0,
95  -1, +1, 0,
96  +1, -1, 0,
97  +1, +1, 0,
98  -1, 0, -1,
99  +1, 0, -1,
100  0, -1, -1,
101  0, +1, -1,
102  -1, -1, -1,
103  -1, +1, -1,
104  +1, -1, -1,
105  +1, +1, -1,
106  -1, 0, +1,
107  +1, 0, +1,
108  0, -1, +1,
109  0, +1, +1,
110  -1, -1, +1,
111  -1, +1, +1,
112  +1, -1, +1,
113  +1, +1, +1,
114  0, 0, -1,
115  0, 0, +1
116  };
117  std::vector<int> coord(3);
118  regionToGridCoord(rid, coord);
119  std::vector<int> nc(3);
120  for (int i = 0; i < 78; i += 3)
121  {
122  nc[0] = coord[0] + offset[i];
123  nc[1] = coord[1] + offset[i+1];
124  nc[2] = coord[2] + offset[i+2];
125  if (nc[0] >= 0 && (int) nc[0] < length_
126  && nc[1] >= 0 && (int) nc[1] < length_
127  && nc[2] >= 0 && (int) nc[2] < length_)
128  neighbors.push_back(nc[0]*length_*length_ + nc[1]*length_ + nc[2]);
129  }
130  }
131  else
132  {
133  computeGridNeighbors (rid, neighbors);
134  }
135 }
136 
138 {
139  std::vector<double> coord(dimension_);
140  project(s, coord);
141  return coordToRegion(coord);
142 }
143 
144 void ompl::control::GridDecomposition::sampleFromRegion(int rid, RNG &rng, std::vector<double> &coord) const
145 {
146  coord.resize(dimension_);
147  const base::RealVectorBounds& regionBounds(getRegionBounds(rid));
148  for (int i = 0; i < dimension_; ++i)
149  coord[i] = rng.uniformReal(regionBounds.low[i], regionBounds.high[i]);
150 }
151 
152 void ompl::control::GridDecomposition::computeGridNeighbors (int rid, std::vector <int> &neighbors) const
153 {
154  std::vector <int> candidate (dimension_, -1);
155  std::vector <int> coord;
156  regionToGridCoord (rid, coord);
157 
158  computeGridNeighborsSub (coord, neighbors, 0, candidate);
159 }
160 
162  std::vector <int> &neighbors,
163  int dim,
164  std::vector <int> &candidate) const
165 {
166  // Stopping condition for recursive method.
167  if (dim == dimension_)
168  {
169  // Make sure we don't push back ourselves as a neighbor
170  bool same = true;
171  for (unsigned int i = 0; i < coord.size() && same; ++i)
172  same = (coord[i] == candidate[i]);
173 
174  if (!same)
175  {
176  neighbors.push_back (gridCoordToRegion (candidate));
177  }
178  }
179  else
180  {
181  // Check neighbor in the cell preceding this one in this dimension
182  if (coord[dim] >= 1)
183  {
184  candidate[dim] = coord[dim]-1;
185  computeGridNeighborsSub (coord, neighbors, dim+1, candidate);
186  }
187 
188  // Make sure to include the same coordinate, for neighbors "above", "below", "in front of", "behind", etcetera.
189  candidate[dim] = coord[dim];
190  computeGridNeighborsSub (coord, neighbors, dim+1, candidate);
191 
192  // Check neighbor in the cell after this one in this dimension
193  if (coord[dim] +1 < length_)
194  {
195  candidate[dim] = coord[dim]+1;
196  computeGridNeighborsSub (coord, neighbors, dim+1, candidate);
197  }
198  }
199 }
200 
201 void ompl::control::GridDecomposition::regionToGridCoord(int rid, std::vector<int>& coord) const
202 {
203  coord.resize(dimension_);
204  for (int i = dimension_-1; i >= 0; --i)
205  {
206  int remainder = rid % length_;
207  coord[i] = remainder;
208  rid /= length_;
209  }
210 }
211 
212 int ompl::control::GridDecomposition::gridCoordToRegion (const std::vector <int> &coord) const
213 {
214  int region = 0;
215  for (unsigned int i = 0; i < coord.size (); i++)
216  {
217  // Computing length_^(dimension of coord -1)
218  int multiplicand = 1;
219  for (unsigned int j = 1; j < coord.size () - i; j++)
220  multiplicand *= length_;
221 
222  region += (coord[i] * multiplicand);
223  }
224  return region;
225 }
226 
227 int ompl::control::GridDecomposition::coordToRegion(const std::vector<double>& coord) const
228 {
229  int region = 0;
230  int factor = 1;
231  int index;
232  for (int i = dimension_-1; i >= 0; --i)
233  {
234  index = (int) (length_*(coord[i]-bounds_.low[i])/(bounds_.high[i]-bounds_.low[i]));
235 
236  // There is an edge case when the coordinate lies exactly on the upper bound where
237  // the region index will be out of bounds. Ensure index lies within [0, length_)
238  if (index >= length_)
239  index = length_-1;
240 
241  region += factor*index;
242  factor *= length_;
243  }
244  return region;
245 }
246 
247 void ompl::control::GridDecomposition::coordToGridCoord(const std::vector<double>& coord, std::vector<int>& gridCoord) const
248 {
249  gridCoord.resize(dimension_);
250  for (int i = 0; i < dimension_; ++i)
251  {
252  gridCoord[i] = (int) (length_*(coord[i]-bounds_.low[i])/(bounds_.high[i]-bounds_.low[i]));
253 
254  // There is an edge case when the coordinate lies exactly on the upper bound where
255  // the region index will be out of bounds. Ensure index lies within [0, length_)
256  if (gridCoord[i] >= length_)
257  gridCoord[i] = length_-1;
258  }
259 }
260 
262 {
263  if (regToBounds_.count(rid) > 0)
264  return *regToBounds_[rid].get();
265  ompl::base::RealVectorBounds *regionBounds = new ompl::base::RealVectorBounds(dimension_);
266  std::vector<int> rc(dimension_);
267  regionToGridCoord(rid, rc);
268  for (int i = 0; i < dimension_; ++i)
269  {
270  const double length = (bounds_.high[i] - bounds_.low[i]) / length_;
271  regionBounds->low[i] = bounds_.low[i] + length*rc[i];
272  regionBounds->high[i] = regionBounds->low[i] + length;
273  }
274  regToBounds_[rid] = boost::shared_ptr<ompl::base::RealVectorBounds>(regionBounds);
275  return *regToBounds_[rid].get();
276 }
277 
278 namespace
279 {
280  int calcNumGridCells(int len, int dim)
281  {
282  int numCells = 1;
283  for (int i = 0; i < dim; ++i)
284  numCells *= len;
285  return numCells;
286  }
287 }
virtual const base::RealVectorBounds & getRegionBounds(int rid) const
Helper method to return the bounds of a given region.
std::vector< double > low
Lower bound.
void coordToGridCoord(const std::vector< double > &coord, std::vector< int > &gridCoord) const
Converts a decomposition space coordinate to a grid coordinate.
virtual int locateRegion(const base::State *s) const
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.
virtual void sampleFromRegion(int rid, RNG &rng, std::vector< double > &coord) const
Samples a projected coordinate from a given region.
A Decomposition is a partition of a bounded Euclidean space into a fixed number of regions which are ...
Definition: Decomposition.h:62
Random number generation. An instance of this class cannot be used by multiple threads at once (membe...
Definition: RandomNumbers.h:54
std::vector< double > high
Upper bound.
double uniformReal(double lower_bound, double upper_bound)
Generate a random real within given bounds: [lower_bound, upper_bound)
Definition: RandomNumbers.h:68
Definition of an abstract state.
Definition: State.h:50
virtual void project(const base::State *s, std::vector< double > &coord) const =0
Project a given State to a set of coordinates in R^k, where k is the dimension of this Decomposition...
GridDecomposition(int len, int dim, const base::RealVectorBounds &b)
Constructor. Creates a GridDecomposition as a hypercube with a given dimension, side length...
The lower and upper bounds for an Rn space.
void regionToGridCoord(int rid, std::vector< int > &coord) const
Converts a given region to a coordinate in the grid.
virtual void getNeighbors(int rid, std::vector< int > &neighbors) const
Stores a given region&#39;s neighbors into a given vector.
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