def Pr2FoVTask(robot, dt):
task = MetaTaskVisualPoint('FoV', robot.dynamic, 'head', 'gaze')
task.opmodif = matrixToTuple(Pr2headMcam)
task.task = TaskInequality('taskFoVineq')
task.task.add(task.feature.name)
[Xmax, Ymax] = [0.38, 0.28]
task.task.referenceInf.value = (-Xmax, -Ymax) # Xmin, Ymin
task.task.referenceSup.value = (Xmax, Ymax) # Xmax, Ymax
task.task.dt.value = dt
task.task.controlGain.value = 0.01
task.featureDes.xy.value = (0, 0)
return task
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 createTaskInternal(name, feature, equality):
# Define a equality task
if equality:
task = Task(name)
task.controlGain.value = 1
# Define a inequality task
else:
dt = 0.005 #todo
task = TaskInequality(name)
task.dt.value = dt
task.controlGain.value = task.dt.value
task.add(feature.name)
return task
