navrobot
diff --git a/‎roboticstoolbox/models/URDF/UR3.py
Lines changed: 2 additions & 2 deletions b/‎roboticstoolbox/models/URDF/UR3.py
Lines changed: 2 additions & 2 deletions
diff --git a/‎roboticstoolbox/robot/ELink.py
Lines changed: 61 additions & 22 deletions b/‎roboticstoolbox/robot/ELink.py
Lines changed: 61 additions & 22 deletions
@@ -32,8 +32,8 @@ def __init__(self):
             "ur_description/urdf/ur3_joint_limited_robot.urdf.xacro")
 
         super().__init__(
-                args[0].upper(),
-                name=args[1],
+                args[0],
+                name=args[1].upper(),
                 manufacturer='Universal Robotics'
             )
 
 
@@ -4,10 +4,10 @@
 """
 
 # import numpy as np
-from spatialmath import SE3
+from spatialmath import SE3, SE2, SMPose
 # from spatialmath.base.argcheck import getvector, verifymatrix, isscalar
 import roboticstoolbox as rp
-from roboticstoolbox.robot.ETS import ETS
+from roboticstoolbox.robot.ETS import ETS, SuperETS
 from roboticstoolbox.robot.Link import Link
 
 
@@ -41,7 +41,15 @@ class ELink(Link):
 
     It inherits from the Link class which provides common functionality such
     as joint and link such as kinematics parameters,
-    .
+    
+
+    The transform to the next link is given as an ETS with the joint
+    variable, if present, as the last term.  This is preprocessed and
+    the object stores:
+
+        * ``Ts`` the constant part as a NumPy array, or None
+        * ``v`` a pointer to an ETS object representing the joint variable.
+          or None
 
     :references:
         - Kinematic Derivatives using the Elementary Transform Sequence,
@@ -62,11 +70,12 @@ def __init__(
         super(ELink, self).__init__(**kwargs)
 
         # check we have an ETS
-        if isinstance(ets, ETS):
-            self._ets = ets
-        else:
+        if not isinstance(ets, SuperETS):
             raise TypeError(
                 'The ets argument must be of type ETS')
+        self._ndims = ets._ndims
+
+        self._ets = ets
 
         if v is None and len(ets) > 0 and ets[-1].isjoint:
             v = ets.pop()
@@ -76,10 +85,10 @@ def __init__(
                 jindex = v.jindex
 
         # TODO simplify this logic, can be ELink class or None
-        if isinstance(parent, list):
-            raise TypeError(
-                'Only one parent link can be present')
-        elif not isinstance(parent, ELink) and parent is not None:
+        # if isinstance(parent, list):
+        #     raise TypeError(
+        #         'Only one parent link can be present')
+        if not isinstance(parent, ELink) and parent is not None:
             raise TypeError(
                 'Parent must be of type ELink')
 
@@ -95,7 +104,7 @@ def __init__(
         # Check the variable joint
         if v is None:
             self._joint = False
-        elif not isinstance(v, ETS):
+        elif not isinstance(v, SuperETS):
             raise TypeError('v must be of type ETS')
         elif not v[0].isjoint:
             raise ValueError('v must be a variable ETS')
@@ -111,20 +120,28 @@ def _init_Ts(self):
         # Number of transforms in the ETS excluding the joint variable
         self._M = len(self._ets)
 
-        # Initialise joints
-        if isinstance(self._ets, ETS):
-            self._Ts = SE3()
-            for i in range(self.M):
-                if self._ets[i].isjoint:
+        # Compute the leading, constant, part of the ETS
+        # TODO probably should use ETS.compile()
+
+        if isinstance(self._ets, SuperETS):
+            first = True
+            T = None
+
+            for et in self._ets:
+                # constant transforms only
+                if et.isjoint:
                     raise ValueError('The transforms in ets must be constant')
 
-                if not isinstance(self._ets[i].T(), SE3):
-                    self._Ts *= SE3(self._ets[i].T())
+                if first:
+                    T = et.T()
+                    first = False
                 else:
-                    self._Ts *= self._ets[i].T()
+                    T = T @ et.T()
+            self._Ts = T
 
         elif isinstance(self._ets, SE3):
             self._Ts = self._ets
+            raise RuntimeError('this shouldnt happen')
 
     def __repr__(self):
         name = self.__class__.__name__
@@ -365,21 +382,43 @@ def A(self, q=None, fast=False):
           transformation from the previous to the current link frame to
           the next, which depends on the joint coordinate ``q``.
 
+        If ``fast`` is True return a NumPy array, either SE(2) or SE(3).
+        A value of None means that it is the identity matrix.
+
+        If ``fast`` is False return an ``SE2`` or ``SE3`` instance.
+
         """
 
+        # we work with NumPy arrays for speed
         if self.isjoint:
             # a variable joint
             if q is None:
                 raise ValueError("q is required for variable joints")
-            T = self.Ts.A @ self.v.T(q)
+
+            # premultiply variable part by constant part if present
+            Ts = self.Ts
+            if Ts is None:
+                T = self.v.T(q)
+            else:
+                T = Ts @ self.v.T(q)
         else:
             # a fixed joint
-            T = self.Ts.A
+            T = self.Ts
 
         if fast:
             return T
+
+        if T is None:
+            if self._ndims == 3:
+                return SE3()
+            elif self._ndims == 2:
+                return SE2()
         else:
-            return SE3(T, check=False)
+            if self._ndims == 3:
+                return SE3(T, check=False)
+            elif self._ndims == 2:
+                return SE2(T, check=False)
+
 
     def ets(self):
         if self.v is None:
Original file line number	Diff line number	Diff line change
`@@ -32,8 +32,8 @@ def __init__(self):`
`32`	`32`	`"ur_description/urdf/ur3_joint_limited_robot.urdf.xacro")`
`33`	`33`
`34`	`34`	`super().__init__(`
`35`		`- args[0].upper(),`
`36`		`- name=args[1],`
	`35`	`+ args[0],`
	`36`	`+ name=args[1].upper(),`
`37`	`37`	`manufacturer='Universal Robotics'`
`38`	`38`	`)`
`39`	`39`