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):
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 hessian(robot, q, crossterms=False):
'''
Compute the Hessian tensor of all the robot joints.
If crossterms, then also compute the Si x Si terms,
that are not part of the true Hessian but enters in some computations like VRNEA.
'''
lambdas.setRobotArgs(robot)
H = np.zeros([6, robot.model.nv, robot.model.nv])
se3.computeJointJacobians(robot.model, robot.data, q)
J = robot.data.J
skiplast = -1 if not crossterms else None
for joint_i in range(1, robot.model.njoints):
for joint_j in ancestors(joint_i)[:skiplast]: # j is a child of i
for i in iv(joint_i):
for j in iv(joint_j):
Si = J[:, i]
Sj = J[:, j]
H[:, i, j] = np.asarray(Mcross(Sj, Si))[:, 0]
return H
def hessian(robot,q,crossterms=False):
'''
Compute the Hessian tensor of all the robot joints.
If crossterms, then also compute the Si x Si terms,
that are not part of the true Hessian but enters in some computations like VRNEA.
'''
lambdas.setRobotArgs(robot)
H=np.zeros([6,robot.model.nv,robot.model.nv])
pin.computeJointJacobians(robot.model,robot.data,q)
J = robot.data.J
skiplast = -1 if not crossterms else None
for joint_i in range(1,robot.model.njoints):
for joint_j in ancestors(joint_i)[:skiplast]: # j is a child of i
for i in iv(joint_i):
for j in iv(joint_j):
Si = J[:,i]
Sj = J[:,j]
H[:,i,j] = np.asarray(Mcross(Sj, Si))[:,0]
return H
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):
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
'/home/nmansard/src/pinocchio/pinocchio/models/romeo/',
], se3.JointModelFreeFlyer())
q = rand(robot.model.nq)
q[3:7] /= norm(q[3:7])
vq = rand(robot.model.nv)
# --- HESSIAN ---
# --- HESSIAN ---
# --- HESSIAN ---
H = hessian(robot, q)
# Compute the Hessian matrix H using RNEA, so that acor = v*H*v
vq1 = vq * 0
Htrue = np.zeros([6, robot.model.nv, robot.model.nv])
for joint_i in range(1, robot.model.njoints):
for joint_j in ancestors(joint_i)[:-1]:
for i in iv(joint_i):
for j in iv(joint_j):
vq1 *= 0
vq1[i] = vq1[j] = 1.0
se3.computeAllTerms(robot.model, robot.data, q, vq1)
Htrue[:, i, j] = (robot.data.oMi[joint_i] *
robot.data.a[joint_i]).vector.T
print('Check hessian = \t\t', norm(H - Htrue))
# --- dCRBA ---
# --- dCRBA ---
# --- dCRBA ---
dcrba = DCRBA(robot)
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
[ '/home/nmansard/src/pinocchio/pinocchio/models/romeo/', ],
pin.JointModelFreeFlyer()
)
q = rand(robot.model.nq); q[3:7] /= norm(q[3:7])
vq = rand(robot.model.nv)
# --- HESSIAN ---
# --- HESSIAN ---
# --- HESSIAN ---
H = hessian(robot,q)
# Compute the Hessian matrix H using RNEA, so that acor = v*H*v
vq1 = vq*0
Htrue = np.zeros([6,robot.model.nv,robot.model.nv])
for joint_i in range(1,robot.model.njoints):
for joint_j in ancestors(joint_i)[:-1]:
for i in iv(joint_i):
for j in iv(joint_j):
vq1 *= 0
vq1[i] = vq1[j] = 1.0
pin.computeAllTerms(robot.model,robot.data,q,vq1)
Htrue[:,i,j] = (robot.data.oMi[joint_i]*robot.data.a[joint_i]).vector.T
print('Check hessian = \t\t', norm(H-Htrue))
# --- dCRBA ---
# --- dCRBA ---
# --- dCRBA ---
dcrba = DCRBA(robot)
dcrba.pre(q)
