dcrba.py
Go to the documentation of this file.
1 #
2 # Copyright (c) 2016 CNRS
3 #
4 
5 from __future__ import print_function
6 
7 import pinocchio as pin
8 from pinocchio.robot_wrapper import RobotWrapper
9 from pinocchio.utils import *
10 from numpy.linalg import norm
11 import lambdas
12 from lambdas import Mcross,ancestors,parent,iv,td,quad,adj,adjdual
13 
14 def hessian(robot,q,crossterms=False):
15  '''
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.
19  '''
20  lambdas.setRobotArgs(robot)
21  H=np.zeros([6,robot.model.nv,robot.model.nv])
22  pin.computeJointJacobians(robot.model,robot.data,q)
23  J = robot.data.J
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]: # j is a child of i
27  for i in iv(joint_i):
28  for j in iv(joint_j):
29  Si = J[:,i]
30  Sj = J[:,j]
31  H[:,i,j] = np.asarray(Mcross(Sj, Si))[:,0]
32  return H
33 
34 
35 # CRBA ====> Mp[i,j] += Si.transpose()*Yk*Sj
36 # D-CRBA ==> TSi Y Sj + Si Y TSj - Si YSdx Sj + Si SdxY Sj
37 # Term a = term d && term b = term c --- or are null
38 # -> term a is null if j<=diff
39 # -> term d is null if diff>k
40 # -> then a-d is nonzero only when k>=diff>j
41 # Due to simplification, terms cancel to the following:
42 # if i<d (always true) M[i,j,d] = Si.T Yd TjSd (= -Si.T Yd Sd x Sj)
43 # if j<d (possibly false) M[i,j,d] += TdSi.T Yd Sd (= +Si.T Sdx Yd Sj)
44 # where Yd is the composite inertia of the minimal subtree rooted either at j or d.
45 
46 class DCRBA:
47  '''
48  Compute the derivative (tangent application) of the mass matrix M wrt q,
49  which is a NVxNVxNV tensor.
50  '''
51 
52  def __init__(self,robot):
53  self.robot = robot
54  lambdas.setRobotArgs(robot)
55 
56  def pre(self,q):
57  robot = self.robot
58  self.H = hessian(robot,q)
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) ]
62 
63  self.dM = np.zeros([robot.model.nv,]*3)
64 
65  def __call__(self):
66  robot = self.robot
67  J = self.J
68  Y = self.Y
69  dM = self.dM
70  H = self.H
71 
72  for j in range(robot.model.njoints-1,0,-1):
73  for joint_diff in range(1,robot.model.njoints): # diff should be a descendant or ancestor of j
74  if j not in ancestors(joint_diff) and joint_diff not in ancestors(j): continue
75 
76  for i in ancestors(min(parent(joint_diff),j)):
77 
78  i0,i1 = iv(i)[0],iv(i)[-1]+1
79  j0,j1 = iv(j)[0],iv(j)[-1]+1
80 
81  Si = J[:,i0:i1]
82  Sj = J[:,j0:j1]
83 
84  for d in iv(joint_diff):
85 
86  T_iSd = np.matrix(H[:,d,i0:i1]) # this is 0 if d<=i (<=j)
87  T_jSd = np.matrix(H[:,d,j0:j1]) # this is 0 is d<=j
88 
89  '''
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 )
94  '''
95 
96  Yd = Y[max(j,joint_diff)]
97 
98  dM [i0:i1,j0:j1,d] = T_iSd.T * Yd * Sj
99  if j<joint_diff:
100  dM[i0:i1,j0:j1,d] += Si.T * Yd * T_jSd
101 
102  # Make dM triangular by copying strict-upper triangle to lower one.
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
105 
106  return dM
107 
108 # -----------------------------------------------------------------------------------
109 # -----------------------------------------------------------------------------------
110 # -----------------------------------------------------------------------------------
111 class VRNEA:
112  '''
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')
116 
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
122  '''
123 
124  def __init__(self,robot):
125  self.robot = robot
126  lambdas.setRobotArgs(robot)
127  self.YJ = zero([6,robot.model.nv])
128  self.Q = np.zeros([robot.model.nv,]*3)
129 
130  def __call__(self,q):
131  robot = self.robot
132  H = hessian(robot,q,crossterms=True)
133  J = robot.data.J
134  YJ = self.YJ
135  Q = self.Q
136 
137  # The terms in SxS YS corresponds to f = vxYv
138  # The terms in S YSxS corresponds to a = vxv (==> f = Yvxv)
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()
142  Sk = J[:,k0:k1]
143  for j in ancestors(k): # Fill YJ = [ ... Yk*Sj ... ]_j
144  j0,j1 = iv(j)[0],iv(j)[-1]+1
145  YJ[:,j0:j1] = Yk*J[:,j0:j1]
146 
147  # Fill the diagonal of the current level of Q = Q[k,:k,:k]
148  for i in ancestors(k):
149  i0,i1 = iv(i)[0],iv(i)[-1]+1
150  Si = J[:,i0:i1]
151 
152  Q[k0:k1,i0:i1,i0:i1] = -td(H[:,k0:k1,i0:i1],YJ[:,i0:i1],[0,0]) # = (Si x Sk)' * Yk Si
153 
154  # Fill the nondiag of the current level Q[k,:k,:k]
155  for j in ancestors(i)[:-1]:
156  j0,j1 = iv(j)[0],iv(j)[-1]+1
157  Sj = J[:,j0:j1]
158 
159  # = Sk' * Yk * Si x Sj
160  Q[k0:k1,i0:i1,j0:j1] = td(YJ[:,k0:k1], H[:,i0:i1,j0:j1],[0,0])
161  # = (Si x Sk)' * Yk Sj
162  Q[k0:k1,i0:i1,j0:j1] -= td(H[:,k0:k1,i0:i1],YJ[:,j0:j1], [0,0])
163  # = (Sj x Sk)' * Yk Si
164  Q[k0:k1,j0:j1,i0:i1] =- td(H[:,k0:k1,j0:j1],YJ[:,i0:i1], [0,0])
165 
166  # Fill the border elements of levels below k Q[kk,k,:] and Q[kk,:,k] with kk<k
167  for kk in ancestors(k)[:-1]:
168  kk0,kk1 = iv(kk)[0],iv(kk)[-1]+1
169  Skk = J[:,kk0:kk1]
170 
171  for j in ancestors(k):
172  j0,j1 = iv(j)[0],iv(j)[-1]+1
173  Sj = J[:,j0:j1]
174 
175  # = Skk' Yk Sk x Sj = (Yk Skk)' Sk x Sj
176  if k!=j:
177  Q[kk0:kk1,k0:k1,j0:j1] = td(YJ[:,kk0:kk1],H[:,k0:k1,j0:j1 ],[0,0])
178  # = (Sk x Skk)' Yk Sj
179  Q[kk0:kk1,k0:k1,j0:j1] += td(H[:,k0:k1,kk0:kk1].T,YJ[:,j0:j1], [2,0])
180  # = (Sj x Skk)' Yk Sk
181  # Switch because j can be > or < than kk
182  if j<kk:
183  Q[kk0:kk1,j0:j1,k0:k1] = -Q[j0:j1,kk0:kk1,k0:k1].transpose(1,0,2)
184  elif j>=kk:
185  Q[kk0:kk1,j0:j1,k0:k1] = td(H[:,j0:j1,kk0:kk1].T,YJ[:,k0:k1], [2,0])
186 
187  return Q
188 
189 # -----------------------------------------------------------------------------------
190 # -----------------------------------------------------------------------------------
191 # -----------------------------------------------------------------------------------
192 class Coriolis:
193  def __init__(self,robot):
194  self.robot = robot
195  lambdas.setRobotArgs(robot)
196  NV = robot.model.nv
197  NJ = robot.model.njoints
198  self.C = zero([NV,NV])
199  self.YS = zero([6,NV])
200  self.Sxv = zero([6,NV])
201 
202  def __call__(self,q,vq):
203  robot = self.robot
204  NV = robot.model.nv
205  NJ = robot.model.njoints
206  C = self.C
207  YS = self.YS
208  Sxv = self.Sxv
209  H = hessian(robot,q)
210 
211  pin.computeAllTerms(robot.model,robot.data,q,vq)
212  J = robot.data.J
213  oMi=robot.data.oMi
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) ]
216 
217  # --- Precomputations
218  # Centroidal matrix
219  for j in range(NJ):
220  j0,j1 = iv(j)[0],iv(j)[-1]+1
221  YS[:,j0:j1] = Y[j]*J[:,j0:j1]
222 
223  # velocity-jacobian cross products.
224  for j in range(NJ):
225  j0,j1 = iv(j)[0],iv(j)[-1]+1
226  vj = v[j]
227  Sxv[:,j0:j1] = adj(vj)*J[:,j0:j1]
228 
229  # --- Main loop
230  for i in range(NJ-1,0,-1):
231  i0,i1 = iv(i)[0],iv(i)[-1]+1
232  Yi = Y[i]
233  Si = J[:,i0:i1]
234  vp = v[robot.model.parents[i]]
235 
236  SxvY = Sxv[:,i0:i1].T*Yi
237  for j in ancestors(i):
238  j0,j1 = iv(j)[0],iv(j)[-1]+1
239  Sj = J[:,j0: j1 ]
240 
241  # COR ===> Si' Yi Sj x vj
242  C[i0:i1,j0:j1] = YS[:,i0:i1].T*Sxv[:,j0:j1]
243  # CEN ===> Si' vi x Yi Sj = - (Si x vi)' Yi Sj
244  C[i0:i1,j0:j1] -= SxvY*Sj
245 
246  vxYS = adjdual(v[i])*Yi*Si
247  YSxv = Yi*Sxv[:,i0:i1]
248  Yv = Yi*vp
249  for ii in ancestors(i)[:-1]:
250  ii0,ii1 = iv(ii)[0],iv(ii)[-1]+1
251  Sii = J[:,ii0:ii1]
252  # COR ===> Sii' Yi Si x vi
253  C[ii0:ii1,i0:i1] = Sii.T*YSxv
254  # CEN ===> Sii' vi x Yi Si = (Sii x vi)' Yi Si
255  C[ii0:ii1,i0:i1] += Sii.T*vxYS
256  # CEN ===> Sii' Si x Yi vi = (Sii x Si)' Yi vi
257  C[ii0:ii1,i0:i1] += np.matrix(td(H[:,i0:i1,ii0:ii1],Yv,[0,0])[:,:,0]).T
258 
259  return C
260 
261 # --- DRNEA -------------------------------------------------------------------------
262 # --- DRNEA -------------------------------------------------------------------------
263 # --- DRNEA -------------------------------------------------------------------------
264 
265 from lambdas import MCross,FCross
266 
267 
268 class DRNEA:
269  def __init__(self,robot):
270  self.robot = robot
271  lambdas.setRobotArgs(robot)
272 
273  def __call__(self,q,vq,aq):
274  robot = self.robot
275  pin.rnea(robot.model,robot.data,q,vq,aq)
276  NJ = robot.model.njoints
277  NV = robot.model.nv
278  J = robot.data.J
279  oMi = robot.data.oMi
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) ]
285 
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:])] ])
289 
290  R = self.R = zero([NV,NV])
291 
292  for i in reversed(range(1,NJ)): # i is the torque index : dtau_i = R[i,:] dq
293  i0,i1 = iv(i)[0],iv(i)[-1]+1
294  Yi = Y[i]
295  Yci = Ycrb[i]
296  Si = J[:,i0:i1]
297  vi = v[i]
298  ai = a[i]
299  fi = f[i]
300  aqi = aq[i0:i1]
301  vqi = vq[i0:i1]
302  dvi = Si*vqi
303  li = parent(i)
304 
305  Yvx[ i] += Y[i]*adj(v[i])
306  Yvx[ i] -= adjf(Y[i]*v[i])
307  Yvx[ i] -= adjdual(v[i])*Yi
308 
309  Yvx[li] += Yvx[i]
310 
311  for k in ancestors(i): # k is the derivative index: dtau = R[:,k] dq_k
312  k0,k1 = iv(k)[0],iv(k)[-1]+1
313  Sk = J[:,k0:k1]
314  lk = parent(k)
315 
316  # Si' Yi Tk ai = Si' Yi ( Sk x (ai-alk) + (vi-vlk) x (Sk x vlk) )
317  # Tk Si' Yi ai = Si' Yi ( - Sk x a[lk] + (vi-vlk) x (Sk x vlk) )
318  Tkf = Yci*(- MCross(Sk,a[lk]) + MCross(MCross(Sk,v[lk]),v[lk]))
319 
320  # Tk Si' fs = Tk Si' Ycrb[i] ai + Si' Ys (vs-vi) x (Sk x vlk)
321  # = "" + Si' Ys vs x (Sk x vlk) - Si' Ys vi x (Sk x vlk)
322  Tkf += Yvx[i]*MCross(Sk,v[lk])
323 
324  R[i0:i1,k0:k1] = Si.T*Tkf
325  if i==k:
326  for kk in ancestors(k)[:-1]:
327  kk0,kk1 = iv(kk)[0],iv(kk)[-1]+1
328  Skk = J[:,kk0:kk1]
329  R[kk0:kk1,k0:k1] = Skk.T * FCross(Sk,f[i])
330  R[kk0:kk1,k0:k1] += Skk.T * Tkf
331 
332  self.a = a
333  self.v = v
334  self.f = f
335  self.Y = Y
336  self.Ycrb = Ycrb
337  self.Yvx = Yvx
338 
339  return R
340 
341 # -----------------------------------------------------------------------------------
342 # -----------------------------------------------------------------------------------
343 # -----------------------------------------------------------------------------------
344 # --- UNIT TEST ---
345 if __name__ == '__main__':
346  np.random.seed(0)
347 
348  robot = RobotWrapper('/home/nmansard/src/pinocchio/pinocchio/models/romeo/urdf/romeo.urdf',
349  [ '/home/nmansard/src/pinocchio/pinocchio/models/romeo/', ],
350  pin.JointModelFreeFlyer()
351  )
352  q = rand(robot.model.nq); q[3:7] /= norm(q[3:7])
353  vq = rand(robot.model.nv)
354 
355  # --- HESSIAN ---
356  # --- HESSIAN ---
357  # --- HESSIAN ---
358  H = hessian(robot,q)
359 
360  # Compute the Hessian matrix H using RNEA, so that acor = v*H*v
361  vq1 = vq*0
362  Htrue = np.zeros([6,robot.model.nv,robot.model.nv])
363  for joint_i in range(1,robot.model.njoints):
364  for joint_j in ancestors(joint_i)[:-1]:
365  for i in iv(joint_i):
366  for j in iv(joint_j):
367  vq1 *= 0
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
371 
372  print('Check hessian = \t\t', norm(H-Htrue))
373 
374  # --- dCRBA ---
375  # --- dCRBA ---
376  # --- dCRBA ---
377 
378  dcrba = DCRBA(robot)
379  dcrba.pre(q)
380  Mp = dcrba()
381 
382  # --- Validate dM/dq by finite diff
383  dM = np.zeros([robot.model.nv,]*3)
384  eps = 1e-6
385  dq = zero(robot.model.nv)
386 
387  for diff in range(robot.model.nv):
388 
389  dM[:,:,diff] = -pin.crba(robot.model,robot.data,q)
390 
391  dq *=0; dq[diff] = eps
392  qdq = pin.integrate(robot.model,q,dq)
393 
394  dM[:,:,diff] += pin.crba(robot.model,robot.data,qdq)
395 
396  dM /= eps
397 
398  print('Check dCRBA = \t\t\t', max([ norm(Mp[:,:,diff]-dM[:,:,diff]) for diff in range(robot.model.nv) ]))
399 
400 
401  # --- vRNEA ---
402  # --- vRNEA ---
403  # --- vRNEA ---
404 
405  vrnea = VRNEA(robot)
406  Q = vrnea(q)
407 
408  # --- Compute C from rnea, for future check
409  robot.model.gravity = pin.Motion.Zero()
410  rnea0 = lambda q,vq: pin.nle(robot.model,robot.data,q,vq)
411  vq1 = vq*0
412  C = np.zeros([robot.model.nv,]*3)
413  for i in range(robot.model.nv):
414  vq1 *= 0; vq1[i] = 1
415  C[:,i,i] = rnea0(q,vq1).T
416 
417  for i in range(robot.model.nv):
418  for j in range(robot.model.nv):
419  if i==j: continue
420  vq1 *= 0
421  vq1[i] = vq1[j] = 1.0
422  C[:,i,j] = (rnea0(q,vq1).T-C[:,i,i]-C[:,j,j]) /2
423 
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 ))
429 
430  # --- CORIOLIS
431  # --- CORIOLIS
432  # --- CORIOLIS
433  coriolis = Coriolis(robot)
434  C = coriolis(q,vq)
435  print("Check coriolis \t\t\t",norm(C*vq-rnea0(q,vq)))
436 
437  # --- DRNEA
438  # --- DRNEA
439  # --- DRNEA
440  drnea = DRNEA(robot)
441  aq = rand(robot.model.nv)
442  R = drnea(q,vq,aq)
443 
444  NV = robot.model.nv
445  Rd = zero([NV,NV])
446  eps = 1e-8
447  r0 = pin.rnea(robot.model,robot.data,q,vq,aq).copy()
448  for i in range(NV):
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))
453 
454 
455 
pinocchio::cholesky::min
JointCollectionTpl const DataTpl< Scalar, Options, JointCollectionTpl > const Eigen::MatrixBase< Mat > & min
Definition: cholesky.hpp:71
lambdas.ancestors
int ancestors
Definition: lambdas.py:19
lambdas.parent
parent
Definition: lambdas.py:16
dcrba.vrnea
vrnea
Definition: dcrba.py:405
lambdas.adjdual
adjdual
Definition: lambdas.py:54
dcrba.drnea
drnea
Definition: dcrba.py:440
dcrba.DCRBA.__call__
def __call__(self)
Definition: dcrba.py:65
CppAD::max
AD< Scalar > max(const AD< Scalar > &x, const AD< Scalar > &y)
Definition: autodiff/cppad.hpp:146
dcrba.DRNEA.__call__
def __call__(self, q, vq, aq)
Definition: dcrba.py:273
dcrba.VRNEA.__init__
def __init__(self, robot)
Definition: dcrba.py:124
pinocchio.robot_wrapper
Definition: robot_wrapper.py:1
dcrba.DCRBA.__init__
def __init__(self, robot)
Definition: dcrba.py:52
dcrba
Definition: dcrba.py:1
dcrba.rnea0
rnea0
Definition: dcrba.py:410
pinocchio.robot_wrapper.RobotWrapper
Definition: robot_wrapper.py:12
lambdas.MCross
MCross
Definition: lambdas.py:47
dcrba.VRNEA.__call__
def __call__(self, q)
Definition: dcrba.py:130
lambdas.setRobotArgs
def setRobotArgs(robot)
Definition: lambdas.py:32
dcrba.DRNEA.__init__
def __init__(self, robot)
Definition: dcrba.py:269
lambdas.Mcross
Mcross
Definition: lambdas.py:41
dcrba.Coriolis.__init__
def __init__(self, robot)
Definition: dcrba.py:193
pinocchio::copy
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...
Definition: copy.hpp:52
dcrba.DCRBA.pre
def pre(self, q)
Definition: dcrba.py:56
lambdas.FCross
FCross
Definition: lambdas.py:48
dcrba.Coriolis.__call__
def __call__(self, q, vq)
Definition: dcrba.py:202
dcrba.hessian
def hessian(robot, q, crossterms=False)
Definition: dcrba.py:14
dcrba.coriolis
coriolis
Definition: dcrba.py:433

pinocchio
Author(s):
autogenerated on Tue Jun 1 2021 02:45:02