Beispiel #1
0
 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
Beispiel #2
0
 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
Beispiel #3
0
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]))
Beispiel #4
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 ---------------------------------------------
Beispiel #5
0
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
Beispiel #6
0
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])   )