00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
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 }