diff --git a/roboticstoolbox/robot/Robot.py b/roboticstoolbox/robot/Robot.py index eadab10d..e1f7094f 100644 --- a/roboticstoolbox/robot/Robot.py +++ b/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,15 +1737,6 @@ 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, :] @@ -1754,25 +1744,29 @@ def rne( # 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,19 +1777,38 @@ 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 @@ -1803,9 +1816,27 @@ def rne( # 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] diff --git a/tests/test_ERobot.py b/tests/test_ERobot.py index be698bbb..1a7f00ca 100644 --- a/tests/test_ERobot.py +++ b/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 diff --git a/tests/test_Ticker.py b/tests/test_Ticker.py index c091a2d0..2dc0d5e8 100644 --- a/tests/test_Ticker.py +++ b/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)