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)