All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator
StateSpaceCollection.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: Ioan Sucan */
00036 
00037 #include "ompl/tools/spaces/StateSpaceCollection.h"
00038 #include "ompl/util/Exception.h"
00039 
00040 const std::string& ompl::StateSpaceCollection::getName(void) const
00041 {
00042     return name_;
00043 }
00044 
00045 void ompl::StateSpaceCollection::setName(const std::string &name)
00046 {
00047     name_ = name;
00048 }
00049 
00050 void ompl::StateSpaceCollection::collect(const std::vector<base::StateSpacePtr> &spaces)
00051 {
00052     for (std::size_t i = 0 ; i < spaces.size() ; ++i)
00053         collect(spaces[i]);
00054 }
00055 
00056 void ompl::StateSpaceCollection::collect(const base::StateSpacePtr &space)
00057 {
00058     if (haveSpace(space))
00059         return;
00060 
00061     msg_.debug("Became aware of space '%s'", space->getName().c_str());
00062 
00063     spaces_.push_back(space);
00064     if (space->isCompound())
00065         collect(space->as<base::CompoundStateSpace>()->getSubSpaces());
00066 }
00067 
00068 const ompl::base::StateSpacePtr& ompl::StateSpaceCollection::getSpace(const std::string &name) const
00069 {
00070     for (std::size_t i = 0 ; i < spaces_.size() ; ++i)
00071         if (spaces_[i]->getName() == name)
00072             return spaces_[i];
00073     throw Exception("Collection does not include state space '" + name + "'");
00074 }
00075 
00076 bool ompl::StateSpaceCollection::haveSpace(const std::string &name) const
00077 {
00078     for (std::size_t i = 0 ; i < spaces_.size() ; ++i)
00079         if (spaces_[i]->getName() == name)
00080             return true;
00081     return false;
00082 }
00083 
00084 bool ompl::StateSpaceCollection::haveSpace(const base::StateSpacePtr &space) const
00085 {
00086     return haveSpace(space->getName());
00087 }
00088 
00089 const ompl::base::StateSpacePtr& ompl::StateSpaceCollection::combine(const std::vector<base::StateSpacePtr> &components)
00090 {
00091     std::vector<double> weights(components.size(), 1.0);
00092     return combine(components, weights);
00093 }
00094 
00095 const ompl::base::StateSpacePtr& ompl::StateSpaceCollection::combine(const std::vector<base::StateSpacePtr> &components, const std::vector<bool> &mask)
00096 {
00097     std::vector<double> weights(components.size(), 1.0);
00098     return combine(components, mask, weights);
00099 }
00100 
00101 const ompl::base::StateSpacePtr& ompl::StateSpaceCollection::combine(const std::vector<base::StateSpacePtr> &components,
00102                                                                      const std::vector<bool> &mask, const std::vector<double> &weights)
00103 {
00104     if (components.size() != mask.size())
00105         throw Exception("Number of components not equal to number of mask bits");
00106     if (components.size() != weights.size())
00107         throw Exception("Number of components not equal to number of weights");
00108 
00109     std::vector<base::StateSpacePtr> realComp;
00110     std::vector<double>              realW;
00111     for (std::size_t i = 0 ; i < components.size() ; ++i)
00112         if (mask[i])
00113         {
00114             realComp.push_back(components[i]);
00115             realW.push_back(weights[i]);
00116         }
00117     return combine(realComp, realW);
00118 }
00119 
00120 const ompl::base::StateSpacePtr& ompl::StateSpaceCollection::combine(const std::vector<base::StateSpacePtr> &components, const std::vector<double> &weights)
00121 {
00122     if (components.empty())
00123         throw Exception("No spaces to combine");
00124     if (components.size() != weights.size())
00125         throw Exception("Number of components not equal to number of weights");
00126 
00127     collect(components);
00128 
00129     if (components.size() == 1)
00130         return getSpace(components[0]->getName());
00131 
00132     for (std::size_t i = 0 ; i < spaces_.size() ; ++i)
00133         if (spaces_[i]->isCompound())
00134             if (spaces_[i]->as<base::CompoundStateSpace>()->getSubSpaces() == components &&
00135                 spaces_[i]->as<base::CompoundStateSpace>()->getSubSpaceWeights() == weights)
00136                 return spaces_[i];
00137 
00138     base::StateSpacePtr space(new base::CompoundStateSpace(components, weights));
00139     std::string name = components[0]->getName();
00140     for (std::size_t i = 1 ; i < components.size() ; ++i)
00141         name += join_ + components[i]->getName();
00142     name = prefix_ + name + suffix_;
00143     if (!name_.empty())
00144         name = name_ + ":" + name;
00145     space->setName(name);
00146 
00147     msg_.debug("Created new state space: '%s'", space->getName().c_str());
00148 
00149     spaces_.push_back(space);
00150     return spaces_.back();
00151 }
00152 
00153 void ompl::StateSpaceCollection::setAutomaticNames(const std::string &join, const std::string &prefix, const std::string &suffix)
00154 {
00155     join_ = join;
00156     prefix_ = prefix;
00157     suffix_ = suffix;
00158 }
00159 
00161 namespace ompl
00162 {
00163     static void generateBits(std::vector< std::vector<bool> > &options, std::vector<bool> &bits, unsigned int start)
00164     {
00165         if (start >= bits.size())
00166         {
00167             for (unsigned int i = 0 ; i < bits.size() ; ++i)
00168                 if (bits[i])
00169                 {
00170                     options.push_back(bits);
00171                     break;
00172                 }
00173         }
00174         else
00175         {
00176             bits[start] = false;
00177             generateBits(options, bits, start + 1);
00178             bits[start] = true;
00179             generateBits(options, bits, start + 1);
00180         }
00181     }
00182 
00183     static std::vector< std::vector<bool> > generateCombinations(unsigned int size)
00184     {
00185         std::vector< std::vector<bool> > options;
00186         std::vector<bool> bits(size);
00187         generateBits(options, bits, 0);
00188         return options;
00189     }
00190 }
00192 
00193 std::vector<ompl::base::StateSpacePtr> ompl::StateSpaceCollection::allCombinations(const std::vector<base::StateSpacePtr> &components, const std::vector<double> &weights)
00194 {
00195     std::vector<base::StateSpacePtr> opt;
00196     const std::vector< std::vector<bool> > &comb = generateCombinations(components.size());
00197     for (std::size_t i = 0 ; i < comb.size() ; ++i)
00198         opt.push_back(combine(components, comb[i], weights));
00199     return opt;
00200 }
00201 
00202 std::vector<ompl::base::StateSpacePtr> ompl::StateSpaceCollection::allCombinations(const std::vector<base::StateSpacePtr> &components)
00203 {
00204     std::vector<double> weights(components.size(), 1.0);
00205     return allCombinations(components, weights);
00206 }