def createTasks(self): self.taskGaze = MetaTaskVisualPoint('gaze',self.robot.dynamic,'head','gaze') self.contactRF = self.robot.contactRF self.contactLF = self.robot.contactLF self.taskCom = self.robot.mTasks['com'] self.taskLim = self.robot.taskLim self.taskRH = self.robot.mTasks['rh'] self.taskLH = self.robot.mTasks['lh'] self.taskChest = self.robot.mTasks['chest'] self.taskPosture = self.robot.mTasks['posture'] self.taskGripper = MetaTaskKinePosture(self.robot.dynamic,'gripper') self.taskHalfStitting = MetaTaskKinePosture(self.robot.dynamic,'halfsitting') self.taskCompensate = MetaTaskKine6d('compensate',self.robot.dynamic,'rh','right-wrist')
def createTasks(self,robot): self.taskHalfSitting = MetaTaskKinePosture(self.robot.dynamic,'halfsitting') self.taskInitialPose = MetaTaskKinePosture(self.robot.dynamic,'initial-pose') self.taskGripper = MetaTaskKinePosture(self.robot.dynamic,'gripper') (self.tasks['trunk'],self.gains['trunk'])= self.createTrunkTask(robot, 'trunk') self.taskRF = self.tasks['left-ankle'] self.taskLF = self.tasks['right-ankle'] self.taskCom = self.tasks['com'] self.taskRH = self.tasks['right-wrist'] self.taskLH = self.tasks['left-wrist'] self.taskTrunk = self.tasks['trunk'] self.taskPosture = self.tasks['posture'] self.taskBalance = self.tasks['balance'] self.taskWaist = self.tasks['waist']
def createTasks(robot): # MetaTasks dictonary robot.mTasks = dict() robot.tasksIne = dict() # Foot contacts robot.contactLF = MetaTaskKine6d('contactLF', robot.dynamic, 'LF', robot.OperationalPointsMap['left-ankle']) robot.contactLF.feature.frame('desired') robot.contactLF.gain.setConstant(10) robot.contactRF = MetaTaskKine6d('contactRF', robot.dynamic, 'RF', robot.OperationalPointsMap['right-ankle']) robot.contactRF.feature.frame('desired') robot.contactRF.gain.setConstant(10) # MetaTasksKine6d for other operational points robot.mTasks['waist'] = MetaTaskKine6d('waist', robot.dynamic, 'waist', robot.OperationalPointsMap['waist']) robot.mTasks['chest'] = MetaTaskKine6d('chest', robot.dynamic, 'chest', robot.OperationalPointsMap['chest']) robot.mTasks['rh'] = MetaTaskKine6d('rh', robot.dynamic, 'rh', robot.OperationalPointsMap['right-wrist']) robot.mTasks['lh'] = MetaTaskKine6d('lh', robot.dynamic, 'lh', robot.OperationalPointsMap['left-wrist']) for taskName in robot.mTasks: robot.mTasks[taskName].feature.frame('desired') robot.mTasks[taskName].gain.setConstant(10) handMgrip = eye(4) handMgrip[0:3, 3] = (0, 0, -0.14) robot.mTasks['rh'].opmodif = matrixToTuple(handMgrip) robot.mTasks['lh'].opmodif = matrixToTuple(handMgrip) # CoM Task robot.mTasks['com'] = MetaTaskKineCom(robot.dynamic) robot.dynamic.com.recompute(0) robot.mTasks['com'].featureDes.errorIN.value = robot.dynamic.com.value robot.mTasks['com'].task.controlGain.value = 10 robot.mTasks['com'].feature.selec.value = '011' # Posture Task robot.mTasks['posture'] = MetaTaskKinePosture(robot.dynamic) robot.mTasks['posture'].ref = robot.halfSitting robot.mTasks['posture'].gain.setConstant(5) # TASK INEQUALITY # Task Height featureHeight = FeatureGeneric('featureHeight') plug(robot.dynamic.com, featureHeight.errorIN) plug(robot.dynamic.Jcom, featureHeight.jacobianIN) robot.tasksIne['taskHeight'] = TaskInequality('taskHeight') robot.tasksIne['taskHeight'].add(featureHeight.name) robot.tasksIne['taskHeight'].selec.value = '100' robot.tasksIne['taskHeight'].referenceInf.value = (0., 0., 0.) # Xmin, Ymin robot.tasksIne['taskHeight'].referenceSup.value = (0., 0., 0.80771) # Xmax, Ymax robot.tasksIne['taskHeight'].dt.value = robot.timeStep
def createTasks(self, robot): self.taskHalfSitting = MetaTaskKinePosture(self.robot.dynamic, 'halfsitting') self.taskInitialPose = MetaTaskKinePosture(self.robot.dynamic, 'initial-pose') self.taskGripper = MetaTaskKinePosture(self.robot.dynamic, 'gripper') (self.tasks['trunk'], self.gains['trunk']) = self.createTrunkTask(robot, 'trunk') self.taskRF = self.tasks['left-ankle'] self.taskLF = self.tasks['right-ankle'] self.taskCom = self.tasks['com'] self.taskRH = self.tasks['right-wrist'] self.taskLH = self.tasks['left-wrist'] self.taskTrunk = self.tasks['trunk'] self.taskPosture = self.tasks['posture'] self.taskBalance = self.tasks['balance'] self.taskWaist = self.tasks['waist']
taskElbow.task.controlGain.value = 0.9 # --- TASK SUPPORT -------------------------------------------------- featureSupport = FeatureGeneric('featureSupport') plug(dyn.com,featureSupport.errorIN) plug(dyn.Jcom,featureSupport.jacobianIN) taskSupport=TaskInequality('taskSupport') taskSupport.add(featureSupport.name) taskSupport.selec.value = '011' taskSupport.referenceInf.value = (-0.08,-0.15,0) # Xmin, Ymin taskSupport.referenceSup.value = (0.11,0.15,0) # Xmax, Ymax taskSupport.dt.value=dt # --- POSTURE --- taskPosture = MetaTaskKinePosture(dyn) # --- 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')
# --- TASK SUPPORT SMALL -------------------------------------------- featureSupportSmall = FeatureGeneric('featureSupportSmall') plug(dyn.com, featureSupportSmall.errorIN) plug(dyn.Jcom, featureSupportSmall.jacobianIN) taskSupportSmall = TaskInequality('taskSupportSmall') taskSupportSmall.add(featureSupportSmall.name) taskSupportSmall.selec.value = '011' #taskSupportSmall.referenceInf.value = (-0.02,-0.02,0) # Xmin, Ymin #askSupportSmall.referenceSup.value = (0.02,0.02,0) # Xmax, Ymax taskSupportSmall.referenceInf.value = (-0.02, -0.05, 0) # Xmin, Ymin taskSupportSmall.referenceSup.value = (0.02, 0.05, 0) # Xmax, Ymax taskSupportSmall.dt.value = dt # --- POSTURE --- taskPosture = MetaTaskKinePosture(dyn) # --- GAZE --- taskGaze = MetaTaskVisualPoint('gaze', dyn, 'head', 'gaze') # Head to camera matrix transform # Camera RU headMcam=array([[0.0,0.0,1.0,0.0825],[1.,0.0,0.0,-0.029],[0.0,1.,0.0,0.102],[0.0,0.0,0.0,1.0]]) # Camera LL headMcam = array([[0.0, 0.0, 1.0, 0.081], [1., 0.0, 0.0, 0.072], [0.0, 1., 0.0, 0.031], [0.0, 0.0, 0.0, 1.0]]) headMcam = dot(headMcam, rotate('x', 10 * pi / 180)) taskGaze.opmodif = matrixToTuple(headMcam) # --- FOV --- taskFoV = MetaTaskVisualPoint('FoV', dyn, 'head', 'gaze') taskFoV.opmodif = matrixToTuple(headMcam)
class Hrp2Bike(Application): tracesRealTime = True initialPose = ( # Free flyer 0., 0., 0., 0., 0., 0., # Legs 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., # Chest and head 0., 0., 0., 0., # Arms 0., 0., 0., 0., 0., 0., 0.1, 0., 0., 0., 0., 0., 0., 0.1, ) def __init__(self, robot, hands=True): #, forceSeqplay=True): Application.__init__(self, robot) self.sot = self.solver.sot self.hands = hands # self.robot=robot # self.seq=Seqplay('seqplay') self.createTasks(robot) self.initTasks() self.initTaskGains() self.initSolver() self.initialStack() def printSolver(self): #show tasks in solver controling the robot print self.solver #----------TASKS------------------------------- #------------------CREATION-------------------- def createTrunkTask(self, robot, taskName, ingain=1.0): task = Task(taskName) task.add(self.features['chest'].name) task.add(self.features['waist'].name) task.add(self.features['gaze'].name) gain = GainAdaptive(taskName + 'gain') gain.setConstant(ingain) plug(gain.gain, task.controlGain) plug(task.error, gain.error) return (task, gain) def createTasks(self, robot): self.taskHalfSitting = MetaTaskKinePosture(self.robot.dynamic, 'halfsitting') self.taskInitialPose = MetaTaskKinePosture(self.robot.dynamic, 'initial-pose') self.taskGripper = MetaTaskKinePosture(self.robot.dynamic, 'gripper') (self.tasks['trunk'], self.gains['trunk']) = self.createTrunkTask(robot, 'trunk') self.taskRF = self.tasks['left-ankle'] self.taskLF = self.tasks['right-ankle'] self.taskCom = self.tasks['com'] self.taskRH = self.tasks['right-wrist'] self.taskLH = self.tasks['left-wrist'] self.taskTrunk = self.tasks['trunk'] self.taskPosture = self.tasks['posture'] self.taskBalance = self.tasks['balance'] self.taskWaist = self.tasks['waist'] def openGripper(self): self.taskGripper.gotoq(None, rhand=(self.gripperOpen, ), lhand=(self.gripperOpen, )) def closeGripper(self): self.taskGripper.gotoq(None, rhand=(self.gripperClose, ), lhand=(self.gripperClose, )) #------------------INIT-TASK------------------ def initTasks(self): self.initTaskBalance() self.initTaskPosture() if self.hands: self.initTaskGripper() self.initTaskHalfSitting() self.initTaskInitialPose() def initTaskBalance(self): # --- BALANCE --- self.features['chest'].frame('desired') self.features['waist'].frame('desired') self.features['chest'].selec.value = '111000' self.features['waist'].selec.value = '111000' self.featureCom.selec.value = '111' def initTaskHalfSitting(self): self.taskHalfSitting.gotoq(None,\ rleg =(self.robot.halfSitting[6:12]), \ lleg =(self.robot.halfSitting[12:18]), \ chest=(self.robot.halfSitting[18:20]), \ head =(self.robot.halfSitting[20:22]), \ rarm =(self.robot.halfSitting[22:28]), \ larm =(self.robot.halfSitting[29:35])) def initTaskInitialPose(self): self.taskInitialPose.gotoq(None,\ rleg =(self.initialPose[6:12]), \ lleg =(self.initialPose[12:18]), \ chest=(self.initialPose[18:20]), \ head =(self.initialPose[20:22]), \ rarm =(self.initialPose[22:28]), \ larm =(self.initialPose[29:35])) def initTaskPosture(self): # --- LEAST NORM weight_ff = 0 weight_leg = 3 weight_knee = 5 weight_chest = 1 weight_chesttilt = 12 weight_head = 0.3 weight_arm = 0.8 weight = diag((weight_ff, ) * 6 + (weight_leg, ) * 12 + (weight_chest, ) * 2 + (weight_head, ) * 2 + (weight_arm, ) * 14) weight[9, 9] = weight_knee weight[15, 15] = weight_knee weight[19, 19] = weight_chesttilt #weight = weight[6:,:] self.featurePosture.jacobianIN.value = matrixToTuple(weight) self.featurePostureDes.errorIN.value = self.robot.halfSitting mask = '1' * 36 # mask = 6*'0'+12*'0'+4*'1'+14*'0' # mask = '000000000000111100000000000000000000000000' # robot.dynamic.displaySignals () # robot.dynamic.Jchest.value self.features['posture'].selec.value = mask def initTaskGripper(self): self.gripperOpen = degToRad(30) self.gripperClose = degToRad(3) self.openGripper() #------------------INIT-GAIN------------------ def initTaskGains(self): self.taskHalfSitting.gain.setByPoint(2, 0.2, 0.01, 0.8) self.taskGripper.gain.setConstant(3) self.taskInitialPose.gain.setByPoint(2, 0.2, 0.01, 0.8) #----------SOLVER------------------------------ def initSolver(self): plug(self.sot.control, self.robot.device.control) def push(self, task): #push task in solver if isinstance(task, str): taskName = task elif "task" in task.__dict__: taskName = task.task.name else: taskName = task.name if taskName not in toList(self.sot): self.sot.push(taskName) if taskName != "posture" and "posture" in toList(self.sot): self.sot.down("posture") def removeTasks(self, task): #remove task from solver if isinstance(task, str): taskName = task elif "task" in task.__dict__: taskName = task.task.name else: taskName = task.name if taskName in toList(self.sot): self.sot.remove(taskName) def printSolver(self): #show tasks in solver controlling the robot print self.solver def showTasks(self): #show library of precomputed tasks self.tasks # --- TRACES ----------------------------------------------------------- def withTraces(self): if self.tracesRealTime: self.robot.initializeTracer() else: self.robot.tracer = Tracer('trace') self.robot.device.after.addSignal('{0}.triger'.format( self.robot.tracer.name)) self.robot.tracer.open('/tmp/', '', '.dat') self.robot.startTracer() def stopTraces(self): self.robot.stopTracer() def trace(self): self.robot.tracer.dump() #----------RUN--------------------------------- def goInitialPose(self): self.sot.clear() # self.push(self.taskBalance) self.push(self.taskLF) self.push(self.taskRF) self.push(self.taskPosture) if self.hands: self.push(self.taskGripper) self.push(self.taskInitialPose) def initialStack(self): self.push(self.taskBalance) self.push(self.taskTrunk) self.push(self.taskPosture) if self.hands: self.push(self.taskGripper) def goHalfSitting(self): self.sot.clear() self.push(self.taskBalance) self.push(self.taskPosture) if self.hands: self.push(self.taskGripper) self.push(self.taskHalfSitting) #------------parameters of bike------- xg = 0.0 zg = 0.29 #center of crank gear R = 0.17 #pedal-center of crank gear def goBikeSitting(self): self.sot.clear() # self.push(self.taskBalance) # self.push(self.tasks['chest']) change6dPositionReference(self.taskWaist,self.features['waist'],\ self.gains['waist'],\ (-0.22,0.0,0.58,0,0,0),'111111') self.push(self.taskWaist) if self.hands: self.push(self.taskGripper) change6dPositionReference(self.taskRH,self.features['right-wrist'],\ self.gains['right-wrist'],\ (0.25,-0.255,0.90,-pi/2,0,pi/2),'111111') self.push(self.taskRH) change6dPositionReference(self.taskLH,self.features['left-wrist'],\ self.gains['left-wrist'],\ (0.25,0.255,0.90,pi/2,0,-pi/2),'111111') self.push(self.taskLH) change6dPositionReference(self.taskRF,self.features['right-ankle'],\ self.gains['right-ankle'],\ (0.0,-0.1125,0.12,0,0,degToRad(-8)),'111111') self.push(self.taskRF) change6dPositionReference(self.taskLF,self.features['left-ankle'],\ self.gains['left-ankle'],\ (0.0,0.1125,0.46,0,0,degToRad(8)),'111111') self.push(self.taskLF) self.push(self.taskPosture) def stopMove(self): self.rotation = False def circle( self, nbPoint=16, rotation=True): #nbPoint=number of point to dicretise the circle self.rotation = rotation self.stepCircle = (2 * pi) / nbPoint circleL = [] circleR = [] for j in range(0, nbPoint): circleL.append(-(j * self.stepCircle) + (pi / 2)) circleR.append(-(j * self.stepCircle) + (3 * pi / 2)) self.sot.clear() change6dPositionReference(self.taskWaist,self.features['waist'],\ self.gains['waist'],\ (-0.22,0.0,0.58,0,0,0),'111111') self.push(self.taskWaist) if self.hands: self.push(self.taskGripper) change6dPositionReference(self.taskRH,self.features['right-wrist'],\ self.gains['right-wrist'],\ (0.25,-0.255,0.90,-pi/2,0,pi/2),'111111') self.push(self.taskRH) change6dPositionReference(self.taskLH,self.features['left-wrist'],\ self.gains['left-wrist'],\ (0.25,0.255,0.90,pi/2,0,-pi/2),'111111') self.push(self.taskLH) #--------rotation-------- self.push(self.taskRF) self.push(self.taskLF) self.push(self.taskPosture) #while self.rotation: for i in range(0, nbPoint): xpL = (cos(circleL[i]) * self.R) + self.xg zpL = (sin(circleL[i]) * self.R) + self.zg xpR = (cos(circleR[i]) * self.R) + self.xg zpR = (sin(circleR[i]) * self.R) + self.zg change6dPositionReference(self.taskRF,self.features['right-ankle'],\ self.gains['right-ankle'],\ (xpR,-0.1125,zpR,0,0,degToRad(-8)),'111111') change6dPositionReference(self.taskLF,self.features['left-ankle'],\ self.gains['left-ankle'],\ (xpL,0.1125,zpL,0,0,degToRad(8)),'111111') time.sleep(5) # --- SEQUENCER --- step = 1 def sequencer(self, stepSeq=None): if stepSeq != None: self.step = stepSeq #-----initial position------ if self.step == 0: print "Step : ", self.step self.goInitialPose() print('Initial Pose') self.step += 1 elif self.step == 1: print "Step : ", self.step self.goBikeSitting() print('Bike Sitting') self.step += 1 elif self.step == 2: print "Step : ", self.step if self.hands: self.closeGripper() print('Close Gripper') self.step += 1 #-----move start------ elif self.step == 3: print "Step : ", self.step self.circle() print('Start movement') self.step += 1 #-----end of move----- #elif self.step==4: #print "Step : ", self.step #self.stopMove() #print('Stop movement') # self.step+=1 elif self.step == 4: #5: print "Step : ", self.step if self.hands: self.openGripper() print('Open Gripper') self.step += 1 else: print "Step : ", self.step self.goHalfSitting() print('Half-Sitting') self.step += 1 def __call__(self): self.sequencer() def __repr__(self): self.sequencer() return str()
taskElbow.task.controlGain.value = 0.9 # --- TASK SUPPORT -------------------------------------------------- featureSupport = FeatureGeneric('featureSupport') plug(dyn.com, featureSupport.errorIN) plug(dyn.Jcom, featureSupport.jacobianIN) taskSupport = TaskInequality('taskSupport') taskSupport.add(featureSupport.name) taskSupport.selec.value = '011' taskSupport.referenceInf.value = (-0.08, -0.15, 0) # Xmin, Ymin taskSupport.referenceSup.value = (0.11, 0.15, 0) # Xmax, Ymax taskSupport.dt.value = dt # --- POSTURE --- taskPosture = MetaTaskKinePosture(dyn) # --- 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')
class CompensaterApplication: threePhaseScrew = True tracesRealTime = False def __init__(self,robot): self.robot = robot plug(self.robot.dynamic.com,self.robot.device.zmp) self.initGeneralApplication() self.sot = self.solver.sot self.createTasks() self.initTasks() self.initTaskGains() self.initSolver() self.initialStack() def initGeneralApplication(self): from dynamic_graph.sot.application.velocity.precomputed_meta_tasks import initialize self.solver = initialize(self.robot) # --- TASKS -------------------------------------------------------------------- # --- TASKS -------------------------------------------------------------------- # --- TASKS -------------------------------------------------------------------- def createTasks(self): self.taskGaze = MetaTaskVisualPoint('gaze',self.robot.dynamic,'head','gaze') self.contactRF = self.robot.contactRF self.contactLF = self.robot.contactLF self.taskCom = self.robot.mTasks['com'] self.taskLim = self.robot.taskLim self.taskRH = self.robot.mTasks['rh'] self.taskLH = self.robot.mTasks['lh'] self.taskChest = self.robot.mTasks['chest'] self.taskPosture = self.robot.mTasks['posture'] self.taskGripper = MetaTaskKinePosture(self.robot.dynamic,'gripper') self.taskHalfStitting = MetaTaskKinePosture(self.robot.dynamic,'halfsitting') self.taskCompensate = MetaTaskKine6d('compensate',self.robot.dynamic,'rh','right-wrist') def openGripper(self): self.taskGripper.gotoq(None,rhand=(self.gripperOpen,),lhand=(self.gripperOpen,)) def closeGripper(self): self.taskGripper.gotoq(None,rhand=(self.gripperClose,),lhand=(self.gripperClose,)) #initialization is separated from the creation of the tasks because if we want to switch #to second order controlm the initialization will remain while the creation is #changed def initTasks(self): self.initTaskContact() self.initTaskBalance() self.initTaskPosture() self.initTaskHalfSitting() self.initTaskCompensate() def initTaskHalfSitting(self): self.taskHalfStitting.gotoq(None,self.robot.halfSitting) def initTaskContact(self): # --- CONTACTS self.contactRF.feature.frame('desired') self.contactLF.feature.frame('desired') def initTaskBalance(self): # --- BALANCE --- self.taskChest.feature.frame('desired') self.taskChest.feature.selec.value = '011000' ljl = matrix(self.robot.dynamic.lowerJl.value).T ujl = matrix(self.robot.dynamic.upperJl.value).T ljl[9] = 0.65 ljl[15] = 0.65 self.taskLim.referenceInf.value = vectorToTuple(ljl) self.taskLim.referenceSup.value = vectorToTuple(ujl) def initTaskPosture(self): # --- LEAST NORM weight_ff = 0 weight_leg = 3 weight_knee = 5 weight_chest = 1 weight_chesttilt = 10 weight_head = 0.3 weight_arm = 1 weight = diag( (weight_ff,)*6 + (weight_leg,)*12 + (weight_chest,)*2 + (weight_head,)*2 + (weight_arm,)*14) weight[9,9] = weight_knee weight[15,15] = weight_knee weight[19,19] = weight_chesttilt weight = weight[6:,:] self.taskPosture.task.jacobian.value = matrixToTuple(weight) self.taskPosture.task.task.value = (0,)*weight.shape[0] def initTaskGains(self, setup = "medium"): if setup == "medium": self.contactRF.gain.setConstant(10) self.contactLF.gain.setConstant(10) self.taskChest.gain.setConstant(10) self.taskRH.gain.setByPoint(2,0.8,0.01,0.8) self.taskCompensate.gain.setByPoint(2,0.8,0.01,0.8) self.taskHalfStitting.gain.setByPoint(2,0.8,0.01,0.8) # --- SOLVER ---------------------------------------------------------------- def initSolver(self): plug(self.sot.control,self.robot.device.control) def push(self,task,keep=False): if isinstance(task,str): taskName=task elif "task" in task.__dict__: taskName=task.task.name else: taskName=task.name if taskName not in self.sot.toList(): self.sot.push(taskName) if taskName!="taskposture" and "taskposture" in self.sot.toList(): self.sot.down("taskposture") if keep: task.keep() def rm(self,task): if isinstance(task,str): taskName=task elif "task" in task.__dict__: taskName=task.task.name else: taskName=task.name if taskName in self.sot.toList(): self.sot.rm(taskName) # --- DISPLAY ------------------------------------------------------------- def initDisplay(self): self.robot.device.viewer.updateElementConfig('red',[0,0,-1,0,0,0]) self.robot.device.viewer.updateElementConfig('yellow',[0,0,-1,0,0,0]) def updateDisplay(self): '''Display the various objects corresponding to the calcul graph. ''' None # --- TRACES ----------------------------------------------------------- def withTraces(self): if self.tracesRealTime: self.robot.initializeTracer() else: self.robot.tracer = Tracer('trace') self.robot.device.after.addSignal('{0}.triger'.format(self.robot.tracer.name)) self.robot.tracer.open('/tmp/','','.dat') self.robot.tracer.add( self.taskRH.task.name+'.error','erh' ) self.robot.startTracer() def stopTraces(self): self.robot.stopTracer() def trace(self): self.robot.tracer.dump() # --- RUN -------------------------------------------------------------- def initialStack(self): self.sot.clear() self.push(self.contactLF,True) self.push(self.contactRF,True) self.push(self.taskLim) self.push(self.taskCom) self.push(self.taskChest,True) self.push(self.taskPosture) def moveToInit(self): '''Go to initial pose.''' gotoNd(self.taskRH,(0.3,-0.2,1.1,0,-pi/2,0),'111001') self.push(self.taskRH) None def goHalfSitting(self): '''End of application, go to final pose.''' self.sot.clear() self.push(self.contactLF,True) self.push(self.contactRF,True) self.push(self.taskLim) self.push(self.taskHalfStitting) # --- SEQUENCER --- seqstep = 0 def nextStep(self,step=None): if step!=None: self.seqstep = step if self.seqstep==0: self.moveToInit() elif self.seqstep==1: self.startCompensate() elif self.seqstep==2: self.goHalfSitting() self.seqstep += 1 def __add__(self,i): self.nextStep() # COMPENATION ###################################### def initTaskCompensate(self): # w is the world, c is the egocenter (the central reference point of # the robot kinematics). # The constraint is: # wMh0 !!=!! wMh = wMa0 a0Ma aMc cMh # or written in cMh # cMh !!=!! cMa aMa0 a0Mh0 # c : central frame of the robot # cc : central frame for the controller (without the flexibility) # ccMc= flexibility self.ccMc = PoseUThetaToMatrixHomo('ccMc') self.ccMc.sin.value = [0,]*6 # href : the reference 'desired' position of the hand self.ccMhref = Multiply_of_matrixHomo('ccMhref') plug(self.ccMc.sout,self.ccMhref.sin1) # You need to set up a reference value here: plug( -- ,self.ccMhref.sin2) plug(self.ccMhref.sout,self.taskCompensate.featureDes.position) self.taskCompensate.feature.frame('desired') def startCompensate(self): '''Start to compensate for the hand movements.''' ccMh0 = matrix(self.robot.dynamic.rh.value) self.ccMhref.sin2.value = matrixToTuple(ccMh0) print ccMh0 print print matrix(self.robot.dynamic.RF.value) self.rm(self.taskRH) self.push(self.taskCompensate)
taskSupport.dt.value=dt # --- TASK SUPPORT SMALL -------------------------------------------- featureSupportSmall = FeatureGeneric('featureSupportSmall') plug(robot.dynamic.com,featureSupportSmall.errorIN) plug(robot.dynamic.Jcom,featureSupportSmall.jacobianIN) taskSupportSmall=TaskInequality('taskSupportSmall') taskSupportSmall.add(featureSupportSmall.name) taskSupportSmall.selec.value = '011' taskSupportSmall.referenceInf.value = (-0.02,-0.05,0) # Xmin, Ymin taskSupportSmall.referenceSup.value = (0.02,0.05,0) # Xmax, Ymax taskSupportSmall.dt.value=dt # --- POSTURE --- taskPosture = MetaTaskKinePosture(robot.dynamic) # --- GAZE --- taskGaze = MetaTaskVisualPoint('gaze',robot.dynamic,'head','gaze') # Head to camera matrix transform # Camera RU headMcam=array([[0.0,0.0,1.0,0.0825],[1.,0.0,0.0,-0.029],[0.0,1.,0.0,0.102],[0.0,0.0,0.0,1.0]]) # Camera LL headMcam=array([[0.0,0.0,1.0,0.081],[1.,0.0,0.0,0.072],[0.0,1.,0.0,0.031],[0.0,0.0,0.0,1.0]]) headMcam = dot(headMcam,rotate('x',10*pi/180)) taskGaze.opmodif = matrixToTuple(headMcam) # --- FOV --- taskFoV = MetaTaskVisualPoint('FoV',robot.dynamic,'head','gaze') taskFoV.opmodif = matrixToTuple(headMcam) taskFoV.task=TaskInequality('taskFoVineq')
class Hrp2Bike(Application): tracesRealTime = True initialPose = ( # Free flyer 0., 0., 0., 0., 0. , 0., # Legs 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., # Chest and head 0., 0., 0., 0., # Arms 0., 0., 0., 0., 0., 0., 0.1, 0., 0., 0., 0., 0., 0., 0.1, ) def __init__(self,robot,hands=True):#, forceSeqplay=True): Application.__init__(self,robot) self.sot=self.solver.sot self.hands=hands # self.robot=robot # self.seq=Seqplay('seqplay') self.createTasks(robot) self.initTasks() self.initTaskGains() self.initSolver() self.initialStack() def printSolver(self): #show tasks in solver controling the robot print self.solver #----------TASKS------------------------------- #------------------CREATION-------------------- def createTrunkTask (self, robot, taskName, ingain=1.0): task = Task (taskName) task.add (self.features['chest'].name) task.add (self.features['waist'].name) task.add (self.features['gaze'].name) gain = GainAdaptive(taskName+'gain') gain.setConstant(ingain) plug(gain.gain, task.controlGain) plug(task.error, gain.error) return (task, gain) def createTasks(self,robot): self.taskHalfSitting = MetaTaskKinePosture(self.robot.dynamic,'halfsitting') self.taskInitialPose = MetaTaskKinePosture(self.robot.dynamic,'initial-pose') self.taskGripper = MetaTaskKinePosture(self.robot.dynamic,'gripper') (self.tasks['trunk'],self.gains['trunk'])= self.createTrunkTask(robot, 'trunk') self.taskRF = self.tasks['left-ankle'] self.taskLF = self.tasks['right-ankle'] self.taskCom = self.tasks['com'] self.taskRH = self.tasks['right-wrist'] self.taskLH = self.tasks['left-wrist'] self.taskTrunk = self.tasks['trunk'] self.taskPosture = self.tasks['posture'] self.taskBalance = self.tasks['balance'] self.taskWaist = self.tasks['waist'] def openGripper(self): self.taskGripper.gotoq(None,rhand=(self.gripperOpen,),lhand=(self.gripperOpen,)) def closeGripper(self): self.taskGripper.gotoq(None,rhand=(self.gripperClose,),lhand=(self.gripperClose,)) #------------------INIT-TASK------------------ def initTasks(self): self.initTaskBalance() self.initTaskPosture() if self.hands: self.initTaskGripper() self.initTaskHalfSitting() self.initTaskInitialPose() def initTaskBalance(self): # --- BALANCE --- self.features['chest'].frame('desired') self.features['waist'].frame('desired') self.features['chest'].selec.value = '111000' self.features['waist'].selec.value = '111000' self.featureCom.selec.value = '111' def initTaskHalfSitting(self): self.taskHalfSitting.gotoq(None,\ rleg =(self.robot.halfSitting[6:12]), \ lleg =(self.robot.halfSitting[12:18]), \ chest=(self.robot.halfSitting[18:20]), \ head =(self.robot.halfSitting[20:22]), \ rarm =(self.robot.halfSitting[22:28]), \ larm =(self.robot.halfSitting[29:35])) def initTaskInitialPose(self): self.taskInitialPose.gotoq(None,\ rleg =(self.initialPose[6:12]), \ lleg =(self.initialPose[12:18]), \ chest=(self.initialPose[18:20]), \ head =(self.initialPose[20:22]), \ rarm =(self.initialPose[22:28]), \ larm =(self.initialPose[29:35])) def initTaskPosture(self): # --- LEAST NORM weight_ff = 0 weight_leg = 3 weight_knee = 5 weight_chest = 1 weight_chesttilt = 12 weight_head = 0.3 weight_arm = 0.8 weight = diag( (weight_ff,)*6 + (weight_leg,)*12 + (weight_chest,)*2 + (weight_head,)*2 + (weight_arm,)*14) weight[9,9] = weight_knee weight[15,15] = weight_knee weight[19,19] = weight_chesttilt #weight = weight[6:,:] self.featurePosture.jacobianIN.value = matrixToTuple(weight) self.featurePostureDes.errorIN.value = self.robot.halfSitting mask = '1'*36 # mask = 6*'0'+12*'0'+4*'1'+14*'0' # mask = '000000000000111100000000000000000000000000' # robot.dynamic.displaySignals () # robot.dynamic.Jchest.value self.features['posture'].selec.value = mask def initTaskGripper(self): self.gripperOpen = degToRad(30) self.gripperClose = degToRad(3) self.openGripper() #------------------INIT-GAIN------------------ def initTaskGains(self): self.taskHalfSitting.gain.setByPoint(2,0.2,0.01,0.8) self.taskGripper.gain.setConstant(3) self.taskInitialPose.gain.setByPoint(2,0.2,0.01,0.8) #----------SOLVER------------------------------ def initSolver(self): plug(self.sot.control,self.robot.device.control) def push(self,task): #push task in solver if isinstance(task,str): taskName=task elif "task" in task.__dict__: taskName=task.task.name else: taskName=task.name if taskName not in toList(self.sot): self.sot.push(taskName) if taskName!="posture" and "posture" in toList(self.sot): self.sot.down("posture") def removeTasks(self,task) : #remove task from solver if isinstance(task,str): taskName=task elif "task" in task.__dict__: taskName=task.task.name else: taskName=task.name if taskName in toList(self.sot): self.sot.remove(taskName) def printSolver(self): #show tasks in solver controlling the robot print self.solver def showTasks(self) : #show library of precomputed tasks self.tasks # --- TRACES ----------------------------------------------------------- def withTraces(self): if self.tracesRealTime: self.robot.initializeTracer() else: self.robot.tracer = Tracer('trace') self.robot.device.after.addSignal('{0}.triger'.format(self.robot.tracer.name)) self.robot.tracer.open('/tmp/','','.dat') self.robot.startTracer() def stopTraces(self): self.robot.stopTracer() def trace(self): self.robot.tracer.dump() #----------RUN--------------------------------- def goInitialPose(self): self.sot.clear() # self.push(self.taskBalance) self.push(self.taskLF) self.push(self.taskRF) self.push(self.taskPosture) if self.hands: self.push(self.taskGripper) self.push(self.taskInitialPose) def initialStack(self): self.push(self.taskBalance) self.push(self.taskTrunk) self.push(self.taskPosture) if self.hands: self.push(self.taskGripper) def goHalfSitting(self): self.sot.clear() self.push(self.taskBalance) self.push(self.taskPosture) if self.hands: self.push(self.taskGripper) self.push(self.taskHalfSitting) #------------parameters of bike------- xg=0.0; zg=0.29 #center of crank gear R=0.17 #pedal-center of crank gear def goBikeSitting(self): self.sot.clear() # self.push(self.taskBalance) # self.push(self.tasks['chest']) change6dPositionReference(self.taskWaist,self.features['waist'],\ self.gains['waist'],\ (-0.22,0.0,0.58,0,0,0),'111111') self.push(self.taskWaist) if self.hands: self.push(self.taskGripper) change6dPositionReference(self.taskRH,self.features['right-wrist'],\ self.gains['right-wrist'],\ (0.25,-0.255,0.90,-pi/2,0,pi/2),'111111') self.push(self.taskRH) change6dPositionReference(self.taskLH,self.features['left-wrist'],\ self.gains['left-wrist'],\ (0.25,0.255,0.90,pi/2,0,-pi/2),'111111') self.push(self.taskLH) change6dPositionReference(self.taskRF,self.features['right-ankle'],\ self.gains['right-ankle'],\ (0.0,-0.1125,0.12,0,0,degToRad(-8)),'111111') self.push(self.taskRF) change6dPositionReference(self.taskLF,self.features['left-ankle'],\ self.gains['left-ankle'],\ (0.0,0.1125,0.46,0,0,degToRad(8)),'111111') self.push(self.taskLF) self.push(self.taskPosture) def stopMove(self): self.rotation=False def circle(self,nbPoint=16,rotation=True): #nbPoint=number of point to dicretise the circle self.rotation=rotation self.stepCircle=(2*pi)/nbPoint circleL=[] circleR=[] for j in range(0,nbPoint): circleL.append(-(j*self.stepCircle)+(pi/2)) circleR.append(-(j*self.stepCircle)+(3*pi/2)) self.sot.clear() change6dPositionReference(self.taskWaist,self.features['waist'],\ self.gains['waist'],\ (-0.22,0.0,0.58,0,0,0),'111111') self.push(self.taskWaist) if self.hands: self.push(self.taskGripper) change6dPositionReference(self.taskRH,self.features['right-wrist'],\ self.gains['right-wrist'],\ (0.25,-0.255,0.90,-pi/2,0,pi/2),'111111') self.push(self.taskRH) change6dPositionReference(self.taskLH,self.features['left-wrist'],\ self.gains['left-wrist'],\ (0.25,0.255,0.90,pi/2,0,-pi/2),'111111') self.push(self.taskLH) #--------rotation-------- self.push(self.taskRF) self.push(self.taskLF) self.push(self.taskPosture) #while self.rotation: for i in range(0,nbPoint): xpL=(cos(circleL[i])*self.R)+self.xg zpL=(sin(circleL[i])*self.R)+self.zg xpR=(cos(circleR[i])*self.R)+self.xg zpR=(sin(circleR[i])*self.R)+self.zg change6dPositionReference(self.taskRF,self.features['right-ankle'],\ self.gains['right-ankle'],\ (xpR,-0.1125,zpR,0,0,degToRad(-8)),'111111') change6dPositionReference(self.taskLF,self.features['left-ankle'],\ self.gains['left-ankle'],\ (xpL,0.1125,zpL,0,0,degToRad(8)),'111111') time.sleep(5) # --- SEQUENCER --- step=1 def sequencer(self,stepSeq=None): if stepSeq!=None: self.step=stepSeq #-----initial position------ if self.step==0: print "Step : ", self.step self.goInitialPose() print('Initial Pose') self.step+=1 elif self.step==1: print "Step : ", self.step self.goBikeSitting() print('Bike Sitting') self.step+=1 elif self.step==2: print "Step : ", self.step if self.hands: self.closeGripper() print('Close Gripper') self.step+=1 #-----move start------ elif self.step==3: print "Step : ", self.step self.circle() print('Start movement') self.step+=1 #-----end of move----- #elif self.step==4: #print "Step : ", self.step #self.stopMove() #print('Stop movement') # self.step+=1 elif self.step==4:#5: print "Step : ", self.step if self.hands: self.openGripper() print('Open Gripper') self.step+=1 else: print "Step : ", self.step self.goHalfSitting() print('Half-Sitting') self.step+=1 def __call__(self): self.sequencer() def __repr__(self): self.sequencer() return str()
# ---- TASKS ------------------------------------------------------------------- # ---- TASKS ------------------------------------------------------------------- # ---- TASKS ------------------------------------------------------------------- # ---- TASK GRIP --- taskRH=MetaTaskKine6d('rh',dyn,'rh','right-wrist') handMgrip=eye(4); handMgrip[0:3,3] = (0,0,-0.17) taskRH.opmodif = matrixToTuple(handMgrip) # --- STATIC COM (if not walking) taskCom0 = MetaTaskKineCom(dyn) # --- POSTURE --- taskPosture = MetaTaskKinePosture(dyn) # --- GAZE --- taskGaze = MetaTaskKine6d('gaze',dyn,'head','gaze') # Head to camera matrix transform headMcam=array([[0.0,0.0,1.0,0.0825],[1.,0.0,0.0,-0.029],[0.0,1.,0.0,0.102],[0.0,0.0,0.0,1.0]]) taskGaze.opmodif = matrixToTuple(headMcam) taskGaze.feature.frame('current') taskGaze.feature.selec.value = '011' # --- Task Joint Limits ----------------------------------------- dyn.upperJl.recompute(0) dyn.lowerJl.recompute(0) taskJL = TaskJointLimits('taskJL') plug(dyn.position,taskJL.position) taskJL.controlGain.value = 10
class HandCompensaterKine: threePhaseScrew = True tracesRealTime = True def __init__(self,robot,twoHands=True): self.robot = robot self.twoHands = twoHands plug(self.robot.dynamic.com,self.robot.device.zmp) self.initGeneralApplication() self.sot = self.solver.sot self.createTasks() self.initTasks() self.initTaskGains() self.initSolver() self.initialStack() def initGeneralApplication(self): self.solver = initialize(self.robot) # --- TASKS -------------------------------------------------------------------- # --- TASKS -------------------------------------------------------------------- # --- TASKS -------------------------------------------------------------------- def createTasks(self): self.taskGaze = MetaTaskVisualPoint('gaze',self.robot.dynamic,'head','gaze') self.contactRF = self.robot.contactRF self.contactLF = self.robot.contactLF self.taskCom = self.robot.mTasks['com'] self.taskLim = self.robot.taskLim self.taskRH = self.robot.mTasks['rh'] if self.twoHands: self.taskLH = self.robot.mTasks['lh'] self.taskChest = self.robot.mTasks['chest'] self.taskPosture = self.robot.mTasks['posture'] self.taskGripper = MetaTaskKinePosture(self.robot.dynamic,'gripper') self.taskHalfStitting = MetaTaskKinePosture(self.robot.dynamic,'halfsitting') self.taskCompensateR = MetaTaskKine6d('compensateR',self.robot.dynamic,'rh','right-wrist') if self.twoHands: self.taskCompensateL = MetaTaskKine6d('compensateL',self.robot.dynamic,'lh','left-wrist') def openGripper(self): self.taskGripper.gotoq(None,rhand=(self.gripperOpen,),lhand=(self.gripperOpen,)) def closeGripper(self): self.taskGripper.gotoq(None,rhand=(self.gripperClose,),lhand=(self.gripperClose,)) #initialization is separated from the creation of the tasks because if we want to switch #to second order controlm the initialization will remain while the creation is #changed def initTasks(self): self.initTaskContact() self.initTaskBalance() self.initTaskPosture() self.initTaskHalfSitting() self.initTaskCompensate() def initTaskHalfSitting(self): self.taskHalfStitting.gotoq(None,self.robot.halfSitting) def initTaskContact(self): # --- CONTACTS self.contactRF.feature.frame('desired') self.contactLF.feature.frame('desired') def initTaskBalance(self): # --- BALANCE --- self.taskChest.feature.frame('desired') #self.taskChest.feature.selec.value = '111111' self.taskChest.feature.selec.value = '111000' #self.taskCom.feature.selec.value = '111' ljl = matrix(self.robot.dynamic.lowerJl.value).T ujl = matrix(self.robot.dynamic.upperJl.value).T ljl[9] = 0.65 ljl[15] = 0.65 self.taskLim.referenceInf.value = vectorToTuple(ljl) self.taskLim.referenceSup.value = vectorToTuple(ujl) def initTaskPosture(self): # --- LEAST NORM weight_ff = 0 weight_leg = 3 weight_knee = 5 weight_chest = 1 weight_chesttilt = 10 weight_head = 0.3 weight_arm = 1 weight = diag( (weight_ff,)*6 + (weight_leg,)*12 + (weight_chest,)*2 + (weight_head,)*2 + (weight_arm,)*14) weight[9,9] = weight_knee weight[15,15] = weight_knee weight[19,19] = weight_chesttilt weight = weight[6:,:] self.taskPosture.task.jacobian.value = matrixToTuple(weight) self.taskPosture.task.task.value = (0,)*weight.shape[0] mask = '000000000000111100000000000000' # robot.dynamic.displaySignals () # robot.dynamic.Jchest.value self.taskPosture.feature.selec.value = mask def initTaskGains(self, setup = "medium"): if setup == "medium": self.contactRF.gain.setConstant(10) self.contactLF.gain.setConstant(10) self.taskChest.gain.setConstant(10) self.taskRH.gain.setByPoint(4,0.2,0.01,0.8) self.taskCompensateR.gain.setByPoint(4,0.2,0.01,0.8) if self.twoHands: self.taskCompensateL.gain.setByPoint(4,0.2,0.01,0.8) self.taskLH.gain.setByPoint(4,0.2,0.01,0.8) #self.taskCompensate.gain.setConstant(2) self.taskHalfStitting.gain.setByPoint(2,0.2,0.01,0.8) # --- SOLVER ---------------------------------------------------------------- def initSolver(self): plug(self.sot.control,self.robot.device.control) def push(self,task,keep=False): if isinstance(task,str): taskName=task elif "task" in task.__dict__: taskName=task.task.name else: taskName=task.name if taskName not in self.sot.toList(): self.sot.push(taskName) if taskName!="taskposture" and "taskposture" in self.sot.toList(): self.sot.down("taskposture") if keep: task.keep() def rm(self,task): if isinstance(task,str): taskName=task elif "task" in task.__dict__: taskName=task.task.name else: taskName=task.name if taskName in self.sot.toList(): self.sot.rm(taskName) # --- DISPLAY ------------------------------------------------------------- def initDisplay(self): self.robot.device.viewer.updateElementConfig('red',[0,0,-1,0,0,0]) self.robot.device.viewer.updateElementConfig('yellow',[0,0,-1,0,0,0]) def updateDisplay(self): '''Display the various objects corresponding to the calcul graph. ''' None # --- TRACES ----------------------------------------------------------- def withTraces(self): if self.tracesRealTime: self.robot.tracerSize = 2**26 self.robot.initializeTracer() else: self.robot.tracer = Tracer('trace') self.robot.device.after.addSignal('{0}.triger'.format(self.robot.tracer.name)) self.robot.tracer.open('/tmp/','','.dat') #self.robot.tracer.add( self.taskRH.task.name+'.error','erh' ) def stopTracer(self): self.robot.stopTracer() def dumpTracer(self): self.robot.tracer.dump() def startTracer(self): self.robot.startTracer() # --- RUN -------------------------------------------------------------- def initialStack(self): self.sot.clear() self.push(self.contactLF,True) self.push(self.contactRF,True) self.push(self.taskLim) self.push(self.taskCom) self.push(self.taskChest,True) self.push(self.taskPosture) def moveToInit(self): '''Go to initial pose.''' #gotoNd(self.taskRH,(0.3,-0.2,1.1,0,-pi/2,0),'111001') #gotoNd(self.taskLH,(0.3,0.2,1.1,0,-pi/2,0),'111001') gotoNd(self.taskRH,(0.3,-0.2,1.1,0,-pi/2,0),'111001') self.push(self.taskRH) if self.twoHands: gotoNd(self.taskLH,(0.3,0.2,1.1,0,-pi/2,0),'111001') self.push(self.taskLH) None def goHalfSitting(self): '''End of application, go to final pose.''' self.sot.clear() self.push(self.contactLF,True) self.push(self.contactRF,True) self.push(self.taskLim) self.push(self.taskHalfStitting) # --- SEQUENCER --- seqstep = 0 def nextStep(self,step=None): if step!=None: self.seqstep = step if self.seqstep==0: self.moveToInit() elif self.seqstep==1: self.startCompensate() elif self.seqstep==2: self.goHalfSitting() self.seqstep += 1 def __add__(self,i): self.nextStep() # COMPENATION ###################################### def initTaskCompensate(self): # The constraint is: # cMhref !!=!! cMh = cMcc ccMh # or written in ccMh # ccMh !!=!! ccMc cMhref # c : central frame of the robot # cc : central frame for the controller (without the flexibility) # cMcc= flexibility # ccMc= flexibility inverted self.transformerR = sotso.MovingFrameTransformation('tranformation_right') self.ccMc = self.transformerR.gMl # inverted flexibility self.cMrhref = self.transformerR.lM0 # reference position in the world control frame # You need to set up the inverted flexibility : plug( ..., self.ccMc) # You need to set up a reference value here: plug( ... ,self.cMhref) self.ccVc = self.transformerR.gVl # inverted flexibility velocity self.cVrhref = self.transformerR.lV0 # reference velocity in the world control frame # You need to set up the inverted flexibility velocity : plug( ..., self.ccVc) # You need to set up a reference velocity value here: plug( ... ,self.cVhref) self.ccMrhref = self.transformerR.gM0 # reference matrix h**o in the control frame self.ccVrhref = self.transformerR.gV0 plug(self.ccMrhref,self.taskCompensateR.featureDes.position) plug(self.ccVrhref,self.taskCompensateR.featureDes.velocity) self.taskCompensateR.task.setWithDerivative (True) self.taskCompensateR.feature.frame('desired') ###### if self.twoHands: self.transformerL = sotso.MovingFrameTransformation('tranformation_left') plug(self.ccMrhref,self.taskCompensateL.featureDes.position) plug(self.ccVrhref,self.taskCompensateL.featureDes.velocity) self.sym = Multiply_of_matrixHomo('sym') self.sym.sin1.value =((1, 0, 0, 0), (0, -1, 0, 0), (0, 0, 1, 0), (0, 0, 0, 1)) plug (self.ccMrhref,self.sym.sin2) plug (self.sym.sout,self.taskCompensateL.featureDes.position) self.symVel = Multiply_matrix_vector('symvel') self.symVel.sin1.value =((1,0,0,0,0,0),(0,-1,0,0,0,0),(0,0,0,1,0,0),(0,0,0,1,0,0),(0,0,0,0,-1,0),(0,0,0,0,0,1)) plug (self.ccVrhref,self.symVel.sin2) plug (self.symVel.sout,self.taskCompensateL.featureDes.velocity) self.taskCompensateL.feature.selec.value='000111' self.taskCompensateL.task.setWithDerivative (True) self.taskCompensateL.feature.frame('desired') self.cMrhref.value = (matrixToTuple(diag([1,1,1,1]))) self.cVrhref.value = (0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0) def startCompensate(self): '''Start to compensate for the hand movements.''' self.cMrhref.value = self.robot.dynamic.rh.value self.cVrhref.value = (0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0) print matrix(self.cMrhref.value) ####### #self.cMlhref.value = self.robot.dynamic.lh.value #self.cVlhref.value = (0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0) #print matrix(self.cMlhref.value) ###### self.rm(self.taskRH) self.push(self.taskCompensateR) if self.twoHands: self.rm(self.taskLH) self.push(self.taskCompensateL)
# ---- TASKS ------------------------------------------------------------------- # ---- TASKS ------------------------------------------------------------------- # ---- TASKS ------------------------------------------------------------------- # ---- TASK GRIP --- taskRH = MetaTaskKine6d('rh', dyn, 'rh', 'right-wrist') handMgrip = eye(4) handMgrip[0:3, 3] = (0, 0, -0.17) taskRH.opmodif = matrixToTuple(handMgrip) # --- STATIC COM (if not walking) taskCom0 = MetaTaskKineCom(dyn) # --- POSTURE --- taskPosture = MetaTaskKinePosture(dyn) # --- GAZE --- taskGaze = MetaTaskKine6d('gaze', dyn, 'head', 'gaze') # Head to camera matrix transform headMcam = array([[0.0, 0.0, 1.0, 0.0825], [1., 0.0, 0.0, -0.029], [0.0, 1., 0.0, 0.102], [0.0, 0.0, 0.0, 1.0]]) taskGaze.opmodif = matrixToTuple(headMcam) taskGaze.feature.frame('current') taskGaze.feature.selec.value = '011' # --- Task Joint Limits ----------------------------------------- dyn.upperJl.recompute(0) dyn.lowerJl.recompute(0) taskJL = TaskJointLimits('taskJL') plug(dyn.position, taskJL.position)
# --- TASK SUPPORT SMALL -------------------------------------------- featureSupportSmall = FeatureGeneric('featureSupportSmall') plug(dyn.com,featureSupportSmall.errorIN) plug(dyn.Jcom,featureSupportSmall.jacobianIN) taskSupportSmall=TaskInequality('taskSupportSmall') taskSupportSmall.add(featureSupportSmall.name) taskSupportSmall.selec.value = '011' #taskSupportSmall.referenceInf.value = (-0.02,-0.02,0) # Xmin, Ymin #askSupportSmall.referenceSup.value = (0.02,0.02,0) # Xmax, Ymax taskSupportSmall.referenceInf.value = (-0.02,-0.05,0) # Xmin, Ymin taskSupportSmall.referenceSup.value = (0.02,0.05,0) # Xmax, Ymax taskSupportSmall.dt.value=dt # --- POSTURE --- taskPosture = MetaTaskKinePosture(dyn) # --- GAZE --- taskGaze = MetaTaskVisualPoint('gaze',dyn,'head','gaze') # Head to camera matrix transform # Camera RU headMcam=array([[0.0,0.0,1.0,0.0825],[1.,0.0,0.0,-0.029],[0.0,1.,0.0,0.102],[0.0,0.0,0.0,1.0]]) # Camera LL headMcam=array([[0.0,0.0,1.0,0.081],[1.,0.0,0.0,0.072],[0.0,1.,0.0,0.031],[0.0,0.0,0.0,1.0]]) headMcam = dot(headMcam,rotate('x',10*pi/180)) taskGaze.opmodif = matrixToTuple(headMcam) # --- FOV --- taskFoV = MetaTaskVisualPoint('FoV',dyn,'head','gaze') taskFoV.opmodif = matrixToTuple(headMcam) taskFoV.task=TaskInequality('taskFoVineq')