def test_rotate(self):
for axis, angle, mat in [
('x', np.pi, ((1, 0, 0, 0), (0, -1, 0, 0), (0, 0, -1, 0), (0, 0, 0, 1))),
('y', np.pi, ((-1, 0, 0, 0), (0, 1, 0, 0), (0, 0, -1, 0), (0, 0, 0, 1))),
('z', np.pi, ((-1, 0, 0, 0), (0, -1, 0, 0), (0, 0, 1, 0), (0, 0, 0, 1))),
]:
self.assertEqual(mat, mod.rotate(axis, angle))
def createTasks(robot):
# MetaTasks dictonary
robot.mTasks = dict()
robot.tasksIne = dict()
# Foot contacts
robot.contactLF = MetaTaskDyn6d('contactLF',robot.dynamic,'lf','left-ankle')
robot.contactRF = MetaTaskDyn6d('contactRF',robot.dynamic,'rf','right-ankle')
setContacts(robot.contactLF,robot.contactRF)
# MetaTasksDyn6d for other operational points
robot.mTasks['waist'] = MetaTaskDyn6d('waist', robot.dynamic, 'waist', 'waist')
robot.mTasks['chest'] = MetaTaskDyn6d('chest', robot.dynamic, 'chest', 'chest')
robot.mTasks['rh'] = MetaTaskDyn6d('rh', robot.dynamic, 'rh', 'right-wrist')
robot.mTasks['lh'] = MetaTaskDyn6d('lh', robot.dynamic, 'lh', 'left-wrist')
for taskName in robot.mTasks:
robot.mTasks[taskName].feature.frame('desired')
robot.mTasks[taskName].gain.setConstant(10)
robot.mTasks[taskName].task.dt.value = robot.timeStep
robot.mTasks[taskName].featureDes.velocity.value=(0,0,0,0,0,0)
handMgrip=eye(4); handMgrip[0:3,3] = (0,0,-0.14)
robot.mTasks['rh'].opmodif = matrixToTuple(handMgrip)
robot.mTasks['lh'].opmodif = matrixToTuple(handMgrip)
robot.mTasks['gaze'] = MetaTaskDynVisualPoint('gaze',robot.dynamic,'head','gaze')
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))
robot.mTasks['gaze'].opmodif = matrixToTuple(headMcam)
robot.mTasks['gaze'].featureDes.xy.value = (0,0)
robot.mTasks['gaze'].task.dt.value = robot.timeStep
robot.mTasks['gaze'].gain.setConstant(1)
# CoM Task
robot.mTasks['com'] = MetaTaskDynCom(robot.dynamic,robot.timeStep)
robot.dynamic.com.recompute(0)
robot.mTasks['com'].featureDes.errorIN.value = robot.dynamic.com.value
robot.mTasks['com'].task.controlGain.value = 10
robot.mTasks['com'].feature.selec.value = '011'
# Posture Task
robot.mTasks['posture'] = MetaTaskDynPosture(robot.dynamic,robot.timeStep)
robot.mTasks['posture'].ref = robot.halfSitting
robot.mTasks['posture'].gain.setConstant(5)
CHEST = 50/DEG # 80 ... 90?
WITH_FULL_EXTENTION=False
'''
sot.clear()
contact(contactLF)
contact(contactRF)
taskCom.feature.selec.value = "11"
taskCom.gain.setByPoint(100, 10, 0.005, 0.8)
taskrf.feature.frame('desired')
taskrf.gain.setByPoint(40, 2, 0.002, 0.8)
taskChest.feature.selec.value = '111000'
taskChest.ref = rotate('y', CHEST)
taskChest.gain.setByPoint(30, 3, 1 / DEG, 0.8)
taskPosture.gain.setByPoint(30, 3, 1 / DEG, 0.8)
ql = matrix(dyn.lowerJl.value)
ql[0, 15] = MAX_SUPPORT_EXT
taskLim.referencePosInf.value = vectorToTuple(ql)
#sot.push(taskLim.name)
plug(robot.state, sot.position)
q0 = robot.state.value
rf0 = matrix(taskrf.feature.position.value)[0:3, 3]
MetaTaskDynPosture.postureRange = postureRange[robotName]
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
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)
#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]
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)
CHEST = 50/DEG # 80 ... 90?
WITH_FULL_EXTENTION=False
'''
sot.clear()
contact(contactLF)
contact(contactRF)
taskCom.feature.selec.value = "11"
taskCom.gain.setByPoint(100,10,0.005,0.8)
taskrf.feature.frame('desired')
taskrf.gain.setByPoint(40,2,0.002,0.8)
taskChest.feature.selec.value='111000'
taskChest.ref = rotate('y',CHEST)
taskChest.gain.setByPoint(30,3,1/DEG,0.8)
taskPosture.gain.setByPoint(30,3,1/DEG,0.8)
ql = matrix(robot.dynamic.lowerJl.value)
ql[0,15] = MAX_SUPPORT_EXT
taskLim.referencePosInf.value = vectorToTuple(ql)
#sot.push(taskLim.name)
plug(robot.device.state,sot.position)
q0 = robot.device.state.value
rf0 = matrix(taskrf.feature.position.value)[0:3,3]
MetaTaskDynPosture.postureRange = postureRange['romeo']
