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
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
taskFoV.task.referenceSup.value = (Xmax,Ymax) # Xmax, Ymax
taskFoV.task.dt.value=dt
taskFoV.task.controlGain.value=0.01
taskFoV.featureDes.xy.value = (0,0)
# --- Task Joint Limits -----------------------------------------
robot.dynamic.upperJl.recompute(0)
robot.dynamic.lowerJl.recompute(0)
taskJL = TaskJointLimits('taskJL')
plug(robot.dynamic.position,taskJL.position)
taskJL.controlGain.value = 10
# --- 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]
taskFoV.task.referenceInf.value = (-Xmax, -Ymax) # Xmin, Ymin
taskFoV.task.referenceSup.value = (Xmax, Ymax) # Xmax, Ymax
taskFoV.task.dt.value = dt
taskFoV.task.controlGain.value = 0.01
taskFoV.featureDes.xy.value = (0, 0)
# --- Task Joint Limits -----------------------------------------
dyn.upperJl.recompute(0)
dyn.lowerJl.recompute(0)
taskJL = TaskJointLimits('taskJL')
plug(dyn.position, taskJL.position)
taskJL.controlGain.value = 10
taskJL.referenceInf.value = dyn.lowerJl.value