ompl/control/planners/syclop/src/GridDecomposition.cpp
00001 /********************************************************************* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Copyright (c) 2011, Rice University 00005 * All rights reserved. 00006 * 00007 * Redistribution and use in source and binary forms, with or without 00008 * modification, are permitted provided that the following conditions 00009 * are met: 00010 * 00011 * * Redistributions of source code must retain the above copyright 00012 * notice, this list of conditions and the following disclaimer. 00013 * * Redistributions in binary form must reproduce the above 00014 * copyright notice, this list of conditions and the following 00015 * disclaimer in the documentation and/or other materials provided 00016 * with the distribution. 00017 * * Neither the name of the Rice University nor the names of its 00018 * contributors may be used to endorse or promote products derived 00019 * from this software without specific prior written permission. 00020 * 00021 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00022 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00023 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00024 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00025 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00026 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00027 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00028 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00029 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00030 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00031 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00032 * POSSIBILITY OF SUCH DAMAGE. 00033 *********************************************************************/ 00034 00035 /* Author: Matt Maly */ 00036 00037 #include "ompl/control/planners/syclop/GridDecomposition.h" 00038 00039 namespace 00040 { 00042 static int calcNumGridCells(int len, int dim); 00043 } 00044 00045 ompl::control::GridDecomposition::GridDecomposition(int len, int dim, const base::RealVectorBounds &b) 00046 : Decomposition(dim, b), length_(len), cellVolume_(b.getVolume()), numGridCells_(calcNumGridCells(len, dim)) 00047 { 00048 double lenInv = 1.0 / len; 00049 for (int i = 0; i < dim; ++i) 00050 cellVolume_ *= lenInv; 00051 } 00052 00053 void ompl::control::GridDecomposition::getNeighbors(int rid, std::vector<int>& neighbors) const 00054 { 00055 //We efficiently compute neighbors for dim = 1, 2, or 3; for higher dimensions we use a general approach. 00056 if (dimension_ == 1) 00057 { 00058 if (rid > 0) 00059 neighbors.push_back(rid-1); 00060 if (rid < length_-1) 00061 neighbors.push_back(rid+1); 00062 } 00063 else if (dimension_ == 2) 00064 { 00065 static const int offset[] = { 00066 -1, -1, 00067 0, -1, 00068 +1, -1, 00069 -1, 0, 00070 +1, 0, 00071 -1, +1, 00072 0, +1, 00073 +1, +1 00074 }; 00075 std::vector<int> coord(2); 00076 regionToGridCoord(rid, coord); 00077 std::vector<int> nc(2); 00078 for (std::size_t i = 0; i < 16; i += 2) 00079 { 00080 nc[0] = coord[0] + offset[i]; 00081 nc[1] = coord[1] + offset[i+1]; 00082 if (nc[0] >= 0 && (int) nc[0] < length_ && nc[1] >= 0 00083 && (int) nc[1] < length_) 00084 neighbors.push_back(nc[0]*length_ + nc[1]); 00085 } 00086 } 00087 else if (dimension_ == 3) 00088 { 00089 static const int offset[] = { 00090 -1, 0, 0, 00091 +1, 0, 0, 00092 0, -1, 0, 00093 0, +1, 0, 00094 -1, -1, 0, 00095 -1, +1, 0, 00096 +1, -1, 0, 00097 +1, +1, 0, 00098 -1, 0, -1, 00099 +1, 0, -1, 00100 0, -1, -1, 00101 0, +1, -1, 00102 -1, -1, -1, 00103 -1, +1, -1, 00104 +1, -1, -1, 00105 +1, +1, -1, 00106 -1, 0, +1, 00107 +1, 0, +1, 00108 0, -1, +1, 00109 0, +1, +1, 00110 -1, -1, +1, 00111 -1, +1, +1, 00112 +1, -1, +1, 00113 +1, +1, +1, 00114 0, 0, -1, 00115 0, 0, +1 00116 }; 00117 std::vector<int> coord(3); 00118 regionToGridCoord(rid, coord); 00119 std::vector<int> nc(3); 00120 for (int i = 0; i < 78; i += 3) 00121 { 00122 nc[0] = coord[0] + offset[i]; 00123 nc[1] = coord[1] + offset[i+1]; 00124 nc[2] = coord[2] + offset[i+2]; 00125 if (nc[0] >= 0 && (int) nc[0] < length_ 00126 && nc[1] >= 0 && (int) nc[1] < length_ 00127 && nc[2] >= 0 && (int) nc[2] < length_) 00128 neighbors.push_back(nc[0]*length_*length_ + nc[1]*length_ + nc[2]); 00129 } 00130 } 00131 else 00132 { 00133 computeGridNeighbors (rid, neighbors); 00134 } 00135 } 00136 00137 int ompl::control::GridDecomposition::locateRegion(const base::State *s) const 00138 { 00139 std::vector<double> coord(dimension_); 00140 project(s, coord); 00141 return coordToRegion(coord); 00142 } 00143 00144 void ompl::control::GridDecomposition::sampleFromRegion(int rid, RNG &rng, std::vector<double> &coord) const 00145 { 00146 coord.resize(dimension_); 00147 const base::RealVectorBounds& regionBounds(getRegionBounds(rid)); 00148 for (int i = 0; i < dimension_; ++i) 00149 coord[i] = rng.uniformReal(regionBounds.low[i], regionBounds.high[i]); 00150 } 00151 00152 void ompl::control::GridDecomposition::computeGridNeighbors (int rid, std::vector <int> &neighbors) const 00153 { 00154 std::vector <int> candidate (dimension_, -1); 00155 std::vector <int> coord; 00156 regionToGridCoord (rid, coord); 00157 00158 computeGridNeighborsSub (coord, neighbors, 0, candidate); 00159 } 00160 00161 void ompl::control::GridDecomposition::computeGridNeighborsSub (const std::vector <int> &coord, 00162 std::vector <int> &neighbors, 00163 int dim, 00164 std::vector <int> &candidate) const 00165 { 00166 // Stopping condition for recursive method. 00167 if (dim == dimension_) 00168 { 00169 // Make sure we don't push back ourselves as a neighbor 00170 bool same = true; 00171 for (unsigned int i = 0; i < coord.size() && same; ++i) 00172 same = (coord[i] == candidate[i]); 00173 00174 if (!same) 00175 { 00176 neighbors.push_back (gridCoordToRegion (candidate)); 00177 } 00178 } 00179 else 00180 { 00181 // Check neighbor in the cell preceding this one in this dimension 00182 if (coord[dim] >= 1) 00183 { 00184 candidate[dim] = coord[dim]-1; 00185 computeGridNeighborsSub (coord, neighbors, dim+1, candidate); 00186 } 00187 00188 // Make sure to include the same coordinate, for neighbors "above", "below", "in front of", "behind", etcetera. 00189 candidate[dim] = coord[dim]; 00190 computeGridNeighborsSub (coord, neighbors, dim+1, candidate); 00191 00192 // Check neighbor in the cell after this one in this dimension 00193 if (coord[dim] +1 < length_) 00194 { 00195 candidate[dim] = coord[dim]+1; 00196 computeGridNeighborsSub (coord, neighbors, dim+1, candidate); 00197 } 00198 } 00199 } 00200 00201 void ompl::control::GridDecomposition::regionToGridCoord(int rid, std::vector<int>& coord) const 00202 { 00203 coord.resize(dimension_); 00204 for (int i = dimension_-1; i >= 0; --i) 00205 { 00206 int remainder = rid % length_; 00207 coord[i] = remainder; 00208 rid /= length_; 00209 } 00210 } 00211 00212 int ompl::control::GridDecomposition::gridCoordToRegion (const std::vector <int> &coord) const 00213 { 00214 int region = 0; 00215 for (unsigned int i = 0; i < coord.size (); i++) 00216 { 00217 // Computing length_^(dimension of coord -1) 00218 int multiplicand = 1; 00219 for (unsigned int j = 1; j < coord.size () - i; j++) 00220 multiplicand *= length_; 00221 00222 region += (coord[i] * multiplicand); 00223 } 00224 return region; 00225 } 00226 00227 int ompl::control::GridDecomposition::coordToRegion(const std::vector<double>& coord) const 00228 { 00229 int region = 0; 00230 int factor = 1; 00231 int index; 00232 for (int i = dimension_-1; i >= 0; --i) 00233 { 00234 index = (int) (length_*(coord[i]-bounds_.low[i])/(bounds_.high[i]-bounds_.low[i])); 00235 00236 // There is an edge case when the coordinate lies exactly on the upper bound where 00237 // the region index will be out of bounds. Ensure index lies within [0, length_) 00238 if (index >= length_) 00239 index = length_-1; 00240 00241 region += factor*index; 00242 factor *= length_; 00243 } 00244 return region; 00245 } 00246 00247 void ompl::control::GridDecomposition::coordToGridCoord(const std::vector<double>& coord, std::vector<int>& gridCoord) const 00248 { 00249 gridCoord.resize(dimension_); 00250 for (int i = 0; i < dimension_; ++i) 00251 { 00252 gridCoord[i] = (int) (length_*(coord[i]-bounds_.low[i])/(bounds_.high[i]-bounds_.low[i])); 00253 00254 // There is an edge case when the coordinate lies exactly on the upper bound where 00255 // the region index will be out of bounds. Ensure index lies within [0, length_) 00256 if (gridCoord[i] >= length_) 00257 gridCoord[i] = length_-1; 00258 } 00259 } 00260 00261 const ompl::base::RealVectorBounds& ompl::control::GridDecomposition::getRegionBounds(int rid) const 00262 { 00263 if (regToBounds_.count(rid) > 0) 00264 return *regToBounds_[rid].get(); 00265 ompl::base::RealVectorBounds *regionBounds = new ompl::base::RealVectorBounds(dimension_); 00266 std::vector<int> rc(dimension_); 00267 regionToGridCoord(rid, rc); 00268 for (int i = 0; i < dimension_; ++i) 00269 { 00270 const double length = (bounds_.high[i] - bounds_.low[i]) / length_; 00271 regionBounds->low[i] = bounds_.low[i] + length*rc[i]; 00272 regionBounds->high[i] = regionBounds->low[i] + length; 00273 } 00274 regToBounds_[rid] = boost::shared_ptr<ompl::base::RealVectorBounds>(regionBounds); 00275 return *regToBounds_[rid].get(); 00276 } 00277 00278 namespace 00279 { 00280 int calcNumGridCells(int len, int dim) 00281 { 00282 int numCells = 1; 00283 for (int i = 0; i < dim; ++i) 00284 numCells *= len; 00285 return numCells; 00286 } 00287 }