All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends
PathHybridization.cpp
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2011, Willow Garage, Inc.
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 *
11 * * Redistributions of source code must retain the above copyright
12 * notice, this list of conditions and the following disclaimer.
13 * * Redistributions in binary form must reproduce the above
14 * copyright notice, this list of conditions and the following
15 * disclaimer in the documentation and/or other materials provided
16 * with the distribution.
17 * * Neither the name of the Willow Garage nor the names of its
18 * contributors may be used to endorse or promote products derived
19 * from this software without specific prior written permission.
20 *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 * POSSIBILITY OF SUCH DAMAGE.
33 *********************************************************************/
34 
35 /* Author: Ioan Sucan */
36 
37 #include "ompl/geometric/PathHybridization.h"
38 #include <boost/graph/dijkstra_shortest_paths.hpp>
39 
40 namespace ompl
41 {
42  namespace magic
43  {
45  static const double GAP_COST_FRACTION = 0.05;
46  }
47 }
48 
50  si_(si), stateProperty_(boost::get(vertex_state_t(), g_))
51 {
52  root_ = boost::add_vertex(g_);
53  stateProperty_[root_] = NULL;
54  goal_ = boost::add_vertex(g_);
55  stateProperty_[goal_] = NULL;
56 }
57 
58 ompl::geometric::PathHybridization::~PathHybridization(void)
59 {
60 }
61 
63 {
64  hpath_.reset();
65  paths_.clear();
66 
67  g_.clear();
68  root_ = boost::add_vertex(g_);
69  stateProperty_[root_] = NULL;
70  goal_ = boost::add_vertex(g_);
71  stateProperty_[goal_] = NULL;
72 }
73 
74 void ompl::geometric::PathHybridization::print(std::ostream &out) const
75 {
76  out << "Path hybridization is aware of " << paths_.size() << " paths" << std::endl;
77  int i = 1;
78  for (std::set<PathInfo>::const_iterator it = paths_.begin() ; it != paths_.end() ; ++it, ++i)
79  out << " path " << i << " of length " << it->length_ << std::endl;
80  if (hpath_)
81  out << "Hybridized path of length " << hpath_->length() << std::endl;
82 }
83 
85 {
86  boost::vector_property_map<Vertex> prev(boost::num_vertices(g_));
87  boost::dijkstra_shortest_paths(g_, root_, boost::predecessor_map(prev));
88  if (prev[goal_] != goal_)
89  {
90  PathGeometric *h = new PathGeometric(si_);
91  for (Vertex pos = prev[goal_]; prev[pos] != pos; pos = prev[pos])
92  h->append(stateProperty_[pos]);
93  h->reverse();
94  hpath_.reset(h);
95  }
96 }
97 
99 {
100  return hpath_;
101 }
102 
103 unsigned int ompl::geometric::PathHybridization::recordPath(const base::PathPtr &pp, bool matchAcrossGaps)
104 {
105  PathGeometric *p = dynamic_cast<PathGeometric*>(pp.get());
106  if (!p)
107  {
108  OMPL_ERROR("Path hybridization only works for geometric paths");
109  return 0;
110  }
111 
112  if (p->getSpaceInformation() != si_)
113  {
114  OMPL_ERROR("Paths for hybridization must be from the same space information");
115  return 0;
116  }
117 
118  // skip empty paths
119  if (p->getStateCount() == 0)
120  return 0;
121 
122  PathInfo pi(pp);
123 
124  // if this path was previously included in the hybridization, skip it
125  if (paths_.find(pi) != paths_.end())
126  return 0;
127 
128  // the number of connection attempts
129  unsigned int nattempts = 0;
130 
131  // start from virtual root
132  Vertex v0 = boost::add_vertex(g_);
133  stateProperty_[v0] = pi.states_[0];
134  pi.vertices_.push_back(v0);
135 
136  // add all the vertices of the path, and the edges between them, to the HGraph
137  // also compute the path length for future use (just for computational savings)
138  const HGraph::edge_property_type prop0(0.0);
139  boost::add_edge(root_, v0, prop0, g_);
140  double length = 0.0;
141  for (std::size_t j = 1 ; j < pi.states_.size() ; ++j)
142  {
143  Vertex v1 = boost::add_vertex(g_);
144  stateProperty_[v1] = pi.states_[j];
145  double weight = si_->distance(pi.states_[j-1], pi.states_[j]);
146  const HGraph::edge_property_type properties(weight);
147  boost::add_edge(v0, v1, properties, g_);
148  length += weight;
149  pi.vertices_.push_back(v1);
150  v0 = v1;
151  }
152 
153  // connect to virtual goal
154  boost::add_edge(v0, goal_, prop0, g_);
155  pi.length_ = length;
156 
157  // find matches with previously added paths
158  for (std::set<PathInfo>::const_iterator it = paths_.begin() ; it != paths_.end() ; ++it)
159  {
160  const PathGeometric *q = static_cast<const PathGeometric*>(it->path_.get());
161  std::vector<int> indexP, indexQ;
162  matchPaths(*p, *q, (pi.length_ + it->length_) / (2.0 / magic::GAP_COST_FRACTION), indexP, indexQ);
163 
164  if (matchAcrossGaps)
165  {
166  int lastP = -1;
167  int lastQ = -1;
168  int gapStartP = -1;
169  int gapStartQ = -1;
170  bool gapP = false;
171  bool gapQ = false;
172  for (std::size_t i = 0 ; i < indexP.size() ; ++i)
173  {
174  // a gap is found in p
175  if (indexP[i] < 0)
176  {
177  // remember this as the beginning of the gap, if needed
178  if (!gapP)
179  gapStartP = i;
180  // mark the fact we are now in a gap on p
181  gapP = true;
182  }
183  else
184  {
185  // check if a gap just ended;
186  // if it did, try to match the endpoint with the elements in q
187  if (gapP)
188  for (std::size_t j = gapStartP ; j < i ; ++j)
189  {
190  attemptNewEdge(pi, *it, indexP[i], indexQ[j]);
191  ++nattempts;
192  }
193  // remember the last non-negative index in p
194  lastP = i;
195  gapP = false;
196  }
197  if (indexQ[i] < 0)
198  {
199  if (!gapQ)
200  gapStartQ = i;
201  gapQ = true;
202  }
203  else
204  {
205  if (gapQ)
206  for (std::size_t j = gapStartQ ; j < i ; ++j)
207  {
208  attemptNewEdge(pi, *it, indexP[j], indexQ[i]);
209  ++nattempts;
210  }
211  lastQ = i;
212  gapQ = false;
213  }
214 
215  // try to match corresponding index values and gep beginnings
216  if (lastP >= 0 && lastQ >= 0)
217  {
218  attemptNewEdge(pi, *it, indexP[lastP], indexQ[lastQ]);
219  ++nattempts;
220  }
221  }
222  }
223  else
224  {
225  // attempt new edge only when states align
226  for (std::size_t i = 0 ; i < indexP.size() ; ++i)
227  if (indexP[i] >= 0 && indexQ[i] >= 0)
228  {
229  attemptNewEdge(pi, *it, indexP[i], indexQ[i]);
230  ++nattempts;
231  }
232  }
233  }
234 
235  // remember this path is part of the hybridization
236  paths_.insert(pi);
237  return nattempts;
238 }
239 
240 void ompl::geometric::PathHybridization::attemptNewEdge(const PathInfo &p, const PathInfo &q, int indexP, int indexQ)
241 {
242  if (si_->checkMotion(p.states_[indexP], q.states_[indexQ]))
243  {
244  double weight = si_->distance(p.states_[indexP], q.states_[indexQ]);
245  const HGraph::edge_property_type properties(weight);
246  boost::add_edge(p.vertices_[indexP], q.vertices_[indexQ], properties, g_);
247  }
248 }
249 
251 {
252  return paths_.size();
253 }
254 
256  std::vector<int> &indexP, std::vector<int> &indexQ) const
257 {
258  std::vector<std::vector<double> > C(p.getStateCount());
259  std::vector<std::vector<char> > T(p.getStateCount());
260 
261  for (std::size_t i = 0 ; i < p.getStateCount() ; ++i)
262  {
263  C[i].resize(q.getStateCount(), 0.0);
264  T[i].resize(q.getStateCount(), '\0');
265  for (std::size_t j = 0 ; j < q.getStateCount() ; ++j)
266  {
267  // as far as I can tell, there is a bug in the algorithm as presented in the paper
268  // so I am doing things slightly differently ...
269  double match = si_->distance(p.getState(i), q.getState(j)) + ((i > 0 && j > 0) ? C[i - 1][j - 1] : 0.0);
270  double up = gapCost + (i > 0 ? C[i - 1][j] : 0.0);
271  double left = gapCost + (j > 0 ? C[i][j - 1] : 0.0);
272  if (match <= up && match <= left)
273  {
274  C[i][j] = match;
275  T[i][j] = 'm';
276  }
277  else
278  if (up <= match && up <= left)
279  {
280  C[i][j] = up;
281  T[i][j] = 'u';
282  }
283  else
284  {
285  C[i][j] = left;
286  T[i][j] = 'l';
287  }
288  }
289  }
290  // construct the sequences with gaps (only index positions)
291  int m = p.getStateCount() - 1;
292  int n = q.getStateCount() - 1;
293 
294  indexP.clear();
295  indexQ.clear();
296  indexP.reserve(std::max(n,m));
297  indexQ.reserve(indexP.size());
298 
299  while (n >= 0 && m >= 0)
300  {
301  if (T[m][n] == 'm')
302  {
303  indexP.push_back(m);
304  indexQ.push_back(n);
305  --m; --n;
306  }
307  else
308  if (T[m][n] == 'u')
309  {
310  indexP.push_back(m);
311  indexQ.push_back(-1);
312  --m;
313  }
314  else
315  {
316  indexP.push_back(-1);
317  indexQ.push_back(n);
318  --n;
319  }
320  }
321  while (n >= 0)
322  {
323  indexP.push_back(-1);
324  indexQ.push_back(n);
325  --n;
326  }
327  while (m >= 0)
328  {
329  indexP.push_back(m);
330  indexQ.push_back(-1);
331  --m;
332  }
333 }
static const double GAP_COST_FRACTION
The fraction of the path length to consider as gap cost when aligning paths to be hybridized...
const SpaceInformationPtr & getSpaceInformation(void) const
Get the space information associated to this class.
Definition: Path.h:82
void clear(void)
Clear all the stored paths.
void append(const base::State *state)
Append state to the end of this path. The memory for state is copied.
base::State * getState(unsigned int index)
Get the state located at index along the path.
void print(std::ostream &out=std::cout) const
Print information about the computed path.
PathHybridization(const base::SpaceInformationPtr &si)
The constructor needs to know about the space information of the paths it will operate on...
std::size_t getStateCount(void) const
Get the number of states (way-points) that make up this path.
const base::PathPtr & getHybridPath(void) const
Get the currently computed hybrid path. computeHybridPath() needs to have been called before...
void computeHybridPath(void)
Run Dijkstra's algorithm to find out the shortest path among the mixed ones.
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Definition: Console.h:64
void matchPaths(const geometric::PathGeometric &p, const geometric::PathGeometric &q, double gapCost, std::vector< int > &indexP, std::vector< int > &indexQ) const
Given two geometric paths p and q, compute the alignment of the paths using dynamic programming in an...
void reverse(void)
Reverse the path.
A boost shared pointer wrapper for ompl::base::SpaceInformation.
unsigned int recordPath(const base::PathPtr &pp, bool matchAcrossGaps)
Add a path to the hybridization. If matchAcrossGaps is true, more possible edge connections are evalu...
std::size_t pathCount(void) const
Get the number of paths that are currently considered as part of the hybridization.
Definition of a geometric path.
Definition: PathGeometric.h:55
A boost shared pointer wrapper for ompl::base::Path.