def __call__(self):
robot = self.robot
J = self.J
Y = self.Y
dM = self.dM
H = self.H
for j in range(robot.model.njoints - 1, 0, -1):
for joint_diff in range(
1, robot.model.njoints
): # diff should be a descendant or ancestor of j
if j not in ancestors(
joint_diff) and joint_diff not in ancestors(j):
continue
for i in ancestors(min(parent(joint_diff), j)):
i0, i1 = iv(i)[0], iv(i)[-1] + 1
j0, j1 = iv(j)[0], iv(j)[-1] + 1
Si = J[:, i0:i1]
Sj = J[:, j0:j1]
for d in iv(joint_diff):
T_iSd = np.matrix(H[:, d,
i0:i1]) # this is 0 if d<=i (<=j)
T_jSd = np.matrix(H[:, d, j0:j1]) # this is 0 is d<=j
'''
assert( norm(T_iSd)<1e-6 or not joint_diff<i ) # d<i => TiSd=0
assert( norm(T_jSd)<1e-6 or not joint_diff<j ) # d<j => TjSd=0
assert( norm(T_jSd)<1e-6 or not norm(T_iSd)<1e-6 ) # TiSd=0 => TjSd=0
assert( norm(T_iSd)>1e-6 )
'''
Yd = Y[max(j, joint_diff)]
dM[i0:i1, j0:j1, d] = T_iSd.T * Yd * Sj
if j < joint_diff:
dM[i0:i1, j0:j1, d] += Si.T * Yd * T_jSd
# Make dM triangular by copying strict-upper triangle to lower one.
if i != j: dM[j0:j1, i0:i1, d] = dM[i0:i1, j0:j1, d].T
else:
dM[j0:j1, i0:i1,
d] += np.triu(dM[i0:i1, j0:j1, d], 1).T
return dM
def __call__(self):
robot = self.robot
J = self.J
Y = self.Y
dM = self.dM
H = self.H
for j in range(robot.model.njoints-1,0,-1):
for joint_diff in range(1,robot.model.njoints): # diff should be a descendant or ancestor of j
if j not in ancestors(joint_diff) and joint_diff not in ancestors(j): continue
for i in ancestors(min(parent(joint_diff),j)):
i0,i1 = iv(i)[0],iv(i)[-1]+1
j0,j1 = iv(j)[0],iv(j)[-1]+1
Si = J[:,i0:i1]
Sj = J[:,j0:j1]
for d in iv(joint_diff):
T_iSd = np.matrix(H[:,d,i0:i1]) # this is 0 if d<=i (<=j)
T_jSd = np.matrix(H[:,d,j0:j1]) # this is 0 is d<=j
'''
assert( norm(T_iSd)<1e-6 or not joint_diff<i ) # d<i => TiSd=0
assert( norm(T_jSd)<1e-6 or not joint_diff<j ) # d<j => TjSd=0
assert( norm(T_jSd)<1e-6 or not norm(T_iSd)<1e-6 ) # TiSd=0 => TjSd=0
assert( norm(T_iSd)>1e-6 )
'''
Yd = Y[max(j,joint_diff)]
dM [i0:i1,j0:j1,d] = T_iSd.T * Yd * Sj
if j<joint_diff:
dM[i0:i1,j0:j1,d] += Si.T * Yd * T_jSd
# Make dM triangular by copying strict-upper triangle to lower one.
if i!=j: dM[j0:j1,i0:i1,d] = dM[i0:i1,j0:j1,d].T
else: dM[j0:j1,i0:i1,d] += np.triu(dM[i0:i1,j0:j1,d],1).T
return dM
def __call__(self, q, vq, aq):
robot = self.robot
se3.rnea(robot.model, robot.data, q, vq, aq)
NJ = robot.model.njoints
NV = robot.model.nv
J = robot.data.J
oMi = robot.data.oMi
v = [(oMi[i] * robot.data.v[i]).vector for i in range(NJ)]
a = [(oMi[i] * robot.data.a_gf[i]).vector for i in range(NJ)]
f = [(oMi[i] * robot.data.f[i]).vector for i in range(NJ)]
Y = [(oMi[i] * robot.model.inertias[i]).matrix() for i in range(NJ)]
Ycrb = [(oMi[i] * robot.data.Ycrb[i]).matrix() for i in range(NJ)]
Tkf = [zero([6, NV]) for i in range(NJ)]
Yvx = [zero([6, 6]) for i in range(NJ)]
adjf = lambda f: -np.bmat([[zero([3, 3]), skew(f[:3])],
[skew(f[:3]), skew(f[3:])]])
R = self.R = zero([NV, NV])
for i in reversed(range(
1, NJ)): # i is the torque index : dtau_i = R[i,:] dq
i0, i1 = iv(i)[0], iv(i)[-1] + 1
Yi = Y[i]
Yci = Ycrb[i]
Si = J[:, i0:i1]
vi = v[i]
ai = a[i]
fi = f[i]
aqi = aq[i0:i1]
vqi = vq[i0:i1]
dvi = Si * vqi
li = parent(i)
Yvx[i] += Y[i] * adj(v[i])
Yvx[i] -= adjf(Y[i] * v[i])
Yvx[i] -= adjdual(v[i]) * Yi
Yvx[li] += Yvx[i]
for k in ancestors(
i): # k is the derivative index: dtau = R[:,k] dq_k
k0, k1 = iv(k)[0], iv(k)[-1] + 1
Sk = J[:, k0:k1]
lk = parent(k)
# Si' Yi Tk ai = Si' Yi ( Sk x (ai-alk) + (vi-vlk) x (Sk x vlk) )
# Tk Si' Yi ai = Si' Yi ( - Sk x a[lk] + (vi-vlk) x (Sk x vlk) )
Tkf = Yci * (-MCross(Sk, a[lk]) +
MCross(MCross(Sk, v[lk]), v[lk]))
# Tk Si' fs = Tk Si' Ycrb[i] ai + Si' Ys (vs-vi) x (Sk x vlk)
# = "" + Si' Ys vs x (Sk x vlk) - Si' Ys vi x (Sk x vlk)
Tkf += Yvx[i] * MCross(Sk, v[lk])
R[i0:i1, k0:k1] = Si.T * Tkf
if i == k:
for kk in ancestors(k)[:-1]:
kk0, kk1 = iv(kk)[0], iv(kk)[-1] + 1
Skk = J[:, kk0:kk1]
R[kk0:kk1, k0:k1] = Skk.T * FCross(Sk, f[i])
R[kk0:kk1, k0:k1] += Skk.T * Tkf
self.a = a
self.v = v
self.f = f
self.Y = Y
self.Ycrb = Ycrb
self.Yvx = Yvx
return R
def __call__(self,q,vq,aq):
robot = self.robot
pin.rnea(robot.model,robot.data,q,vq,aq)
NJ = robot.model.njoints
NV = robot.model.nv
J = robot.data.J
oMi = robot.data.oMi
v = [ (oMi[i]*robot.data.v [i]).vector for i in range(NJ) ]
a = [ (oMi[i]*robot.data.a_gf[i]).vector for i in range(NJ) ]
f = [ (oMi[i]*robot.data.f [i]).vector for i in range(NJ) ]
Y = [ (oMi[i]*robot.model.inertias[i]).matrix() for i in range(NJ) ]
Ycrb = [ (oMi[i]*robot.data.Ycrb[i]) .matrix() for i in range(NJ) ]
Tkf = [ zero([6,NV]) for i in range(NJ) ]
Yvx = [ zero([6,6]) for i in range(NJ) ]
adjf = lambda f: -np.bmat([ [zero([3,3]),skew(f[:3])] , [skew(f[:3]),skew(f[3:])] ])
R = self.R = zero([NV,NV])
for i in reversed(range(1,NJ)): # i is the torque index : dtau_i = R[i,:] dq
i0,i1 = iv(i)[0],iv(i)[-1]+1
Yi = Y[i]
Yci = Ycrb[i]
Si = J[:,i0:i1]
vi = v[i]
ai = a[i]
fi = f[i]
aqi = aq[i0:i1]
vqi = vq[i0:i1]
dvi = Si*vqi
li = parent(i)
Yvx[ i] += Y[i]*adj(v[i])
Yvx[ i] -= adjf(Y[i]*v[i])
Yvx[ i] -= adjdual(v[i])*Yi
Yvx[li] += Yvx[i]
for k in ancestors(i): # k is the derivative index: dtau = R[:,k] dq_k
k0,k1 = iv(k)[0],iv(k)[-1]+1
Sk = J[:,k0:k1]
lk = parent(k)
# Si' Yi Tk ai = Si' Yi ( Sk x (ai-alk) + (vi-vlk) x (Sk x vlk) )
# Tk Si' Yi ai = Si' Yi ( - Sk x a[lk] + (vi-vlk) x (Sk x vlk) )
Tkf = Yci*(- MCross(Sk,a[lk]) + MCross(MCross(Sk,v[lk]),v[lk]))
# Tk Si' fs = Tk Si' Ycrb[i] ai + Si' Ys (vs-vi) x (Sk x vlk)
# = "" + Si' Ys vs x (Sk x vlk) - Si' Ys vi x (Sk x vlk)
Tkf += Yvx[i]*MCross(Sk,v[lk])
R[i0:i1,k0:k1] = Si.T*Tkf
if i==k:
for kk in ancestors(k)[:-1]:
kk0,kk1 = iv(kk)[0],iv(kk)[-1]+1
Skk = J[:,kk0:kk1]
R[kk0:kk1,k0:k1] = Skk.T * FCross(Sk,f[i])
R[kk0:kk1,k0:k1] += Skk.T * Tkf
self.a = a
self.v = v
self.f = f
self.Y = Y
self.Ycrb = Ycrb
self.Yvx = Yvx
return R