5 from __future__
import print_function
7 import pinocchio
as pin
10 from numpy.linalg
import norm
12 from lambdas
import Mcross,ancestors,parent,iv,td,quad,adj,adjdual
16 Compute the Hessian tensor of all the robot joints. 17 If crossterms, then also compute the Si x Si terms, 18 that are not part of the true Hessian but enters in some computations like VRNEA. 21 H=np.zeros([6,robot.model.nv,robot.model.nv])
22 pin.computeJointJacobians(robot.model,robot.data,q)
24 skiplast = -1
if not crossterms
else None 25 for joint_i
in range(1,robot.model.njoints):
26 for joint_j
in ancestors(joint_i)[:skiplast]:
31 H[:,i,j] = np.asarray(
Mcross(Sj, Si))[:,0]
48 Compute the derivative (tangent application) of the mass matrix M wrt q, 49 which is a NVxNVxNV tensor. 59 self.
J = robot.data.J.copy()
60 pin.crba(robot.model,robot.data,q)
61 self.
Y =[ (robot.data.oMi[i]*robot.data.Ycrb[i]).matrix()
for i
in range(0,robot.model.njoints) ]
63 self.
dM = np.zeros([robot.model.nv,]*3)
72 for j
in range(robot.model.njoints-1,0,-1):
73 for joint_diff
in range(1,robot.model.njoints):
78 i0,i1 =
iv(i)[0],
iv(i)[-1]+1
79 j0,j1 =
iv(j)[0],
iv(j)[-1]+1
84 for d
in iv(joint_diff):
86 T_iSd = np.array(H[:,d,i0:i1])
87 T_jSd = np.array(H[:,d,j0:j1])
90 assert( norm(T_iSd)<1e-6 or not joint_diff<i ) # d<i => TiSd=0 91 assert( norm(T_jSd)<1e-6 or not joint_diff<j ) # d<j => TjSd=0 92 assert( norm(T_jSd)<1e-6 or not norm(T_iSd)<1e-6 ) # TiSd=0 => TjSd=0 93 assert( norm(T_iSd)>1e-6 ) 96 Yd = Y[
max(j,joint_diff)]
98 dM [i0:i1,j0:j1,d] = T_iSd.T * Yd * Sj
100 dM[i0:i1,j0:j1,d] += Si.T * Yd * T_jSd
103 if i!=j: dM[j0:j1,i0:i1,d] = dM[i0:i1,j0:j1,d].T
104 else: dM[j0:j1,i0:i1,d] += np.triu(dM[i0:i1,j0:j1,d],1).T
113 Compute the C tensor so that nle(q,vq) = vq C vq. 114 Noting Cv = C*vq for an arbitrary robot velocity vq: 115 Since Mdot = (Cv+Cv')/2, then C = dCv/dvq and dM/dq = (Cv+Cv') 117 Q is index by i,j,k i.e. tau_k = v.T*Q[:,:,k]*v 118 At level k, Qk is a lower triangular matrix (same shape as H) where: 119 * rows i s.t. i>k are equals to Sk.T*Ycrb_i*S_ixS_j (j the col index), 120 * rows i s.t. i<=k are equals to Sk.T*Ycrb_k*S_ixS_j (j the col index) 121 To avoid the call to Ycrb_i>k, the first par is set up while computing Q_k 128 self.
Q = np.zeros([robot.model.nv,]*3)
132 H =
hessian(robot,q,crossterms=
True)
139 for k
in range(robot.model.njoints-1,0,-1):
140 k0,k1 =
iv(k)[0],
iv(k)[-1]+1
141 Yk = (robot.data.oMi[k]*robot.data.Ycrb[k]).matrix()
144 j0,j1 =
iv(j)[0],
iv(j)[-1]+1
145 YJ[:,j0:j1] = Yk*J[:,j0:j1]
149 i0,i1 =
iv(i)[0],
iv(i)[-1]+1
152 Q[k0:k1,i0:i1,i0:i1] = -
td(H[:,k0:k1,i0:i1],YJ[:,i0:i1],[0,0])
156 j0,j1 =
iv(j)[0],
iv(j)[-1]+1
160 Q[k0:k1,i0:i1,j0:j1] =
td(YJ[:,k0:k1], H[:,i0:i1,j0:j1],[0,0])
162 Q[k0:k1,i0:i1,j0:j1] -=
td(H[:,k0:k1,i0:i1],YJ[:,j0:j1], [0,0])
164 Q[k0:k1,j0:j1,i0:i1] =-
td(H[:,k0:k1,j0:j1],YJ[:,i0:i1], [0,0])
168 kk0,kk1 =
iv(kk)[0],
iv(kk)[-1]+1
172 j0,j1 =
iv(j)[0],
iv(j)[-1]+1
177 Q[kk0:kk1,k0:k1,j0:j1] =
td(YJ[:,kk0:kk1],H[:,k0:k1,j0:j1 ],[0,0])
179 Q[kk0:kk1,k0:k1,j0:j1] +=
td(H[:,k0:k1,kk0:kk1].T,YJ[:,j0:j1], [2,0])
183 Q[kk0:kk1,j0:j1,k0:k1] = -Q[j0:j1,kk0:kk1,k0:k1].transpose(1,0,2)
185 Q[kk0:kk1,j0:j1,k0:k1] =
td(H[:,j0:j1,kk0:kk1].T,YJ[:,k0:k1], [2,0])
197 NJ = robot.model.njoints
205 NJ = robot.model.njoints
211 pin.computeAllTerms(robot.model,robot.data,q,vq)
214 v = [ (oMi[i]*robot.data.v[i]).vector
for i
in range(NJ) ]
215 Y = [ (oMi[i]*robot.data.Ycrb[i]).matrix()
for i
in range(NJ) ]
220 j0,j1 =
iv(j)[0],
iv(j)[-1]+1
221 YS[:,j0:j1] = Y[j]*J[:,j0:j1]
225 j0,j1 =
iv(j)[0],
iv(j)[-1]+1
227 Sxv[:,j0:j1] =
adj(vj)*J[:,j0:j1]
230 for i
in range(NJ-1,0,-1):
231 i0,i1 =
iv(i)[0],
iv(i)[-1]+1
234 vp = v[robot.model.parents[i]]
236 SxvY = Sxv[:,i0:i1].T*Yi
238 j0,j1 =
iv(j)[0],
iv(j)[-1]+1
242 C[i0:i1,j0:j1] = YS[:,i0:i1].T*Sxv[:,j0:j1]
244 C[i0:i1,j0:j1] -= SxvY*Sj
247 YSxv = Yi*Sxv[:,i0:i1]
250 ii0,ii1 =
iv(ii)[0],
iv(ii)[-1]+1
253 C[ii0:ii1,i0:i1] = Sii.T*YSxv
255 C[ii0:ii1,i0:i1] += Sii.T*vxYS
257 C[ii0:ii1,i0:i1] += np.array(
td(H[:,i0:i1,ii0:ii1],Yv,[0,0])[:,:,0])
265 from lambdas
import MCross,FCross
275 pin.rnea(robot.model,robot.data,q,vq,aq)
276 NJ = robot.model.njoints
280 v = [ (oMi[i]*robot.data.v [i]).vector
for i
in range(NJ) ]
281 a = [ (oMi[i]*robot.data.a_gf[i]).vector
for i
in range(NJ) ]
282 f = [ (oMi[i]*robot.data.f [i]).vector
for i
in range(NJ) ]
283 Y = [ (oMi[i]*robot.model.inertias[i]).matrix()
for i
in range(NJ) ]
284 Ycrb = [ (oMi[i]*robot.data.Ycrb[i]) .matrix()
for i
in range(NJ) ]
286 Tkf = [
zero([6,NV])
for i
in range(NJ) ]
287 Yvx = [
zero([6,6])
for i
in range(NJ) ]
288 adjf =
lambda f: -np.bmat([ [
zero([3,3]),
skew(f[:3])] , [
skew(f[:3]),
skew(f[3:])] ])
292 for i
in reversed(range(1,NJ)):
293 i0,i1 =
iv(i)[0],
iv(i)[-1]+1
305 Yvx[ i] += Y[i]*
adj(v[i])
306 Yvx[ i] -= adjf(Y[i]*v[i])
312 k0,k1 =
iv(k)[0],
iv(k)[-1]+1
322 Tkf += Yvx[i]*
MCross(Sk,v[lk])
324 R[i0:i1,k0:k1] = Si.T*Tkf
327 kk0,kk1 =
iv(kk)[0],
iv(kk)[-1]+1
329 R[kk0:kk1,k0:k1] = Skk.T *
FCross(Sk,f[i])
330 R[kk0:kk1,k0:k1] += Skk.T * Tkf
345 if __name__ ==
'__main__':
348 robot =
RobotWrapper(
'/home/nmansard/src/pinocchio/pinocchio/models/romeo/urdf/romeo.urdf',
349 [
'/home/nmansard/src/pinocchio/pinocchio/models/romeo/', ],
350 pin.JointModelFreeFlyer()
352 q =
rand(robot.model.nq); q[3:7] /= norm(q[3:7])
362 Htrue = np.zeros([6,robot.model.nv,robot.model.nv])
363 for joint_i
in range(1,robot.model.njoints):
365 for i
in iv(joint_i):
366 for j
in iv(joint_j):
368 vq1[i] = vq1[j] = 1.0
369 pin.computeAllTerms(robot.model,robot.data,q,vq1)
370 Htrue[:,i,j] = (robot.data.oMi[joint_i]*robot.data.a[joint_i]).vector.T
372 print(
'Check hessian = \t\t', norm(H-Htrue))
383 dM = np.zeros([robot.model.nv,]*3)
387 for diff
in range(robot.model.nv):
389 dM[:,:,diff] = -pin.crba(robot.model,robot.data,q)
391 dq *=0; dq[diff] = eps
392 qdq = pin.integrate(robot.model,q,dq)
394 dM[:,:,diff] += pin.crba(robot.model,robot.data,qdq)
398 print(
'Check dCRBA = \t\t\t',
max([ norm(Mp[:,:,diff]-dM[:,:,diff])
for diff
in range(robot.model.nv) ]))
409 robot.model.gravity = pin.Motion.Zero()
410 rnea0 =
lambda q,vq: pin.nle(robot.model,robot.data,q,vq)
412 C = np.zeros([robot.model.nv,]*3)
413 for i
in range(robot.model.nv):
415 C[:,i,i] =
rnea0(q,vq1).T
417 for i
in range(robot.model.nv):
418 for j
in range(robot.model.nv):
421 vq1[i] = vq1[j] = 1.0
422 C[:,i,j] = (
rnea0(q,vq1).T-C[:,i,i]-C[:,j,j]) /2
424 print(
"Check d/dv rnea = \t\t",norm(
quad(Q,vq)-
rnea0(q,vq)))
425 print(
"Check C = Q+Q.T = \t\t", norm((Q+Q.transpose(0,2,1))/2-C))
426 print(
"Check dM = C+C.T /2 \t\t", norm( Mp - (C+C.transpose(1,0,2)) ))
427 print(
"Check dM = Q+Q.T+Q.T+Q.T /2 \t", norm( Mp -
428 (Q+Q.transpose(0,2,1)+Q.transpose(1,0,2)+Q.transpose(2,0,1))/2 ))
435 print(
"Check coriolis \t\t\t",norm(C*vq-
rnea0(q,vq)))
447 r0 = pin.rnea(robot.model,robot.data,q,vq,aq).
copy()
449 dq =
zero(NV); dq[i]=eps
450 qdq = pin.integrate(robot.model,q,dq)
451 Rd[:,i] = (pin.rnea(robot.model,robot.data,qdq,vq,aq)-r0)/eps
452 print(
"Check drnea \t\t\t",norm(Rd-R))
JointCollectionTpl const DataTpl< Scalar, Options, JointCollectionTpl > const Eigen::MatrixBase< Mat > & min
AD< Scalar > max(const AD< Scalar > &x, const AD< Scalar > &y)
def __call__(self, q, vq, aq)
def __init__(self, robot)
def __init__(self, robot)
def __init__(self, robot)
def __init__(self, robot)
void copy(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const DataTpl< Scalar, Options, JointCollectionTpl > &origin, DataTpl< Scalar, Options, JointCollectionTpl > &dest, KinematicLevel kinematic_level)
Copy part of the data from origin to dest. Template parameter can be used to select at which differen...
def __call__(self, q, vq)
def hessian(robot, q, crossterms=False)