def createJointsTask(diag = None, gain = 1, selec = toFlags(range(0,robot.dimension)), jointsPos = tuple(numpy.zeros(robot.dimension))):
taskJoints = MetaTaskJointWeights('jointsTask',selec,robot,diag,gain,0)
jointsVel = numpy.zeros(robot.dimension)
taskJoints.task.setVelocity(tuple(jointsVel))
taskJoints.task.setPositionDes(tuple(jointsPos))
plug(robot.dynamic.position,taskJoints.task.position)
return taskJoints
def createJointsTask(diag = None, gain = 1, selec = toFlags(range(0,robot.dimension)), jointsPos = tuple(numpy.zeros(robot.dimension))):
taskJoints = MetaTaskJointWeights('jointsTask',selec,robot,diag,gain,0)
jointsVel = numpy.zeros(robot.dimension)
taskJoints.task.setVelocity(tuple(jointsVel))
taskJoints.task.setPositionDes(tuple(jointsPos))
plug(robot.dynamic.position,taskJoints.task.position)
return taskJoints
def createWeightsTask(diag=None, gain=0.001, sampleInterval=0, selec=toFlags(range(0, robot.dimension))):
taskWeights = MetaTaskJointWeights("jointWeights", selec, robot, diag, gain, sampleInterval)
plug(robot.device.velocity, taskWeights.task.velocity)
jointsPos = numpy.zeros(robot.dimension)
taskWeights.task.setPosition(tuple(jointsPos))
taskWeights.task.setPositionDes(tuple(jointsPos))
taskWeights.task.velocity.recompute(0)
return taskWeights
def createWeightsTask(diag = None, gain = 0.001, sampleInterval = 0, selec = toFlags(range(0,robot.dimension))):
taskWeights = MetaTaskJointWeights('jointWeights',selec,robot,diag,gain,sampleInterval)
plug(robot.device.velocity,taskWeights.task.velocity)
jointsPos = numpy.zeros(robot.dimension)
taskWeights.task.setPosition(tuple(jointsPos))
taskWeights.task.setPositionDes(tuple(jointsPos))
taskWeights.task.velocity.recompute(0)
return taskWeights
def gotoNd(task,position,selec=None,gain=None,resetJacobian=True):
M=generic6dReference(position)
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)
setGain(task.gain,gain)
if 'resetJacobianDerivative' in task.task.__class__.__dict__.keys() and resetJacobian:
task.task.resetJacobianDerivative()
def gotoNd(task,position,selec=None,gain=None,resetJacobian=True):
M=generic6dReference(position)
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)
setGain(task.gain,gain)
if 'resetJacobianDerivative' in task.task.__class__.__dict__.keys() and resetJacobian:
task.task.resetJacobianDerivative()
def change6dPositionReference(task,feature,gain,position,selec=None,ingain=None,resetJacobian=True):
M=generic6dReference(position)
if selec!=None:
if isinstance(selec,str): feature.selec.value = selec
else: feature.selec.value = toFlags(selec)
feature.reference.value = matrixToTuple(M)
if gain!=None: setGain(gain,ingain)
if 'resetJacobianDerivative' in task.__class__.__dict__.keys() and resetJacobian:
task.resetJacobianDerivative()
def Pr2JointLimitsTask(robot,dt):
robot.dynamic.upperJl.recompute(0)
robot.dynamic.lowerJl.recompute(0)
task = TaskJointLimits('taskJL')
plug(robot.dynamic.position,task.position)
task.controlGain.value = 10
task.referenceInf.value = robot.dynamic.lowerJl.value
task.referenceSup.value = robot.dynamic.upperJl.value
task.dt.value = dt
task.selec.value = toFlags(range(18,25)+range(26,27)+range(28,31)+range(32,40)+range(41,42)+range(43,46)+range(47,50))
return task
def zeroPosition():
diag = None
gain = 0.01
jointsPos = numpy.zeros(robot.dimension)
selec = toFlags(range(6, robot.dimension))
taskZeroPosition = MetaTaskJointWeights("zeroPos", selec, robot, diag, gain, 0)
jointsVel = numpy.zeros(robot.dimension)
taskZeroPosition.task.setVelocity(tuple(jointsVel))
taskZeroPosition.task.setPositionDes(tuple(jointsPos))
plug(robot.dynamic.position, taskZeroPosition.task.position)
return taskZeroPosition
def zeroPosition():
diag = None
gain = 0.01
jointsPos = numpy.zeros(robot.dimension)
selec = toFlags(range(6,robot.dimension))
taskZeroPosition = MetaTaskJointWeights('zeroPos',selec,robot,diag,gain,0)
jointsVel = numpy.zeros(robot.dimension)
taskZeroPosition.task.setVelocity(tuple(jointsVel))
taskZeroPosition.task.setPositionDes(tuple(jointsPos))
plug(robot.dynamic.position,taskZeroPosition.task.position)
return taskZeroPosition
def setTaskLim(taskJL,robot):
"""
Sets the parameters for the 'joint-limits'
"""
robot.dynamic.upperJl.recompute(0)
robot.dynamic.lowerJl.recompute(0)
plug(robot.dynamic.position,taskJL.position)
taskJL.controlGain.value = 10
taskJL.referenceInf.value = robot.dynamic.lowerJl.value
taskJL.referenceSup.value = robot.dynamic.upperJl.value
taskJL.dt.value = robot.timeStep
taskJL.selec.value = toFlags(range(6,22)+range(22,28)+range(29,35))
def setTaskLim(taskJL, robot):
"""
Sets the parameters for the 'joint-limits'
"""
robot.dynamic.upperJl.recompute(0)
robot.dynamic.lowerJl.recompute(0)
plug(robot.dynamic.position, taskJL.position)
taskJL.controlGain.value = 10
taskJL.referenceInf.value = robot.dynamic.lowerJl.value
taskJL.referenceSup.value = robot.dynamic.upperJl.value
taskJL.dt.value = robot.timeStep
taskJL.selec.value = toFlags(range(6, 22) + range(22, 28) + range(29, 35))
def gotoNd(task, position, selec, gain=None, resetJacobian=True):
M = eye(4)
if isinstance(position, matrix): position = vectorToTuple(position)
if (len(position) == 3): M[0:3, 3] = position
else:
#print "Position 6D with rotation ... todo" # done?
M = array(rpy2tr(*position[3:7]))
M[0:3, 3] = position[0:3]
if isinstance(selec, str): task.feature.selec.value = selec
else: task.feature.selec.value = toFlags(selec)
task.featureDes.position.value = matrixToTuple(M)
if resetJacobian: task.task.resetJacobianDerivative()
setGain(task.gain, gain)
def gotoNd(task,position,selec,gain=None,resetJacobian=True):
M=eye(4)
if isinstance(position,matrix): position = vectorToTuple(position)
if( len(position)==3 ): M[0:3,3] = position
else:
#print "Position 6D with rotation ... todo" # done?
M = array( rpy2tr(*position[3:7]) )
M[0:3,3] = position[0:3]
if isinstance(selec,str): task.feature.selec.value = selec
else: task.feature.selec.value = toFlags(selec)
task.featureDes.position.value = matrixToTuple(M)
if resetJacobian: task.task.resetJacobianDerivative()
setGain(task.gain,gain)
def Pr2JointLimitsTask(robot, dt):
robot.dynamic.upperJl.recompute(0)
robot.dynamic.lowerJl.recompute(0)
task = TaskJointLimits('taskJL')
plug(robot.dynamic.position, task.position)
task.controlGain.value = 10
task.referenceInf.value = robot.dynamic.lowerJl.value
task.referenceSup.value = robot.dynamic.upperJl.value
task.dt.value = dt
task.selec.value = toFlags(
range(18, 25) + range(26, 27) + range(28, 31) + range(32, 40) +
range(41, 42) + range(43, 46) + range(47, 50))
return task
def safePosition(gain=0.01):
diag = None
safePos = numpy.zeros(robot.dimension)
safePos[9] = 0.2
safePos[16] = 0.2
jointsPos = tuple(safePos)
selec = toFlags(range(6,robot.dimension))
taskSafePosition = MetaTaskJointWeights('safePos',selec,robot,diag,gain,0)
jointsVel = numpy.zeros(robot.dimension)
taskSafePosition.task.setVelocity(tuple(jointsVel))
taskSafePosition.task.setPositionDes(tuple(jointsPos))
plug(robot.dynamic.position,taskSafePosition.task.position)
return taskSafePosition
def safePosition(gain=0.01):
diag = None
safePos = numpy.zeros(robot.dimension)
safePos[9] = 0.2
safePos[16] = 0.2
jointsPos = tuple(safePos)
selec = toFlags(range(6, robot.dimension))
taskSafePosition = MetaTaskJointWeights("safePos", selec, robot, diag, gain, 0)
jointsVel = numpy.zeros(robot.dimension)
taskSafePosition.task.setVelocity(tuple(jointsVel))
taskSafePosition.task.setPositionDes(tuple(jointsPos))
plug(robot.dynamic.position, taskSafePosition.task.position)
return taskSafePosition
def gotoq(self, gain=None, **kwargs):
act = list()
if MetaTaskDynPosture.nbDof == None:
MetaTaskDynPosture.nbDof = len(self.feature.errorIN.value)
qdes = zeros((MetaTaskDynPosture.nbDof, 1))
for n, v in kwargs.items():
r = self.postureRange[n]
act += r
if isinstance(v, matrix): qdes[r, 0] = vectorToTuple(v)
if isinstance(v, ndarray): qdes[r, 0] = vectorToTuple(v)
else: qdes[r, 0] = v
self.ref = vectorToTuple(qdes)
self.feature.selec.value = toFlags(act)
setGain(self.gain, gain)
def gotoq(self,gain=None,**kwargs):
act=list()
if MetaTaskDynPosture.nbDof==None:
MetaTaskDynPosture.nbDof = len(self.feature.errorIN.value)
qdes = zeros((MetaTaskDynPosture.nbDof,1))
for n,v in kwargs.items():
r = self.postureRange[n]
act += r
if isinstance(v,matrix): qdes[r,0] = vectorToTuple(v)
if isinstance(v,ndarray): qdes[r,0] = vectorToTuple(v)
else: qdes[r,0] = v
self.ref = vectorToTuple(qdes)
self.feature.selec.value = toFlags(act)
setGain(self.gain,gain)
def __init__(self, dyn, config=None, name="config"):
self.dyn = dyn
self.name = name
self.config = config
self.feature = FeatureGeneric('feature' + name)
self.featureDes = FeatureGeneric('featureDes' + name)
self.gain = GainAdaptive('gain' + name)
plug(dyn.position, self.feature.errorIN)
robotDim = dyn.getDimension()
self.feature.jacobianIN.value = matrixToTuple(identity(robotDim))
self.feature.setReference(self.featureDes.name)
if config is not None:
self.feature.selec.value = toFlags(self.config)
def setTaskLim(taskJL,robot):
"""
Sets the parameters for the 'joint-limits'
"""
robot.dynamic.upperJl.recompute(0)
robot.dynamic.lowerJl.recompute(0)
plug(robot.dynamic.position,taskJL.position)
taskJL.controlGain.value = 10
refInf = array(robot.dynamic.lowerJl.value)
# Avoid knee singularity
refInf[9] = 0.7
refInf[15] = 0.7
taskJL.referenceInf.value = vectorToTuple(refInf)
taskJL.referenceSup.value = robot.dynamic.upperJl.value
taskJL.dt.value = robot.timeStep
taskJL.selec.value = toFlags(range(6,22)+range(22,28)+range(29,35))
def change6dPositionReference(task,
feature,
gain,
position,
selec=None,
ingain=None,
resetJacobian=True):
M = generic6dReference(position)
if selec != None:
if isinstance(selec, str): feature.selec.value = selec
else: feature.selec.value = toFlags(selec)
feature.reference.value = matrixToTuple(M)
if gain != None: setGain(gain, ingain)
if 'resetJacobianDerivative' in task.__class__.__dict__.keys(
) and resetJacobian:
task.resetJacobianDerivative()
def gotoNdComp(task,position,selec=None,gain=None,resetJacobian=True,comp=[[0,1,0],[0,0,1],[1,0,0]]):
'''
gotoNdComp takes care of the different frame orientations used in jrl-dynamics.
Be careful about the comp matrix, it is specific for reem and reemc (and could be different among
different joint frames)
'''
M = generic6dReference(position.copy())
R = M[0:3,0:3]
R = R*comp
M[0:3,0:3] = R
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)
setGain(task.gain,gain)
if 'resetJacobianDerivative' in task.task.__class__.__dict__.keys() and resetJacobian:
task.task.resetJacobianDerivative()
def gotoNdComp(task,position,selec=None,gain=None,resetJacobian=True,comp=[[0,1,0],[0,0,1],[1,0,0]]):
'''
gotoNdComp takes care of the different frame orientations used in jrl-dynamics.
Be careful about the comp matrix, it is specific for reem and reemc (and could be different among
different joint frames)
'''
M = generic6dReference(position.copy())
R = M[0:3,0:3]
R = R*comp
M[0:3,0:3] = R
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)
setGain(task.gain,gain)
if 'resetJacobianDerivative' in task.task.__class__.__dict__.keys() and resetJacobian:
task.task.resetJacobianDerivative()
def createJointLimitsTask(gain=1, dt=None):
robot.dynamic.upperJl.recompute(0)
robot.dynamic.lowerJl.recompute(0)
taskJL = TaskJointLimits("jointLimits")
plug(robot.dynamic.position, taskJL.position)
"""
The internal gain for the joint limits task is defined as 1/gain, i.e. is used to limit the maximum reachable
velocity in a single time interval (dt).
A high value can be used to limit oscillations around the goal point but it slows down the motion.
"""
taskJL.controlGain.value = gain
taskJL.referenceInf.value = robot.dynamic.lowerJl.value
taskJL.referenceSup.value = robot.dynamic.upperJl.value
taskJL.selec.value = toFlags(range(6, robot.dimension))
if dt:
taskJL.dt.value = dt
else:
plug(robot.device.dt, taskJL.dt)
return taskJL
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 createJointLimitsTask(gain=1, dt=None):
robot.dynamic.upperJl.recompute(0)
robot.dynamic.lowerJl.recompute(0)
taskJL = TaskJointLimits('jointLimits')
plug(robot.dynamic.position, taskJL.position)
"""
The internal gain for the joint limits task is defined as 1/gain, i.e. is used to limit the maximum reachable
velocity in a single time interval (dt).
A high value can be used to limit oscillations around the goal point but it slows down the motion.
"""
taskJL.controlGain.value = gain
taskJL.referenceInf.value = robot.dynamic.lowerJl.value
taskJL.referenceSup.value = robot.dynamic.upperJl.value
taskJL.selec.value = toFlags(range(6, robot.dimension))
if (dt):
taskJL.dt.value = dt
else:
plug(robot.device.dt, taskJL.dt)
return taskJL
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)
taskFoV.task.referenceSup.value = (Xmax,Ymax) # Xmax, Ymax
taskFoV.task.dt.value=dt
taskFoV.task.controlGain.value=0.01
taskFoV.featureDes.xy.value = (0,0)
# --- Task Joint Limits -----------------------------------------
robot.dynamic.upperJl.recompute(0)
robot.dynamic.lowerJl.recompute(0)
taskJL = TaskJointLimits('taskJL')
plug(robot.dynamic.position,taskJL.position)
taskJL.controlGain.value = 10
taskJL.referenceInf.value = robot.dynamic.lowerJl.value
taskJL.referenceSup.value = robot.dynamic.upperJl.value
taskJL.dt.value = dt
taskJL.selec.value = toFlags(range(6,22)+range(22,28)+range(29,35))
# --- CONTACTS
# define contactLF and contactRF
for name,joint in [ ['LF','left-ankle'], ['RF','right-ankle' ] ]:
contact = MetaTaskKine6d('contact'+name,robot.dynamic,name,joint)
contact.feature.frame('desired')
contact.gain.setConstant(10)
locals()['contact'+name] = contact
# --- TRACER -----------------------------------------------------------------
tr = Tracer('tr')
tr.open('/tmp/','','.dat')
tr.start()
robot.device.after.addSignal('tr.triger')
#tr.add(dyn.name+'.ffposition','ff')
tr.add(taskRF.featureDes.name+'.position','refr')
tr.add(taskLF.featureDes.name+'.position','refl')
tr.add('dyn.rf','r')
tr.add('dyn.lf','l')
tr.add('featureComDes.errorIN','comref')
tr.add('dyn.com','com')
tr.add(taskWaist.gain.name+'.gain','gainwaist')
history = History(dyn,1)
# --- RUN -----------------------------------------------------------------
featurePosture.selec.value = toFlags( range(6,36) )
sot.clear()
for task in [taskWaist, taskRF, taskLF]:
task.feature.position.recompute(0)
task.feature.keep()
task.feature.selec.value = '111111'
sot.push(task.task.name)
taskWaist.ref = matrixToTuple(eye(4))
taskWaist.feature.selec.value = '111011'
taskWaist.gain.setByPoint(18,0.1,0.005,0.8)
attime(200
,(lambda : sot.rm(taskWaist.task.name),'Rm waist')
,(lambda : pg.initState(),'Init PG')
def gotoq(qref, selec=None):
featurePostureDes.errorIN.value = vectorToTuple(qref.transpose())
if selec != None:
featurePosture.selec.value = toFlags(selec)
[go, stop, next, n] = loopShortcuts(runner)
# Tasks
taskzero = MetaTaskKine6d('rh', robot.dynamic, 'rh', 'right-wrist')
taskzero.feature.frame('desired')
robot.dynamic.upperJl.recompute(0)
robot.dynamic.lowerJl.recompute(0)
taskJL = TaskJointLimits('taskJL')
plug(robot.dynamic.position, taskJL.position)
taskJL.controlGain.value = 10
taskJL.referenceInf.value = robot.dynamic.lowerJl.value
taskJL.referenceSup.value = robot.dynamic.upperJl.value
taskJL.dt.value = dt
taskJL.selec.value = toFlags(
range(18, 25) + range(26, 27) + range(28, 31) + range(32, 40) +
range(41, 42) + range(43, 46) + range(47, 50))
taskContact = MetaTaskKine6d('contact', robot.dynamic, 'contact', 'left-ankle')
taskContact.feature.frame('desired')
taskContact.feature.selec.value = '011100'
taskContact.gain.setConstant(10)
taskFixed = MetaTaskKine6d('contactFixed', robot.dynamic, 'contact',
'left-ankle')
taskFixed.feature.frame('desired')
taskFixed.gain.setConstant(10)
# Problem
from dynamic_graph.sot.core.meta_tasks_kine import gotoNd
targetRH = (0.65, -0.2, 0.9)
def gotoq( qref,selec=None ):
featurePostureDes.errorIN.value = vectorToTuple( qref.transpose() )
if selec!=None: featurePosture.selec.value = toFlags( selec )
#tr.add(dyn.name+'.ffposition','ff')
tr.add(taskRF.featureDes.name + '.position', 'refr')
tr.add(taskLF.featureDes.name + '.position', 'refl')
tr.add('dyn.rf', 'r')
tr.add('dyn.lf', 'l')
tr.add('featureComDes.errorIN', 'comref')
tr.add('dyn.com', 'com')
tr.add(taskWaist.gain.name + '.gain', 'gainwaist')
history = History(dyn, 1)
# --- RUN -----------------------------------------------------------------
featurePosture.selec.value = toFlags(range(6, 36))
sot.clear()
for task in [taskWaist, taskRF, taskLF]:
task.feature.position.recompute(0)
task.feature.keep()
task.feature.selec.value = '111111'
sot.push(task.task.name)
taskWaist.ref = matrixToTuple(eye(4))
taskWaist.feature.selec.value = '111011'
taskWaist.gain.setByPoint(18, 0.1, 0.005, 0.8)
attime(200, (lambda: sot.rm(taskWaist.task.name), 'Rm waist'),
(lambda: pg.initState(), 'Init PG'),
(lambda: pg.parseCmd(":stepseq " + seqpart), 'Push step list'),
featurePostureDes.errorIN.value = dyn.position.value
taskPosture = Task('taskPosture')
taskPosture.add('featurePosture')
gPosture = GainAdaptive('gPosture')
plug(taskPosture.error,gPosture.error)
plug(gPosture.gain,taskPosture.controlGain)
gPosture.set(1,1,1)
postureSelec = range(0,3) # right leg
postureSelec += range(6,9) # left leg
postureSelec += range(12,16) # chest+head
#postureSelec += range(16,19) # right arm
#postureSelec += range(23,26) # left arm
featurePosture.selec.value = toFlags(postureSelec)
# --- TASK JL ------------------------------------------------------
taskJL = TaskJointLimits('taskJL')
plug(dyn.position,taskJL.position)
plug(dyn.lowerJl,taskJL.referenceInf)
plug(dyn.upperJl,taskJL.referenceSup)
taskJL.dt.value = dt
taskJL.selec.value = toFlags(range(6,robotDim))
# --- SOT KINE OpSpaceH -------------------------------------------
# SOT controller.
sot = SolverKine('sot')
sot.setSize(robotDim)
sot.damping.value = 1e-6
runner=inc()
[go,stop,next,n]=loopShortcuts(runner)
# Tasks
taskzero = MetaTaskKine6d('rh',robot.dynamic,'rh','right-wrist')
taskzero.feature.frame('desired')
robot.dynamic.upperJl.recompute(0)
robot.dynamic.lowerJl.recompute(0)
taskJL = TaskJointLimits('taskJL')
plug(robot.dynamic.position,taskJL.position)
taskJL.controlGain.value = 10
taskJL.referenceInf.value = robot.dynamic.lowerJl.value
taskJL.referenceSup.value = robot.dynamic.upperJl.value
taskJL.dt.value = dt
taskJL.selec.value = toFlags(range(18,25)+range(26,27)+range(28,31)+range(32,40)+range(41,42)+range(43,46)+range(47,50))
taskContact = MetaTaskKine6d('contact',robot.dynamic,'contact','left-ankle')
taskContact.feature.frame('desired')
taskContact.feature.selec.value = '011100'
taskContact.gain.setConstant(10)
taskFixed = MetaTaskKine6d('contactFixed',robot.dynamic,'contact','left-ankle')
taskFixed.feature.frame('desired')
taskFixed.gain.setConstant(10)
# Problem
from dynamic_graph.sot.core.meta_tasks_kine import gotoNd
targetRH = (0.65,-0.2,0.9)
targetLH = (0.1, 0.0,1.2)
#targetLH = targetRH
taskFoV.task.referenceInf.value = (-Xmax, -Ymax) # Xmin, Ymin
taskFoV.task.referenceSup.value = (Xmax, Ymax) # Xmax, Ymax
taskFoV.task.dt.value = dt
taskFoV.task.controlGain.value = 0.01
taskFoV.featureDes.xy.value = (0, 0)
# --- Task Joint Limits -----------------------------------------
dyn.upperJl.recompute(0)
dyn.lowerJl.recompute(0)
taskJL = TaskJointLimits('taskJL')
plug(dyn.position, taskJL.position)
taskJL.controlGain.value = 10
taskJL.referenceInf.value = dyn.lowerJl.value
taskJL.referenceSup.value = dyn.upperJl.value
taskJL.dt.value = dt
taskJL.selec.value = toFlags(range(6, 22) + range(22, 28) + range(29, 35))
# --- CONTACTS
# define contactLF and contactRF
for name, joint in [['LF', 'left-ankle'], ['RF', 'right-ankle']]:
contact = MetaTaskKine6d('contact' + name, dyn, name, joint)
contact.feature.frame('desired')
contact.gain.setConstant(10)
locals()['contact' + name] = contact
# --- TRACER -----------------------------------------------------------------
from dynamic_graph.tracer import *
tr = Tracer('tr')
tr.open('/tmp/', '', '.dat')
tr.start()
robot.after.addSignal('tr.triger')