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/geometric/PathHybridization.h"
00038 #include <boost/graph/dijkstra_shortest_paths.hpp>
00039
00040 namespace ompl
00041 {
00042 namespace magic
00043 {
00045 static const double GAP_COST_FRACTION = 0.05;
00046 }
00047 }
00048
00049 ompl::geometric::PathHybridization::PathHybridization(const base::SpaceInformationPtr &si) :
00050 si_(si), stateProperty_(boost::get(vertex_state_t(), g_))
00051 {
00052 root_ = boost::add_vertex(g_);
00053 stateProperty_[root_] = NULL;
00054 goal_ = boost::add_vertex(g_);
00055 stateProperty_[goal_] = NULL;
00056 }
00057
00058 ompl::geometric::PathHybridization::~PathHybridization(void)
00059 {
00060 }
00061
00062 void ompl::geometric::PathHybridization::clear(void)
00063 {
00064 hpath_.reset();
00065 paths_.clear();
00066
00067 g_.clear();
00068 root_ = boost::add_vertex(g_);
00069 stateProperty_[root_] = NULL;
00070 goal_ = boost::add_vertex(g_);
00071 stateProperty_[goal_] = NULL;
00072 }
00073
00074 void ompl::geometric::PathHybridization::print(std::ostream &out) const
00075 {
00076 out << "Path hybridization is aware of " << paths_.size() << " paths" << std::endl;
00077 int i = 1;
00078 for (std::set<PathInfo>::const_iterator it = paths_.begin() ; it != paths_.end() ; ++it, ++i)
00079 out << " path " << i << " of length " << it->length_ << std::endl;
00080 if (hpath_)
00081 out << "Hybridized path of length " << hpath_->length() << std::endl;
00082 }
00083
00084 void ompl::geometric::PathHybridization::computeHybridPath(void)
00085 {
00086 boost::vector_property_map<Vertex> prev(boost::num_vertices(g_));
00087 boost::dijkstra_shortest_paths(g_, root_, boost::predecessor_map(prev));
00088 if (prev[goal_] != goal_)
00089 {
00090 PathGeometric *h = new PathGeometric(si_);
00091 for (Vertex pos = prev[goal_]; prev[pos] != pos; pos = prev[pos])
00092 h->append(stateProperty_[pos]);
00093 h->reverse();
00094 hpath_.reset(h);
00095 }
00096 }
00097
00098 const ompl::base::PathPtr& ompl::geometric::PathHybridization::getHybridPath(void) const
00099 {
00100 return hpath_;
00101 }
00102
00103 unsigned int ompl::geometric::PathHybridization::recordPath(const base::PathPtr &pp, bool matchAcrossGaps)
00104 {
00105 PathGeometric *p = dynamic_cast<PathGeometric*>(pp.get());
00106 if (!p)
00107 {
00108 msg_.error("Path hybridization only works for geometric paths");
00109 return 0;
00110 }
00111
00112 if (p->getSpaceInformation() != si_)
00113 {
00114 msg_.error("Paths for hybridization must be from the same space information");
00115 return 0;
00116 }
00117
00118
00119 if (p->getStateCount() == 0)
00120 return 0;
00121
00122 PathInfo pi(pp);
00123
00124
00125 if (paths_.find(pi) != paths_.end())
00126 return 0;
00127
00128
00129 unsigned int nattempts = 0;
00130
00131
00132 Vertex v0 = boost::add_vertex(g_);
00133 stateProperty_[v0] = pi.states_[0];
00134 pi.vertices_.push_back(v0);
00135
00136
00137
00138 const HGraph::edge_property_type prop0(0.0);
00139 boost::add_edge(root_, v0, prop0, g_);
00140 double length = 0.0;
00141 for (std::size_t j = 1 ; j < pi.states_.size() ; ++j)
00142 {
00143 Vertex v1 = boost::add_vertex(g_);
00144 stateProperty_[v1] = pi.states_[j];
00145 double weight = si_->distance(pi.states_[j-1], pi.states_[j]);
00146 const HGraph::edge_property_type properties(weight);
00147 boost::add_edge(v0, v1, properties, g_);
00148 length += weight;
00149 pi.vertices_.push_back(v1);
00150 v0 = v1;
00151 }
00152
00153
00154 boost::add_edge(v0, goal_, prop0, g_);
00155 pi.length_ = length;
00156
00157
00158 for (std::set<PathInfo>::const_iterator it = paths_.begin() ; it != paths_.end() ; ++it)
00159 {
00160 const PathGeometric *q = static_cast<const PathGeometric*>(it->path_.get());
00161 std::vector<int> indexP, indexQ;
00162 matchPaths(*p, *q, (pi.length_ + it->length_) / (2.0 / magic::GAP_COST_FRACTION), indexP, indexQ);
00163
00164 if (matchAcrossGaps)
00165 {
00166 int lastP = -1;
00167 int lastQ = -1;
00168 int gapStartP = -1;
00169 int gapStartQ = -1;
00170 bool gapP = false;
00171 bool gapQ = false;
00172 for (std::size_t i = 0 ; i < indexP.size() ; ++i)
00173 {
00174
00175 if (indexP[i] < 0)
00176 {
00177
00178 if (!gapP)
00179 gapStartP = i;
00180
00181 gapP = true;
00182 }
00183 else
00184 {
00185
00186
00187 if (gapP)
00188 for (std::size_t j = gapStartP ; j < i ; ++j)
00189 {
00190 attemptNewEdge(pi, *it, indexP[i], indexQ[j]);
00191 ++nattempts;
00192 }
00193
00194 lastP = i;
00195 gapP = false;
00196 }
00197 if (indexQ[i] < 0)
00198 {
00199 if (!gapQ)
00200 gapStartQ = i;
00201 gapQ = true;
00202 }
00203 else
00204 {
00205 if (gapQ)
00206 for (std::size_t j = gapStartQ ; j < i ; ++j)
00207 {
00208 attemptNewEdge(pi, *it, indexP[j], indexQ[i]);
00209 ++nattempts;
00210 }
00211 lastQ = i;
00212 gapQ = false;
00213 }
00214
00215
00216 if (lastP >= 0 && lastQ >= 0)
00217 {
00218 attemptNewEdge(pi, *it, indexP[lastP], indexQ[lastQ]);
00219 ++nattempts;
00220 }
00221 }
00222 }
00223 else
00224 {
00225
00226 for (std::size_t i = 0 ; i < indexP.size() ; ++i)
00227 if (indexP[i] >= 0 && indexQ[i] >= 0)
00228 {
00229 attemptNewEdge(pi, *it, indexP[i], indexQ[i]);
00230 ++nattempts;
00231 }
00232 }
00233 }
00234
00235
00236 paths_.insert(pi);
00237 return nattempts;
00238 }
00239
00240 void ompl::geometric::PathHybridization::attemptNewEdge(const PathInfo &p, const PathInfo &q, int indexP, int indexQ)
00241 {
00242 if (si_->checkMotion(p.states_[indexP], q.states_[indexQ]))
00243 {
00244 double weight = si_->distance(p.states_[indexP], q.states_[indexQ]);
00245 const HGraph::edge_property_type properties(weight);
00246 boost::add_edge(p.vertices_[indexP], q.vertices_[indexQ], properties, g_);
00247 }
00248 }
00249
00250 std::size_t ompl::geometric::PathHybridization::pathCount(void) const
00251 {
00252 return paths_.size();
00253 }
00254
00255 void ompl::geometric::PathHybridization::matchPaths(const PathGeometric &p, const PathGeometric &q, double gapCost,
00256 std::vector<int> &indexP, std::vector<int> &indexQ) const
00257 {
00258 std::vector<std::vector<double> > C(p.getStateCount());
00259 std::vector<std::vector<char> > T(p.getStateCount());
00260
00261 for (std::size_t i = 0 ; i < p.getStateCount() ; ++i)
00262 {
00263 C[i].resize(q.getStateCount(), 0.0);
00264 T[i].resize(q.getStateCount(), '\0');
00265 for (std::size_t j = 0 ; j < q.getStateCount() ; ++j)
00266 {
00267
00268
00269 double match = si_->distance(p.getState(i), q.getState(j)) + ((i > 0 && j > 0) ? C[i - 1][j - 1] : 0.0);
00270 double up = gapCost + (i > 0 ? C[i - 1][j] : 0.0);
00271 double left = gapCost + (j > 0 ? C[i][j - 1] : 0.0);
00272 if (match <= up && match <= left)
00273 {
00274 C[i][j] = match;
00275 T[i][j] = 'm';
00276 }
00277 else
00278 if (up <= match && up <= left)
00279 {
00280 C[i][j] = up;
00281 T[i][j] = 'u';
00282 }
00283 else
00284 {
00285 C[i][j] = left;
00286 T[i][j] = 'l';
00287 }
00288 }
00289 }
00290
00291 int m = p.getStateCount() - 1;
00292 int n = q.getStateCount() - 1;
00293
00294 indexP.clear();
00295 indexQ.clear();
00296 indexP.reserve(std::max(n,m));
00297 indexQ.reserve(indexP.size());
00298
00299 while (n >= 0 && m >= 0)
00300 {
00301 if (T[m][n] == 'm')
00302 {
00303 indexP.push_back(m);
00304 indexQ.push_back(n);
00305 --m; --n;
00306 }
00307 else
00308 if (T[m][n] == 'u')
00309 {
00310 indexP.push_back(m);
00311 indexQ.push_back(-1);
00312 --m;
00313 }
00314 else
00315 {
00316 indexP.push_back(-1);
00317 indexQ.push_back(n);
00318 --n;
00319 }
00320 }
00321 while (n >= 0)
00322 {
00323 indexP.push_back(-1);
00324 indexQ.push_back(n);
00325 --n;
00326 }
00327 while (m >= 0)
00328 {
00329 indexP.push_back(m);
00330 indexQ.push_back(-1);
00331 --m;
00332 }
00333 }