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 
64 ompl::geometric::PathHybridization::PathHybridization(base::SpaceInformationPtr si, base::OptimizationObjectivePtr obj)
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 
132 const ompl::geometric::PathGeometricPtr &ompl::geometric::PathHybridization::getHybridPath() const
133 {
134  return hpath_;
135 }
136 
137 unsigned int ompl::geometric::PathHybridization::recordPath(const geometric::PathGeometricPtr &p, bool matchAcrossGaps)
138 {
139  if (p->getSpaceInformation() != si_)
140  {
141  OMPL_ERROR("Paths for hybridization must be from the same space information");
142  return 0;
143  }
144 
145  // skip empty paths
146  if (p->getStateCount() == 0)
147  return 0;
148 
149  // interpolate path to get more potential crossover points between paths
150  PathInfo pi(p);
151 
152  // if this path was previously included in the hybridization, skip it
153  if (paths_.find(pi) != paths_.end())
154  return 0;
155 
156  // the number of connection attempts
157  unsigned int nattempts = 0;
158 
159  // start from virtual root
160  Vertex v0 = boost::add_vertex(g_);
161  stateProperty_[v0] = pi.states_[0];
162  pi.vertices_.push_back(v0);
163 
164  // add all the vertices of the path, and the edges between them, to the HGraph
165  // also compute the path cost for future use (just for computational savings)
166  const HGraph::edge_property_type prop0(obj_->identityCost());
167  boost::add_edge(root_, v0, prop0, g_);
168  base::Cost cost = obj_->identityCost();
169  for (std::size_t j = 1; j < pi.states_.size(); ++j)
170  {
171  Vertex v1 = boost::add_vertex(g_);
172  stateProperty_[v1] = pi.states_[j];
173  base::Cost weight = obj_->motionCost(pi.states_[j - 1], pi.states_[j]);
174  const HGraph::edge_property_type properties(weight);
175  boost::add_edge(v0, v1, properties, g_);
176  cost = obj_->combineCosts(cost, weight);
177  pi.vertices_.push_back(v1);
178  v0 = v1;
179  }
180 
181  // connect to virtual goal
182  boost::add_edge(v0, goal_, prop0, g_);
183  pi.cost_ = cost;
184 
185  // find matches with previously added paths
186  for (const auto &path : paths_)
187  {
188  const auto *q = static_cast<const PathGeometric *>(path.path_.get());
189  std::vector<int> indexP, indexQ;
190  matchPaths(*p, *q, obj_->combineCosts(pi.cost_, path.cost_).value() / (2.0 / magic::GAP_COST_FRACTION), indexP,
191  indexQ);
192 
193  if (matchAcrossGaps)
194  {
195  int lastP = -1;
196  int lastQ = -1;
197  int gapStartP = -1;
198  int gapStartQ = -1;
199  bool gapP = false;
200  bool gapQ = false;
201  for (std::size_t i = 0; i < indexP.size(); ++i)
202  {
203  // a gap is found in p
204  if (indexP[i] < 0)
205  {
206  // remember this as the beginning of the gap, if needed
207  if (!gapP)
208  gapStartP = i;
209  // mark the fact we are now in a gap on p
210  gapP = true;
211  }
212  else
213  {
214  // check if a gap just ended;
215  // if it did, try to match the endpoint with the elements in q
216  if (gapP)
217  for (std::size_t j = gapStartP; j < i; ++j)
218  {
219  attemptNewEdge(pi, path, indexP[i], indexQ[j]);
220  ++nattempts;
221  }
222  // remember the last non-negative index in p
223  lastP = i;
224  gapP = false;
225  }
226  if (indexQ[i] < 0)
227  {
228  if (!gapQ)
229  gapStartQ = i;
230  gapQ = true;
231  }
232  else
233  {
234  if (gapQ)
235  for (std::size_t j = gapStartQ; j < i; ++j)
236  {
237  attemptNewEdge(pi, path, indexP[j], indexQ[i]);
238  ++nattempts;
239  }
240  lastQ = i;
241  gapQ = false;
242  }
243 
244  // try to match corresponding index values and gep beginnings
245  if (lastP >= 0 && lastQ >= 0)
246  {
247  attemptNewEdge(pi, path, indexP[lastP], indexQ[lastQ]);
248  ++nattempts;
249  }
250  }
251  }
252  else
253  {
254  // attempt new edge only when states align
255  for (std::size_t i = 0; i < indexP.size(); ++i)
256  if (indexP[i] >= 0 && indexQ[i] >= 0)
257  {
258  attemptNewEdge(pi, path, indexP[i], indexQ[i]);
259  ++nattempts;
260  }
261  }
262  }
263 
264  // remember this path is part of the hybridization
265  paths_.insert(pi);
266  return nattempts;
267 }
268 
269 void ompl::geometric::PathHybridization::attemptNewEdge(const PathInfo &p, const PathInfo &q, int indexP, int indexQ)
270 {
271  if (si_->checkMotion(p.states_[indexP], q.states_[indexQ]))
272  {
273  base::Cost weight = obj_->motionCost(p.states_[indexP], q.states_[indexQ]);
274  const HGraph::edge_property_type properties(weight);
275  boost::add_edge(p.vertices_[indexP], q.vertices_[indexQ], properties, g_);
276  }
277 }
278 
280 {
281  return paths_.size();
282 }
283 
285  std::vector<int> &indexP, std::vector<int> &indexQ) const
286 {
287  using CostMatrix = Eigen::Matrix<base::Cost, Eigen::Dynamic, Eigen::Dynamic>;
288  using TMatrix = Eigen::Matrix<char, Eigen::Dynamic, Eigen::Dynamic>;
289  CostMatrix C = CostMatrix::Constant(p.getStateCount(), q.getStateCount(), obj_->identityCost());
290  TMatrix T = TMatrix::Constant(p.getStateCount(), q.getStateCount(), '\0');
291 
292  base::Cost gapCost(gapValue);
293  for (std::size_t i = 0; i < p.getStateCount(); ++i)
294  {
295  for (std::size_t j = 0; j < q.getStateCount(); ++j)
296  {
297  // as far as I can tell, there is a bug in the algorithm as presented in the paper
298  // so I am doing things slightly differently ...
299  base::Cost match = obj_->combineCosts(obj_->motionCost(p.getState(i), q.getState(j)),
300  ((i > 0 && j > 0) ? C(i - 1, j - 1) : obj_->identityCost()));
301  base::Cost up = obj_->combineCosts(gapCost, (i > 0 ? C(i - 1, j) : obj_->identityCost()));
302  base::Cost left = obj_->combineCosts(gapCost, (j > 0 ? C(i, j - 1) : obj_->identityCost()));
303  if (!(obj_->isCostBetterThan(up, match) || obj_->isCostBetterThan(left, match)))
304  {
305  C(i, j) = match;
306  T(i, j) = 'm';
307  }
308  else if (!(obj_->isCostBetterThan(match, up) || obj_->isCostBetterThan(left, up)))
309  {
310  C(i, j) = up;
311  T(i, j) = 'u';
312  }
313  else
314  {
315  C(i, j) = left;
316  T(i, j) = 'l';
317  }
318  }
319  }
320  // construct the sequences with gaps (only index positions)
321  int m = p.getStateCount() - 1;
322  int n = q.getStateCount() - 1;
323 
324  indexP.clear();
325  indexQ.clear();
326  indexP.reserve(std::max(n, m));
327  indexQ.reserve(indexP.size());
328 
329  while (n >= 0 && m >= 0)
330  {
331  if (T(m, n) == 'm')
332  {
333  indexP.push_back(m);
334  indexQ.push_back(n);
335  --m;
336  --n;
337  }
338  else if (T(m, n) == 'u')
339  {
340  indexP.push_back(m);
341  indexQ.push_back(-1);
342  --m;
343  }
344  else
345  {
346  indexP.push_back(-1);
347  indexQ.push_back(n);
348  --n;
349  }
350  }
351  while (n >= 0)
352  {
353  indexP.push_back(-1);
354  indexQ.push_back(n);
355  --n;
356  }
357  while (m >= 0)
358  {
359  indexP.push_back(m);
360  indexQ.push_back(-1);
361  --m;
362  }
363 }
const std::string & getName() const
Get the name of the algorithm.
unsigned int recordPath(const geometric::PathGeometricPtr &pp, bool matchAcrossGaps)
Add a path to the hybridization. If matchAcrossGaps is true, more possible edge connections are evalu...
void print(std::ostream &out=std::cout) const
Print information about the computed path.
void clear()
Clear all the stored paths.
std::size_t pathCount() const
Get the number of paths that are currently considered as part of the hybridization.
const geometric::PathGeometricPtr & getHybridPath() const
Get the currently computed hybrid path. computeHybridPath() needs to have been called before.
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition: Cost.h:111
static const double GAP_COST_FRACTION
The fraction of the path length to consider as gap cost when aligning paths to be hybridized.
Definition of a geometric path.
Definition: PathGeometric.h:97
void computeHybridPath()
Run Dijkstra's algorithm to find out the lowest-cost path among the mixed ones.
std::size_t getStateCount() const
Get the number of states (way-points) that make up this path.
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...
#define OMPL_WARN(fmt,...)
Log a formatted warning string.
Definition: Console.h:66
PathHybridization(base::SpaceInformationPtr si)
The constructor needs to know about the space information of the paths it will operate on.
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Definition: Console.h:64
base::State * getState(unsigned int index)
Get the state located at index along the path.
Main namespace. Contains everything in this library.