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 createGazeTask(jointName): taskGaze = MetaTaskVisualPoint('gaze', robot.dynamic, jointName, jointName) taskGaze.featureDes.xy.value = (0, 0) return taskGaze
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]
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 Pr2GazeTask(robot): task = MetaTaskVisualPoint('gaze', robot.dynamic, 'head', 'gaze') task.opmodif = matrixToTuple(Pr2headMcam) return task