def addCornerTask( self,cornerInd, opPointRef, Pc):
''' Add task for a corner (extreme of a foot) '''
name = 'cornerTask' + str(cornerInd)
print 'add task ', name
#print
''' Choose the dictionary to be used '''
if (opPointRef == 'right-ankle'):
dictToFloor = self.dictRToFloor
elif(opPointRef == 'left-ankle'):
dictToFloor = self.dictLToFloor
''' Create one task for each corner '''
dictToFloor[cornerInd] = MetaTaskDyn6d(name, self.dyn, name, opPointRef)
dictToFloor[cornerInd].gain.setConstant(1000)
''' Change the operational position to correspond to the corner '''
dictToFloor[cornerInd].opmodif = ((1,0,0,Pc[cornerInd][0]),(0,1,0,Pc[cornerInd][1]),
(0,0,1,Pc[cornerInd][2]),(0,0,0,1))
''' Control the position of the task (points) to keep the current value '''
dictToFloor[cornerInd].feature.position.recompute(self.sot.solution.time)
p0 = dictToFloor[cornerInd].feature.position.value
#gotoNd( cornerTask[i], (p0[0][3], p0[1][3], 0, 0, 0, 0), "000111") # Take to the floor
gotoNd( dictToFloor[cornerInd], (p0[0][3], p0[1][3], 0, 0, 0, 0), "000100") # Take to the floor (z)
self.sot.push(dictToFloor[cornerInd].task.name)
def renamePointTasks(self, contactF, contactKeepTasks, Xold, Yold, Zold, opPointRef):
''' Verify that the name of the tasks (in order) correspond to their index. If so, leave them as they are,
otherwise, delete them and create a new task (to avoid name's problems when adding new tasks) '''
renameOldTasks = False
''' For all the tasks corresponding to the previous points that were kept '''
for i in range(len(Xold)):
name = 'task' + contactF.name + 'supportTask' + str(i)
''' If at least one name does not coincide, perform a renaming '''
if (contactKeepTasks[i].task.name != name):
renameOldTasks = True
break
if (renameOldTasks):
''' Clear the feature associated with each task '''
for i in reversed( range(len(contactKeepTasks)) ):
contactKeepTasks[i].task.clear()
contactKeepTasks.pop(i)
for i in range(len(Xold)):
name = contactF.name + 'supportTask' + str(i)
contactKeepTasks.append( MetaTaskDyn6d(name, self.dyn, name, opPointRef) )
contactKeepTasks[i].gain.setConstant(3000)
contactKeepTasks[i].opmodif = ((1,0,0,Xold[i]),(0,1,0,Yold[i]),(0,0,1,Zold[i]),(0,0,0,1))
contactKeepTasks[i].feature.position.recompute(self.sot.solution.time)
p0 = contactKeepTasks[i].feature.position.value
gotoNd( contactKeepTasks[i], (p0[0][3], p0[1][3], p0[2][3], 0, 0, 0), "000111") # Control only translation
def moveRightHandToTarget(robot, solver, target, gainMax):
############################################################
# Reference and gain setting
############################################################
gotoNd(robot.mTasks['rh'], target, '000111',
(gainMax, gainMax / 50.0, 0.01, 0.9))
############################################################
# Push
############################################################
removeUndesiredTasks(solver)
solver.push(robot.mTasks['rh'].task)
def get_2ht(robot, solver, TwoHandTool, gainMax, gainMin):
#TwoHandTool = (0.4,-0.1,0.9,0.,0.,pi/2)
#TwoHandTool = (xd,yd,zd,roll,pitch,yaw)
# Homogeneous Matrix of the TwoHandTool. Normally given from the camera
refToTwoHandToolMatrix = RPYToMatrix(TwoHandTool)
# Homogeneous Matrixes
refToTriggerMatrix = dot(refToTwoHandToolMatrix,
TwoHandToolToTriggerMatrix)
refToSupportMatrix = dot(refToTwoHandToolMatrix,
TwoHandToolToSupportMatrix)
# ---- TASKS DEFINITION -------------------------------------------------------------------
# Set the targets. Selec is the activation flag (say control only
# the XYZ translation), and gain is the adaptive gain (<arg1> at the target, <arg2>
# far from it, with slope st. at <arg3>m from the target, <arg4>% of the max gain
# value is reached
target = vectorToTuple(refToSupportMatrix[0:3, 3])
gotoNd(robot.mTasks['rh'], target, "000111", (gainMax, gainMin, 0.01, 0.9))
target = vectorToTuple(refToTriggerMatrix[0:3, 3])
gotoNd(robot.mTasks['lh'], target, "000111", (gainMax, gainMin, 0.01, 0.9))
# Orientation RF and LF - Needed featureVector3 to get desired behaviour
if not hasattr(robot.mTasks['rh'], 'featureVec'):
createFeatureVec(robot, 'rh', array([1., 0., 0.]))
if not hasattr(robot.mTasks['lh'], 'featureVec'):
createFeatureVec(robot, 'lh', array([1., 0., 0.]))
robot.mTasks['rh'].featureVec.positionRef.value = dot(
refToTwoHandToolMatrix[0:3, 0:3], array([1., 0., 0.]))
robot.mTasks['lh'].featureVec.positionRef.value = dot(
refToTwoHandToolMatrix[0:3, 0:3], array([-1., 0., 0.]))
tasks = array([robot.mTasks['rh'].task, robot.mTasks['lh'].task])
# sot loading
solver.sot.damping.value = 0.001
removeUndesiredTasks(solver)
for i in range(len(tasks)):
solver.push(tasks[i])
def addPoints(self, supPoints, contact, dyn, opPointRef):
''' supPoints: ((x1,y1,z1),(x2,y2,z2),... )
Create tasks, one for each 3Dpoint
'''
# Convert the contact points to the support points format
# ((x1,x2...),(y1,y2,...),(z1,z2,...))
X=[]; Y=[]; Z=[]
for i in range(len(supPoints)):
X.append(supPoints[i][0])
Y.append(supPoints[i][1])
Z.append(supPoints[i][2])
Support = (tuple(X),tuple(Y),tuple(Z))
# print "\nSupport ", Support
# The contact already exists
if hasattr(self.sot, '_'+contact.name+'_p'):
supportPrev = self.sot.signal("_"+contact.name+"_p").value # Old supporting points
# print "Sup points old: ", supportPrev
indexToKeep = [] # Indexes to keep their tasks
Xnew=[]; Ynew=[]; Znew=[] # New values for X,Y,Z (support)
Xold=[]; Yold=[]; Zold=[] # Old values for the support that will be kept
# Compare the new points with the old ones to see if they are close enough
for i in range(len( Support[0] )):
keepNew = True
for j in range(len( supportPrev[0] )):
# Get the L2 norm distance between the olds and current support points
dist = sqrt( (supportPrev[0][j]-Support[0][i])**2 + (supportPrev[1][j]-Support[1][i])**2 +
(supportPrev[2][j]-Support[2][i])**2 )
# If the distance is less than a certain value, discard the new value, keep the old one, and 'continue'
#if (dist < 0.001):
if (dist < 0.05):
indexToKeep.append(j) # Add the index for the pointTasks that will be kept
keepNew = False
continue
if (keepNew == True):
Xnew.append(Support[0][i])
Ynew.append(Support[1][i])
Znew.append(Support[2][i])
# print "New Values: ", Xnew, ", ", Ynew, ", ", Znew
# print "indexTokeep: ", indexToKeep
# Go through the old support contacts (in decreasing order of the index)
for j in reversed( range(len(supportPrev[0])) ):
# If the contact has to be kept, append it to X,Y,Zold
if j in indexToKeep:
# Add the support contacts
Xold.append(supportPrev[0][j])
Yold.append(supportPrev[1][j])
Zold.append(supportPrev[2][j])
# If the contact has to be removed, remove the task associated with it
else:
self.pointTasks[j].task.clear()
self.pointTasks.pop(j)
# print "Old Values: ", Xold, ", ", Yold, ", ", Zold
# Verify that the name of the tasks (in order) correspond to their index. If so, leave them as they are,
# otherwise, delete them and create a new task (to avoid name's problems when adding new tasks)
renameOldTasks = False
for i in range(len(Xold)):
name = 'task' + contact.name + 'supportTask' + str(i)
if (self.pointTasks[i].task.name != name):
renameOldTasks = True
break
if (renameOldTasks):
for i in reversed( range(len(self.pointTasks)) ):
self.pointTasks[i].task.clear()
self.pointTasks.pop(i)
for i in range(len(Xold)):
name = contact.name + 'supportTask' + str(i)
self.pointTasks.append( MetaTaskDyn6d(name, dyn, name, opPointRef) )
self.pointTasks[i].gain.setConstant(3000)
self.pointTasks[i].opmodif = ((1,0,0,Xold[i]),(0,1,0,Yold[i]),(0,0,1,Zold[i]),(0,0,0,1))
self.pointTasks[i].feature.position.recompute(self.sot.solution.time)
p0 = self.pointTasks[i].feature.position.value
gotoNd( self.pointTasks[i], (p0[0][3], p0[1][3], p0[2][3], 0, 0, 0), "000111") # Control only translation
# Generate the new support composed of the old points and the new points
NewSupport = (tuple(Xold+Xnew), tuple(Yold+Ynew), tuple(Zold+Znew))
contact.support = NewSupport
# print "New Support: ", NewSupport
self.sot.signal("_"+contact.name+"_p").value = contact.support
#contact.task.resetJacobianDerivative()
Nold = len(Xold)
taskVectors = []
# print "Before adding Xnew tasks: ", self.pointTasks
# Only for the new points
for i in range(len(Xnew)):
name = contact.name + 'supportTask' + str(Nold+i)
self.pointTasks.append( MetaTaskDyn6d(name, dyn, name, opPointRef) )
self.pointTasks[i+Nold].gain.setConstant(3000)
self.pointTasks[i+Nold].opmodif = ((1,0,0,Xnew[i]),(0,1,0,Ynew[i]),(0,0,1,Znew[i]),(0,0,0,1))
self.pointTasks[i+Nold].feature.position.recompute(self.sot.solution.time)
p0 = self.pointTasks[i+Nold].feature.position.value
gotoNd( self.pointTasks[i+Nold], (p0[0][3], p0[1][3], p0[2][3], 0, 0, 0), "000111") # Control only translation
# print "After adding Xnew tasks: ", self.pointTasks
# Update the values of the tasks
taskVectors = []
for i in range(len(self.pointTasks)):
self.pointTasks[i].task.taskVector.recompute(self.sot.solution.time)
taskVectors.append( self.pointTasks[i].task.taskVector.value, )
# print "task Vectors: ", taskVectors
self.sot.signal("_"+contact.name+"_ddx3").value = tuple(taskVectors)
# Else (the contact does not exist), create the contact with the specified points
else:
contact.support = Support
self.sot.addContactFromMetaTask(contact)
self.pointTasks = []
taskVectors = []
for i in range(len(supPoints)):
name = contact.name + 'supportTask' + str(i)
self.pointTasks.append( MetaTaskDyn6d(name, dyn, name, opPointRef) )
self.pointTasks[i].gain.setConstant(3000)
self.pointTasks[i].opmodif = ((1,0,0,supPoints[i][0]),(0,1,0,supPoints[i][1]),(0,0,1,supPoints[i][2]),(0,0,0,1))
self.pointTasks[i].feature.position.recompute(self.sot.solution.time)
p0 = self.pointTasks[i].feature.position.value
gotoNd( self.pointTasks[i], (p0[0][3], p0[1][3], p0[2][3], 0, 0, 0), "000111") # Control only translation
self.pointTasks[i].task.taskVector.recompute(self.sot.solution.time)
taskVectors.append( self.pointTasks[i].task.taskVector.value, )
self.sot.signal("_"+contact.name+"_ddx3").value = tuple(taskVectors)
def addContactRemoveCornerTasks(self, Pc, contactF, contactKeepTasks, cornerTask, indCornerInContact, opPointRef):
''' Detect the collision of the points. If a point is closer within a radius of 'th' to the
contactTask, then, remove that contactTask, and add the point to the contact
- Pc: ((x1,y1,z1),(x2,y2,z2),... ) -- contact points wrt ankle
- contactF: foot contact task (to keep the points fixed)
- contactKeepTasks: tasks to obtain the acceleration ddx3 (one for each point)
- cornerTask: tasks to take the corners to the ground
- indCornerInContact: indices of the corners that are already in contact
- opPointRef: reference point (right-ankle or left-ankle)
'''
''' Get the corners values and the value of dict '''
if (opPointRef=='right-ankle'):
corner = self.cornersRf # corner points
dictToFloor = self.dictRToFloor # dictionary that stores the corner tasks
elif (opPointRef=='left-anlke'):
corner = self.cornersLf
dictToFloor = self.dictLToFloor
''' If the collision is close to a corner task point, remove the point and add it to the
contact foot '''
# print '\nPc: ', Pc
# print 'corner:', corner
ThDist = 0.005
for i in range(len(Pc)):
for j in range(len(self.indCornerToFloor)):
''' Check if the dictionary is not empty '''
if dictToFloor.has_key(self.indCornerToFloor[j])==False:
continue
if ( self.distance(Pc[i], corner[self.indCornerToFloor[j]]) < ThDist ):
''' Remove CornerToFloorTask and remove from the dictionary '''
# print 'i=',i,' -- Pc: ', Pc[i]
# print 'j=', j, '-- corner: ', corner[self.indCornerToFloor[j]]
# print 'dict: ', dictToFloor
self.sot.rm(dictToFloor[self.indCornerToFloor[j]].task.name)
dictToFloor.pop(self.indCornerToFloor[j])
''' ---------------------------------- '''
''' addToContact '''
tolerance = 0.002
[Xnew, Ynew, Znew, Xold, Yold, Zold] = self.removeClosePoints(tolerance, Pc, contactF, contactKeepTasks)
self.renamePointTasks( contactF, contactKeepTasks, Xold, Yold, Zold, opPointRef)
''' Generate the new support composed of the old points (and the new points, if there are) '''
NewSupport = (tuple(Xold+Xnew), tuple(Yold+Ynew), tuple(Zold+Znew))
contactF.support = NewSupport
self.sot.signal("_"+contactF.name+"_p").value = contactF.support
contactF.task.resetJacobianDerivative()
Nold = len(Xold)
''' For the new points: Create one task for each point '''
for k in range(len(Xnew)):
name = contactF.name + 'supportTask' + str(Nold+k)
contactKeepTasks.append( MetaTaskDyn6d(name, self.dyn, name, opPointRef) )
contactKeepTasks[k+Nold].gain.setConstant(3000)
contactKeepTasks[k+Nold].opmodif = ((1,0,0,Xnew[k]),(0,1,0,Ynew[k]),(0,0,1,Znew[k]),(0,0,0,1))
contactKeepTasks[k+Nold].feature.position.recompute(self.sot.solution.time)
p0 = contactKeepTasks[k+Nold].feature.position.value
gotoNd( contactKeepTasks[k+Nold], (p0[0][3], p0[1][3], p0[2][3], 0, 0, 0), "000111") # Control only translation
#print NewSupport
if (len(NewSupport[0])<3):
''' Find the corner points that generate the greatest polygon '''
indCornerToFloorN = self.findCornersGreatestPolygon(NewSupport, opPointRef) # Returns the indices
print "corners to floor: ", indCornerToFloorN
''' Add the tasks to move the 'proper' corners to the floor '''
self.addFootExtremes(indCornerToFloorN, indCornerInContact, opPointRef)
''' ---------------------------------- '''
print 'task to be removed: ', self.indCornerToFloor[j]
''' Add the corner to the in contact list '''
indCornerInContact.append(self.indCornerToFloor[j])
self.setVelocityNullAll()
def addPointsToContact(self, Pc, contactF, contactKeepTasks, cornerTask, indCornerInContact, opPointRef):
''' Add contact points as support points, create tasks (one for each 3Dpoint),
take the extremes fo the foot to he ground.
- Pc: ((x1,y1,z1),(x2,y2,z2),... ) -- contact points wrt ankle
- contactF: foot contact task (to keep the points fixed)
- contactKeepTasks: tasks to obtain the acceleration ddx3 (one for each point)
- cornerTask: tasks to take the corners to the ground
- indCornerInContact: indices of the corners that are already in contact
- opPointRef: reference point (right-ankle or left-ankle)
'''
#print "collision: ", Pc
#support = self.formatPointsToSupport(Pc)
''' Check if the contact exists '''
if hasattr(self.sot, '_'+contactF.name+'_p'): # If the contact exists
tolerance = 0.002
[Xnew, Ynew, Znew, Xold, Yold, Zold] = self.removeClosePoints(tolerance, Pc, contactF, contactKeepTasks)
self.renamePointTasks( contactF, contactKeepTasks, Xold, Yold, Zold, opPointRef)
''' Generate the new support composed of the old points (and the new points, if there are) '''
NewSupport = (tuple(Xold+Xnew), tuple(Yold+Ynew), tuple(Zold+Znew))
contactF.support = NewSupport
#print 'New support in add points to contact: ', NewSupport
self.sot.signal("_"+contactF.name+"_p").value = contactF.support
contactF.task.resetJacobianDerivative()
Nold = len(Xold)
''' For the new points: Create one task for each point '''
for i in range(len(Xnew)):
name = contactF.name + 'supportTask' + str(Nold+i)
contactKeepTasks.append( MetaTaskDyn6d(name, self.dyn, name, opPointRef) )
contactKeepTasks[i+Nold].gain.setConstant(3000)
contactKeepTasks[i+Nold].opmodif = ((1,0,0,Xnew[i]),(0,1,0,Ynew[i]),(0,0,1,Znew[i]),(0,0,0,1))
contactKeepTasks[i+Nold].feature.position.recompute(self.sot.solution.time)
p0 = contactKeepTasks[i+Nold].feature.position.value
gotoNd( contactKeepTasks[i+Nold], (p0[0][3], p0[1][3], p0[2][3], 0, 0, 0), "000111") # Control only translation
#print NewSupport
if (len(NewSupport[0])<3):
''' Find the corner points that generate the greatest polygon '''
indCornerToFloorN = self.findCornersGreatestPolygon(NewSupport, opPointRef) # Returns the indices
print "corners to floor: ", indCornerToFloorN
''' Add the tasks to move the 'proper' corners to the floor '''
self.addFootExtremes(indCornerToFloorN, indCornerInContact, opPointRef)
#self.setVelocityNull(opPointRef)
''' The contact does not exist: create it with the specified points '''
else:
self.sot.addContactFromTask(contactF.task.name, contactF.name)
self.sot.signal("_"+contactF.name+"_p").value = self.formatPointsToSupport(Pc)
contactF.task.resetJacobianDerivative()
print ' -- Contact created -- '
taskVectors = []
[contactKeepTasks.pop() for z in xrange(len(contactKeepTasks))] # Clear the list 'by reference'
for i in range( len(Pc) ):
name = contactF.name + 'SupportTask' + str(i)
''' Create one task for each contact point '''
contactKeepTasks.append( MetaTaskDyn6d(name, self.dyn, name, opPointRef) )
contactKeepTasks[i].gain.setConstant(3000)
contactKeepTasks[i].featureDes.velocity.value=(0,0,0)
contactKeepTasks[i].feature.errordot.value=(0,0,0)
''' Change the operational position to correspond to the contact point '''
contactKeepTasks[i].opmodif = ((1,0,0,Pc[i][0]),(0,1,0,Pc[i][1]),(0,0,1,Pc[i][2]),(0,0,0,1))
''' Control the position of the task (points) to keep the current value '''
contactKeepTasks[i].feature.position.recompute(self.sot.solution.time)
p0 = contactKeepTasks[i].feature.position.value
gotoNd( contactKeepTasks[i], (p0[0][3], p0[1][3], p0[2][3], 0, 0, 0), "000111") # Control translation
''' Get the acceleration of the points (taskVector) to use it for the ddx3 in the drift '''
contactKeepTasks[i].task.taskVector.recompute(self.sot.solution.time)
taskVectors.append( contactKeepTasks[i].task.taskVector.value, )
self.sot.signal("_"+contactF.name+"_ddx3").value = tuple(taskVectors)
#print 'contactKeepTasks: ', contactKeepTasks
# ''' Check the greatest polygon to add a task to move to that point '''
# self.findCornersGreatestPolygon(NewSupport)
#self.setVelocityNull(opPointRef)
self.setVelocityNullAll()
lh0 = tuple([x[3] for x in dyn.lh.value])
rh0 = tuple([x[3] for x in dyn.rh.value])
r = radians
sot.clear()
contact(contactLF)
contact(contactRF)
sot.push(taskLim.task.name)
plug(robot.state,sot.position)
attime(2
,(lambda: sot.push(taskChest.task.name), "Add Chest to the SoT")
,(lambda: taskChest.feature.keep(), "Keep Chest")
,(lambda: sot.push(taskHead.task.name), "Add Head to the SoT")
,(lambda: gotoNd(taskHead, (0, 0, 0, 0, r(25), -r(40)), "111000"), "Rotate Head to the right")
,(lambda: sot.push(taskrh.task.name), "Add Right hand to the SoT")
,(lambda: gotoNd(taskrh, (rh0[0]-0.3, rh0[1]-0.4, 0), "000011"), "Move right hand")
)
attime(200
,(lambda: gotoNd( taskHead, (0, 0, 0, 0, r(25), r(40)), "111000"), "Rotate Head to the left")
,(lambda: sot.push(tasklh.task.name), "Add Left hand to the SoT")
,(lambda: gotoNd(tasklh, (lh0[0]+0.3, lh0[1]+0.4, 0), "000011"), "Move left hand")
)
attime(400
,(lambda: gotoNd(taskHead, (0, 0, 0, 0, 0, 0), "111000"), "Rotate Head to the center")
,(lambda: gotoNd(tasklh, (lh0[0], lh0[1], 0), "000011"), "Return left hand to initial position")
,(lambda: gotoNd(taskrh, (rh0[0], rh0[1], 0), "000011"), "Return right hand to initial position")
)
taskCom.feature.selec.value = "11"
taskCom.gain.setByPoint(100,10,0.005,0.8)
r = radians
sigset = ( lambda s, v : s.__class__.value.__set__(s,v) )
refset = ( lambda mt,v : mt.__class__.ref.__set__(mt,v) )
# attime(2 ,(lambda: sot.push(taskCom.task.name),"Add CoM")
# ,(lambda: refset(taskCom, ( 0.01, 0.09, 0.7 )), "Com to left foot")
# )
attime(2 ,
(lambda: sot.push(taskChest.task.name), "Add Chest"),
(lambda: taskChest.feature.keep(), "Keep Chest"),
(lambda: sot.push(taskHead.task.name), "Add Head"),
(lambda: gotoNd(taskHead, (0, 0, 0, 0, r(25), -r(40)), "111000"), "Rotate Head to the right"),
)
attime(200,
(lambda: gotoNd( taskHead, (0, 0, 0, 0, r(25), r(40)), "111000"), "Rotate Head to the left")
)
attime(400 ,
(lambda: gotoNd(taskHead, (0, 0, 0, 0, 0, 0), "111000"), "Rotate Head to the center")
)
attime(600, stop, "Stopped")
go()
dyn.rh.recompute(0)
lh0 = tuple([x[3] for x in dyn.lh.value])
rh0 = tuple([x[3] for x in dyn.rh.value])
r = radians
sot.clear()
contact(contactLF)
contact(contactRF)
sot.push(taskLim.name)
plug(robot.state, sot.position)
attime(2, (lambda: sot.push(taskChest.task.name), "Add Chest to the SoT"),
(lambda: taskChest.feature.keep(), "Keep Chest"),
(lambda: sot.push(taskHead.task.name), "Add Head to the SoT"),
(lambda: gotoNd(taskHead, (0, 0, 0, 0, r(25), -r(40)), "111000"),
"Rotate Head to the right"),
(lambda: sot.push(taskrh.task.name), "Add Right hand to the SoT"),
(lambda: gotoNd(taskrh, (rh0[0] - 0.3, rh0[1] - 0.4, 0), "000011"),
"Move right hand"))
attime(200, (lambda: gotoNd(taskHead, (0, 0, 0, 0, r(25), r(40)), "111000"),
"Rotate Head to the left"),
(lambda: sot.push(tasklh.task.name), "Add Left hand to the SoT"),
(lambda: gotoNd(tasklh, (lh0[0] + 0.3, lh0[1] + 0.4, 0), "000011"),
"Move left hand"))
attime(400, (lambda: gotoNd(taskHead, (0, 0, 0, 0, 0, 0), "111000"),
"Rotate Head to the center"),
(lambda: gotoNd(tasklh, (lh0[0], lh0[1], 0), "000011"),
"Return left hand to initial position"),
rh0 = tuple([x[3] for x in dyn.rh.value])
r = radians
sot.clear()
contact(contactLF)
contact(contactRF)
sot.push(taskLim.task.name)
plug(robot.state, sot.position)
attime(
2,
(lambda: sot.push(taskChest.task.name), "Add Chest to the SoT"),
(lambda: taskChest.feature.keep(), "Keep Chest"),
(lambda: sot.push(taskHead.task.name), "Add Head to the SoT"),
(lambda: gotoNd(taskHead, (0, 0, 0, 0, r(25), -r(40)), "111000"), "Rotate Head to the right"),
(lambda: sot.push(taskrh.task.name), "Add Right hand to the SoT"),
(lambda: gotoNd(taskrh, (rh0[0] - 0.3, rh0[1] - 0.4, 0), "000011"), "Move right hand"),
)
attime(
200,
(lambda: gotoNd(taskHead, (0, 0, 0, 0, r(25), r(40)), "111000"), "Rotate Head to the left"),
(lambda: sot.push(tasklh.task.name), "Add Left hand to the SoT"),
(lambda: gotoNd(tasklh, (lh0[0] + 0.3, lh0[1] + 0.4, 0), "000011"), "Move left hand"),
)
attime(
400,
(lambda: gotoNd(taskHead, (0, 0, 0, 0, 0, 0), "111000"), "Rotate Head to the center"),
(lambda: gotoNd(tasklh, (lh0[0], lh0[1], 0), "000011"), "Return left hand to initial position"),
taskPosture.ref = pose
# Set the target for RH and LH task. Selec is the activation flag (say control only
# the XYZ translation), and gain is the adaptive gain (10 at the target, 0.1
# far from it, with slope st. at 0.01m from the target, 90% of the max gain
# value is reached
displacementMatrix = eye(4)
displacementMatrix[0:3, 3] = array([0.1, 0., 0.])
taskRH.feature.position.recompute(0)
taskLH.feature.position.recompute(0)
targetRH = vectorToTuple(
array(
matrixToRPY(
dot(displacementMatrix, array(taskRH.feature.position.value)))))
gotoNd(taskRH, targetRH, "111111", (50, 1, 0.01, 0.9))
RHPos = taskRH.feature.position.value
LHPos = taskLH.feature.position.value
#3RHPos = vectorToTuple(array(taskRH.feature.position.value)[0:3,3])
#LHPos = vectorToTuple(array(taskLH.feature.position.value)[0:3,3])
gotoNdRel(taskRel, RHPos, LHPos, '110111', (50, 1, 0.01, 0.9))
taskRel.feature.errordot.value = (0, 0, 0, 0, 0) # not to forget!!
############################################################
# Production of variable reference vector
HToR = HomoToRotation("HToR")
plug(taskLH.feature.position, HToR.sin)
RxV = Multiply_matrix_vector("RxV")