From 0e4489473f2798dfacb51a41ab8fb2472a063ad3 Mon Sep 17 00:00:00 2001
From: Joseph Mirabel <jmirabel@laas.fr>
Date: Wed, 11 May 2016 17:06:20 +0200
Subject: [PATCH] graph::helper handles cases with no grasp or no placement.
* No grasp: when the robot has contact surface to be put somewhere
* (humanoid feet, for instance)
* No place: when an object has 2 handles and is already grasped on one
* handle, grasping the object on the other handle does not require
* placement.
---
include/hpp/manipulation/graph/helper.hh | 7 ++-
src/graph/helper.cc | 57 ++++++++++++++++++++----
2 files changed, 55 insertions(+), 9 deletions(-)
diff --git a/include/hpp/manipulation/graph/helper.hh b/include/hpp/manipulation/graph/helper.hh
index b426a71..6ad3c14 100644
--- a/include/hpp/manipulation/graph/helper.hh
+++ b/include/hpp/manipulation/graph/helper.hh
@@ -161,7 +161,12 @@ namespace hpp {
PlacementConstraint_t;
typedef std::vector <HandlePtr_t> Handles_t;
typedef std::vector <GripperPtr_t> Grippers_t;
- typedef boost::tuple <PlacementConstraint_t, Handles_t> Object_t;
+ /// Tuple representing an object as follows:
+ /// \li PlacementConstraint_t constraint to place the object
+ /// \li Handles_t list of handles of the object
+ /// \li std::size_t the index of this tuple in Objects_t.
+ /// \note the index must be unique, as object equallity is checked using this index.
+ typedef boost::tuple <PlacementConstraint_t, Handles_t, std::size_t> Object_t;
typedef std::vector <Object_t> Objects_t;
/// Fill a Graph
diff --git a/src/graph/helper.cc b/src/graph/helper.cc
index 5fd2e5d..9ae8eaf 100644
--- a/src/graph/helper.cc
+++ b/src/graph/helper.cc
@@ -306,7 +306,11 @@ namespace hpp {
for (std::size_t i = K; i < Nedges - 1 + K; ++i)
e[i]->setShort (true);
// The border B
- const std::size_t B = (pregrasp?1:0) + (intersec?1:0);
+ std::size_t B;
+ if ((gCase & NoGrasp)) // There is no grasp
+ B = 0;
+ else // There is a grasp
+ B = 1 + (pregrasp?1:0);
for (std::size_t i = 0; i < B ; ++i) e[i]->node (n[0]);
for (std::size_t i = B; i < Nedges; ++i) e[i]->node (n[Nnodes-1]);
}
@@ -583,6 +587,18 @@ namespace hpp {
throw std::out_of_range ("Handle index");
}
+ /// Check is an object is grasped by the GraspV_t
+ bool isObjectGrasped (const GraspV_t& idxOH,
+ const Object_t& o) const
+ {
+ assert (idxOH.size () == nG);
+ for (std::size_t i = 0; i < idxOH.size (); ++i)
+ if (idxOH[i] < nOH) // This grippers grasps an object
+ if (o.get<2>() == object(idxOH[i]).get<2>())
+ return true;
+ return false;
+ }
+
/// Get a node name from a set of grasps
std::string name (const GraspV_t& idxOH, bool abbrev = false) const {
assert (idxOH.size () == nG);
@@ -698,14 +714,20 @@ namespace hpp {
{
const NodeAndManifold_t& from = makeNode (r, gFrom, priority),
to = makeNode (r, gTo, priority+1);
+ const Object_t& o = r.object (gTo[iG]);
+
+ /// Detect when grasping an object already grasped.
+ bool noPlace = r.isObjectGrasped (gFrom, o);
+
FoliatedManifold grasp, pregrasp, place, preplace,
submanifold;
r.graspManifold (iG, gTo[iG], grasp, pregrasp);
- const Object_t& o = r.object (gTo[iG]);
- relaxedPlacementManifold (o.get<0>().get<0>(),
- o.get<0>().get<1>(),
- o.get<0>().get<2>(),
- place, preplace);
+ if (!noPlace) {
+ relaxedPlacementManifold (o.get<0>().get<0>(),
+ o.get<0>().get<1>(),
+ o.get<0>().get<2>(),
+ place, preplace);
+ }
std::pair<std::string, std::string> names =
r.name (gFrom, gTo, iG);
{
@@ -737,7 +759,16 @@ namespace hpp {
}
}
if (pregrasp.empty ()) {
- if (preplace.empty ())
+ if (noPlace)
+ createEdges <GraspOnly | NoPlace> (
+ names.first , names.second,
+ from.get<0> () , to.get<0>(),
+ 1 , 1,
+ grasp , pregrasp,
+ place , preplace,
+ grasp.foliated () , place.foliated(),
+ submanifold);
+ else if (preplace.empty ())
createEdges <GraspOnly | PlaceOnly> (
names.first , names.second,
from.get<0> () , to.get<0>(),
@@ -759,7 +790,16 @@ namespace hpp {
submanifold); // */
}
} else {
- if (preplace.empty ())
+ if (noPlace)
+ createEdges <WithPreGrasp | NoPlace> (
+ names.first , names.second,
+ from.get<0> () , to.get<0>(),
+ 1 , 1,
+ grasp , pregrasp,
+ place , preplace,
+ grasp.foliated () , place.foliated(),
+ submanifold);
+ else if (preplace.empty ())
createEdges <WithPreGrasp | PlaceOnly> (
names.first , names.second,
from.get<0> () , to.get<0>(),
@@ -853,6 +893,7 @@ namespace hpp {
bool prePlace = (prePlaceWidth > 0);
BOOST_FOREACH (const ObjectDef_t& od, objs) {
// Create handles
+ objects[i].get<2> () = i;
objects[i].get<1> ().resize (od.handles.size());
Handles_t::iterator it = objects[i].get<1> ().begin();
BOOST_FOREACH (const std::string hn, od.handles) {
--
GitLab