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