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):
robot = self.robot
H = hessian(robot,q,crossterms=True)
J = robot.data.J
YJ = self.YJ
Q = self.Q
# The terms in SxS YS corresponds to f = vxYv
# The terms in S YSxS corresponds to a = vxv (==> f = Yvxv)
for k in range(robot.model.njoints-1,0,-1):
k0,k1 = iv(k)[0],iv(k)[-1]+1
Yk = (robot.data.oMi[k]*robot.data.Ycrb[k]).matrix()
Sk = J[:,k0:k1]
for j in ancestors(k): # Fill YJ = [ ... Yk*Sj ... ]_j
j0,j1 = iv(j)[0],iv(j)[-1]+1
YJ[:,j0:j1] = Yk*J[:,j0:j1]
# Fill the diagonal of the current level of Q = Q[k,:k,:k]
for i in ancestors(k):
i0,i1 = iv(i)[0],iv(i)[-1]+1
Si = J[:,i0:i1]
Q[k0:k1,i0:i1,i0:i1] = -td(H[:,k0:k1,i0:i1],YJ[:,i0:i1],[0,0]) # = (Si x Sk)' * Yk Si
# Fill the nondiag of the current level Q[k,:k,:k]
for j in ancestors(i)[:-1]:
j0,j1 = iv(j)[0],iv(j)[-1]+1
Sj = J[:,j0:j1]
# = Sk' * Yk * Si x Sj
Q[k0:k1,i0:i1,j0:j1] = td(YJ[:,k0:k1], H[:,i0:i1,j0:j1],[0,0])
# = (Si x Sk)' * Yk Sj
Q[k0:k1,i0:i1,j0:j1] -= td(H[:,k0:k1,i0:i1],YJ[:,j0:j1], [0,0])
# = (Sj x Sk)' * Yk Si
Q[k0:k1,j0:j1,i0:i1] =- td(H[:,k0:k1,j0:j1],YJ[:,i0:i1], [0,0])
# Fill the border elements of levels below k Q[kk,k,:] and Q[kk,:,k] with kk<k
for kk in ancestors(k)[:-1]:
kk0,kk1 = iv(kk)[0],iv(kk)[-1]+1
Skk = J[:,kk0:kk1]
for j in ancestors(k):
j0,j1 = iv(j)[0],iv(j)[-1]+1
Sj = J[:,j0:j1]
# = Skk' Yk Sk x Sj = (Yk Skk)' Sk x Sj
if k!=j:
Q[kk0:kk1,k0:k1,j0:j1] = td(YJ[:,kk0:kk1],H[:,k0:k1,j0:j1 ],[0,0])
# = (Sk x Skk)' Yk Sj
Q[kk0:kk1,k0:k1,j0:j1] += td(H[:,k0:k1,kk0:kk1].T,YJ[:,j0:j1], [2,0])
# = (Sj x Skk)' Yk Sk
# Switch because j can be > or < than kk
if j<kk:
Q[kk0:kk1,j0:j1,k0:k1] = -Q[j0:j1,kk0:kk1,k0:k1].transpose(1,0,2)
elif j>=kk:
Q[kk0:kk1,j0:j1,k0:k1] = td(H[:,j0:j1,kk0:kk1].T,YJ[:,k0:k1], [2,0])
return Q
def __call__(self, q):
robot = self.robot
H = hessian(robot, q, crossterms=True)
J = robot.data.J
YJ = self.YJ
Q = self.Q
# The terms in SxS YS corresponds to f = vxYv
# The terms in S YSxS corresponds to a = vxv (==> f = Yvxv)
for k in range(robot.model.njoints - 1, 0, -1):
k0, k1 = iv(k)[0], iv(k)[-1] + 1
Yk = (robot.data.oMi[k] * robot.data.Ycrb[k]).matrix()
Sk = J[:, k0:k1]
for j in ancestors(k): # Fill YJ = [ ... Yk*Sj ... ]_j
j0, j1 = iv(j)[0], iv(j)[-1] + 1
YJ[:, j0:j1] = Yk * J[:, j0:j1]
# Fill the diagonal of the current level of Q = Q[k,:k,:k]
for i in ancestors(k):
i0, i1 = iv(i)[0], iv(i)[-1] + 1
Si = J[:, i0:i1]
Q[k0:k1, i0:i1, i0:i1] = -td(H[:, k0:k1, i0:i1], YJ[:, i0:i1],
[0, 0]) # = (Si x Sk)' * Yk Si
# Fill the nondiag of the current level Q[k,:k,:k]
for j in ancestors(i)[:-1]:
j0, j1 = iv(j)[0], iv(j)[-1] + 1
Sj = J[:, j0:j1]
# = Sk' * Yk * Si x Sj
Q[k0:k1, i0:i1, j0:j1] = td(YJ[:, k0:k1], H[:, i0:i1,
j0:j1], [0, 0])
# = (Si x Sk)' * Yk Sj
Q[k0:k1, i0:i1, j0:j1] -= td(H[:, k0:k1, i0:i1],
YJ[:, j0:j1], [0, 0])
# = (Sj x Sk)' * Yk Si
Q[k0:k1, j0:j1,
i0:i1] = -td(H[:, k0:k1, j0:j1], YJ[:, i0:i1], [0, 0])
# Fill the border elements of levels below k Q[kk,k,:] and Q[kk,:,k] with kk<k
for kk in ancestors(k)[:-1]:
kk0, kk1 = iv(kk)[0], iv(kk)[-1] + 1
Skk = J[:, kk0:kk1]
for j in ancestors(k):
j0, j1 = iv(j)[0], iv(j)[-1] + 1
Sj = J[:, j0:j1]
# = Skk' Yk Sk x Sj = (Yk Skk)' Sk x Sj
if k != j:
Q[kk0:kk1, k0:k1,
j0:j1] = td(YJ[:, kk0:kk1], H[:, k0:k1, j0:j1],
[0, 0])
# = (Sk x Skk)' Yk Sj
Q[kk0:kk1, k0:k1, j0:j1] += td(H[:, k0:k1, kk0:kk1].T,
YJ[:, j0:j1], [2, 0])
# = (Sj x Skk)' Yk Sk
# Switch because j can be > or < than kk
if j < kk:
Q[kk0:kk1, j0:j1,
k0:k1] = -Q[j0:j1, kk0:kk1, k0:k1].transpose(
1, 0, 2)
elif j >= kk:
Q[kk0:kk1, j0:j1, k0:k1] = td(H[:, j0:j1, kk0:kk1].T,
YJ[:, k0:k1], [2, 0])
return Q
