def jointRPY(self, j, t):
return matrixToRPY(self.jointPosition(j, t))
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]))
step2 = (dot(inv(dot(wMa, lf[0])), (dot(wMb, rf[1]))))
step3 = (dot(inv(rf[1]), lf[1]))
seqpart = ''
for s in [step1, step2, step3]:
si = inv(s)
seqpart += str(-si[0, 3]) + ' ' + str(-si[1, 3]) + ' ' + str(
-arctan2(si[1, 0], si[0, 0]) * 180 / pi) + ' '
# --- MAIN LOOP ------------------------------------------
def inc():
robot.increment(dt)
def jointRPY( self,j,t): return matrixToRPY( self.jointPosition(j,t) )
def linkPosition(self,l,t):
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]) )
step2 = ( dot( inv( dot(wMa,lf[0]) ), (dot( wMb,rf[1])) ) )
step3 = ( dot( inv(rf[1]), lf[1] ))
seqpart = ''
for s in [step1,step2, step3]:
si = inv(s)
seqpart+= str(-si[0,3])+' '+str(-si[1,3])+' '+str(-arctan2(si[1,0],si[0,0])*180/pi)+' '
# --- MAIN LOOP ------------------------------------------
def inc():
robot.increment(dt)
attime.run(robot.control.time)
