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 }
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition: Cost.h:48
Definition of a geometric path.
Definition: PathGeometric.h:66
std::size_t getStateCount() const
Get the number of states (way-points) that make up this path.
base::State * getState(unsigned int index)
Get the state located at index along the path.
const std::string & getName() const
Get the name of the algorithm.
const geometric::PathGeometricPtr & getHybridPath() const
Get the currently computed hybrid path. computeHybridPath() needs to have been called before.
void clear()
Clear all the stored paths.
void computeHybridPath()
Run Dijkstra's algorithm to find out the lowest-cost path among the mixed ones.
void print(std::ostream &out=std::cout) const
Print information about the computed path.
PathHybridization(base::SpaceInformationPtr si)
The constructor needs to know about the space information of the paths it will operate on.
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...
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...
std::size_t pathCount() const
Get the number of paths that are currently considered as part of the hybridization.
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Definition: Console.h:64
#define OMPL_WARN(fmt,...)
Log a formatted warning string.
Definition: Console.h:66
static const double GAP_COST_FRACTION
The fraction of the path length to consider as gap cost when aligning paths to be hybridized.
Main namespace. Contains everything in this library.
Definition: AppBase.h:22