Fix bug in ETS for tip to tip case

petercorke · petercorke · commit c1e023f636f4 · 2021-04-08T19:23:48.000+10:00
diff --git a/roboticstoolbox/robot/ELink.py b/roboticstoolbox/robot/ELink.py
@@ -13,9 +13,9 @@
 
 class BaseELink(Link):
 
-    def __init__(self, name=None, parent=None, joint_name=None):
+    def __init__(self, name=None, parent=None, joint_name=None, **kwargs):
 
-        super().__init__()
+        super().__init__(kwargs)
 
         self._name = name
 
@@ -256,6 +256,7 @@ def ets(self):
         if self.v is None:
             return self._ets
         else:
+            self.v.jindex = self.jindex
             return self._ets * self.v
 
     @collision.setter
diff --git a/roboticstoolbox/robot/ERobot.py b/roboticstoolbox/robot/ERobot.py
@@ -505,7 +505,7 @@ def ets(self, start=None, end=None, explored=None, path=None):
             >>> panda = rtb.models.ETS.Panda()
             >>> panda.ets()
         """
-        v = self._getlink(start, self.base_link)
+        link = self._getlink(start, self.base_link)
         if end is None and len(self.ee_links) > 1:
             raise ValueError(
                 'ambiguous, specify which end-effector is required')
@@ -515,34 +515,38 @@ def ets(self, start=None, end=None, explored=None, path=None):
             explored = set()
         toplevel = path is None
 
-        explored.add(v)
-        if v == end:
+        explored.add(link)
+        if link == end:
             return path
 
         # unlike regular DFS, the neighbours of the node are its children
         # and its parent.
 
-        # visit child nodes
+        # visit child nodes below start
         if toplevel:
-            path = v.ets()
-        for w in v.children:
-            if w not in explored:
-                p = self.ets(w, end, explored, path * w.ets())
-                if p:
+            path = link.ets()
+        for child in link.children:
+            if child not in explored:
+                p = self.ets(child, end, explored, path * child.ets())
+                if p is not None:
                     return p
 
-        # visit parent node
+        # we didn't find the node below, keep going up a level, and recursing
+        # down again
         if toplevel:
-            path = ETS()
-        if v.parent is not None:
-            w = v.parent
-            if w not in explored:
-                p = self.ets(w, end, explored, path * v.ets().inv())
-                if p:
+            path = None
+        if link.parent is not None:
+            parent = link.parent  # go up one level toward the root
+            if parent not in explored:
+                if path is None:
+                    p = self.ets(parent, end, explored, link.ets().inv())
+                else:
+                    p = self.ets(parent, end, explored, path * link.ets().inv())
+                if p is not None:
                     return p
-
         return None
 
+
 # --------------------------------------------------------------------- #
 
     def showgraph(self, **kwargs):
diff --git a/roboticstoolbox/robot/ETS.py b/roboticstoolbox/robot/ETS.py
@@ -10,7 +10,7 @@
 import numpy as np
 from spatialmath import SE3, SE2
 from spatialmath.base import getvector, getunit, trotx, troty, trotz, \
-    issymbol, tr2jac, transl2, trot2, removesmall, trinv, \
+    issymbol, tr2jac, transl2, trot2, removesmall, trinv, trinv2, \
     verifymatrix, iseye, tr2jac2
 
 class BaseETS(UserList, ABC):
@@ -730,7 +730,7 @@ def __mul__(self, rest):
         return prod
 
     def __imul__(self, rest):
-        prod = ETS()
+        prod = self.__class__()
         prod.data = self.data + rest.data
         return prod
 
@@ -911,7 +911,7 @@ def inv(self):
             the reversed order of the transforms.
         """  # noqa
 
-        inv = ETS()
+        inv = self.__class__()
         for ns in reversed(self.data):
             # get the namespace from the list
 
@@ -920,12 +920,12 @@ def inv(self):
             if nsi.joint:
                 nsi.flip ^= True   # toggle flip status
             elif nsi.axis[0] == 'C':
-                nsi.T = trinv(nsi.T)
+                nsi.T = self._inverse(nsi.T)
             elif nsi.eta is not None:
-                nsi.T = trinv(nsi.T)
+                nsi.T = self._inverse(nsi.T)
                 nsi.eta = -nsi.eta
-            et = ETS()  # create a new ETS instance
-            et.data = [nsi]  # set it's data from the dict
+            et = self.__class__()  # create a new ETS instance
+            et.data = [nsi]  # set its data from the dict
             inv *= et
         return inv
 
@@ -1004,6 +1004,9 @@ def __init__(self, *pos, **kwargs):
         super().__init__(*pos, **kwargs)
         self._ndims = 3
 
+    def _inverse(self, T):
+        return trinv(T)
+
     @property
     def s(self):
         if self.axis[1] == 'x':
@@ -1481,6 +1484,9 @@ def __init__(self, *pos, **kwargs):
         super().__init__(*pos, **kwargs)
         self._ndims = 2
 
+    def _inverse(self, T):
+        return trinv2(T)
+
     @classmethod
     def r(cls, eta=None, unit='rad', **kwargs):
         """
