Commit 29c9c516 authored by Joseph Mirabel's avatar Joseph Mirabel Committed by Joseph Mirabel

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

parent 4463d8da
......@@ -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 }
......
......@@ -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'):
......
Markdown is supported
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