@@ -336,12 +336,28 @@ def jacobe_pinv(self, q, end=None, analytical=None):
336
336
self ._dict [key ] = np .linalg .pinv (J )
337
337
return self ._dict [key ]
338
338
339
+ def inertia (self , q ):
340
+ key = ("inertia" , self ._qhash (q ), self ._qhash (qd ))
341
+ if key not in self ._dict :
342
+ # cache miss, compute it
343
+ C = self .inertia (q )
344
+ self ._dict [key ] = self ._robot .inertia (q )
345
+ return self ._dict [key ]
346
+
339
347
def coriolis (self , q , qd ):
340
348
key = ("coriolis" , self ._qhash (q ), self ._qhash (qd ))
341
349
if key not in self ._dict :
342
350
# cache miss, compute it
343
351
C = self .coriolis (q , qd )
344
- self ._dict [key ] = self ._robot .coriolis_x (q , qd )
352
+ self ._dict [key ] = self ._robot .coriolis (q , qd )
353
+ return self ._dict [key ]
354
+
355
+ def gravload (self , q ):
356
+ key = ("gravload" , self ._qhash (q ), self ._qhash (qd ))
357
+ if key not in self ._dict :
358
+ # cache miss, compute it
359
+ C = self .gravload (q )
360
+ self ._dict [key ] = self ._robot .gravload (q )
345
361
return self ._dict [key ]
346
362
347
363
def inertia_x (self , q , pinv = False , analytical = "rpy-xyz" ):
@@ -361,18 +377,18 @@ def coriolis_x(self, q, qd, pinv=False, analytical="rpy-xyz"):
361
377
key = ("coriolis_x" , self ._qhash (q ), self ._qhash (qd ), pinv , analytical )
362
378
if key not in self ._dict :
363
379
# cache miss, compute it
364
-
380
+ Ja = self . jacob0 ( q , pinv = pinv , analytical = analytical )
365
381
# get Jacobian inv or pinv from cache
366
382
if pinv :
367
383
Ji = self .jacobe0_pinv (analytical = analytical )
368
384
else :
369
385
Ji = self .jacobe0_inv (analytical = analytical )
370
386
# get inertia, Jacobian dot and Coriolis from cache
371
387
Mx = self .inertia_x (q , pinv = pinv , analytical = analytical )
372
- Jd = self .jacob_dot (q , J0 = self .jacob0 (q , analytical = analytical ))
388
+ Jd = self .jacob_dot (q , J0 = self .jacob0 (q , J0 = Ja , analytical = analytical ))
373
389
C = self .coriolis (q , qd )
374
390
self ._dict [key ] = self ._robot .coriolis_x (q , qd , pinv , analytical ,
375
- J , Ji , Jd , C , Mx )
391
+ J = Ja , Ji = Ji , Jd = JD , C = C , Mx = Mx )
376
392
return self ._dict [key ]
377
393
378
394
def gravload_x (self , q , pinv = False , analytical = "rpy-xyz" ):
@@ -388,6 +404,27 @@ def gravload_x(self, q, pinv=False, analytical="rpy-xyz"):
388
404
self ._dict [key ] = self ._robot .gravload_x (q , Ji = Ji )
389
405
return self ._dict [key ]
390
406
407
+ def accel_x (self , q , qd , xd , wrench , pinv = False , analytical = "rpy-xyz" ):
408
+ key = ("accel_x" , self ._qhash (q ), pinv , analytical )
409
+ if key not in self ._dict :
410
+ # cache miss, compute it
411
+
412
+ Ja = self .jacob0 (q , pinv = pinv , analytical = analytical )
413
+ # get Jacobian inv or pinv from cache
414
+ if pinv :
415
+ Ji = self .jacobe0_pinv (analytical = analytical )
416
+ else :
417
+ Ji = self .jacobe0_inv (analytical = analytical )
418
+ C = self .coriolis (q , qd )
419
+ Jd = self .jacob_dot (q , J0 = Ja )
420
+
421
+ Mx = self .inertia_x (q , Ji = Ji , pinv = pinv , analytical = analytical )
422
+ Cx = self .coriolis_x (q , qd , pinv = pinv , analytical = analytical ,
423
+ J = Ja , Ji = Ji , Jd = Jd , C = C , Mx = Mx )
424
+ Gx = self .gravload_x (q , Ji = Ji )
425
+ self ._dict [key ] = np .linalg .inv (Mx ) @ (wrench - Cx @ xd - Gx )
426
+ return self ._dict [key ]
427
+
391
428
392
429
if __name__ == "__main__" :
393
430
q = np .r_ [1 , 2 , 3 , 4 , 5 , 6 , 7 ]
0 commit comments