From 9fc2fd10407887aaef8ab5d69055e66e2a552539 Mon Sep 17 00:00:00 2001
From: Joseph Mirabel <jmirabel@laas.fr>
Date: Tue, 16 Feb 2016 20:50:19 +0100
Subject: [PATCH] Graph helper: properly order nodes in NodeSelector
---
src/graph/helper.cc | 29 +++++++++++++++--------------
1 file changed, 15 insertions(+), 14 deletions(-)
diff --git a/src/graph/helper.cc b/src/graph/helper.cc
index 45653f34..192697a4 100644
--- a/src/graph/helper.cc
+++ b/src/graph/helper.cc
@@ -645,12 +645,14 @@ namespace hpp {
}
};
- const NodeAndManifold_t& makeNode (Result& r, const GraspV_t& g)
+ const NodeAndManifold_t& makeNode (Result& r, const GraspV_t& g,
+ const int priority)
{
NodeAndManifold_t& nam = r (g);
if (!nam.get<0>()) {
hppDout (info, "Creating node " << r.name (g));
- nam.get<0>() = r.graph->nodeSelector ()->createNode (r.name (g));
+ nam.get<0>() = r.graph->nodeSelector ()->createNode
+ (r.name (g), false, priority);
// Loop over the grippers and create grasping constraints if required
FoliatedManifold unused;
std::set <index_t> idxsOH;
@@ -691,10 +693,10 @@ namespace hpp {
/// \li for all i != iG, gTo[iG] == gFrom[iG]
void makeEdge (Result& r,
const GraspV_t& gFrom, const GraspV_t& gTo,
- const index_t iG)
+ const index_t iG, const int priority)
{
- const NodeAndManifold_t& from = makeNode (r, gFrom),
- to = makeNode (r, gTo);
+ const NodeAndManifold_t& from = makeNode (r, gFrom, priority),
+ to = makeNode (r, gTo, priority+1);
FoliatedManifold grasp, pregrasp, place, preplace,
submanifold;
r.graspManifold (iG, gTo[iG], grasp, pregrasp);
@@ -780,7 +782,7 @@ namespace hpp {
/// idx are the available grippers
void recurseGrippers (Result& r,
const IndexV_t& idx_g, const IndexV_t& idx_oh,
- const GraspV_t& grasps)
+ const GraspV_t& grasps, const int depth)
{
if (idx_g.empty () || idx_oh.empty ()) return;
IndexV_t nIdx_g (idx_g.size() - 1);
@@ -793,18 +795,17 @@ namespace hpp {
);
for (IndexV_t::const_iterator itx_oh = idx_oh.begin ();
itx_oh != idx_oh.end (); ++itx_oh) {
- // Copy all element except itx_oh
- std::copy (boost::next (itx_oh), idx_oh.end (),
- std::copy (idx_oh.begin (), itx_oh, nIdx_oh.begin ())
- );
-
// Create the edge for the selected grasp
GraspV_t nGrasps = grasps;
nGrasps [*itx_g] = *itx_oh;
- makeEdge (r, grasps, nGrasps, *itx_g);
+ makeEdge (r, grasps, nGrasps, *itx_g, depth);
+ // Copy all element except itx_oh
+ std::copy (boost::next (itx_oh), idx_oh.end (),
+ std::copy (idx_oh.begin (), itx_oh, nIdx_oh.begin ())
+ );
// Do all the possible combination below this new grasp
- recurseGrippers (r, nIdx_g, nIdx_oh, nGrasps);
+ recurseGrippers (r, nIdx_g, nIdx_oh, nGrasps, depth + 2);
}
}
}
@@ -827,7 +828,7 @@ namespace hpp {
GraspV_t iG (r.nG, r.nOH);
- recurseGrippers (r, availG, availOH, iG);
+ recurseGrippers (r, availG, availOH, iG, 0);
}
GraphPtr_t graphBuilder (
--
GitLab