Loading...
Searching...
No Matches
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
43namespace 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
64ompl::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
79ompl::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
93void 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
132const ompl::geometric::PathGeometricPtr &ompl::geometric::PathHybridization::getHybridPath() const
133{
134 return hpath_;
135}
136
137unsigned 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
269void 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.
STL namespace.