37 #include "ompl/control/planners/syclop/GridDecomposition.h"
40 Decomposition(dim, b, calcNumRegions(len,dim)), length_(len), cellVolume_(b.getVolume())
42 double lenInv = 1.0 / len;
43 for (
unsigned int i = 0; i < dim; ++i)
44 cellVolume_ *= lenInv;
53 neighbors.push_back(rid-1);
55 neighbors.push_back(rid+1);
57 else if (dimension_ == 2)
59 static const int offset[] = {
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)
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]);
81 else if (dimension_ == 3)
83 static const int offset[] = {
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)
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]);
127 computeGridNeighbors (rid, neighbors);
133 std::vector<double> coord(dimension_);
135 return coordToRegion(coord);
140 coord.resize(dimension_);
142 for (
unsigned int i = 0; i < dimension_; ++i)
148 std::vector <unsigned int> candidate (dimension_, -1);
149 std::vector <unsigned int> coord;
150 regionToGridCoord (rid, coord);
152 computeGridNeighborsSub (coord, neighbors, 0, candidate);
156 std::vector <unsigned int> &neighbors,
158 std::vector <unsigned int> &candidate)
const
161 if (dim == dimension_)
165 for (
unsigned int i = 0; i < coord.size () && same; ++i)
166 same = (coord[i] == candidate[i]);
170 neighbors.push_back (gridCoordToRegion (candidate));
178 candidate[dim] = coord[dim]-1;
179 computeGridNeighborsSub (coord, neighbors, dim+1, candidate);
183 candidate[dim] = coord[dim];
184 computeGridNeighborsSub (coord, neighbors, dim+1, candidate);
187 if (coord[dim] +1 < length_)
189 candidate[dim] = coord[dim]+1;
190 computeGridNeighborsSub (coord, neighbors, dim+1, candidate);
197 coord.resize(dimension_);
198 for (
int i = dimension_-1; i >= 0; --i)
200 unsigned int remainder = rid % length_;
201 coord[i] = remainder;
208 unsigned int region = 0;
209 for (
unsigned int i = 0; i < coord.size (); i++)
212 unsigned int multiplicand = 1;
213 for (
unsigned int j = 1; j < coord.size () - i; j++)
214 multiplicand *= length_;
216 region += (coord[i] * multiplicand);
223 unsigned int region = 0;
224 unsigned int factor = 1;
226 for (
int i = dimension_-1; i >= 0; --i)
228 index = (
unsigned int) (length_*(coord[i]-bounds_.low[i])/(bounds_.high[i]-bounds_.low[i]));
232 if (index >= length_)
235 region += factor*index;
243 gridCoord.resize(dimension_);
244 for (
unsigned int i = 0; i < dimension_; ++i)
246 gridCoord[i] = (
unsigned int) (length_*(coord[i]-bounds_.low[i])/(bounds_.high[i]-bounds_.low[i]));
250 if (gridCoord[i] >= length_)
251 gridCoord[i] = length_-1;
257 if (regToBounds_.count(rid) > 0)
258 return *regToBounds_[rid].
get();
260 std::vector<unsigned int> rc(dimension_);
261 regionToGridCoord(rid, rc);
262 for (
unsigned int i = 0; i < dimension_; ++i)
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;
268 regToBounds_[rid] = boost::shared_ptr<ompl::base::RealVectorBounds>(regionBounds);
269 return *regToBounds_[rid].get();
272 unsigned int ompl::control::GridDecomposition::calcNumRegions(
unsigned int len,
unsigned int dim)
const
274 unsigned int numRegions = 1;
275 for (
unsigned int i = 0; i < dim; ++i)
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 ...
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...
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 of an abstract state.
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.