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