Make Ts None if no static transform

Author: jhavl

roboticstoolbox/robot/ET.py

@@ -561,7 +561,7 @@ def A(self, q: Union[float, Sym] = 0.0) -> ndarray:
             # We can't use the fast version, lets use Python instead
             if self.isjoint:
                 if self.isflip:
-                    q = -q
+                    q = -q  # type: ignore
 
                 if self.axis_func is not None:
                     return self.axis_func(q)

roboticstoolbox/robot/Link.py

@@ -226,25 +226,25 @@ def dynpar(self, name, value, default):
     def _init_Ts(self):
         # Compute the leading, constant, part of the ETS
 
-        # Ts can not be equal to None otherwise things seem
-        # to break everywhere, so initialise Ts np be identity
-
         if isinstance(self, Link2):
-            T = np.eye(3)
+            T = None
         else:
-            T = np.eye(4)
+            T = None
 
         for et in self._ets:
             # constant transforms only
             if et.isjoint:
                 break
             else:
-                T = T @ et.A()
+                if T is None:
+                    T = et.A()
+                else:
+                    T = T @ et.A()
 
         self._Ts = T
 
     @property
-    def Ts(self) -> NDArray:
+    def Ts(self) -> Union[NDArray, None]:
         """
         Constant part of link ETS
 
@@ -733,7 +733,6 @@ def I(self) -> NDArray:  # noqa
     @I.setter
     @_listen_dyn
     def I(self, I_new: ArrayLike):  # noqa
-
         if ismatrix(I_new, (3, 3)):
             # 3x3 matrix passed
             if np.any(np.abs(I_new - I_new.T) > 1e-8):  # type: ignore
@@ -947,7 +946,6 @@ def collision(self) -> SceneGroup:
 
     @collision.setter
     def collision(self, coll: Union[SceneGroup, List[Shape], Shape]):
-
         if isinstance(coll, list):
             self.collision.scene_children = coll  # type: ignore
         elif isinstance(coll, Shape):
@@ -957,7 +955,6 @@ def collision(self, coll: Union[SceneGroup, List[Shape], Shape]):
 
     @geometry.setter
     def geometry(self, geom: Union[SceneGroup, List[Shape], Shape]):
-
         if isinstance(geom, list):
             self.geometry.scene_children = geom  # type: ignore
         elif isinstance(geom, Shape):
@@ -1616,7 +1613,6 @@ class Link(BaseLink):
     def __init__(
         self, ets: Union[ETS, ET] = ETS(), jindex: Union[None, int] = None, **kwargs
     ):
-
         # process common options
         super().__init__(ets=ets, **kwargs)
 
@@ -1651,14 +1647,19 @@ def A(self, q: float = 0.0) -> SE3:
 
         """
         if self.isjoint:
-            return SE3(self._Ts @ self._ets[-1].A(q), check=False)
-        else:
+            if self._Ts is not None:
+                return SE3(self._Ts @ self._ets[-1].A(q), check=False)
+            else:
+                return SE3(self._ets[-1].A(q), check=False)
+
+        elif self._Ts is not None:
             return SE3(self._Ts, check=False)
+        else:
+            return SE3()
 
 
 class Link2(BaseLink):
     def __init__(self, ets: ETS2 = ETS2(), jindex: Union[int, None] = None, **kwargs):
-
         # process common options
         super().__init__(ets=ets, **kwargs)
 
