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")