From 0438c4bce0bf21b5f5c7eea567e9f6eec5ecb811 Mon Sep 17 00:00:00 2001
From: Joseph Mirabel <jmirabel@laas.fr>
Date: Mon, 15 Feb 2016 16:42:57 +0100
Subject: [PATCH] Fix submanifold definition in helpers
---
src/graph/helper.cc | 89 ++++++++++++++++++++++++++++++++++++++++-----
1 file changed, 79 insertions(+), 10 deletions(-)
diff --git a/src/graph/helper.cc b/src/graph/helper.cc
index e771e04..73f98ec 100644
--- a/src/graph/helper.cc
+++ b/src/graph/helper.cc
@@ -740,6 +740,7 @@ namespace hpp {
struct Result {
GraphPtr_t graph;
std::vector<NodeAndManifold_t> nodes;
+ std::vector< boost::array<NumericalConstraintPtr_t,3> > graspCs;
index_t nG, nOH;
GraspV_t dims;
const Grippers_t& gs;
@@ -756,6 +757,7 @@ namespace hpp {
for (index_t i = 1; i < nG; ++i)
dims[i] = dims[i-1] * (nOH + 1);
nodes.resize (dims[nG-1] * (nOH + 1));
+ graspCs.resize (nG * nOH);
}
NodeAndManifold_t& operator() (const GraspV_t& iG)
@@ -766,6 +768,24 @@ namespace hpp {
return nodes [iGOH];
}
+ inline boost::array<NumericalConstraintPtr_t,3>& graspConstraint (
+ const index_t& iG, const index_t& iOH)
+ {
+ boost::array<NumericalConstraintPtr_t,3>& gcs =
+ graspCs [iG * nOH + iOH];
+ if (!gcs[0]) {
+ hppDout (info, "Create grasps constraints for ("
+ << iG << ", " << iOH << ")");
+ const GripperPtr_t& g (gs[iG]);
+ const HandlePtr_t& h (handle (iOH));
+ gcs[0] = h->createGrasp (g);
+ gcs[1] = h->createGraspComplement (g);
+ const value_type c = h->clearance () + g->clearance ();
+ gcs[2] = h->createPreGrasp (g, c);
+ }
+ return gcs;
+ }
+
const Object_t& object (const index_t& iOH) const {
index_t iH = iOH;
BOOST_FOREACH (const Object_t& o, ohs) {
@@ -824,6 +844,27 @@ namespace hpp {
ss << "Loop" << sep << nf;
return ss.str();
}
+
+ void graspManifold (const index_t& iG, const index_t& iOH,
+ FoliatedManifold& grasp, FoliatedManifold& pregrasp)
+ {
+ boost::array<NumericalConstraintPtr_t,3>& gcs
+ = graspConstraint (iG, iOH);
+ grasp.nc.nc.push_back (gcs[0]);
+ grasp.nc.pdof.push_back (SizeIntervals_t ());
+ grasp.nc_path.nc.push_back (gcs[0]);
+ // TODO: see function declaration
+ grasp.nc_path.pdof.push_back (SizeIntervals_t ());
+ if (gcs[1]->function ().outputSize () > 0) {
+ grasp.nc_fol.nc.push_back (gcs[1]);
+ grasp.nc_fol.pdof.push_back (SizeIntervals_t());
+ }
+
+ pregrasp.nc.nc.push_back (gcs[2]);
+ pregrasp.nc.pdof.push_back (SizeIntervals_t());
+ pregrasp.nc_path.nc.push_back (gcs[2]);
+ pregrasp.nc_path.pdof.push_back (SizeIntervals_t());
+ }
};
const NodeAndManifold_t& makeNode (Result& r, const GraspV_t& g)
@@ -838,8 +879,7 @@ namespace hpp {
for (index_t i = 0; i < r.nG; ++i) {
if (g[i] < r.nOH) {
idxsOH.insert (g[i]);
- graspManifold (r.gs[i], r.handle (g[i]),
- nam.get<1>(), unused);
+ r.graspManifold (i, g[i], nam.get<1>(), unused);
}
}
index_t iOH = 0;
@@ -861,7 +901,8 @@ namespace hpp {
createLoopEdge (r.nameLoopEdge (g),
nam.get<0>(), 1,
- nam.get<1>().foliated(),
+ false,
+ // TODO nam.get<1>().foliated(),
nam.get<1>());
}
return nam;
@@ -876,9 +917,9 @@ namespace hpp {
{
const NodeAndManifold_t& from = makeNode (r, gFrom),
to = makeNode (r, gTo);
- FoliatedManifold grasp, pregrasp, place, preplace;
- graspManifold (r.gs[iG], r.handle (gTo[iG]),
- grasp, pregrasp);
+ 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>(),
@@ -886,6 +927,34 @@ namespace hpp {
place, preplace);
std::pair<std::string, std::string> names =
r.name (gFrom, gTo, iG);
+ {
+ FoliatedManifold unused;
+ std::set <index_t> idxsOH;
+ for (index_t i = 0; i < r.nG; ++i) {
+ if (gFrom[i] < r.nOH) {
+ idxsOH.insert (gFrom[i]);
+ r.graspManifold (i, gFrom[i], submanifold, unused);
+ }
+ }
+ index_t iOH = 0;
+ BOOST_FOREACH (const Object_t& o, r.ohs) {
+ bool oIsGrasped = false;
+ const index_t iOHstart = iOH;
+ for (; iOH < iOHstart + o.get<1>().size(); ++iOH) {
+ if (iOH == gTo [iG]) {
+ oIsGrasped = true;
+ iOH = iOHstart + o.get<1>().size();
+ break;
+ }
+ if (idxsOH.erase (iOH) == 1) oIsGrasped = true;
+ }
+ if (!oIsGrasped)
+ relaxedPlacementManifold (o.get<0>().get<0>(),
+ o.get<0>().get<1>(),
+ o.get<0>().get<2>(),
+ submanifold, unused);
+ }
+ }
if (pregrasp.empty ()) {
if (preplace.empty ())
createEdges <GraspOnly | PlaceOnly> (
@@ -895,7 +964,7 @@ namespace hpp {
grasp , pregrasp,
place , preplace,
grasp.foliated () , place.foliated(),
- from.get<1> ());
+ submanifold);
else {
hppDout (error, "GraspOnly | WithPrePlace not implemeted yet");
/*
@@ -906,7 +975,7 @@ namespace hpp {
grasp , pregrasp,
place , preplace,
grasp.foliated () , place.foliated(),
- from.get<1> ()); // */
+ submanifold); // */
}
} else {
if (preplace.empty ())
@@ -917,7 +986,7 @@ namespace hpp {
grasp , pregrasp,
place , preplace,
grasp.foliated () , place.foliated(),
- from.get<1> ());
+ submanifold);
else
createEdges <WithPreGrasp | WithPrePlace> (
names.first , names.second,
@@ -926,7 +995,7 @@ namespace hpp {
grasp , pregrasp,
place , preplace,
grasp.foliated () , place.foliated(),
- from.get<1> ());
+ submanifold);
}
}
--
GitLab