Add class Affordance to store data related to each (gripper, handle)

sot_hpp/factory.py

@@ -2,6 +2,23 @@ from hpp.corbaserver.manipulation.constraint_graph_factory import ConstraintFact
 from tools import Manifold, Grasp, idx, idx_zip, OpFrame, EEPosture
 from dynamic_graph.sot.core import SOT
 
+class Affordance(object):
+    def __init__ (self, gripper, handle, **kwargs):
+        self.gripper = gripper
+        self.handle  = handle
+        self.setControl (**kwargs)
+    def setControl (self, refOpen, refClose, openType = "position", closeType="position"):
+        if openType != "position" or closeType != "position":
+            raise NotImplementedError ("Only position control is implemented for gripper opening/closure.")
+        self.controlType = {
+                "open": openType,
+                "close": closeType,
+                }
+        self.ref = {
+                "open": refOpen,
+                "close": refClose,
+                }
+
 class TaskFactory(ConstraintFactoryAbstract):
     gfields = ('grasp', 'pregrasp', 'gripper_open', 'gripper_close')
     pfields = ()
@@ -9,12 +26,26 @@ class TaskFactory(ConstraintFactoryAbstract):
     def __init__ (self, graphfactory):
         super(TaskFactory, self).__init__ (graphfactory)
 
+    def _buildGripper (self, type, gripper, handle):
+        try:
+            aff = self.graphfactory.affordances[(gripper, handle)]
+        except KeyError:
+            # If there are no affordance, do not add a task.
+            return Manifold()
+        gf = self.graphfactory
+        robot = gf.sotrobot
+        if aff.controlType[type] == "position":
+            return EEPosture (robot, gf.gripperFrames[gripper], aff.ref[type])
+        else:
+            raise NotImplementedError ("Only position control is implemented for gripper closure.")
+
     def buildGrasp (self, g, h, otherGrasp=None):
         gf = self.graphfactory
         if h is None:
-            return { 'gripper_open': EEPosture (gf.sotrobot, gf.gripperFrames [g], [0]) }
+            gripper_open = self._buildGripper ("open", g, h)
+            return { 'gripper_open': gripper_open }
 
-        gripper_close  = EEPosture (gf.sotrobot, gf.gripperFrames [g], [-0.2])
+        gripper_close  = self._buildGripper ("close", g, h)
         pregrasp = Grasp (gf.gripperFrames [g],
                           gf.handleFrames [h],
                           otherGrasp,
@@ -66,6 +97,7 @@ class Factory(GraphFactoryAbstract):
             for ig, ih in idx_zip (grasps):
                 if ih is not None:
                     # Add task gripper_close
+                    self.manifold += tasks.g (factory.grippers[ig], factory.handles[ih], 'gripper_close')
                     otherGrasp = objectsAlreadyGrasped.get(factory.objectFromHandle[ih])
                     self.manifold += tasks.g (factory.grippers[ig], factory.handles[ih], 'grasp', otherGrasp)
                     objectsAlreadyGrasped[factory.objectFromHandle[ih]] = tasks.g (factory.grippers[ig], factory.handles[ih], 'grasp', otherGrasp)
@@ -78,12 +110,17 @@ class Factory(GraphFactoryAbstract):
         self.tasks = TaskFactory (self)
         self.hpTasks = supervisor.hpTasks
         self.lpTasks = supervisor.lpTasks
+        self.affordances = dict()
         self.sots = dict()
         self.postActions = dict()
         self.preActions = dict()
 
         self.supervisor = supervisor
 
+    def addAffordance (self, aff):
+        assert isinstance(aff, Affordance)
+        self.affordances [(aff.gripper, aff.handle)] = aff
+
     def finalize (self, hppclient):
         graph, elmts = hppclient.manipulation.graph.getGraph()
         ids = { n.name: n.id for n in elmts.edges }

sot_hpp/tools.py

@@ -177,7 +177,6 @@ class Grasp (Manifold):
 
             self.tasks = [ self.graspTask.task ]
             #self.tasks = []
-        self += EEPosture (sotrobot, self.gripper, [-0.2 if self.closeGripper else 0])
 
     def opPointExist(self,dyn,opPoint):
         sigsP = filter(lambda x: x.getName().split(':')[-1] == opPoint,
@@ -195,16 +194,9 @@ class EEPosture (Manifold):
 
         super(EEPosture, self).__init__()
         self.gripper = gripper
-
-        # Get joint position in posture
+        robotName, gripperName = parseHppName (gripper.name)
+        self.jointNames = sotrobot.grippers[gripperName]
         pinmodel = sotrobot.dynamic.model
-        idJ = pinmodel.getJointId(gripper.sotjoint)
-        assert idJ < pinmodel.njoints
-        joint = sotrobot.dynamic.model.joints[idJ]
-        assert joint.nq == len(position)
-
-        idx_q = joint.idx_q + 1
-        idx_v = joint.idx_v + 1
 
         n = "eeposture" + Posture.sep + gripper.name + Posture.sep + str(position)
 
@@ -214,23 +206,34 @@ class EEPosture (Manifold):
 
         plug(sotrobot.dynamic.position, self.tp.feature.state)
         q = list(sotrobot.dynamic.position.value)
-        q[idx_v:idx_v + joint.nv] = position
+        # Define the reference and the selected DoF
+        rank = 0
+        ranks = []
+        for name in self.jointNames:
+            idJ = pinmodel.getJointId(name)
+            assert idJ < pinmodel.njoints
+            joint = pinmodel.joints[idJ]
+            idx_v = joint.idx_v
+            nv = joint.nv
+            ranks += range(idx_v, idx_v + nv)
+            q[idx_v:idx_v+nv] = position[rank: rank + nv]
+            rank += nv
+        assert rank == len(position)
+
         self.tp.feature.posture.value = q
+        for i in ranks:
+            self.tp.feature.selectDof (i, True)
 
         self.tp.gain = GainAdaptive("gain_"+n)
         robotDim = sotrobot.dynamic.getDimension()
-        # for i in range (6, robotDim):
-            # self.tp.feature.selectDof (i, False)
-        # print idx_q, idx_v
-        for i in range(joint.nv):
-            self.tp.feature.selectDof (idx_v + i, True)
         self.tp.add(self.tp.feature.name)
 
         # Set the gain of the posture task
         setGain(self.tp.gain,(4.9,0.9,0.01,0.9))
         plug(self.tp.gain.gain, self.tp.controlGain)
         plug(self.tp.error, self.tp.gain.error)
-        self.tasks = [ self.tp ]
+        if len(self.jointNames) > 0:
+            self.tasks = [ self.tp ]
 
 class Foot (Manifold):
     def __init__ (self, footname, sotrobot, selec='111111'):