Skip to content
Snippets Groups Projects
Commit bda55a86 authored by Joseph Mirabel's avatar Joseph Mirabel Committed by Joseph Mirabel
Browse files

Add missing implementation of createEdges

parent 903d2372
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
Showing
with 200 additions and 85 deletions
src/graph/helper.cc
+ 200
− 85
View file @ bda55a86
...@@ -224,6 +224,118 @@ namespace hpp { ...@@ -224,6 +224,118 @@ namespace hpp {
return std::make_pair (weForw, weBack); return std::make_pair (weForw, weBack);
} }
template <> EdgePair_t
createEdges <WithPreGrasp | PlaceOnly> (
const std::string& forwName, const std::string& backName,
const NodePtr_t& from, const NodePtr_t& to,
const size_type& wForw, const size_type& wBack,
const FoliatedManifold& grasp, const FoliatedManifold& pregrasp,
const FoliatedManifold& place, const FoliatedManifold&,
const bool levelSetGrasp, const bool levelSetPlace,
const FoliatedManifold& submanifoldDef)
{
// Create the edges
WaypointEdgePtr_t weForw = HPP_DYNAMIC_PTR_CAST (WaypointEdge,
from->linkTo (forwName, to, wForw, WaypointEdge::create)),
weBack = HPP_DYNAMIC_PTR_CAST (WaypointEdge,
to-> linkTo (backName, from, wBack, WaypointEdge::create));
weForw->nbWaypoints (2);
weBack->nbWaypoints (2);
std::string name = forwName;
NodeSelectorPtr_t ns = weForw->parentGraph ()->nodeSelector ();
NodePtr_t n0 = from,
n1 = ns->createNode (name + "_pregrasp", true),
n2 = ns->createNode (name + "_intersec", true),
n3 = to;
EdgePtr_t e01 = n0->linkTo (name + "_e01", n1, -1, Edge::create),
e12 = n1->linkTo (name + "_e12", n2, -1, Edge::create),
e23 = weForw;
LevelSetEdgePtr_t e12_ls;
if (levelSetGrasp)
e12_ls = HPP_DYNAMIC_PTR_CAST (LevelSetEdge,
n1->linkTo (name + "_e12_ls", n2, -1, LevelSetEdge::create));
// Set the edges properties
e01->node (n0);
e12->node (n0); e12->setShort (true);
e23->node (n3);
// set the nodes constraints
// From and to are not populated automatically to avoid duplicates.
place.addToNode (n1);
pregrasp.addToNode (n1);
submanifoldDef.addToNode (n1);
place.addToNode (n2);
grasp.addToNode (n2);
submanifoldDef.addToNode (n2);
// Set the edges constraints
place.addToEdge (e01);
submanifoldDef.addToEdge (e01);
place.addToEdge (e12);
submanifoldDef.addToEdge (e12);
grasp.addToEdge (e23);
submanifoldDef.addToEdge (e23);
// Set the waypoints
weForw->setWaypoint (0, e01, n1);
weForw->setWaypoint (1, (levelSetGrasp)?e12_ls:e12, n2);
// Populate bacward edge
EdgePtr_t e32 = n3->linkTo (name + "_e32", n2, -1, Edge::create),
e21 = n2->linkTo (name + "_e21", n1, -1, Edge::create),
e10 = weBack;
LevelSetEdgePtr_t e32_ls;
if (levelSetPlace)
e32_ls = HPP_DYNAMIC_PTR_CAST (LevelSetEdge,
n3->linkTo (name + "_e32_ls", n2, -1, LevelSetEdge::create));
e32->node (n3);
e21->node (n0); e21->setShort (true);
e10->node (n0);
place.addToEdge (e10);
submanifoldDef.addToEdge (e10);
place.addToEdge (e21);
submanifoldDef.addToEdge (e21);
grasp.addToEdge (e32);
submanifoldDef.addToEdge (e32);
weForw->setWaypoint (0, (levelSetGrasp)?e32_ls:e32, n2);
weBack->setWaypoint (1, e21, n1);
if (levelSetPlace) {
if (!place.foliated ()) {
hppDout (warning, "You asked for a LevelSetEdge for placement, "
"but did not specify the target foliation. "
"It will have no effect");
}
e32_ls->node (n3);
e32_ls->setShort (true);
grasp.addToEdge (e32_ls);
place.specifyFoliation (e32_ls);
submanifoldDef.addToEdge (e32_ls);
}
if (levelSetGrasp) {
if (!grasp.foliated ()) {
hppDout (warning, "You asked for a LevelSetEdge for grasping, "
"but did not specify the target foliation. "
"It will have no effect");
}
e12_ls->node (n0);
e12_ls->setShort (true);
place.addToEdge (e12_ls);
grasp.specifyFoliation (e12_ls);
submanifoldDef.addToEdge (e12_ls);
}
return std::make_pair (weForw, weBack);
}
template <> EdgePair_t template <> EdgePair_t
createEdges <GraspOnly | PlaceOnly>( createEdges <GraspOnly | PlaceOnly>(
const std::string& forwName, const std::string& backName, const std::string& forwName, const std::string& backName,
...@@ -588,7 +700,7 @@ namespace hpp { ...@@ -588,7 +700,7 @@ namespace hpp {
BOOST_FOREACH (const Object_t& o, r.ohs) { BOOST_FOREACH (const Object_t& o, r.ohs) {
bool oIsGrasped = false; bool oIsGrasped = false;
// TODO: use the fact that the set is sorted. // TODO: use the fact that the set is sorted.
// BOOST_FOREACH (const HandlePtr_t& h, o.get<1>()) { // BOOST_FOREACH (const HandlePtr_t& h, o.get<1>())
for (index_t i = 0; i < o.get<1>().size(); ++i) { for (index_t i = 0; i < o.get<1>().size(); ++i) {
if (idxsOH.erase (iOH) == 1) oIsGrasped = true; if (idxsOH.erase (iOH) == 1) oIsGrasped = true;
++iOH; ++iOH;
...@@ -600,99 +712,102 @@ namespace hpp { ...@@ -600,99 +712,102 @@ namespace hpp {
nam.get<1>(), unused); nam.get<1>(), unused);
} }
nam.get<1>().addToNode (nam.get<0>()); nam.get<1>().addToNode (nam.get<0>());
}
return nam;
} }
return nam;
}
/// Arguments are such that /// Arguments are such that
/// \li gTo[iG] != gFrom[iG] /// \li gTo[iG] != gFrom[iG]
/// \li for all i != iG, gTo[iG] == gFrom[iG] /// \li for all i != iG, gTo[iG] == gFrom[iG]
void makeEdge (Result& r, void makeEdge (Result& r,
const GraspV_t& gFrom, const GraspV_t& gTo, const GraspV_t& gFrom, const GraspV_t& gTo,
const index_t iG) const index_t iG)
{ {
const NodeAndManifold_t& from = makeNode (r, gFrom), const NodeAndManifold_t& from = makeNode (r, gFrom),
to = makeNode (r, gTo); to = makeNode (r, gTo);
FoliatedManifold grasp, pregrasp, place, preplace; FoliatedManifold grasp, pregrasp, place, preplace;
graspManifold (r.gs[iG], r.handle (gTo[iG]), graspManifold (r.gs[iG], r.handle (gTo[iG]),
grasp, pregrasp); grasp, pregrasp);
const Object_t& o = r.object (gTo[iG]); const Object_t& o = r.object (gTo[iG]);
relaxedPlacementManifold (o.get<0>().get<0>(), relaxedPlacementManifold (o.get<0>().get<0>(),
o.get<0>().get<1>(), o.get<0>().get<1>(),
o.get<0>().get<2>(), o.get<0>().get<2>(),
place, preplace); place, preplace);
if (pregrasp.empty ()) { if (pregrasp.empty ()) {
if (preplace.empty ()) if (preplace.empty ())
createEdges <GraspOnly | PlaceOnly> ( createEdges <GraspOnly | PlaceOnly> (
"forwName" , "backName", "forwName" , "backName",
from.get<0> () , to.get<0>(), from.get<0> () , to.get<0>(),
1 , 1, 1 , 1,
grasp , pregrasp, grasp , pregrasp,
place , preplace, place , preplace,
grasp.foliated () , place.foliated(), grasp.foliated () , place.foliated(),
from.get<1> ()); from.get<1> ());
else else {
createEdges <GraspOnly | WithPrePlace> ( hppDout (error, "GraspOnly | WithPrePlace not implemeted yet");
"forwName" , "backName", /*
from.get<0> () , to.get<0>(), createEdges <GraspOnly | WithPrePlace> (
1 , 1, "forwName" , "backName",
grasp , pregrasp, from.get<0> () , to.get<0>(),
place , preplace, 1 , 1,
grasp.foliated () , place.foliated(), grasp , pregrasp,
from.get<1> ()); place , preplace,
} else { grasp.foliated () , place.foliated(),
if (preplace.empty ()) from.get<1> ()); // */
createEdges <WithPreGrasp | PlaceOnly> (
"forwName" , "backName",
from.get<0> () , to.get<0>(),
1 , 1,
grasp , pregrasp,
place , preplace,
grasp.foliated () , place.foliated(),
from.get<1> ());
else
createEdges <WithPreGrasp | WithPrePlace> (
"forwName" , "backName",
from.get<0> () , to.get<0>(),
1 , 1,
grasp , pregrasp,
place , preplace,
grasp.foliated () , place.foliated(),
from.get<1> ());
} }
} else {
if (preplace.empty ())
createEdges <WithPreGrasp | PlaceOnly> (
"forwName" , "backName",
from.get<0> () , to.get<0>(),
1 , 1,
grasp , pregrasp,
place , preplace,
grasp.foliated () , place.foliated(),
from.get<1> ());
else
createEdges <WithPreGrasp | WithPrePlace> (
"forwName" , "backName",
from.get<0> () , to.get<0>(),
1 , 1,
grasp , pregrasp,
place , preplace,
grasp.foliated () , place.foliated(),
from.get<1> ());
} }
}
/// idx are the available grippers /// idx are the available grippers
void recurseGrippers (Result& r, void recurseGrippers (Result& r,
const IndexV_t& idx_g, const IndexV_t& idx_oh, const IndexV_t& idx_g, const IndexV_t& idx_oh,
const GraspV_t& grasps) const GraspV_t& grasps)
{ {
IndexV_t nIdx_g (idx_g.size() - 1); IndexV_t nIdx_g (idx_g.size() - 1);
IndexV_t nIdx_oh (idx_oh.size() - 1); IndexV_t nIdx_oh (idx_oh.size() - 1);
for (IndexV_t::const_iterator itx_g = idx_g.begin (); for (IndexV_t::const_iterator itx_g = idx_g.begin ();
itx_g != idx_g.end (); ++itx_g) { itx_g != idx_g.end (); ++itx_g) {
// Copy all element except itx_g // Copy all element except itx_g
std::copy (boost::next (itx_g), idx_g.end (), std::copy (boost::next (itx_g), idx_g.end (),
std::copy (idx_g.begin (), itx_g, nIdx_g.begin ()) std::copy (idx_g.begin (), itx_g, nIdx_g.begin ())
);
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 ())
); );
for (IndexV_t::const_iterator itx_oh = idx_oh.begin ();
// Create the edge for the selected grasp itx_oh != idx_oh.end (); ++itx_oh) {
GraspV_t nGrasps = grasps; // Copy all element except itx_oh
nGrasps [*itx_g] = *itx_oh; std::copy (boost::next (itx_oh), idx_oh.end (),
makeEdge (r, grasps, nGrasps, *itx_g); std::copy (idx_oh.begin (), itx_oh, nIdx_oh.begin ())
);
// Do all the possible combination below this new grasp
recurseGrippers (r, nIdx_oh, nIdx_oh, nGrasps); // Create the edge for the selected grasp
GraspV_t nGrasps = grasps;
nGrasps [*itx_g] = *itx_oh;
makeEdge (r, grasps, nGrasps, *itx_g);
// Do all the possible combination below this new grasp
recurseGrippers (r, nIdx_oh, nIdx_oh, nGrasps);
}
} }
} }
} }
}
void graphBuilder ( void graphBuilder (
const Objects_t& objects, const Objects_t& objects,
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment