def buildGrasp(self, g, h, otherGrasp=None):
gf = self.graphfactory
if h is None:
gripper_open = self._buildGripper("open", g, h)
return {'gripper_open': gripper_open}
gripper = gf.gripperFrames[g]
handle = gf.handleFrames[h]
# get affordance
try:
aff = gf.affordances[(g, h)]
useMeasurementOfObjectPose = aff.useMeasurementOfObjectPose(handle)
useMeasurementOfGripperPose = aff.useMeasurementOfGripperPose(
gripper)
except KeyError:
useMeasurementOfObjectPose = handle.hasVisualTag
useMeasurementOfGripperPose = gripper.hasVisualTag
gripper_close = self._buildGripper("close", g, h)
pregrasp = PreGrasp(gripper, handle, otherGrasp)
pregrasp.makeTasks(gf.sotrobot, useMeasurementOfObjectPose,
useMeasurementOfGripperPose)
if not gripper.enabled:
# TODO If otherGrasp is not None,
# we should include the grasp function of otherGrasp, not pregrasp function...
grasp = Manifold()
else:
grasp = Grasp(gripper, handle, otherGrasp)
grasp.makeTasks(gf.sotrobot)
return {
'grasp': grasp,
'pregrasp': pregrasp,
'gripper_close': gripper_close
}
def __init__(self, tasks, grasps, factory):
self.name = factory._stateName(grasps)
self.grasps = grasps
self.manifold = Manifold()
objectsAlreadyGrasped = {}
for ig, ih in enumerate(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)
else:
# Add task gripper_open
self.manifold += tasks.g(factory.grippers[ig], None,
'gripper_open')
def _buildGripper(self, type, gripper, handle):
key = (type, gripper, handle)
if self._grippers.has_key(key):
return self._grippers[key]
try:
aff = self.graphfactory.affordances[(gripper, handle)]
except KeyError:
# If there are no affordance, do not add a task.
self._grippers[key] = Manifold()
return self._grippers[key]
gf = self.graphfactory
gripperFrame = gf.gripperFrames[gripper]
if not gripperFrame.enabled:
self._grippers[key] = Manifold()
return self._grippers[key]
robot = gf.sotrobot
if aff.controlType[type] == "position":
ee = EndEffector(
robot, gripperFrame,
"p" + type + ("_" + handle if handle is not None else ""))
ee.makePositionControl(aff.ref["angle_" + type])
self._grippers[key] = ee
elif aff.controlType[type] == "torque" or aff.controlType[
type] == "position_torque":
ee = EndEffector(
robot, gripperFrame,
"pt_" + type + ("_" + handle if handle is not None else ""))
ee.makeAdmittanceControl(aff,
type,
period=gf.parameters["period"],
simulateTorqueFeedback=gf.parameters.get(
"simulateTorqueFeedback", False))
if gf.parameters["addTracerToAdmittanceController"]:
tracer = ee.ac.addTracerRealTime(robot)
gf.tracers[tracer.name] = tracer
self._grippers[key] = ee
else:
raise NotImplementedError("Control type " + type +
" is not implemented for gripper.")
return self._grippers[key]
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 _manifold (self, idxs):
if self.grasps.has_key(idxs):
return self.grasps[idxs]
if len(idxs) == 1:
m = Grasp(self.grippers[idxs[0][0]], self.handles[idxs[0][1]])
m.makeTasks(self.sotrobot)
elif len(idxs) == 0:
m = Manifold()
else:
subm = self._manifold(idxs[:-1])
# TODO Find relative
m = Grasp(self.grippers[idxs[-1][0]], self.handles[idxs[-1][1]])
m.makeTasks(self.sotrobot)
m += subm
self.grasps[idxs] = m
return m
def _hpTasks(sotrobot):
return Manifold()