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 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines