def createScrewTask(robot, TwoHandTool): refToTwoHandToolMatrix = array(RPYToMatrix(TwoHandTool)) #RH-TwoHandTool Homogeneous Transformation Matrix (fixed in time) robot.mTasks['rh'].feature.position.recompute(robot.device.control.time) RHToTwoHandToolMatrix = dot( linalg.inv(array(robot.mTasks['rh'].feature.position.value)), refToTwoHandToolMatrix) #!!!!!! RH and Support are different references, because the X rotation is not controlled in positioning!!!!!!!!!!!!!!!!!!!!!!!!!! RHToScrewMatrix = dot(RHToTwoHandToolMatrix, TwoHandToolToScrewMatrix) # Screw Lenght - unused at the moment #screw_len = 0.03 # TASK Screw robot.mTasks['screw'] = MetaTaskDyn6d('screw', robot.dynamic, 'screw', 'right-wrist') handMgrip = array(robot.mTasks['rh'].opmodif) robot.mTasks['screw'].opmodif = matrixToTuple( dot(handMgrip, RHToScrewMatrix)) robot.mTasks['screw'].featureDes.velocity.value = (0, 0, 0, 0, 0, 0) robot.mTasks['screw'].feature.selec.value = '000111' # TASK Screw orientation robot.mTasks['screw'].featureVec = FeatureVector3("featureVecScrew") plug(robot.mTasks['screw'].opPointModif.position, robot.mTasks['screw'].featureVec.signal('position')) plug(robot.mTasks['screw'].opPointModif.jacobian, robot.mTasks['screw'].featureVec.signal('Jq')) robot.mTasks['screw'].featureVec.vector.value = array([0., 0., 1.]) robot.mTasks['screw'].task.add(robot.mTasks['screw'].featureVec.name)
def createTasks(robot): # MetaTasks dictonary robot.mTasks = dict() robot.tasksIne = dict() # Foot contacts robot.contactLF = MetaTaskDyn6d('contactLF', robot.dynamic, 'lf', 'left-ankle') robot.contactRF = MetaTaskDyn6d('contactRF', robot.dynamic, 'rf', 'right-ankle') setContacts(robot.contactLF, robot.contactRF) # MetaTasksDyn6d for other operational points robot.mTasks['waist'] = MetaTaskDyn6d('waist', robot.dynamic, 'waist', 'waist') robot.mTasks['chest'] = MetaTaskDyn6d('chest', robot.dynamic, 'chest', 'chest') robot.mTasks['rh'] = MetaTaskDyn6d('rh', robot.dynamic, 'rh', 'right-wrist') robot.mTasks['lh'] = MetaTaskDyn6d('lh', robot.dynamic, 'lh', 'left-wrist') for taskName in robot.mTasks: robot.mTasks[taskName].feature.frame('desired') robot.mTasks[taskName].gain.setConstant(10) robot.mTasks[taskName].task.dt.value = robot.timeStep robot.mTasks[taskName].featureDes.velocity.value = (0, 0, 0, 0, 0, 0) 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'] = MetaTaskDynCom(robot.dynamic, robot.timeStep) 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'] = MetaTaskDynPosture(robot.dynamic, robot.timeStep) 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'] = TaskDynInequality('taskHeight') plug(robot.dynamic.velocity, robot.tasksIne['taskHeight'].qdot) 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
dyn.ffposition.unplug() dyn.ffvelocity.unplug() dyn.ffacceleration.unplug() dyn.setProperty('ComputeBackwardDynamics', 'true') dyn.setProperty('ComputeAccelerationCoM', 'true') robot.control.unplug() #----------------------------------------------------------------------------- # --- OPERATIONAL TASKS (For HRP2-14)--------------------------------------------- #----------------------------------------------------------------------------- # --- Op task for the waist ------------------------------ taskWaist = MetaTaskDyn6d('taskWaist', dyn, 'waist', 'waist') taskChest = MetaTaskDyn6d('taskChest', dyn, 'chest', 'chest') taskHead = MetaTaskDyn6d('taskHead', dyn, 'head', 'gaze') taskrh = MetaTaskDyn6d('rh', dyn, 'rh', 'right-wrist') tasklh = MetaTaskDyn6d('lh', dyn, 'lh', 'left-wrist') taskrf = MetaTaskDyn6d('rf', dyn, 'rf', 'right-ankle') for task in [taskWaist, taskChest, taskHead, taskrh, tasklh, taskrf]: task.feature.frame('current') task.gain.setConstant(50) task.task.dt.value = dt # --- TASK COM ------------------------------------------------------ taskCom = MetaTaskDynCom(dyn, dt) # --- TASK POSTURE --------------------------------------------------
plug(robot.device.velocity, robot.dynamic.velocity) robot.dynamic.acceleration.value = robot.dimension * (0., ) #unplug the waist: it is not fixed in the universe anymore. robot.dynamic.ffposition.unplug() robot.dynamic.ffvelocity.unplug() robot.dynamic.ffacceleration.unplug() robot.dynamic.setProperty('ComputeBackwardDynamics', 'true') robot.dynamic.setProperty('ComputeAccelerationCoM', 'true') robot.device.control.unplug() # --- Task Dyn ----------------------------------------- # Task right hand task = MetaTaskDyn6d('rh', robot.dynamic, 'task') task.ref = ((0, 0, -1, 0.2), (0, 1, 0, -0.2), (1, 0, 0, 1.00), (0, 0, 0, 1)) # Task LFoot: Move the left foot up. taskLF = MetaTaskDyn6d('lf', robot.dynamic, 'lf', 'left-ankle') taskLF.ref = ((1, 0, 0, 0.0), (0, 1, 0, +0.29), (0, 0, 1, .15), (0, 0, 0, 1)) # Task RFoot: Move the right foot up. taskRF = MetaTaskDyn6d('rf', robot.dynamic, 'rf', 'right-ankle') taskRF.ref = ((1, 0, 0, 0.0102), (0, 1, 0, -0.096), (0, 0, 1, 0.1), (0, 0, 0, 1)) # --- TASK COM ------------------------------------------------------ robot.dynamic.setProperty('ComputeCoM', 'true') featureCom = FeatureGeneric('featureCom')
dyn.acceleration.value = robotDim * (0., ) dyn.ffposition.unplug() dyn.ffvelocity.unplug() dyn.ffacceleration.unplug() dyn.setProperty('ComputeBackwardDynamics', 'true') dyn.setProperty('ComputeAccelerationCoM', 'true') robot.control.unplug() #----------------------------------------------------------------------------- # --- OPERATIONAL TASKS (For HRP2-14)----------------------------------------- #----------------------------------------------------------------------------- taskRH = MetaTaskDyn6d('rh', dyn, 'rh', 'right-wrist') taskLH = MetaTaskDyn6d('lh', dyn, 'lh', 'left-wrist') taskRel = MetaTaskDyn6dRel('rel', dyn, 'rh', 'lh', 'right-wrist', 'left-wrist') for task in [taskRH, taskLH, taskRel]: task.feature.frame('current') task.gain.setConstant(10) task.task.dt.value = dt task.featureDes.velocity.value = (0, 0, 0, 0, 0, 0) handMgrip = eye(4) handMgrip[0:3, 3] = (0, 0, -0.14) taskRH.opmodif = matrixToTuple(handMgrip) taskLH.opmodif = matrixToTuple(handMgrip) # RELATIVE POSITION TASK
plug(robot.state, dyn.position) plug(robot.velocity, dyn.velocity) dyn.acceleration.value = robotDim * (0., ) dyn.ffposition.unplug() dyn.ffvelocity.unplug() dyn.ffacceleration.unplug() dyn.setProperty('ComputeBackwardDynamics', 'true') dyn.setProperty('ComputeAccelerationCoM', 'true') robot.control.unplug() # --- Task Dyn ----------------------------------------- # Task right hand task = MetaTaskDyn6d('rh', dyn, 'rh') # task.ref = ((0,0,-1,0.22),(0,1,0,-0.37),(1,0,0,.74),(0,0,0,1)) task.ref = ((0, 0, -1, 0.2), (0, 1, 0, -0.2), (1, 0, 0, 1.00), (0, 0, 0, 1)) # constraint only the position: _ _ _ Z Y X task.feature.selec.value = '111000' # --- TASK COM ------------------------------------------------------ dyn.setProperty('ComputeCoM', 'true') featureCom = FeatureGeneric('featureCom') featureComDes = FeatureGeneric('featureComDes') plug(dyn.com, featureCom.errorIN) plug(dyn.Jcom, featureCom.jacobianIN) featureCom.setReference('featureComDes') featureComDes.errorIN.value = (0.0478408688115, -0.0620357207995, 0.684865189311)