Skip to content

Commit ce31c3a

Browse files
committed
add more methods. probably redundant now with @lru_cache
1 parent 13e461a commit ce31c3a

File tree

1 file changed

+41
-4
lines changed

1 file changed

+41
-4
lines changed

roboticstoolbox/robot/KinematicCache.py

Lines changed: 41 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -336,12 +336,28 @@ def jacobe_pinv(self, q, end=None, analytical=None):
336336
self._dict[key] = np.linalg.pinv(J)
337337
return self._dict[key]
338338

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+
339347
def coriolis(self, q, qd):
340348
key = ("coriolis", self._qhash(q), self._qhash(qd))
341349
if key not in self._dict:
342350
# cache miss, compute it
343351
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)
345361
return self._dict[key]
346362

347363
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"):
361377
key = ("coriolis_x", self._qhash(q), self._qhash(qd), pinv, analytical)
362378
if key not in self._dict:
363379
# cache miss, compute it
364-
380+
Ja = self.jacob0(q, pinv=pinv, analytical=analytical)
365381
# get Jacobian inv or pinv from cache
366382
if pinv:
367383
Ji = self.jacobe0_pinv(analytical=analytical)
368384
else:
369385
Ji = self.jacobe0_inv(analytical=analytical)
370386
# get inertia, Jacobian dot and Coriolis from cache
371387
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))
373389
C = self.coriolis(q, qd)
374390
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)
376392
return self._dict[key]
377393

378394
def gravload_x(self, q, pinv=False, analytical="rpy-xyz"):
@@ -388,6 +404,27 @@ def gravload_x(self, q, pinv=False, analytical="rpy-xyz"):
388404
self._dict[key] = self._robot.gravload_x(q, Ji=Ji)
389405
return self._dict[key]
390406

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+
391428

392429
if __name__ == "__main__":
393430
q = np.r_[1, 2, 3, 4, 5, 6, 7]

0 commit comments

Comments
 (0)