37 #include "ompl/geometric/PathHybridization.h" 38 #include "ompl/base/objectives/PathLengthOptimizationObjective.h" 39 #include <boost/graph/dijkstra_shortest_paths.hpp> 54 , obj_(new base::PathLengthOptimizationObjective(si_))
55 , stateProperty_(
boost::get(vertex_state_t(), g_))
56 , name_(
"PathHybridization")
58 root_ = boost::add_vertex(g_);
59 stateProperty_[root_] =
nullptr;
60 goal_ = boost::add_vertex(g_);
61 stateProperty_[goal_] =
nullptr;
66 , obj_(
std::move(obj))
67 , stateProperty_(
boost::get(vertex_state_t(), g_))
68 , name_(
"PathHybridization")
71 ss <<
"PathHybridization over " << obj_->getDescription() <<
" cost";
73 root_ = boost::add_vertex(g_);
74 stateProperty_[root_] =
nullptr;
75 goal_ = boost::add_vertex(g_);
76 stateProperty_[goal_] =
nullptr;
79 ompl::geometric::PathHybridization::~PathHybridization() =
default;
87 root_ = boost::add_vertex(g_);
88 stateProperty_[root_] =
nullptr;
89 goal_ = boost::add_vertex(g_);
90 stateProperty_[goal_] =
nullptr;
95 out <<
"Path hybridization is aware of " << paths_.size() <<
" paths" << std::endl;
97 for (
auto it = paths_.begin(); it != paths_.end(); ++it, ++i)
98 out <<
" path " << i <<
" of cost " << it->cost_.value() << std::endl;
100 out <<
"Hybridized path of cost " << hpath_->cost(obj_) << std::endl;
110 boost::vector_property_map<Vertex> prev(boost::num_vertices(g_));
111 boost::dijkstra_shortest_paths(
113 boost::predecessor_map(prev)
114 .distance_compare([
this](
base::Cost c1,
base::Cost c2) {
return obj_->isCostBetterThan(c1, c2); })
116 .distance_inf(obj_->infiniteCost())
117 .distance_zero(obj_->identityCost()));
118 if (prev[goal_] != goal_)
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]);
142 OMPL_ERROR(
"Path hybridization only works for geometric paths");
146 if (p->getSpaceInformation() != si_)
148 OMPL_ERROR(
"Paths for hybridization must be from the same space information");
153 if (p->getStateCount() == 0)
159 if (paths_.find(pi) != paths_.end())
163 unsigned int nattempts = 0;
166 Vertex v0 = boost::add_vertex(g_);
167 stateProperty_[v0] = pi.states_[0];
168 pi.vertices_.push_back(v0);
172 const HGraph::edge_property_type prop0(obj_->identityCost());
173 boost::add_edge(root_, v0, prop0, g_);
175 for (std::size_t j = 1; j < pi.states_.size(); ++j)
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);
188 boost::add_edge(v0, goal_, prop0, g_);
192 for (
const auto &path : paths_)
194 const auto *q =
static_cast<const PathGeometric *
>(path.path_.get());
195 std::vector<int> indexP, indexQ;
207 for (std::size_t i = 0; i < indexP.size(); ++i)
223 for (std::size_t j = gapStartP; j < i; ++j)
225 attemptNewEdge(pi, path, indexP[i], indexQ[j]);
241 for (std::size_t j = gapStartQ; j < i; ++j)
243 attemptNewEdge(pi, path, indexP[j], indexQ[i]);
251 if (lastP >= 0 && lastQ >= 0)
253 attemptNewEdge(pi, path, indexP[lastP], indexQ[lastQ]);
261 for (std::size_t i = 0; i < indexP.size(); ++i)
262 if (indexP[i] >= 0 && indexQ[i] >= 0)
264 attemptNewEdge(pi, path, indexP[i], indexQ[i]);
275 void ompl::geometric::PathHybridization::attemptNewEdge(
const PathInfo &p,
const PathInfo &q,
int indexP,
int indexQ)
277 if (si_->checkMotion(p.states_[indexP], q.states_[indexQ]))
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_);
287 return paths_.size();
291 std::vector<int> &indexP, std::vector<int> &indexQ)
const 293 using CostMatrix = Eigen::Matrix<base::Cost, Eigen::Dynamic, Eigen::Dynamic>;
294 using TMatrix = Eigen::Matrix<char, Eigen::Dynamic, Eigen::Dynamic>;
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)))
314 else if (!(obj_->isCostBetterThan(match, up) || obj_->isCostBetterThan(left, up)))
332 indexP.reserve(std::max(n, m));
333 indexQ.reserve(indexP.size());
335 while (n >= 0 && m >= 0)
344 else if (T(m, n) ==
'u')
347 indexQ.push_back(-1);
352 indexP.push_back(-1);
359 indexP.push_back(-1);
366 indexQ.push_back(-1);
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.
const std::string & getName() const
Get the name of the algorithm.
void computeHybridPath()
Run Dijkstra'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.
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
#define OMPL_WARN(fmt,...)
Log a formatted warning string.
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.
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.
A shared pointer wrapper for ompl::base::Path.