@@ -1693,6 +1694,12 @@ def A(self, q: float = 0.0) -> SE2:
         """
 
         if self.isjoint:
-            return SE2(self._Ts @ self._ets[-1].A(q), check=False)
-        else:
+            if self._Ts is not None:
+                return SE2(self._Ts @ self._ets[-1].A(q), check=False)
+            else:
+                return SE2(self._ets[-1].A(q), check=False)
+
+        elif self._Ts is not None:
             return SE2(self._Ts, check=False)
+        else:
+            return SE2()

roboticstoolbox/robot/Robot.py

@@ -63,7 +63,6 @@
 
 
 class Robot(BaseRobot[Link], RobotKinematicsMixin):
-
     _color = True
 
     def __init__(
@@ -83,7 +82,6 @@ def __init__(
         urdf_string: Union[str, None] = None,
         urdf_filepath: Union[Path, PurePosixPath, None] = None,
     ):
-
         # Process links
         if isinstance(arg, Robot):
             # We're passed a Robot, clone it
@@ -117,7 +115,6 @@ def __init__(
             self._urdf_string = arg.urdf_string
             self._urdf_filepath = arg.urdf_filepath
         else:
-
             if isinstance(arg, ETS):
                 # We're passed an ETS string
                 links = []
@@ -161,11 +158,9 @@ def __init__(
     # --------------------------------------------------------------------- #
 
     def _to_dict(self, robot_alpha=1.0, collision_alpha=0.0):
-
         ob = []
 
         for link in self.links:
-
             if robot_alpha > 0:
                 for gi in link.geometry:
                     gi.set_alpha(robot_alpha)
@@ -178,7 +173,6 @@ def _to_dict(self, robot_alpha=1.0, collision_alpha=0.0):
         # Do the grippers now
         for gripper in self.grippers:
             for link in gripper.links:
-
                 if robot_alpha > 0:
                     for gi in link.geometry:
                         gi.set_alpha(robot_alpha)
@@ -198,7 +192,6 @@ def _fk_dict(self, robot_alpha=1.0, collision_alpha=0.0):
 
         # Do the robot
         for link in self.links:
-
             if robot_alpha > 0:
                 for gi in link.geometry:
                     ob.append(gi.fk_dict())
@@ -1049,7 +1042,6 @@ def jacob0_dot(
         qd = np.array(qd)
 
         if representation is None:
-
             if J0 is None:
                 J0 = self.jacob0(q)
             H = self.hessian0(q, J0=J0)
@@ -1767,7 +1759,7 @@ def rne(
                     I[j] = SpatialInertia(m=link.m, r=link.r)
                     if symbolic and link.Ts is None:  # pragma: nocover
                         Xtree[j] = SE3(np.eye(4, dtype="O"), check=False)
-                    else:
+                    elif link.Ts is not None:
                         Xtree[j] = Ts * SE3(link.Ts, check=False)
 
                     if link.v is not None:
@@ -1780,7 +1772,8 @@ def rne(
                     Ts = SE3()
                 else:  # pragma nocover
                     # TODO Keep track of inertia and transform???
-                    Ts *= SE3(link.Ts, check=False)
+                    if link.Ts is not None:
+                        Ts *= SE3(link.Ts, check=False)
 
             if gravity is None:
                 a_grav = -SpatialAcceleration(self.gravity)
@@ -1808,7 +1801,6 @@ def rne(
 
             # backward recursion
             for j in reversed(range(0, n)):
-
                 # next line could be dot(), but fails for symbolic arguments
                 Q[k, j] = sum(f[j].A * s[j])
 
@@ -1829,7 +1821,6 @@ def rne(
 
 class Robot2(BaseRobot[Link2]):
     def __init__(self, arg, **kwargs):
-
         if isinstance(arg, ETS2):
             # we're passed an ETS string
             links = []

tests/test_ELink.py

@@ -166,7 +166,6 @@ def test_properties(self):
         self.assertEqual(l0.Jm, 0.0)
 
     def test_fail_parent(self):
-
         with self.assertRaises(TypeError):
             rtb.Link(parent=1)
 
@@ -243,7 +242,7 @@ def test_init_ets2(self):
         e1 = rtb.ET2.R()
         link = BaseLink(e1)
 
-        nt.assert_almost_equal(link.Ts, np.eye(4))
+        self.assertEqual(link.Ts, None)
 
     def test_get_ets(self):
         e1 = rtb.ETS(rtb.ET.Ry())
@@ -334,7 +333,6 @@ def test_dyn2list(self):
         self.assertEqual(s, ans)
 
     def test_init_fail4(self):
-
         with self.assertRaises(TypeError):
             rtb.Link(2.0)  # type: ignore
 
@@ -352,5 +350,4 @@ def test_ets2_A2(self):
 
 
 if __name__ == "__main__":
-
     unittest.main()