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
17 Compute the Hessian tensor of all the robot joints.
18 If crossterms, then also compute the Si x Si terms,
19 that are not part of the true Hessian but enters in some computations like VRNEA.
22 H = np.zeros([6, robot.model.nv, robot.model.nv])
23 pin.computeJointJacobians(robot.model, robot.data, q)
25 skiplast = -1
if not crossterms
else None
26 for joint_i
in range(1, robot.model.njoints):
27 for joint_j
in ancestors(joint_i)[:skiplast]:
32 H[:, i, j] = np.asarray(
Mcross(Sj, Si))[:, 0]
50 Compute the derivative (tangent application) of the mass matrix M wrt q,
51 which is a NVxNVxNV tensor.
61 self.
J = robot.data.J.copy()
62 pin.crba(robot.model, robot.data, q)
64 (robot.data.oMi[i] * robot.data.Ycrb[i]).matrix()
65 for i
in range(0, robot.model.njoints)
82 for j
in range(robot.model.njoints - 1, 0, -1):
83 for joint_diff
in range(
84 1, robot.model.njoints
90 i0, i1 =
iv(i)[0],
iv(i)[-1] + 1
91 j0, j1 =
iv(j)[0],
iv(j)[-1] + 1
96 for d
in iv(joint_diff):
97 T_iSd = np.array(H[:, d, i0:i1])
98 T_jSd = np.array(H[:, d, j0:j1])
101 assert( norm(T_iSd)<1e-6 or not joint_diff<i ) # d<i => TiSd=0
102 assert( norm(T_jSd)<1e-6 or not joint_diff<j ) # d<j => TjSd=0
103 assert( norm(T_jSd)<1e-6 or not norm(T_iSd)<1e-6 ) # TiSd=0 => TjSd=0
104 assert( norm(T_iSd)>1e-6 )
107 Yd = Y[
max(j, joint_diff)]
109 dM[i0:i1, j0:j1, d] = T_iSd.T * Yd * Sj
111 dM[i0:i1, j0:j1, d] += Si.T * Yd * T_jSd
115 dM[j0:j1, i0:i1, d] = dM[i0:i1, j0:j1, d].T
117 dM[j0:j1, i0:i1, d] += np.triu(dM[i0:i1, j0:j1, d], 1).T
127 Compute the C tensor so that nle(q,vq) = vq C vq.
128 Noting Cv = C*vq for an arbitrary robot velocity vq:
129 Since Mdot = (Cv+Cv')/2, then C = dCv/dvq and dM/dq = (Cv+Cv')
131 Q is index by i,j,k i.e. tau_k = v.T*Q[:,:,k]*v
132 At level k, Qk is a lower triangular matrix (same shape as H) where:
133 * rows i s.t. i>k are equals to Sk.T*Ycrb_i*S_ixS_j (j the col index),
134 * rows i s.t. i<=k are equals to Sk.T*Ycrb_k*S_ixS_j (j the col index)
135 To avoid the call to Ycrb_i>k, the first par is set up while computing Q_k
151 H =
hessian(robot, q, crossterms=
True)
158 for k
in range(robot.model.njoints - 1, 0, -1):
159 k0, k1 =
iv(k)[0],
iv(k)[-1] + 1
160 Yk = (robot.data.oMi[k] * robot.data.Ycrb[k]).matrix()
163 j0, j1 =
iv(j)[0],
iv(j)[-1] + 1
164 YJ[:, j0:j1] = Yk * J[:, j0:j1]
168 i0, i1 =
iv(i)[0],
iv(i)[-1] + 1
171 Q[k0:k1, i0:i1, i0:i1] = -
td(
172 H[:, k0:k1, i0:i1], YJ[:, i0:i1], [0, 0]
177 j0, j1 =
iv(j)[0],
iv(j)[-1] + 1
181 Q[k0:k1, i0:i1, j0:j1] =
td(
182 YJ[:, k0:k1], H[:, i0:i1, j0:j1], [0, 0]
185 Q[k0:k1, i0:i1, j0:j1] -=
td(
186 H[:, k0:k1, i0:i1], YJ[:, j0:j1], [0, 0]
189 Q[k0:k1, j0:j1, i0:i1] = -
td(
190 H[:, k0:k1, j0:j1], YJ[:, i0:i1], [0, 0]
195 kk0, kk1 =
iv(kk)[0],
iv(kk)[-1] + 1
199 j0, j1 =
iv(j)[0],
iv(j)[-1] + 1
204 Q[kk0:kk1, k0:k1, j0:j1] =
td(
205 YJ[:, kk0:kk1], H[:, k0:k1, j0:j1], [0, 0]
208 Q[kk0:kk1, k0:k1, j0:j1] +=
td(
209 H[:, k0:k1, kk0:kk1].T, YJ[:, j0:j1], [2, 0]
214 Q[kk0:kk1, j0:j1, k0:k1] = -Q[j0:j1, kk0:kk1, k0:k1].transpose(
218 Q[kk0:kk1, j0:j1, k0:k1] =
td(
219 H[:, j0:j1, kk0:kk1].T, YJ[:, k0:k1], [2, 0]
233 NJ = robot.model.njoints
241 NJ = robot.model.njoints
247 pin.computeAllTerms(robot.model, robot.data, q, vq)
250 v = [(oMi[i] * robot.data.v[i]).vector
for i
in range(NJ)]
251 Y = [(oMi[i] * robot.data.Ycrb[i]).matrix()
for i
in range(NJ)]
256 j0, j1 =
iv(j)[0],
iv(j)[-1] + 1
257 YS[:, j0:j1] = Y[j] * J[:, j0:j1]
261 j0, j1 =
iv(j)[0],
iv(j)[-1] + 1
263 Sxv[:, j0:j1] =
adj(vj) * J[:, j0:j1]
266 for i
in range(NJ - 1, 0, -1):
267 i0, i1 =
iv(i)[0],
iv(i)[-1] + 1
270 vp = v[robot.model.parents[i]]
272 SxvY = Sxv[:, i0:i1].T * Yi
274 j0, j1 =
iv(j)[0],
iv(j)[-1] + 1
278 C[i0:i1, j0:j1] = YS[:, i0:i1].T * Sxv[:, j0:j1]
280 C[i0:i1, j0:j1] -= SxvY * Sj
283 YSxv = Yi * Sxv[:, i0:i1]
286 ii0, ii1 =
iv(ii)[0],
iv(ii)[-1] + 1
289 C[ii0:ii1, i0:i1] = Sii.T * YSxv
291 C[ii0:ii1, i0:i1] += Sii.T * vxYS
293 C[ii0:ii1, i0:i1] += np.array(
294 td(H[:, i0:i1, ii0:ii1], Yv, [0, 0])[:, :, 0]
304 from lambdas
import MCross, FCross
314 pin.rnea(robot.model, robot.data, q, vq, aq)
315 NJ = robot.model.njoints
319 v = [(oMi[i] * robot.data.v[i]).vector
for i
in range(NJ)]
320 a = [(oMi[i] * robot.data.a_gf[i]).vector
for i
in range(NJ)]
321 f = [(oMi[i] * robot.data.f[i]).vector
for i
in range(NJ)]
322 Y = [(oMi[i] * robot.model.inertias[i]).matrix()
for i
in range(NJ)]
323 Ycrb = [(oMi[i] * robot.data.Ycrb[i]).matrix()
for i
in range(NJ)]
325 Tkf = [
zero([6, NV])
for i
in range(NJ)]
326 Yvx = [
zero([6, 6])
for i
in range(NJ)]
327 adjf =
lambda f: -np.bmat(
336 i0, i1 =
iv(i)[0],
iv(i)[-1] + 1
348 Yvx[i] += Y[i] *
adj(v[i])
349 Yvx[i] -= adjf(Y[i] * v[i])
355 k0, k1 =
iv(k)[0],
iv(k)[-1] + 1
365 Tkf += Yvx[i] *
MCross(Sk, v[lk])
367 R[i0:i1, k0:k1] = Si.T * Tkf
370 kk0, kk1 =
iv(kk)[0],
iv(kk)[-1] + 1
372 R[kk0:kk1, k0:k1] = Skk.T *
FCross(Sk, f[i])
373 R[kk0:kk1, k0:k1] += Skk.T * Tkf
389 if __name__ ==
"__main__":
393 "/home/nmansard/src/pinocchio/pinocchio/models/romeo/urdf/romeo.urdf",
395 "/home/nmansard/src/pinocchio/pinocchio/models/romeo/",
397 pin.JointModelFreeFlyer(),
400 q[3:7] /= norm(q[3:7])
410 Htrue = np.zeros([6, robot.model.nv, robot.model.nv])
411 for joint_i
in range(1, robot.model.njoints):
413 for i
in iv(joint_i):
414 for j
in iv(joint_j):
416 vq1[i] = vq1[j] = 1.0
417 pin.computeAllTerms(robot.model, robot.data, q, vq1)
419 robot.data.oMi[joint_i] * robot.data.a[joint_i]
422 print(
"Check hessian = \t\t", norm(H - Htrue))
442 for diff
in range(robot.model.nv):
443 dM[:, :, diff] = -pin.crba(robot.model, robot.data, q)
447 qdq = pin.integrate(robot.model, q, dq)
449 dM[:, :, diff] += pin.crba(robot.model, robot.data, qdq)
454 "Check dCRBA = \t\t\t",
455 max([norm(Mp[:, :, diff] - dM[:, :, diff])
for diff
in range(robot.model.nv)]),
466 robot.model.gravity = pin.Motion.Zero()
467 rnea0 =
lambda q, vq: pin.nle(robot.model, robot.data, q, vq)
475 for i
in range(robot.model.nv):
478 C[:, i, i] =
rnea0(q, vq1).T
480 for i
in range(robot.model.nv):
481 for j
in range(robot.model.nv):
485 vq1[i] = vq1[j] = 1.0
486 C[:, i, j] = (
rnea0(q, vq1).T - C[:, i, i] - C[:, j, j]) / 2
488 print(
"Check d/dv rnea = \t\t", norm(
quad(Q, vq) -
rnea0(q, vq)))
489 print(
"Check C = Q+Q.T = \t\t", norm((Q + Q.transpose(0, 2, 1)) / 2 - C))
490 print(
"Check dM = C+C.T /2 \t\t", norm(Mp - (C + C.transpose(1, 0, 2))))
492 "Check dM = Q+Q.T+Q.T+Q.T /2 \t",
495 - (Q + Q.transpose(0, 2, 1) + Q.transpose(1, 0, 2) + Q.transpose(2, 0, 1))
505 print(
"Check coriolis \t\t\t", norm(C * vq -
rnea0(q, vq)))
517 r0 = pin.rnea(robot.model, robot.data, q, vq, aq).
copy()
521 qdq = pin.integrate(robot.model, q, dq)
522 Rd[:, i] = (pin.rnea(robot.model, robot.data, qdq, vq, aq) - r0) / eps
523 print(
"Check drnea \t\t\t", norm(Rd - R))