Python MetaTaskVisualPoint.task Examples

Example #1

File: pr2_tasks.py Project: florent-lamiraux/sot_pr2

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

Example #2

File: pr2_tasks.py Project: florent-lamiraux/sot_pr2

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

Example #3

File: tools.py Project: francois-keith/sot-robohow-demo

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

Example #4

File: sot-concept.py Project: elie-moussy/sot-dyninv

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

Example #5

# --- 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