From 670014f5090db759125ea124619d98db08d77b6a Mon Sep 17 00:00:00 2001 From: niederha Date: Thu, 16 Nov 2023 12:03:38 +0100 Subject: [PATCH 01/12] Added unittest to check for inertia with a URDF robot containing fixed links --- tests/test_ERobot.py | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/tests/test_ERobot.py b/tests/test_ERobot.py index be698bbb..9c99e1ba 100644 --- a/tests/test_ERobot.py +++ b/tests/test_ERobot.py @@ -152,6 +152,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): From 51b6d445cdc845831d71c3343f5b81315b52d1ab Mon Sep 17 00:00:00 2001 From: niederha Date: Thu, 16 Nov 2023 15:05:10 +0100 Subject: [PATCH 02/12] Switched test from UR10 to panda since its urdf is easier to interpret --- tests/test_ERobot.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tests/test_ERobot.py b/tests/test_ERobot.py index 9c99e1ba..e4514fb3 100644 --- a/tests/test_ERobot.py +++ b/tests/test_ERobot.py @@ -153,7 +153,7 @@ def test_invdyn(self): nt.assert_array_almost_equal(tau, np.r_[d11 + d12, d21 + d22]) def test_URDF_inertia(self): - robot = rtb.models.URDF.UR10() + robot = rtb.models.URDF.Panda() try: robot.inertia(robot.q) except TypeError: From 404467e122c2650cdbdced95a9d81db9c659c0f8 Mon Sep 17 00:00:00 2001 From: niederha Date: Thu, 16 Nov 2023 15:08:52 +0100 Subject: [PATCH 03/12] Fixed joint transform matrix q parameter in RNE --- roboticstoolbox/robot/Robot.py | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/roboticstoolbox/robot/Robot.py b/roboticstoolbox/robot/Robot.py index eadab10d..6bedf946 100644 --- a/roboticstoolbox/robot/Robot.py +++ b/roboticstoolbox/robot/Robot.py @@ -1784,7 +1784,10 @@ def rne( vJ = SpatialVelocity(s[j] * qdk[j]) # transform from parent(j) to j - Xup[j] = SE3(self.links[j].A(qk[j])).inv() + if self.links[j].jindex is not None: + Xup[j] = SE3(self.links[j].A(qk[self.links[j].jindex])).inv() + else: + Xup[j] = SE3(self.links[j].A()).inv() if self.links[j].parent is None: v[j] = vJ From 4041ad59028f06216063fc9f7236724466f99a68 Mon Sep 17 00:00:00 2001 From: niederha Date: Thu, 16 Nov 2023 15:36:08 +0100 Subject: [PATCH 04/12] Removed Xtree variables and all related since it was not used --- roboticstoolbox/robot/Robot.py | 25 +++---------------------- 1 file changed, 3 insertions(+), 22 deletions(-) diff --git a/roboticstoolbox/robot/Robot.py b/roboticstoolbox/robot/Robot.py index 6bedf946..65736b0c 100644 --- a/roboticstoolbox/robot/Robot.py +++ b/roboticstoolbox/robot/Robot.py @@ -1719,7 +1719,6 @@ def rne( # allocate intermediate variables Xup = SE3.Alloc(n) - Xtree = SE3.Alloc(n) v = SpatialVelocity.Alloc(n) a = SpatialAcceleration.Alloc(n) @@ -1738,28 +1737,15 @@ 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: + for j, link in enumerate(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) @@ -1767,13 +1753,6 @@ def rne( # Increment the joint counter j += 1 - # 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) - if gravity is None: a_grav = -SpatialAcceleration(self.gravity) else: # pragma nocover @@ -1787,6 +1766,7 @@ def rne( if self.links[j].jindex is not None: Xup[j] = SE3(self.links[j].A(qk[self.links[j].jindex])).inv() else: + # If fixed link between parent and child Xup[j] = SE3(self.links[j].A()).inv() if self.links[j].parent is None: @@ -1794,6 +1774,7 @@ def rne( a[j] = Xup[j] * a_grav + SpatialAcceleration(s[j] * qddk[j]) else: jp = self.links[j].parent.jindex # type: ignore + breakpoint() v[j] = Xup[j] * v[jp] + vJ a[j] = ( Xup[j] * a[jp] + SpatialAcceleration(s[j] * qddk[j]) + v[j] @ vJ From 0fed750ca5c2c73d7e90276cd38e990164d697f3 Mon Sep 17 00:00:00 2001 From: niederha Date: Fri, 17 Nov 2023 10:23:35 +0100 Subject: [PATCH 05/12] Fixed skip condition in ticker test --- tests/test_Ticker.py | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) 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) From 0ec78a6749fe4e368a9a0e0908b025f57a799545 Mon Sep 17 00:00:00 2001 From: niederha Date: Fri, 17 Nov 2023 10:41:33 +0100 Subject: [PATCH 06/12] Fixed and improved variable initialization except for the intertia issue --- roboticstoolbox/robot/Robot.py | 27 ++++++++++++++++++--------- 1 file changed, 18 insertions(+), 9 deletions(-) diff --git a/roboticstoolbox/robot/Robot.py b/roboticstoolbox/robot/Robot.py index 65736b0c..36809d0f 100644 --- a/roboticstoolbox/robot/Robot.py +++ b/roboticstoolbox/robot/Robot.py @@ -1724,7 +1724,7 @@ def rne( 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)) @@ -1743,15 +1743,25 @@ def rne( qddk = qdd[k, :] # initialize intermediate variables - for j, link in enumerate(self.links): - if link.isjoint: - I[j] = SpatialInertia(m=link.m, r=link.r) + for link in self.links: - if link.v is not None: - s.append(link.v.s) + # Find closest joint + reference_link = link + while reference_link is not None and reference_link.jindex is None: + 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 + + # TODO: Stack static link inertias + joint_idx = reference_link.jindex + I[joint_idx] = SpatialInertia(m=link.m, r=link.r) + + # Get joint subspace + if link.v is not None: + s[link.jindex] = link.v.s if gravity is None: a_grav = -SpatialAcceleration(self.gravity) @@ -1774,7 +1784,6 @@ def rne( a[j] = Xup[j] * a_grav + SpatialAcceleration(s[j] * qddk[j]) else: jp = self.links[j].parent.jindex # type: ignore - breakpoint() v[j] = Xup[j] * v[jp] + vJ a[j] = ( Xup[j] * a[jp] + SpatialAcceleration(s[j] * qddk[j]) + v[j] @ vJ From 1b0418debe333e12588db28c384a51619134107c Mon Sep 17 00:00:00 2001 From: niederha Date: Fri, 17 Nov 2023 11:38:52 +0100 Subject: [PATCH 07/12] Fixed joint inertia --- roboticstoolbox/robot/Robot.py | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/roboticstoolbox/robot/Robot.py b/roboticstoolbox/robot/Robot.py index 36809d0f..57758a5d 100644 --- a/roboticstoolbox/robot/Robot.py +++ b/roboticstoolbox/robot/Robot.py @@ -1747,7 +1747,9 @@ def rne( # 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 # This means we are linked to the base link w/o joints inbetween @@ -1755,9 +1757,11 @@ def rne( if reference_link is None or reference_link.jindex is None: continue - # TODO: Stack static link inertias + # Adding inertia of fixed links joint_idx = reference_link.jindex - I[joint_idx] = SpatialInertia(m=link.m, r=link.r) + I[joint_idx] = I[joint_idx] + SpatialInertia(m=link.m, + r=transform_matrix * link.r, + I=link.I) # Get joint subspace if link.v is not None: From cf2a54e9a6c0d9b339216402d58840641455218b Mon Sep 17 00:00:00 2001 From: niederha Date: Fri, 17 Nov 2023 14:44:23 +0100 Subject: [PATCH 08/12] Fixed backwardpass for robot with fixed links and intertia addition --- roboticstoolbox/robot/Robot.py | 54 ++++++++++++++++++++++++---------- 1 file changed, 39 insertions(+), 15 deletions(-) diff --git a/roboticstoolbox/robot/Robot.py b/roboticstoolbox/robot/Robot.py index 57758a5d..9cf176d7 100644 --- a/roboticstoolbox/robot/Robot.py +++ b/roboticstoolbox/robot/Robot.py @@ -1759,9 +1759,17 @@ def rne( # Adding inertia of fixed links joint_idx = reference_link.jindex - I[joint_idx] = I[joint_idx] + SpatialInertia(m=link.m, - r=transform_matrix * link.r, - I=link.I) + + # Trick to initialize empty I slots that don't have I components, + # leading to an error on addition + try: + I[joint_idx] = I[joint_idx] + SpatialInertia(m=link.m, + r=transform_matrix * link.r, + I=link.I) + except AttributeError: + I[joint_idx] = SpatialInertia(m=link.m, + r=transform_matrix * link.r, + I=link.I) # Get joint subspace if link.v is not None: @@ -1776,23 +1784,39 @@ def rne( for j in range(0, n): vJ = SpatialVelocity(s[j] * qdk[j]) + # Find link attached to joint n + 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 j 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 - if self.links[j].jindex is not None: - Xup[j] = SE3(self.links[j].A(qk[self.links[j].jindex])).inv() - else: - # If fixed link between parent and child - Xup[j] = SE3(self.links[j].A()).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 From 5f726c4d591f87d39d757ca4304db477eddfdbd1 Mon Sep 17 00:00:00 2001 From: niederha Date: Fri, 17 Nov 2023 15:20:22 +0100 Subject: [PATCH 09/12] Fixed backwards pass --- roboticstoolbox/robot/Robot.py | 29 +++++++++++++++++++++++------ 1 file changed, 23 insertions(+), 6 deletions(-) diff --git a/roboticstoolbox/robot/Robot.py b/roboticstoolbox/robot/Robot.py index 9cf176d7..4f8f296d 100644 --- a/roboticstoolbox/robot/Robot.py +++ b/roboticstoolbox/robot/Robot.py @@ -1784,7 +1784,7 @@ def rne( for j in range(0, n): vJ = SpatialVelocity(s[j] * qdk[j]) - # Find link attached to joint n + # Find link attached to joint j current_link = None for current_link in self.links: # We have already added the inertia of all fixed @@ -1792,14 +1792,13 @@ def rne( # only the link closest to the joint if current_link.jindex == j: break - if j is None: + 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 @@ -1824,9 +1823,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] From 4ccf155ec6f55c2e68546027b3a3db78ad1d5309 Mon Sep 17 00:00:00 2001 From: niederha Date: Fri, 17 Nov 2023 15:21:16 +0100 Subject: [PATCH 10/12] Fixed unit test for RNE sym. New implementation add some multiplicaiton by one so the string approach didnt work --- tests/test_ERobot.py | 35 +++++++++++++++++++++++++---------- 1 file changed, 25 insertions(+), 10 deletions(-) diff --git a/tests/test_ERobot.py b/tests/test_ERobot.py index e4514fb3..11776e44 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 @@ -210,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 From 2695992f448ac6cb7590c4b22226e256c17c57ee Mon Sep 17 00:00:00 2001 From: niederha Date: Fri, 17 Nov 2023 15:31:07 +0100 Subject: [PATCH 11/12] Changed to a more complex robot for unittesting URDF inertia --- tests/test_ERobot.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tests/test_ERobot.py b/tests/test_ERobot.py index 11776e44..1a7f00ca 100644 --- a/tests/test_ERobot.py +++ b/tests/test_ERobot.py @@ -154,7 +154,7 @@ def test_invdyn(self): nt.assert_array_almost_equal(tau, np.r_[d11 + d12, d21 + d22]) def test_URDF_inertia(self): - robot = rtb.models.URDF.Panda() + robot = rtb.models.URDF.UR10() try: robot.inertia(robot.q) except TypeError: From 594de5282b4a8bfce2839330eafd9bc8b860b9f2 Mon Sep 17 00:00:00 2001 From: niederha Date: Mon, 20 Nov 2023 15:57:57 +0100 Subject: [PATCH 12/12] Bug fix: circumventin addition bug for spatialinertia in the spatial library --- roboticstoolbox/robot/Robot.py | 15 ++++----------- 1 file changed, 4 insertions(+), 11 deletions(-) diff --git a/roboticstoolbox/robot/Robot.py b/roboticstoolbox/robot/Robot.py index 4f8f296d..e1f7094f 100644 --- a/roboticstoolbox/robot/Robot.py +++ b/roboticstoolbox/robot/Robot.py @@ -1759,17 +1759,10 @@ def rne( # Adding inertia of fixed links joint_idx = reference_link.jindex - - # Trick to initialize empty I slots that don't have I components, - # leading to an error on addition - try: - I[joint_idx] = I[joint_idx] + SpatialInertia(m=link.m, - r=transform_matrix * link.r, - I=link.I) - except AttributeError: - I[joint_idx] = SpatialInertia(m=link.m, - r=transform_matrix * link.r, - I=link.I) + + # 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: