Fixing RNE for URDFs robots with fixed joints

roboticstoolbox/robot/Robot.py

@@ -1719,13 +1719,12 @@ def rne(
 
         # allocate intermediate variables
         Xup = SE3.Alloc(n)
-        Xtree = SE3.Alloc(n)
 
         v = SpatialVelocity.Alloc(n)
         a = SpatialAcceleration.Alloc(n)
         f = SpatialForce.Alloc(n)
         I = SpatialInertia.Alloc(n)  # noqa
-        s = []  # joint motion subspace
+        s = [[]] * n  # joint motion subspace
 
         # Handle trajectory case
         q = getmatrix(q, (None, None))
@@ -1738,41 +1737,36 @@ def rne(
         else:
             Q = np.empty((l, n))  # joint torque/force
 
-        # TODO Should the dynamic parameters of static links preceding joint be
-        # somehow merged with the joint?
-
-        # A temp variable to handle static joints
-        Ts = SE3()
-
-        # A counter through joints
-        j = 0
-
         for k in range(l):
             qk = q[k, :]
             qdk = qd[k, :]
             qddk = qdd[k, :]
 
             # initialize intermediate variables
             for link in self.links:
-                if link.isjoint:
-                    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)
-                    elif link.Ts is not None:
-                        Xtree[j] = Ts * SE3(link.Ts, check=False)
 
-                    if link.v is not None:
-                        s.append(link.v.s)
+                # Find closest joint
+                reference_link = link
+                transform_matrix = SE3()
+                while reference_link is not None and reference_link.jindex is None:
+                    transform_matrix = transform_matrix * SE3(reference_link.A())
+                    reference_link = reference_link.parent
 
-                    # Increment the joint counter
-                    j += 1
+                # This means we are linked to the base link w/o joints inbetween
+                # Meaning no inertia to be computed
+                if reference_link is None or reference_link.jindex is None:
+                    continue
 
-                    # Reset the Ts tracker
-                    Ts = SE3()
-                else:  # pragma nocover
-                    # TODO Keep track of inertia and transform???
-                    if link.Ts is not None:
-                        Ts *= SE3(link.Ts, check=False)
+                # Adding inertia of fixed links
+                joint_idx = reference_link.jindex
+
+                # Adding inertia. This is the way found to circumvent a bug in the library
+                I.data[joint_idx] = I[joint_idx].A + \
+                                    (SpatialInertia(m=link.m, r=transform_matrix * link.r, I=link.I).data[0])
+
+                # Get joint subspace
+                if link.v is not None:
+                    s[link.jindex] = link.v.s
 
             if gravity is None:
                 a_grav = -SpatialAcceleration(self.gravity)
@@ -1783,29 +1777,66 @@ def rne(
             for j in range(0, n):
                 vJ = SpatialVelocity(s[j] * qdk[j])
 
+                # Find link attached to joint j
+                current_link = None
+                for current_link in self.links:
+                    # We have already added the inertia of all fixed
+                    # links connected to this joint so we can consider
+                    # only the link closest to the joint
+                    if current_link.jindex == j:
+                        break
+                if current_link is None:
+                    raise ValueError(f"Joint index {j} not found in "
+                                     f"robot {self.name}, is the model correct?")
+
+                # Find previous joint
+                previous_link = current_link.parent
+                j_previous = None
+                while j_previous is None:
+                    if previous_link is None:
+                        break
+                    j_previous = previous_link.jindex
+                    previous_link = previous_link.parent
+
                 # transform from parent(j) to j
-                Xup[j] = SE3(self.links[j].A(qk[j])).inv()
+                Xup[j] = SE3(current_link.A(qk[current_link.jindex])).inv()
 
-                if self.links[j].parent is None:
+                # compute velocity and acceleration
+                if j_previous is None:
                     v[j] = vJ
                     a[j] = Xup[j] * a_grav + SpatialAcceleration(s[j] * qddk[j])
                 else:
-                    jp = self.links[j].parent.jindex  # type: ignore
-                    v[j] = Xup[j] * v[jp] + vJ
-                    a[j] = (
-                        Xup[j] * a[jp] + SpatialAcceleration(s[j] * qddk[j]) + v[j] @ vJ
-                    )
-
+                    v[j] = Xup[j] * v[j_previous] + vJ
+                    a[j] = Xup[j] * a[j_previous] + \
+                           SpatialAcceleration(s[j] * qddk[j]) + v[j] @ vJ
                 f[j] = I[j] * a[j] + v[j] @ (I[j] * v[j])
 
             # 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])
 
-                if self.links[j].parent is not None:
-                    jp = self.links[j].parent.jindex  # type: ignore
-                    f[jp] = f[jp] + Xup[j] * f[j]
+                # Find link attached to joint j
+                current_link = None
+                for current_link in self.links:
+                    if current_link.jindex == j:
+                        break
+                if current_link is None:
+                    raise ValueError(f"Joint index {j} not found in "
+                                     f"robot {self.name}, is the model correct?")
+
+                # Find previous joint
+                previous_link = current_link.parent
+                j_previous = None
+                while j_previous is None:
+                    if previous_link is None:
+                        break
+                    j_previous = previous_link.jindex
+                    previous_link = previous_link.parent
+
+                # Compute backward pass
+                if j_previous is not None:
+                    f[j_previous] = f[j_previous] + Xup[j] * f[j]
 
         if l == 1:
             return Q[0]

tests/test_ERobot.py

@@ -16,8 +16,9 @@
 from math import pi, sin, cos
 
 try:
-    from sympy import symbols
-
+    from sympy import symbols, simplify
+    from sympy import sin as ssin
+    from sympy import cos as scos
     _sympy = True
 except ModuleNotFoundError:
     _sympy = False
@@ -152,6 +153,13 @@ def test_invdyn(self):
         tau = robot.rne(q, z, np.array([1, 1]))
         nt.assert_array_almost_equal(tau, np.r_[d11 + d12, d21 + d22])
 
+    def test_URDF_inertia(self):
+        robot = rtb.models.URDF.UR10()
+        try:
+            robot.inertia(robot.q)
+        except TypeError:
+            self.fail("inertia() with a robot containing fixed links raised an error")
+
 
 class TestERobot2(unittest.TestCase):
     def test_plot(self):
@@ -203,19 +211,33 @@ def test_symdyn(self):
         link2 = Link(ET.tx(a1) * ET.Ry(flip=True), m=m2, r=[r2, 0, 0], name="link1")
         robot = ERobot([link1, link2])
 
+        # Define symbols
         q = symbols("q:2")
         qd = symbols("qd:2")
         qdd = symbols("qdd:2")
+        q0, q1 = q
+        qd0, qd1 = qd
+        qdd0, qdd1 = qdd
+
         Q = robot.rne(q, qd, qdd, gravity=[0, 0, g], symbolic=True)
 
-        self.assertEqual(
-            str(Q[0]),
-            "a1**2*m2*qd0**2*sin(q1)*cos(q1) + a1*qd0*(-a1*m2*qd0*cos(q1) - m2*r2*(qd0 + qd1))*sin(q1) - a1*(m2*(a1*qd0*qd1*cos(q1) - a1*qdd0*sin(q1) - g*sin(q0)*cos(q1) - g*sin(q1)*cos(q0)) + (qd0 + qd1)*(-a1*m2*qd0*cos(q1) - m2*r2*(qd0 + qd1)))*sin(q1) - a1*(-a1*m2*qd0*(-qd0 - qd1)*sin(q1) - m2*r2*(qdd0 + qdd1) + m2*(-a1*qd0*qd1*sin(q1) - a1*qdd0*cos(q1) + g*sin(q0)*sin(q1) - g*cos(q0)*cos(q1)))*cos(q1) + g*m1*r1*cos(q0) + m1*qdd0*r1**2 + m2*r2**2*(qdd0 + qdd1) - m2*r2*(-a1*qd0*qd1*sin(q1) - a1*qdd0*cos(q1) + g*sin(q0)*sin(q1) - g*cos(q0)*cos(q1))",
-        )
-        self.assertEqual(
-            str(Q[1]),
-            "a1**2*m2*qd0**2*sin(q1)*cos(q1) + a1*qd0*(-a1*m2*qd0*cos(q1) - m2*r2*(qd0 + qd1))*sin(q1) + m2*r2**2*(qdd0 + qdd1) - m2*r2*(-a1*qd0*qd1*sin(q1) - a1*qdd0*cos(q1) + g*sin(q0)*sin(q1) - g*cos(q0)*cos(q1))",
-        )
+        solution_0 = a1**2*m2*qd0**2*ssin(q1)*scos(q1) + a1*qd0*(-a1*m2*qd0*scos(q1) - \
+                     m2*r2*(qd0 + qd1))*ssin(q1) - a1*(m2*(a1*qd0*qd1*scos(q1) - \
+                     a1*qdd0*ssin(q1) - g*ssin(q0)*scos(q1) - g*ssin(q1)*scos(q0)) + \
+                     (qd0 + qd1)*(-a1*m2*qd0*scos(q1) - m2*r2*(qd0 + qd1)))*ssin(q1) - \
+                     a1*(-a1*m2*qd0*(-qd0 - qd1)*ssin(q1) - m2*r2*(qdd0 + qdd1) + \
+                     m2*(-a1*qd0*qd1*ssin(q1) - a1*qdd0*scos(q1) + g*ssin(q0)*ssin(q1) - \
+                     g*scos(q0)*scos(q1)))*scos(q1) + g*m1*r1*scos(q0) + m1*qdd0*r1**2 + \
+                     m2*r2**2*(qdd0 + qdd1) - m2*r2*(-a1*qd0*qd1*ssin(q1) - \
+                     a1*qdd0*scos(q1) + g*ssin(q0)*ssin(q1) - g*scos(q0)*scos(q1))
+
+        solution_1 = a1**2*m2*qd0**2*ssin(q1)*scos(q1) + a1*qd0*(-a1*m2*qd0*scos(q1) - \
+                     m2*r2*(qd0 + qd1))*ssin(q1) + m2*r2**2*(qdd0 + qdd1) - \
+                     m2*r2*(-a1*qd0*qd1*ssin(q1) - a1*qdd0*scos(q1) + g*ssin(q0)*ssin(q1) - \
+                     g*scos(q0)*scos(q1))
+
+        self.assertEqual(simplify(Q[0]-solution_0), 0)
+        self.assertEqual(simplify(Q[1]-solution_1), 0)
 
 
 if __name__ == "__main__":  # pragma nocover

tests/test_Ticker.py

@@ -5,12 +5,15 @@
 
 from roboticstoolbox.tools.Ticker import Ticker
 import unittest
+import platform
 
 
 class TestTicker(unittest.TestCase):
 
     def test_ticker(self):
-        self.skipTest('Not working on windows or mac')
+
+        if platform.system() in ['Windows', 'Darwin']:
+            self.skipTest('Not working on windows or mac')
 
         t = Ticker(0.1)