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 "ompl/base/objectives/PathLengthOptimizationObjective.h"
39 #include <boost/graph/dijkstra_shortest_paths.hpp>
40 #include <utility>
41 #include <Eigen/Core>
42 
43 namespace ompl
44 {
45  namespace magic
46  {
48  static const double GAP_COST_FRACTION = 0.05;
49  } // namespace magic
50 } // namespace ompl
51 
53  : si_(std::move(si))
54  , obj_(new base::PathLengthOptimizationObjective(si_))
55  , stateProperty_(boost::get(vertex_state_t(), g_))
56  , name_("PathHybridization")
57 {
58  root_ = boost::add_vertex(g_);
59  stateProperty_[root_] = nullptr;
60  goal_ = boost::add_vertex(g_);
61  stateProperty_[goal_] = nullptr;
62 }
63 
65  : si_(std::move(si))
66  , obj_(std::move(obj))
67  , stateProperty_(boost::get(vertex_state_t(), g_))
68  , name_("PathHybridization")
69 {
70  std::stringstream ss;
71  ss << "PathHybridization over " << obj_->getDescription() << " cost";
72  name_ = ss.str();
73  root_ = boost::add_vertex(g_);
74  stateProperty_[root_] = nullptr;
75  goal_ = boost::add_vertex(g_);
76  stateProperty_[goal_] = nullptr;
77 }
78 
79 ompl::geometric::PathHybridization::~PathHybridization() = default;
80 
82 {
83  hpath_.reset();
84  paths_.clear();
85 
86  g_.clear();
87  root_ = boost::add_vertex(g_);
88  stateProperty_[root_] = nullptr;
89  goal_ = boost::add_vertex(g_);
90  stateProperty_[goal_] = nullptr;
91 }
92 
93 void ompl::geometric::PathHybridization::print(std::ostream &out) const
94 {
95  out << "Path hybridization is aware of " << paths_.size() << " paths" << std::endl;
96  int i = 1;
97  for (auto it = paths_.begin(); it != paths_.end(); ++it, ++i)
98  out << " path " << i << " of cost " << it->cost_.value() << std::endl;
99  if (hpath_)
100  out << "Hybridized path of cost " << hpath_->cost(obj_) << std::endl;
101 }
102 
104 {
105  return name_;
106 }
107 
109 {
110  boost::vector_property_map<Vertex> prev(boost::num_vertices(g_));
111  boost::dijkstra_shortest_paths(
112  g_, root_,
113  boost::predecessor_map(prev)
114  .distance_compare([this](base::Cost c1, base::Cost c2) { return obj_->isCostBetterThan(c1, c2); })
115  .distance_combine([this](base::Cost c1, base::Cost c2) { return obj_->combineCosts(c1, c2); })
116  .distance_inf(obj_->infiniteCost())
117  .distance_zero(obj_->identityCost()));
118  if (prev[goal_] != goal_)
119  {
120  auto h(std::make_shared<PathGeometric>(si_));
121  for (Vertex pos = prev[goal_]; prev[pos] != pos; pos = prev[pos])
122  h->append(stateProperty_[pos]);
123  h->reverse();
124  hpath_ = h;
125  }
126  else
127  {
128  OMPL_WARN("No path to goal was found");
129  }
130 }
131 
133 {
134  return hpath_;
135 }
136 
137 unsigned int ompl::geometric::PathHybridization::recordPath(const base::PathPtr &pp, bool matchAcrossGaps)
138 {
139  auto *p = dynamic_cast<PathGeometric *>(pp.get());
140  if (!p)
141  {
142  OMPL_ERROR("Path hybridization only works for geometric paths");
143  return 0;
144  }
145 
146  if (p->getSpaceInformation() != si_)
147  {
148  OMPL_ERROR("Paths for hybridization must be from the same space information");
149  return 0;
150  }
151 
152  // skip empty paths
153  if (p->getStateCount() == 0)
154  return 0;
155 
156  PathInfo pi(pp);
157 
158  // if this path was previously included in the hybridization, skip it
159  if (paths_.find(pi) != paths_.end())
160  return 0;
161 
162  // the number of connection attempts
163  unsigned int nattempts = 0;
164 
165  // start from virtual root
166  Vertex v0 = boost::add_vertex(g_);
167  stateProperty_[v0] = pi.states_[0];
168  pi.vertices_.push_back(v0);
169 
170  // add all the vertices of the path, and the edges between them, to the HGraph
171  // also compute the path cost for future use (just for computational savings)
172  const HGraph::edge_property_type prop0(obj_->identityCost());
173  boost::add_edge(root_, v0, prop0, g_);
174  base::Cost cost = obj_->identityCost();
175  for (std::size_t j = 1; j < pi.states_.size(); ++j)
176  {
177  Vertex v1 = boost::add_vertex(g_);
178  stateProperty_[v1] = pi.states_[j];
179  base::Cost weight = obj_->motionCost(pi.states_[j - 1], pi.states_[j]);
180  const HGraph::edge_property_type properties(weight);
181  boost::add_edge(v0, v1, properties, g_);
182  cost = obj_->combineCosts(cost, weight);
183  pi.vertices_.push_back(v1);
184  v0 = v1;
185  }
186 
187  // connect to virtual goal
188  boost::add_edge(v0, goal_, prop0, g_);
189  pi.cost_ = cost;
190 
191  // find matches with previously added paths
192  for (const auto &path : paths_)
193  {
194  const auto *q = static_cast<const PathGeometric *>(path.path_.get());
195  std::vector<int> indexP, indexQ;
196  matchPaths(*p, *q, obj_->combineCosts(pi.cost_, path.cost_).value() / (2.0 / magic::GAP_COST_FRACTION), indexP,
197  indexQ);
198 
199  if (matchAcrossGaps)
200  {
201  int lastP = -1;
202  int lastQ = -1;
203  int gapStartP = -1;
204  int gapStartQ = -1;
205  bool gapP = false;
206  bool gapQ = false;
207  for (std::size_t i = 0; i < indexP.size(); ++i)
208  {
209  // a gap is found in p
210  if (indexP[i] < 0)
211  {
212  // remember this as the beginning of the gap, if needed
213  if (!gapP)
214  gapStartP = i;
215  // mark the fact we are now in a gap on p
216  gapP = true;
217  }
218  else
219  {
220  // check if a gap just ended;
221  // if it did, try to match the endpoint with the elements in q
222  if (gapP)
223  for (std::size_t j = gapStartP; j < i; ++j)
224  {
225  attemptNewEdge(pi, path, indexP[i], indexQ[j]);
226  ++nattempts;
227  }
228  // remember the last non-negative index in p
229  lastP = i;
230  gapP = false;
231  }
232  if (indexQ[i] < 0)
233  {
234  if (!gapQ)
235  gapStartQ = i;
236  gapQ = true;
237  }
238  else
239  {
240  if (gapQ)
241  for (std::size_t j = gapStartQ; j < i; ++j)
242  {
243  attemptNewEdge(pi, path, indexP[j], indexQ[i]);
244  ++nattempts;
245  }
246  lastQ = i;
247  gapQ = false;
248  }
249 
250  // try to match corresponding index values and gep beginnings
251  if (lastP >= 0 && lastQ >= 0)
252  {
253  attemptNewEdge(pi, path, indexP[lastP], indexQ[lastQ]);
254  ++nattempts;
255  }
256  }
257  }
258  else
259  {
260  // attempt new edge only when states align
261  for (std::size_t i = 0; i < indexP.size(); ++i)
262  if (indexP[i] >= 0 && indexQ[i] >= 0)
263  {
264  attemptNewEdge(pi, path, indexP[i], indexQ[i]);
265  ++nattempts;
266  }
267  }
268  }
269 
270  // remember this path is part of the hybridization
271  paths_.insert(pi);
272  return nattempts;
273 }
274 
275 void ompl::geometric::PathHybridization::attemptNewEdge(const PathInfo &p, const PathInfo &q, int indexP, int indexQ)
276 {
277  if (si_->checkMotion(p.states_[indexP], q.states_[indexQ]))
278  {
279  base::Cost weight = obj_->motionCost(p.states_[indexP], q.states_[indexQ]);
280  const HGraph::edge_property_type properties(weight);
281  boost::add_edge(p.vertices_[indexP], q.vertices_[indexQ], properties, g_);
282  }
283 }
284 
286 {
287  return paths_.size();
288 }
289 
291  std::vector<int> &indexP, std::vector<int> &indexQ) const
292 {
293  using CostMatrix = Eigen::Matrix<base::Cost, Eigen::Dynamic, Eigen::Dynamic>;
294  using TMatrix = Eigen::Matrix<char, Eigen::Dynamic, Eigen::Dynamic>;
295  CostMatrix C = CostMatrix::Constant(p.getStateCount(), q.getStateCount(), obj_->identityCost());
296  TMatrix T = TMatrix::Constant(p.getStateCount(), q.getStateCount(), '\0');
297 
298  base::Cost gapCost(gapValue);
299  for (std::size_t i = 0; i < p.getStateCount(); ++i)
300  {
301  for (std::size_t j = 0; j < q.getStateCount(); ++j)
302  {
303  // as far as I can tell, there is a bug in the algorithm as presented in the paper
304  // so I am doing things slightly differently ...
305  base::Cost match = obj_->combineCosts(obj_->motionCost(p.getState(i), q.getState(j)),
306  ((i > 0 && j > 0) ? C(i - 1, j - 1) : obj_->identityCost()));
307  base::Cost up = obj_->combineCosts(gapCost, (i > 0 ? C(i - 1, j) : obj_->identityCost()));
308  base::Cost left = obj_->combineCosts(gapCost, (j > 0 ? C(i, j - 1) : obj_->identityCost()));
309  if (!(obj_->isCostBetterThan(up, match) || obj_->isCostBetterThan(left, match)))
310  {
311  C(i, j) = match;
312  T(i, j) = 'm';
313  }
314  else if (!(obj_->isCostBetterThan(match, up) || obj_->isCostBetterThan(left, up)))
315  {
316  C(i, j) = up;
317  T(i, j) = 'u';
318  }
319  else
320  {
321  C(i, j) = left;
322  T(i, j) = 'l';
323  }
324  }
325  }
326  // construct the sequences with gaps (only index positions)
327  int m = p.getStateCount() - 1;
328  int n = q.getStateCount() - 1;
329 
330  indexP.clear();
331  indexQ.clear();
332  indexP.reserve(std::max(n, m));
333  indexQ.reserve(indexP.size());
334 
335  while (n >= 0 && m >= 0)
336  {
337  if (T(m, n) == 'm')
338  {
339  indexP.push_back(m);
340  indexQ.push_back(n);
341  --m;
342  --n;
343  }
344  else if (T(m, n) == 'u')
345  {
346  indexP.push_back(m);
347  indexQ.push_back(-1);
348  --m;
349  }
350  else
351  {
352  indexP.push_back(-1);
353  indexQ.push_back(n);
354  --n;
355  }
356  }
357  while (n >= 0)
358  {
359  indexP.push_back(-1);
360  indexQ.push_back(n);
361  --n;
362  }
363  while (m >= 0)
364  {
365  indexP.push_back(m);
366  indexQ.push_back(-1);
367  --m;
368  }
369 }
static const double GAP_COST_FRACTION
The fraction of the path length to consider as gap cost when aligning paths to be hybridized...
void clear()
Clear all the stored paths.
const base::PathPtr & getHybridPath() const
Get the currently computed hybrid path. computeHybridPath() needs to have been called before...
std::size_t getStateCount() const
Get the number of states (way-points) that make up this path.
STL namespace.
const std::string & getName() const
Get the name of the algorithm.
void computeHybridPath()
Run Dijkstra&#39;s algorithm to find out the lowest-cost path among the mixed ones.
base::State * getState(unsigned int index)
Get the state located at index along the path.
Main namespace. Contains everything in this library.
Definition: AppBase.h:21
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Definition: Console.h:64
A shared pointer wrapper for ompl::base::SpaceInformation.
#define OMPL_WARN(fmt,...)
Log a formatted warning string.
Definition: Console.h:66
void print(std::ostream &out=std::cout) const
Print information about the computed path.
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() const
Get the number of paths that are currently considered as part of the hybridization.
A shared pointer wrapper for ompl::base::OptimizationObjective.
void matchPaths(const geometric::PathGeometric &p, const geometric::PathGeometric &q, double gapValue, 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...
Definition of a geometric path.
Definition: PathGeometric.h:60
PathHybridization(base::SpaceInformationPtr si)
The constructor needs to know about the space information of the paths it will operate on...
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition: Cost.h:47
A shared pointer wrapper for ompl::base::Path.