def gotoq(self, gain=None, qdes=None, **kwargs):
act = list()
if qdes is not None:
if isinstance(qdes, tuple):
qdes = matrix(qdes).T
else:
if MetaTaskPosture.nbDof is None:
MetaTaskPosture.nbDof = len(self.feature.errorIN.value)
qdes = zeros((MetaTaskPosture.nbDof, 1))
act = [
False,
] * MetaTaskPosture.nbDof
for limbName, jointValues in kwargs.items():
limbRange = self.postureRange[limbName]
for i in limbRange:
act[i] = True
if jointValues != []:
if isinstance(jointValues, matrix):
qdes[limbRange, 0] = vectorToTuple(jointValues)
else:
qdes[limbRange, 0] = jointValues
self.ref = vectorToTuple(qdes)
if len(act) > 0:
self.feature.selec.value = Flags(act)
setGain(self.gain, gain)
def goto3D(self, point3D, gain=None, ref2D=None, selec=None):
self.target = point3D
if ref2D is not None:
self.ref = ref2D
if selec is not None:
self.feature.selec.value = selec
setGain(self.gain, gain)
def goForTheBall():
'''The hand goes for the hand.'''
taskRH.feature.selec.value = '111'
setGain(taskRH.gain,(5,0.5,0.03,0.9))
taskWaist.feature.selec.value = '011000'
ballTraj.elasticXY = 0.0
ballTraj.gravity = (0,0,-3.0)
attime(ALWAYS,graspIf)
def goForTheBall():
'''The hand goes for the hand.'''
taskRH.feature.selec.value = '111'
setGain(taskRH.gain, (5, 0.5, 0.03, 0.9))
taskWaist.feature.selec.value = '011000'
ballTraj.elasticXY = 0.0
ballTraj.gravity = (0, 0, -3.0)
attime(ALWAYS, graspIf)
def goto6dRel(task,position,positionRef,gain=None,resetJacobian=True): M=generic6dReference(position) MRef=generic6dReference(positionRef) task.featureDes.position.value = matrixToTuple(M) task.featureDes.positionRef.value = matrixToTuple(MRef) task.feature.selec.value = "111111" setGain(task.gain,gain) if 'resetJacobianDerivative' in task.task.__class__.__dict__.keys() and resetJacobian: task.task.resetJacobianDerivative()
def goto6dRel(task, position, positionRef, gain=None, resetJacobian=True):
M = generic6dReference(position)
MRef = generic6dReference(positionRef)
task.featureDes.position.value = matrixToTuple(M)
task.featureDes.positionRef.value = matrixToTuple(MRef)
task.feature.selec.value = Flags("111111")
setGain(task.gain, gain)
if 'resetJacobianDerivative' in task.task.__class__.__dict__.keys() and resetJacobian:
task.task.resetJacobianDerivative()
def grasp():
'''Ball is in the hand.'''
del attime.events[ALWAYS]
attime(ALWAYS,ballInHand)
sot.rm(taskRH.task.name)
sot.rm(taskGaze.task.name)
q0 = halfSittingConfig[robotName]
taskPosture.gotoq((5,1,0.05,0.9),q0,rarm=[],larm=[],chest=[],head=[],rhand=[0,])
setGain(taskWaist.gain,(2,0.2,0.03,0.9))
taskWaist.feature.selec.value = '011100'
def gotoNdRel(task,position,positionRef,selec=None,gain=None,resetJacobian=True): M=generic6dReference(position) MRef=generic6dReference(positionRef) if selec!=None: if isinstance(selec,str): task.feature.selec.value = selec else: task.feature.selec.value = toFlags(selec) task.featureDes.position.value = matrixToTuple(M) task.featureDes.positionRef.value = matrixToTuple(MRef) setGain(task.gain,gain) if 'resetJacobianDerivative' in task.task.__class__.__dict__.keys() and resetJacobian: task.task.resetJacobianDerivative()
def gotoNdRel(task, position, positionRef, selec=None, gain=None, resetJacobian=True):
M = generic6dReference(position)
MRef = generic6dReference(positionRef)
if selec is not None:
if not isinstance(selec, Flags):
selec = Flags(selec)
task.feature.selec.value = selec
task.featureDes.position.value = matrixToTuple(M)
task.featureDes.positionRef.value = matrixToTuple(MRef)
setGain(task.gain, gain)
if 'resetJacobianDerivative' in task.task.__class__.__dict__.keys() and resetJacobian:
task.task.resetJacobianDerivative()
def makeTasks(self, sotrobot):
from dynamic_graph.sot.core.matrix_util import matrixToTuple
from dynamic_graph.sot.core import Multiply_of_matrix, Multiply_of_matrixHomo, OpPointModifier
from dynamic_graph import plug
if self.relative:
self.graspTask = MetaTaskKine6d (
Grasp.sep + self.gripper.name + Grasp.sep + self.otherGripper.name,
sotrobot.dynamic,
self.gripper.sotjoint,
self.gripper.sotjoint)
# Creates the matrix transforming the goal gripper's position into the desired moving gripper's position:
# on the other handle of the object
#M = ((-1.0, 0.0, 0.0, 0.0), (0.0, 1.0, 0.0, 0.0), (0.0, 0.0, -1.0, -0.55), (0.0, 0.0, 0.0, 1.0))
M = transformToMatrix(self.otherGripper.sotpose * self.otherHandle.sotpose.inverse() * self.handle.sotpose * self.gripper.sotpose.inverse())
self.graspTask.opmodif = matrixToTuple(M)
self.graspTask.feature.frame("current")
setGain(self.graspTask.gain,(100,0.9,0.01,0.9))
self.graspTask.task.setWithDerivative (False)
# Sets the position and velocity of the other gripper as the goal of the first gripper pose
if not self.opPointExist(sotrobot.dynamic, self.otherGripper.sotjoint):
sotrobot.dynamic.createOpPoint(self.otherGripper.sotjoint, self.otherGripper.sotjoint)
sotrobot.dynamic.createVelocity("vel_"+self.otherGripper.sotjoint, self.otherGripper.sotjoint)
plug(sotrobot.dynamic.signal(self.otherGripper.sotjoint), self.graspTask.featureDes.position)
plug(sotrobot.dynamic.signal("vel_"+self.otherGripper.sotjoint), self.graspTask.featureDes.velocity)
#plug(sotrobot.dynamic.signal("J"+self.otherGripper.sotjoint), self.graspTask.featureDes.Jq)
# We will need a less barbaric method to do this...
# Zeroes the jacobian for the joints we don't want to use.
# - Create the selection matrix for the desired joints
mat = ()
for i in range(38):
mat += ((0,)*i + (1 if i in range(29, 36) else 0,) + (0,)*(37-i),)
# - Multiplies it with the computed jacobian matrix of the gripper
mult = Multiply_of_matrix('mult')
mult.sin2.value = mat
plug(self.graspTask.opPointModif.jacobian, mult.sin1)
plug(mult.sout, self.graspTask.feature.Jq)
self.tasks = [ self.graspTask.task ]
#self.tasks = []
self += EEPosture (sotrobot, self.gripper, [-0.2 if self.closeGripper else 0])
def grasp():
'''Ball is in the hand.'''
del attime.events[ALWAYS]
attime(ALWAYS, ballInHand)
sot.rm(taskRH.task.name)
sot.rm(taskGaze.task.name)
q0 = halfSittingConfig[robotName]
taskPosture.gotoq((5, 1, 0.05, 0.9),
q0,
rarm=[],
larm=[],
chest=[],
head=[],
rhand=[
0,
])
setGain(taskWaist.gain, (2, 0.2, 0.03, 0.9))
taskWaist.feature.selec.value = '011100'
def makePositionControl(self, position):
q = list(self.tp.feature.state.value)
# Define the reference
ip = 0
for iq, nv in self.jointRanks:
q[iq:iq + nv] = position[ip:ip + nv]
ip += nv
assert ip == len(position)
self.tp.feature.posture.value = q
setGain(self.tp.gain, (4.9, 0.9, 0.01, 0.9))
from .events import norm_inferior_to
n, c = norm_inferior_to(self.name + "_positioncmp", self.tp.error,
0.001)
self.events = {
"done_close": c.sout,
"done_open": c.sout,
}
def __init__ (self, sotrobot, gripper, position):
from dynamic_graph.sot.core import Task, FeatureGeneric, GainAdaptive, Selec_of_vector
from dynamic_graph.sot.core.matrix_util import matrixToTuple
from dynamic_graph import plug
from numpy import identity, hstack, zeros
super(EEPosture, self).__init__()
self.gripper = gripper
# Get joint position in posture
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)
self.tp = Task ('task' + n)
self.tp.dyn = sotrobot.dynamic
self.tp.feature = FeaturePosture ('feature_' + n)
plug(sotrobot.dynamic.position, self.tp.feature.state)
q = list(sotrobot.dynamic.position.value)
q[idx_v:idx_v + joint.nv] = position
self.tp.feature.posture.value = q
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 ]
def __init__(self, sotrobot, gripper, position):
from dynamic_graph.sot.core import Task, GainAdaptive
super(EEPosture, self).__init__()
self.gripper = gripper
self.jointNames = gripper.joints
pinmodel = sotrobot.dynamic.model
n = "eeposture" + Posture.sep + gripper.name + Posture.sep + str(
list(position))
self.tp = Task('task' + n)
self.tp.dyn = sotrobot.dynamic
self.tp.feature = FeaturePosture('feature_' + n)
plug(sotrobot.dynamic.position, self.tp.feature.state)
q = list(sotrobot.dynamic.position.value)
# 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)
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)
if len(self.jointNames) > 0:
self.tasks = [self.tp]
def gotoq(self,gain=None,qdes=None,**kwargs):
act=list()
if qdes!=None:
if isinstance(qdes,tuple): qdes=matrix(qdes).T
else:
if MetaTaskPosture.nbDof==None:
MetaTaskPosture.nbDof = len(self.feature.errorIN.value)
qdes = zeros((MetaTaskPosture.nbDof,1))
for limbName,jointValues in kwargs.items():
limbRange = self.postureRange[limbName]
act += limbRange
if jointValues!=[]:
if isinstance(jointValues,matrix):
qdes[limbRange,0] = vectorToTuple(jointValues)
else: qdes[limbRange,0] = jointValues
self.ref = vectorToTuple(qdes)
if(len(act)>0): self.feature.selec.value = toFlags(act)
setGain(self.gain,gain)
def __init__(self, name, sotrobot):
super(Posture, self).__init__()
from dynamic_graph.sot.core import Task, FeatureGeneric, GainAdaptive
from dynamic_graph.sot.core.matrix_util import matrixToTuple
from numpy import identity
n = Posture.sep + name
self.tp = Task('task' + n)
self.tp.dyn = sotrobot.dynamic
self.tp.feature = FeaturePosture('feature_' + n)
q = list(sotrobot.dynamic.position.value)
self.tp.feature.state.value = q
self.tp.feature.posture.value = q
robotDim = sotrobot.dynamic.getDimension()
for i in range(6, robotDim):
self.tp.feature.selectDof(i, True)
self.tp.gain = GainAdaptive("gain_" + n)
self.tp.add(self.tp.feature.name)
# Connects the dynamics to the current feature of the posture task
plug(sotrobot.dynamic.position, self.tp.feature.state)
self.tp.setWithDerivative(True)
# 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]
self.topics = {
name: {
"type": "vector",
"topic": "/hpp/target/position",
"signalGetters": [self._signalPositionRef]
},
"vel_" + name: {
"type": "vector",
"topic": "/hpp/target/velocity",
"signalGetters": [self._signalVelocityRef]
},
}
def __init__(self,
footname,
sotrobot,
selec='111111',
withDerivative=False):
super(Foot, self).__init__()
robotname, sotjoint = parseHppName(footname)
basename = self._name(footname)
# Create the FeaturePose
self.feature = FeaturePose('pose' + basename)
dyn = sotrobot.dynamic
if sotjoint not in dyn.signals():
dyn.createOpPoint(sotjoint, sotjoint)
plug(dyn.signal(sotjoint), self.feature.oMjb)
plug(dyn.signal('J' + sotjoint), self.feature.jbJjb)
# Wrap it into a Task
self.task = SotTask('task' + basename)
self.gain = GainAdaptive('gain' + basename)
self.task.add(self.feature.name)
plug(self.task.error, self.gain.error)
plug(self.gain.gain, self.task.controlGain)
setGain(self.gain, (4.9, 0.9, 0.01, 0.9))
self.task.setWithDerivative(withDerivative)
if selec != '111111':
self.feature.selec.value = selec
self.tasks = [
self.task,
]
self.addHppJointTopic(footname,
signalGetters=[self._signalPositionRef])
if withDerivative:
self.addHppJointTopic("vel_" + footname,
footname,
velocity=True,
signalGetters=[self._signalVelocityRef])
def __init__(self, sotrobot, gripper, name_suffix):
from dynamic_graph.sot.core import Task, GainAdaptive
super(EndEffector, self).__init__()
self.gripper = gripper
self.jointNames = gripper.joints
self.robot = sotrobot
pinmodel = sotrobot.dynamic.model
self.name = Posture.sep.join(["ee", gripper.name, name_suffix])
self.tp = Task('task' + self.name)
self.tp.dyn = sotrobot.dynamic
self.tp.feature = FeaturePosture('feature_' + self.name)
plug(sotrobot.dynamic.position, self.tp.feature.state)
# Select the dofs
self.tp.feature.posture.value = sotrobot.dynamic.position.value
# Define the reference and the selected DoF
self.jointRanks = []
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
self.jointRanks.append((idx_v, nv))
for idx_v, nv in self.jointRanks:
for i in range(idx_v, idx_v + nv):
self.tp.feature.selectDof(i, True)
self.tp.gain = GainAdaptive("gain_" + self.name)
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)
if len(self.jointNames) > 0:
self.tasks = [self.tp]
def __init__(self, name, sotrobot, withDerivative=False):
super(Posture, self).__init__()
n = self._name(name)
self._task = SotTask(n + '_task')
self._task.dyn = sotrobot.dynamic
self._feature = FeaturePosture(n + '_feature_')
q = list(sotrobot.dynamic.position.value)
self._feature.state.value = q
self._feature.posture.value = q
robotDim = sotrobot.dynamic.getDimension()
for i in range(6, robotDim):
self._feature.selectDof(i, True)
self._gain = GainAdaptive(n + "_gain")
self._task.add(self._feature.name)
# Connects the dynamics to the current feature of the posture task
plug(sotrobot.dynamic.position, self._feature.state)
self._task.setWithDerivative(withDerivative)
# Set the gain of the posture task
setGain(self._gain, (4.9, 0.9, 0.01, 0.9))
plug(self._gain.gain, self._task.controlGain)
plug(self._task.error, self._gain.error)
self.tasks = [self._task]
self.topics = {
name: {
"type": "vector",
"topic": "/hpp/target/position",
"signalGetters": frozenset([self._signalPositionRef])
},
}
if withDerivative:
self.topics["vel_" + name] = {
"type": "vector",
"topic": "/hpp/target/velocity",
"signalGetters": frozenset([self._signalVelocityRef])
}
def __init__ (self, name, sotrobot):
super(Posture, self).__init__()
from dynamic_graph.sot.core import Task, FeatureGeneric, GainAdaptive, Selec_of_vector
from dynamic_graph.sot.core.matrix_util import matrixToTuple
from dynamic_graph import plug
from numpy import identity, hstack, zeros
n = Posture.sep + name
self.tp = Task ('task' + n)
self.tp.dyn = sotrobot.dynamic
self.tp.feature = FeatureGeneric('feature_'+n)
self.tp.featureDes = FeatureGeneric('feature_des_'+n)
self.tp.gain = GainAdaptive("gain_"+n)
robotDim = sotrobot.dynamic.getDimension()
self.tp.feature.jacobianIN.value = matrixToTuple( identity(robotDim) )
self.tp.feature.setReference(self.tp.featureDes.name)
self.tp.add(self.tp.feature.name)
# Connects the dynamics to the current feature of the posture task
plug(sotrobot.dynamic.position, self.tp.feature.errorIN)
self.tp.setWithDerivative (True)
# Set the gain of the posture task
setGain(self.tp.gain,10)
plug(self.tp.gain.gain, self.tp.controlGain)
plug(self.tp.error, self.tp.gain.error)
self.tasks = [ self.tp ]
self.topics = {
name: {
"type": "vector",
"topic": "/hpp/target/position",
"signalGetters": [ self._signalPositionRef ] },
"vel_" + name: {
"type": "vector",
"topic": "/hpp/target/velocity",
"signalGetters": [ self._signalVelocityRef ] },
}
def makeTasks(self, sotrobot):
if self.relative:
self.graspTask = MetaTaskKine6dRel(
Grasp.sep + self.gripper.name + Grasp.sep +
self.otherGripper.name, sotrobot.dynamic, self.gripper.joint,
self.otherGripper.joint, self.gripper.joint,
self.otherGripper.joint)
M = se3ToTuple(self.gripper.pose * self.handle.pose.inverse())
self.graspTask.opPointModif.activ = True
self.graspTask.opPointModif.setTransformation(M)
# Express the velocities in local frame.
# This is the default.
# self.graspTask.opPointModif.setEndEffector(True)
M = se3ToTuple(self.otherGripper.pose *
self.otherHandle.pose.inverse())
self.graspTask.opPointModifBase.activ = True
self.graspTask.opPointModifBase.setTransformation(M)
# Express the velocities in local frame.
# This is the default.
# self.graspTask.opPointModif.setEndEffector(True)
# Plug the signals
plug(self.graspTask.opPointModif.signal('position'),
self.graspTask.feature.position)
plug(self.graspTask.opPointModif.signal('jacobian'),
self.graspTask.feature.Jq)
plug(self.graspTask.opPointModifBase.signal('position'),
self.graspTask.feature.positionRef)
plug(self.graspTask.opPointModifBase.signal('jacobian'),
self.graspTask.feature.JqRef)
setGain(self.graspTask.gain, (4.9, 0.9, 0.01, 0.9))
self.graspTask.task.setWithDerivative(False)
self.tasks = [self.graspTask.task]
def makeTasks(self, sotrobot, withDerivative = False):
if self.relative:
basename = self._name(self.gripper.name,
self.handle.name,
self.otherGripper.name,
self.otherHandle.name)
def set(oMj, jMf, jJj, g, h):
# Create the operational points
_createOpPoint (sotrobot, g.link)
jMf.value = se3ToTuple(g.lMf * h.lMf.inverse())
plug(sotrobot.dynamic.signal( g.link), oMj)
plug(sotrobot.dynamic.signal('J'+g.link), jJj)
# Create the FeaturePose
self.feature = FeaturePose (basename+"_feature")
set(self.feature.oMja, self.feature.jaMfa, self.feature.jaJja,
self.otherGripper, self.otherHandle)
set(self.feature.oMjb, self.feature.jbMfb, self.feature.jbJjb,
self.gripper, self.handle)
# Wrap it into a Task
self.task = SotTask (basename+"_task")
self.gain = GainAdaptive (basename+"_gain")
self.task.add (self.feature.name)
plug (self.task.error, self.gain.error)
plug (self.gain.gain , self.task.controlGain)
setGain(self.gain,(4.9,0.9,0.01,0.9))
if withDerivative:
print("Grasp constraint with derivative is not implemented yet.")
self.task.setWithDerivative (False)
self.tasks = [ self.task ]
"/home/ostasse/devel-src/robotpkg-test3/install/share/pyrene-motions/identification/OEM_arms_60s_500Hz"
)
aSimpleSeqPlay.setTimeToStart(10.0)
# Connects the sequence player to the posture task
from dynamic_graph.sot.core import Selec_of_vector
from dynamic_graph import plug
plug(aSimpleSeqPlay.posture, taskPosture.featureDes.errorIN)
# Connects the dynamics to the current feature of the posture task
getPostureValue = Selec_of_vector("current_posture")
getPostureValue.selec(6, robotDim)
plug(robot.dynamic.position, getPostureValue.sin)
plug(getPostureValue.sout, taskPosture.feature.errorIN)
plug(getPostureValue.sout, aSimpleSeqPlay.currentPosture)
# Set the gain of the posture task
setGain(taskPosture.gain, (4.9, 0.9, 0.01, 0.9))
plug(taskPosture.gain.gain, taskPosture.controlGain)
plug(taskPosture.error, taskPosture.gain.error)
# Create the solver
from dynamic_graph.sot.core import SOT
sot = SOT('sot')
sot.setSize(robot.dynamic.getDimension())
plug(sot.control, robot.device.control)
taskPosture.featureDes.errorIN.recompute(0)
# Push the posture task in the solver
featureVectorRH.positionRef.value = (1,0,0)
taskVectorRH = Task('taskVectorRH')
taskVectorRH.add(featureVectorRH.name)
taskVectorRH.controlGain.value = 10
featureVectorLH = FeatureVector3('fv3lh')
plug(dyn.lh,featureVectorLH.position)
plug(dyn.Jlh,featureVectorLH.Jq)
featureVectorLH.vector.value = (1,0,0)
featureVectorLH.positionRef.value = (1,0,0)
taskVectorLH = Task('taskVectorLH')
taskVectorLH.add(featureVectorLH.name)
gainVectorLH = GainAdaptive("gainVectorLH")
plug(taskVectorLH.error,gainVectorLH.error)
plug(gainVectorLH.gain,taskVectorLH.controlGain)
setGain(gainVectorLH,(10,1.5,0.01,0.9))
# Control the orientation of the gripper toward the continuous handle.
taskRHOrient=MetaTaskKine6d('rhorient',dyn,'rh','right-wrist')
taskRHOrient.opmodif = matrixToTuple(handMgrip)
taskRHOrient.feature.frame('current')
taskRHOrient.feature.selec.value = '011'
taskRHOrient.featureVector = FeatureVector3('feature-rhorient-v3')
plug(taskRHOrient.feature.position,taskRHOrient.featureVector.position)
plug(taskRHOrient.feature.Jq,taskRHOrient.featureVector.Jq)
taskRHOrient.featureVector.vector.value = (1,0,0)
taskRHOrient.task.add(taskRHOrient.featureVector.name)
setGain(taskRHOrient.gain,1)
taskLHOrient=MetaTaskKine6d('lhorient',dyn,'lh','left-wrist')
taskLHOrient.opmodif = matrixToTuple(handMgrip)
def _makeAbsolute(self, sotrobot, withMeasurementOfObjectPos,
withMeasurementOfGripperPos):
name = PreGrasp.sep.join(
["", "pregrasp", self.gripper.name, self.handle.fullName])
self.graspTask = MetaTaskKine6d(name, sotrobot.dynamic,
self.gripper.joint, self.gripper.joint)
setGain(self.graspTask.gain, (4.9, 0.9, 0.01, 0.9))
self.graspTask.task.setWithDerivative(False)
# Current gripper position
self.graspTask.opmodif = se3ToTuple(self.gripper.pose)
# Express the velocities in local frame. This is the default.
# self.graspTask.opPointModif.setEndEffector(True)
# Desired gripper position:
# Planned handle pose H_p = object_planned_pose * self.handle.pose
# Real handle pose H_r = object_real_pose * self.handle.pose
# Displacement M = H_p^-1 * H_r
# planned gripper pose G_p= joint_planned_pose * self.gripper.pose
# The derised position is
# G*_r = G_p * M = G_p * h^-1 * O_p^-1 * O_r * h
# = J_p * g * h^-1 * O_p^-1 * O_r * h
self.gripper_desired_pose = Multiply_of_matrixHomo(name + "_desired")
nsignals = 2
if withMeasurementOfGripperPos: nsignals += 1
if withMeasurementOfObjectPos: nsignals += 5
self.gripper_desired_pose.setSignalNumber(nsignals)
isignal = 0
def sig(i):
return self.gripper_desired_pose.signal("sin" + str(i))
# Object calibration
if withMeasurementOfObjectPos:
h = self.handle.pose
# TODO Integrate measurement of h_r: position error of O_r^-1 * G_r
# (for the release phase and computed only at time 0)
# self.gripper_desired_pose.sin0 -> plug to handle link planning pose
self.topics[self.handle.fullLink] = {
"velocity": False,
"type": "matrixHomo",
"handler": "hppjoint",
"hppjoint": self.handle.fullLink,
"signalGetters": [lambda: self.gripper_desired_pose.sin0],
}
isignal += 1
sig(isignal).value = se3ToTuple(h.inverse())
isignal += 1
delta_h = sig(isignal)
self.topics["delta_" + self.handle.fullLink] = {
"velocity": False,
"type": "matrixHomo",
"handler": "tf_listener",
"frame0": self.handle.fullLink,
"frame1": self.handle.fullLink + "_estimated",
"signalGetters": [lambda: delta_h],
}
isignal += 1
sig(isignal).value = se3ToTuple(h)
isignal += 1
self._oMh_p_inv = Inverse_of_matrixHomo(self.handle.fullLink +
"_invert_planning_pose")
self.topics[self.handle.fullLink]["signalGetters"] \
.append (lambda: self._oMh_p_inv.sin)
plug(self._oMh_p_inv.sout, sig(isignal))
isignal += 1
# Joint planning pose
joint_planning_pose = sig(isignal)
self.topics[self.gripper.fullJoint] = {
"velocity": False,
"type": "matrixHomo",
"handler": "hppjoint",
"hppjoint": self.gripper.fullJoint,
"signalGetters": [lambda: joint_planning_pose],
}
isignal += 1
# Joint calibration
if withMeasurementOfGripperPos:
self._delta_g_inv = Inverse_of_matrixHomo(self.gripper.fullJoint +
"_delta_inv")
self.topics["delta_" + self.gripper.fullJoint] = {
"velocity": False,
"type": "matrixHomo",
"handler": "tf_listener",
"frame0": self.gripper.fullJoint,
"frame1": self.gripper.fullJoint + "_estimated",
"signalGetters": [lambda: self._delta_g_inv.sin],
}
plug(self._delta_g_inv.sout, self.gripper_desired_pose.sin6)
isignal += 1
sig(isignal).value = se3ToTuple(self.gripper.pose)
isignal += 1
assert isignal == nsignals
plug(self.gripper_desired_pose.sout,
self.graspTask.featureDes.position)
self.tasks = [self.graspTask.task]
def _makeRelativeTask(self, sotrobot, withMeasurementOfObjectPos,
withMeasurementOfGripperPos):
assert self.handle.robotName == self.otherHandle.robotName
assert self.handle.link == self.otherHandle.link
name = PreGrasp.sep.join([
"", "pregrasp", self.gripper.name, self.handle.fullName, "based",
self.otherGripper.name, self.handle.fullName
])
self.graspTask = MetaTaskKine6dRel(name, sotrobot.dynamic,
self.gripper.joint,
self.otherGripper.joint,
self.gripper.joint,
self.otherGripper.joint)
self.graspTask.opmodif = se3ToTuple(self.gripper.pose *
self.handle.pose.inverse())
# Express the velocities in local frame. This is the default.
# self.graspTask.opPointModif.setEndEffector(True)
self.graspTask.opmodifBase = se3ToTuple(
self.otherGripper.pose * self.otherHandle.pose.inverse())
# Express the velocities in local frame. This is the default.
# self.graspTask.opPointModif.setEndEffector(True)
setGain(self.graspTask.gain, (4.9, 0.9, 0.01, 0.9))
self.graspTask.task.setWithDerivative(False)
self.gripper_desired_pose = Multiply_of_matrixHomo(name + "_desired1")
self.otherGripper_desired_pose = Multiply_of_matrixHomo(name +
"_desired2")
if withMeasurementOfObjectPos:
# TODO Integrate measurement of h1_r and h2_r: position error
# O_r^-1 * G1_r and O_r^-1 * G2_r
# (for the release phase and computed only at time 0)
print("Relative grasp with measurement is NOT IMPLEMENTED")
# else:
# G2*_r = H1_p * h1^-1 * h2 = G2_p = J2_p * G
self.gripper_desired_pose.setSignalNumber(2)
# self.gripper_desired_pose.sin0 -> plug to joint planning pose
self.gripper_desired_pose.sin1.value = se3ToTuple(
self.gripper.pose * self.handle.pose.inverse())
self.otherGripper_desired_pose.setSignalNumber(2)
# self.otherGripper_desired_pose.sin0 -> plug to otherJoint planning pose
self.otherGripper_desired_pose.sin1.value = se3ToTuple(
self.otherGripper.pose * self.otherHandle.pose.inverse())
plug(self.gripper_desired_pose.sout,
self.graspTask.featureDes.position)
plug(self.otherGripper_desired_pose.sout,
self.graspTask.featureDes.positionRef)
self.topics = {
self.gripper.fullJoint: {
"velocity": False,
"type": "matrixHomo",
"handler": "hppjoint",
"hppjoint": self.gripper.fullJoint,
"signalGetters": [
lambda: self.gripper_desired_pose.sin0,
]
},
self.otherGripper.fullJoint: {
"velocity": False,
"type": "matrixHomo",
"handler": "hppjoint",
"hppjoint": self.otherGripper.fullJoint,
"signalGetters": [
lambda: self.otherGripper_desired_pose.sin0,
]
},
}
self.tasks = [self.graspTask.task]
def _makeAbsoluteBasedOnOther(self, sotrobot, withMeasurementOfObjectPos,
withMeasurementOfGripperPos):
assert self.handle.robotName == self.otherHandle.robotName
assert self.handle.link == self.otherHandle.link
name = PreGrasp.sep.join([
"", "pregrasp", self.gripper.fullName, self.handle.fullName,
"based", self.otherGripper.name, self.otherHandle.fullName
])
self.graspTask = MetaTaskKine6d(name, sotrobot.dynamic,
self.otherGripper.joint,
self.otherGripper.joint)
setGain(self.graspTask.gain, (4.9, 0.9, 0.01, 0.9))
self.graspTask.task.setWithDerivative(False)
# Current gripper position
self.graspTask.opmodif = se3ToTuple(self.otherGripper.pose)
# Express the velocities in local frame. This is the default.
# self.graspTask.opPointModif.setEndEffector(True)
# Desired gripper position:
# Displacement gripper1: M = G1_p^-1 * G1_r
# The derised position of handle1 is
# H1*_r = H1_p * M = H1_p * G1_p^-1 * G1_r
#
# Moreover, the relative position of handle2 wrt to gripper2 may not be perfect so
# h2_r = O_r^-1 * G2_r
#
# G2*_r = O*_r * h2_r = H1_p * G1_p^-1 * G1_r * h1^-1 * h2_r
# O_p * h1 * G1_p^-1 * G1_r * h1^-1 * h2_r
# O_p * h1 * g1^-1 * J1_p^-1 * J1_r * g1 * h1^-1 * h2_r
# where h2_r can be a constant value of the expression above.
self.gripper_desired_pose = Multiply_of_matrixHomo(name + "_desired")
if withMeasurementOfObjectPos:
# TODO Integrate measurement of h1_r and h2_r: position error
# O_r^-1 * G1_r and O_r^-1 * G2_r
# (for the release phase and computed only at time 0)
self.gripper_desired_pose.setSignalNumber(5)
## self.gripper_desired_pose.setSignalNumber (7)
# self.gripper_desired_pose.sin0 -> plug to object1 planning pose
# self.gripper_desired_pose.sin1.value = se3ToTuple(self.handle.pose)
self.gripper_desired_pose.sin1.value = se3ToTuple(
self.handle.pose * self.gripper.pose.inverse())
self._invert_planning_pose = Inverse_of_matrixHomo(
self.gripper.fullLink + "_invert_planning_pose")
# self._invert_planning_pose.sin -> plug to link1 planning pose
plug(self._invert_planning_pose.sout,
self.gripper_desired_pose.sin2)
# self.gripper_desired_pose.sin3 -> plug to link1 real pose
# self.gripper_desired_pose.sin4.value = se3ToTuple (self.handle.pose.inverse() * self.otherHandle.pose)
self.gripper_desired_pose.sin4.value = se3ToTuple(
self.gripper.pose * self.handle.pose.inverse() *
self.otherHandle.pose)
## self.gripper_desired_pose.sin4.value = se3ToTuple (self.gripper.pose * self.handle.pose.inverse())
## self.gripper_desired_pose.sin5 -> plug to object real pose
## if self.otherGripper.joint not in sotrobot.dyn.signals():
## sotrobot.dyn.createOpPoint (self.otherGripper.joint, self.otherGripper.joint)
## plug(sotrobot.dyn.signal(self.otherGripper.joint), self.gripper_desired_pose.sin6)
plug(self.gripper_desired_pose.sout,
self.graspTask.featureDes.position)
self.topics = {
self.handle.fullLink: {
"velocity": False,
"type": "matrixHomo",
"handler": "hppjoint",
"hppjoint": self.handle.fullLink,
"signalGetters": [
lambda: self.gripper_desired_pose.sin0,
]
},
self.gripper.fullLink: {
"velocity": False,
"type": "matrixHomo",
"handler": "hppjoint",
"hppjoint": self.gripper.fullLink,
"signalGetters": [
lambda: self._invert_planning_pose.sin,
]
},
"measurement_" + self.gripper.fullLink: {
"velocity": False,
"type": "matrixHomo",
"handler": "tf_listener",
"frame0": "world",
"frame1": self.gripper.fullLink,
"signalGetters": [
lambda: self.gripper_desired_pose.sin3,
]
},
## "measurement_" + self.otherHandle.fullLink: {
## "velocity": False,
## "type": "matrixHomo",
## "handler": "tf_listener",
## "frame0": "world",
## "frame1": self.otherHandle.fullLink,
## "signalGetters": [ self.gripper_desired_pose.sin5 ] },
}
else:
# G2*_r = H1_p * h1^-1 * h2 = G2_p = J2_p * G
self.gripper_desired_pose.setSignalNumber(2)
# self.gripper_desired_pose.sin0 -> plug to joint planning pose
self.gripper_desired_pose.sin1.value = se3ToTuple(
self.otherGripper.pose)
self.topics = {
self.otherGripper.fullJoint: {
"velocity": False,
"type": "matrixHomo",
"handler": "hppjoint",
"hppjoint": self.otherGripper.fullJoint,
"signalGetters": [
lambda: self.gripper_desired_pose.sin0,
]
},
}
plug(self.gripper_desired_pose.sout,
self.graspTask.featureDes.position)
self.tasks = [self.graspTask.task]
def makeAdmittanceControl(self,
affordance,
type,
period,
simulateTorqueFeedback=False,
filterCurrents=True):
# Make the admittance controller
from agimus_sot.control.gripper import AdmittanceControl, PositionAndAdmittanceControl
from .events import norm_superior_to
# type = "open" or "close"
desired_torque = affordance.ref["torque"]
estimated_theta_close = affordance.ref["angle_close"]
wn, z, nums, denoms = affordance.getControlParameter()
if affordance.controlType[type] == "torque":
self.ac = AdmittanceControl("AC_" + self.name + "_" + type,
estimated_theta_close, desired_torque,
period, nums, denoms)
elif affordance.controlType[type] == "position_torque":
theta_open = affordance.ref["angle_open"]
threshold_up = tuple([x / 10. for x in desired_torque])
threshold_down = tuple([x / 100. for x in desired_torque])
self.ac = PositionAndAdmittanceControl(
"AC_" + self.name + "_" + type, theta_open,
estimated_theta_close, desired_torque, period, threshold_up,
threshold_down, wn, z, nums, denoms)
else:
raise NotImplementedError("Control type " + type +
" is not implemented for gripper.")
if simulateTorqueFeedback:
# Get torque from an internal simulation
M, d, k, x0 = affordance.getSimulationParameters()
self.ac.setupFeedbackSimulation(M, d, k, x0)
# Must be done after setupFeedbackSimulation
self.ac.readPositionsFromRobot(self.robot, self.jointNames)
else:
self.ac.readPositionsFromRobot(self.robot, self.jointNames)
# Get torque from robot sensors (or external simulation)
# TODO allows to switch between current and torque sensors
self.ac.readCurrentsFromRobot(self.robot, self.jointNames,
(self.gripper.torque_constant, ),
filterCurrents)
# self.ac.readTorquesFromRobot(self.robot, self.jointNames)
from dynamic_graph.sot.core.operator import Mix_of_vector
mix_of_vector = Mix_of_vector(self.name + "_control_to_robot_control")
mix_of_vector.default.value = tuple([
0,
] * len(self.tp.feature.posture.value))
mix_of_vector.setSignalNumber(2)
for idx_v, nv in self.jointRanks:
mix_of_vector.addSelec(1, idx_v, nv)
# Plug the admittance controller to the posture task
setGain(self.tp.gain, 1.)
plug(self.ac.outputPosition, mix_of_vector.signal("sin1"))
plug(mix_of_vector.sout, self.tp.feature.posture)
# TODO plug posture dot ?
# I do not think it is necessary.
# TODO should we send to the posture task
# - the current robot posture
# - the postureDot from self.ac.outputDerivative
# This would avoid the last integration in self.ac.
# This integration does not know the initial point so
# there might be some drift (removed the position controller at the beginning)
n, c = norm_superior_to(self.name + "_torquecmp",
self.ac.currentTorqueIn,
0.9 * np.linalg.norm(desired_torque))
self.events = {
"done_close": c.sout,
}
def goto3D(self,point3D,gain=None,ref2D=None,selec=None):
self.target = point3D
if ref2D!=None: self.ref=ref2D
if selec!=None: self.feature.selec.value = selec
setGain(self.gain,gain)
robotDim = robot.dynamic.getDimension()
assert robotDim == len(q)
with_wheels = robotDim == 19
for i in range(8 if with_wheels else 6, robotDim):
tp.feature.selectDof(i, True)
tp.gain = GainAdaptive("gain_" + n)
tp.add(tp.feature.name)
# Connects the dynamics to the current feature of the posture task
plug(robot.dynamic.position, tp.feature.state)
# Set the gain of the posture task
setGain(tp.gain, (4.9, 0.9, 0.01, 0.9))
plug(tp.gain.gain, tp.controlGain)
plug(tp.error, tp.gain.error)
def getJointIdxQ(name):
model = robot.pinocchioModel
jid = model.getJointId(name)
return model.joints[jid].idx_q - 1
# Set initial state.
# q[getJointIdxQ("torso_lift_joint")] = 0.25
# q[getJointIdxQ("arm_1_joint")] = 0.2
# q[getJointIdxQ("arm_2_joint")] = 0.35
# q[getJointIdxQ("arm_3_joint")] = -0.2