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 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 createFoVTasks(robot):
headMcam=array([[0.0,0.0,1.0,0],[1.,0.0,0.0,0],[0.0,1.,0.0,0],[0.0,0.0,0.0,1.0]])
taskFoV = MetaTaskVisualPoint('FoV',robot.dynamic,'head','gaze')
taskFoV.opmodif = matrixToTuple(headMcam)
taskFoV.task=TaskInequality('taskFoVineq')
taskFoV.task.add(taskFoV.feature.name)
[Xmax,Ymax]=[0.38,0.28]
taskFoV.task.referenceInf.value = (-Xmax,-Ymax) # Xmin, Ymin
taskFoV.task.referenceSup.value = (Xmax,Ymax) # Xmax, Ymax
taskFoV.task.dt.value=5e-3
taskFoV.task.controlGain.value=0.01
taskFoV.featureDes.xy.value = (0,0)
rightWristPosition = MatrixHomoToPose('rightWristPosition')
plug(robot.frames['rightGripper'].position, rightWristPosition.sin)
# taskFoV.goto3D((0,-1.1,0.9))
#rightWristPosition.sout.recompute(1)
#taskFoV.goto3D(rightWristPosition.sout.value)
plug(rightWristPosition.sout, taskFoV.target)
return taskFoV.task
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')
taskFoV.task.add(taskFoV.feature.name)
[Xmax,Ymax]=[0.38,0.28]
taskFoV.task.referenceInf.value = (-Xmax,-Ymax) # Xmin, Ymin
def createGazeTask(jointName):
taskGaze = MetaTaskVisualPoint('gaze', robot.dynamic, jointName, jointName)
taskGaze.featureDes.xy.value = (0, 0)
return taskGaze
def __init__(self, name, dyn, opPoint, opPointRef='right-wrist'):
MetaTaskVisualPoint.__init__(self, name, dyn, opPoint, opPointRef)
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')
taskFoV.task.add(taskFoV.feature.name)
[Xmax, Ymax] = [0.38, 0.28]
def __init__(self,name,dyn,opPoint,opPointRef='right-wrist'): MetaTaskVisualPoint.__init__(self, name, dyn, opPoint, opPointRef)
def Pr2GazeTask(robot):
task = MetaTaskVisualPoint('gaze',robot.dynamic,'head','gaze')
task.opmodif = matrixToTuple(Pr2headMcam)
return task
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)
# --- 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)
# --- 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
def Pr2GazeTask(robot):
task = MetaTaskVisualPoint('gaze', robot.dynamic, 'head', 'gaze')
task.opmodif = matrixToTuple(Pr2headMcam)
return task
