def alignHrpFrames( self ):
namesT1 = ['Larm2','Lhand','Lleg1','Lleg2','Rarm2','Rhand','Rleg1','Rleg2','head','chest']
namesT2 = ['Larm1','Rarm1']
transf1 = matrix(rotate('x',-pi))*matrix(rotate('z',-pi/2))
transf2 = matrix(rotate('y',pi/2))*matrix(rotate('z',pi/2))
for joint in self.joints.values():
if joint.name in namesT1: joint.Ki = joint.Ki*transf1
if joint.name in namesT2: joint.Ki = joint.Ki*transf2
def alignHrpFrames(self):
namesT1 = [
'Larm2', 'Lhand', 'Lleg1', 'Lleg2', 'Rarm2', 'Rhand', 'Rleg1',
'Rleg2', 'head', 'chest'
]
namesT2 = ['Larm1', 'Rarm1']
transf1 = matrix(rotate('x', -pi)) * matrix(rotate('z', -pi / 2))
transf2 = matrix(rotate('y', pi / 2)) * matrix(rotate('z', pi / 2))
for joint in self.joints.values():
if joint.name in namesT1: joint.Ki = joint.Ki * transf1
if joint.name in namesT2: joint.Ki = joint.Ki * transf2
if abs(rMl0[2, 2] - 1) > 1e-3 or abs(rMl0[2, 3]) > 1e-3:
print 'Error: the feet should be parallele on a same Z plane (initial pose)'
a0 = arctan2(rMl0[1, 0], rMl0[0, 0]) / 2
a1 = arctan2(rMl1[1, 0], rMl1[0, 0]) / 2
t0 = rMl0[0:3, 3]
t1 = rMl1[0:3, 3]
if isinstance(options.direction, int) and options.direction == 1:
options.direction = "forward"
if isinstance(options.direction, int) and options.direction == -1:
options.direction = "backward"
if options.direction[0:3] == "for": t1[0] -= 0.2
if options.direction[0:3] == "bac": t1[0] += 0.2
Ma0 = array(rotate('z', a0))
Ma0[0:3, 3] = t0 / 2
Ma1 = array(rotate('z', a1))
Ma1[0:3, 3] = t1 / 2
wMa = inv(dot(Ma0, rf[0]))
wMb = inv(dot(Ma1, rf[1]))
Mview = dot(wMa, RPYToMatrix(q[0][0:6]))
Mup = eye(4)
Mup[2, 3] = 0.105
Mview = dot(Mup, Mview)
robot.set(tuple(matrixToRPY(Mview)) + q[0][6:])
step1 = (dot(wMa, lf[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(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]
# --- Events ---------------------------------------------
contact(contactLF)
contact(contactRF)
taskCom.feature.selec.value = "01"
taskCom.gain.setByPoint(100,10,0.005,0.8)
taskSupport.selec.value = "10"
taskSupport.referenceInf.value = (-0.015,-0.09,0) # Xmin, Ymin
taskSupport.referenceSup.value = (0.035,0.09,0) # Xmax, Ymax
taskSupport.controlGain.value = 15
mrh=eye(4)
mrh[0:3,3] = (0,0,-0.18)
taskrh.opmodif = matrixToTuple(mrh)
taskrh.gain.setByPoint(120,5,0.03,0.8)
drh=array(matrix(rotate('x',-45/DEG))*matrix(rotate('y',180/DEG)))
taskrh.ref = matrixToTuple(drh)
taskrh.feature.selec.value = '111000'
tasklh.opmodif = matrixToTuple(mrh)
tasklh.gain.setByPoint(120,5,0.03,0.8)
dlh=array(matrix(rotate('x',+45/DEG))*matrix(rotate('y',180/DEG)))
tasklh.ref = matrixToTuple(dlh)
tasklh.feature.selec.value = '111000'
taskWaist.gain.setByPoint(60,5,0.01,0.8)
taskWaist.feature.frame('desired')
[s.recompute(0) for s in robot.state, dyn.com, dyn.rh, dyn.lh ]
q0 = robot.state.value
if abs(rMl1[2,2]-1)>1e-3 or abs(rMl1[2,3])>1e-3:
print 'Error: the feet should be parallele on a same Z plane (final pose)'
if abs(rMl0[2,2]-1)>1e-3 or abs(rMl0[2,3])>1e-3:
print 'Error: the feet should be parallele on a same Z plane (initial pose)'
a0 = arctan2(rMl0[1,0],rMl0[0,0])/2
a1 = arctan2(rMl1[1,0],rMl1[0,0])/2
t0 = rMl0[0:3,3]
t1 = rMl1[0:3,3];
if isinstance(options.direction,int) and options.direction == 1: options.direction = "forward"
if isinstance(options.direction,int) and options.direction == -1: options.direction = "backward"
if options.direction[0:3] == "for": t1[0]-=0.2
if options.direction[0:3] == "bac": t1[0]+=0.2
Ma0 = array(rotate('z',a0))
Ma0[0:3,3] = t0/2
Ma1 = array(rotate('z',a1))
Ma1[0:3,3] = t1/2
wMa = inv(dot(Ma0,rf[0]))
wMb = inv(dot(Ma1,rf[1]))
Mview = dot(wMa,RPYToMatrix(q[0][0:6]))
Mup = eye(4); Mup[2,3] = 0.105
Mview = dot( Mup,Mview )
robot.set( tuple(matrixToRPY( Mview ))+q[0][6:] )
step1 = ( dot(wMa,lf[0]) )
