def __call__(self, q, vq):
robot = self.robot
NV = robot.model.nv
NJ = robot.model.njoints
C = self.C
YS = self.YS
Sxv = self.Sxv
H = hessian(robot, q)
se3.computeAllTerms(robot.model, robot.data, q, vq)
J = robot.data.J
oMi = robot.data.oMi
v = [(oMi[i] * robot.data.v[i]).vector for i in range(NJ)]
Y = [(oMi[i] * robot.data.Ycrb[i]).matrix() for i in range(NJ)]
# --- Precomputations
# Centroidal matrix
for j in range(NJ):
j0, j1 = iv(j)[0], iv(j)[-1] + 1
YS[:, j0:j1] = Y[j] * J[:, j0:j1]
# velocity-jacobian cross products.
for j in range(NJ):
j0, j1 = iv(j)[0], iv(j)[-1] + 1
vj = v[j]
Sxv[:, j0:j1] = adj(vj) * J[:, j0:j1]
# --- Main loop
for i in range(NJ - 1, 0, -1):
i0, i1 = iv(i)[0], iv(i)[-1] + 1
Yi = Y[i]
Si = J[:, i0:i1]
vp = v[robot.model.parents[i]]
SxvY = Sxv[:, i0:i1].T * Yi
for j in ancestors(i):
j0, j1 = iv(j)[0], iv(j)[-1] + 1
Sj = J[:, j0:j1]
# COR ===> Si' Yi Sj x vj
C[i0:i1, j0:j1] = YS[:, i0:i1].T * Sxv[:, j0:j1]
# CEN ===> Si' vi x Yi Sj = - (Si x vi)' Yi Sj
C[i0:i1, j0:j1] -= SxvY * Sj
vxYS = adjdual(v[i]) * Yi * Si
YSxv = Yi * Sxv[:, i0:i1]
Yv = Yi * vp
for ii in ancestors(i)[:-1]:
ii0, ii1 = iv(ii)[0], iv(ii)[-1] + 1
Sii = J[:, ii0:ii1]
# COR ===> Sii' Yi Si x vi
C[ii0:ii1, i0:i1] = Sii.T * YSxv
# CEN ===> Sii' vi x Yi Si = (Sii x vi)' Yi Si
C[ii0:ii1, i0:i1] += Sii.T * vxYS
# CEN ===> Sii' Si x Yi vi = (Sii x Si)' Yi vi
C[ii0:ii1, i0:i1] += np.matrix(
td(H[:, i0:i1, ii0:ii1], Yv, [0, 0])[:, :, 0]).T
return C
def __call__(self,q,vq):
robot = self.robot
NV = robot.model.nv
NJ = robot.model.njoints
C = self.C
YS = self.YS
Sxv = self.Sxv
H = hessian(robot,q)
pin.computeAllTerms(robot.model,robot.data,q,vq)
J = robot.data.J
oMi=robot.data.oMi
v = [ (oMi[i]*robot.data.v[i]).vector for i in range(NJ) ]
Y = [ (oMi[i]*robot.data.Ycrb[i]).matrix() for i in range(NJ) ]
# --- Precomputations
# Centroidal matrix
for j in range(NJ):
j0,j1 = iv(j)[0],iv(j)[-1]+1
YS[:,j0:j1] = Y[j]*J[:,j0:j1]
# velocity-jacobian cross products.
for j in range(NJ):
j0,j1 = iv(j)[0],iv(j)[-1]+1
vj = v[j]
Sxv[:,j0:j1] = adj(vj)*J[:,j0:j1]
# --- Main loop
for i in range(NJ-1,0,-1):
i0,i1 = iv(i)[0],iv(i)[-1]+1
Yi = Y[i]
Si = J[:,i0:i1]
vp = v[robot.model.parents[i]]
SxvY = Sxv[:,i0:i1].T*Yi
for j in ancestors(i):
j0,j1 = iv(j)[0],iv(j)[-1]+1
Sj = J[:,j0: j1 ]
# COR ===> Si' Yi Sj x vj
C[i0:i1,j0:j1] = YS[:,i0:i1].T*Sxv[:,j0:j1]
# CEN ===> Si' vi x Yi Sj = - (Si x vi)' Yi Sj
C[i0:i1,j0:j1] -= SxvY*Sj
vxYS = adjdual(v[i])*Yi*Si
YSxv = Yi*Sxv[:,i0:i1]
Yv = Yi*vp
for ii in ancestors(i)[:-1]:
ii0,ii1 = iv(ii)[0],iv(ii)[-1]+1
Sii = J[:,ii0:ii1]
# COR ===> Sii' Yi Si x vi
C[ii0:ii1,i0:i1] = Sii.T*YSxv
# CEN ===> Sii' vi x Yi Si = (Sii x vi)' Yi Si
C[ii0:ii1,i0:i1] += Sii.T*vxYS
# CEN ===> Sii' Si x Yi vi = (Sii x Si)' Yi vi
C[ii0:ii1,i0:i1] += np.matrix(td(H[:,i0:i1,ii0:ii1],Yv,[0,0])[:,:,0]).T
return C
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
