From 0e59ae655d7e80f444f4e8a8d65aba06221ec400 Mon Sep 17 00:00:00 2001 From: Kamal Karki Date: Tue, 29 Sep 2026 12:02:53 +0530 Subject: [PATCH] fix(urdf): rotate inertia tensors from the inertial frame into the link frame URDF gives each link's inertia tensor in its frame, which may rotate relative to the link frame. The loader kept only the origin translation and passed the tensor through unrotated, so links with rotated inertial frames got wrong inertia, rne, coriolis and gravload results. Apply I_link = R I R^T. Related to #686 and #688. --- src/roboticstoolbox/models/URDF/URDFRobot.py | 31 ++++++-- tests/test_urdf_inertial.py | 82 ++++++++++++++++++++ 2 files changed, 107 insertions(+), 6 deletions(-) create mode 100644 tests/test_urdf_inertial.py diff --git a/src/roboticstoolbox/models/URDF/URDFRobot.py b/src/roboticstoolbox/models/URDF/URDFRobot.py index efcaa7359..a6a620e64 100644 --- a/src/roboticstoolbox/models/URDF/URDFRobot.py +++ b/src/roboticstoolbox/models/URDF/URDFRobot.py @@ -324,18 +324,37 @@ def _parse_urdf(urdf_str: str, source: str = ""): raise +def _inertia_in_link_frame(inertial) -> "tuple[np.ndarray | None, np.ndarray | None]": + """Centre of mass and inertia tensor of a URDF link, in the link frame. + + URDF gives the inertia tensor in the link's *inertial* frame, whose pose + relative to the link frame is ````. The toolbox + wants the tensor about the centre of mass but with axes parallel to the + link frame (see :attr:`Link.I`), so rotate it: ``I_link = R I R^T``. + ``xyz`` is already the centre of mass in the link frame. + + :param inertial: a parsed :class:`~roboticstoolbox.tools.urdf.Inertial` + :returns: ``(r, I)``, either of which is ``None`` when not given + """ + origin = inertial.origin + r = None if origin is None else origin[:3, 3] + + I = inertial.inertia + if I is not None and origin is not None: + R = origin[:3, :3] + I = R @ I @ R.T + I = 0.5 * (I + I.T) # remove rounding asymmetry; Link.I checks to 1e-8 + return r, I + + def _links_from_urdf(urdf: URDF): """Convert a parsed :class:`URDF` into (elinks, name).""" elinks = [] elinkdict = {} for link in urdf._links: - elink = Link( - name=link.name, - m=link.inertial.mass, - r=link.inertial.origin[:3, 3] if link.inertial.origin is not None else None, - I=link.inertial.inertia, - ) + r, I = _inertia_in_link_frame(link.inertial) + elink = Link(name=link.name, m=link.inertial.mass, r=r, I=I) elinks.append(elink) elinkdict[link.name] = elink diff --git a/tests/test_urdf_inertial.py b/tests/test_urdf_inertial.py new file mode 100644 index 000000000..7481584d5 --- /dev/null +++ b/tests/test_urdf_inertial.py @@ -0,0 +1,82 @@ +""" +URDF inertia tensors must be rotated from the inertial frame into the link frame. + +URDF gives each link's inertia tensor in its frame, which may be +rotated relative to the link frame by . The toolbox's +Link.I is the tensor about the centre of mass with axes parallel to the link +frame, so the loader has to apply I_link = R I R^T. It used to keep only the +translation of the inertial origin and pass the tensor through unrotated, +silently giving wrong dynamics for any link with a rotated inertial frame. +""" + +import io +import unittest + +import numpy as np +import numpy.testing as nt +from spatialmath import SE3 + +import roboticstoolbox as rtb +from roboticstoolbox.models.URDF.URDFRobot import URDF_file + +TEMPLATE = """ + + + + + + + + + + + + + + + + + +""" + + +def arm_link(xyz="0 0 0", rpy="0 0 0"): + links, _, _ = URDF_file(io.StringIO(TEMPLATE.format(xyz=xyz, rpy=rpy))) + return next(link for link in links if link.name == "arm") + + +class TestURDFInertialFrame(unittest.TestCase): + def test_unrotated_inertial_frame_is_unchanged(self): + link = arm_link() + nt.assert_array_almost_equal(link.I, np.diag([1, 2, 3])) + + def test_yaw_swaps_x_and_y_moments(self): + link = arm_link(rpy=f"0 0 {np.pi / 2}") + nt.assert_array_almost_equal(link.I, np.diag([2, 1, 3])) + + def test_general_rotation_matches_R_I_Rt(self): + rpy = [0.3, -0.7, 1.1] + link = arm_link(rpy=" ".join(str(a) for a in rpy)) + R = SE3.RPY(rpy).R + nt.assert_array_almost_equal(link.I, R @ np.diag([1, 2, 3]) @ R.T) + nt.assert_array_almost_equal(link.I, link.I.T) + + def test_centre_of_mass_is_the_origin_translation(self): + link = arm_link(xyz="0.1 0.2 0.3", rpy="0.4 0.5 0.6") + nt.assert_array_almost_equal(link.r, [0.1, 0.2, 0.3]) + + def test_rne_uses_the_rotated_tensor(self): + # Joint rotates about z. Rolling the inertial frame by 90 deg about x + # puts the principal moment iyy=2 on the link z axis, so a unit joint + # acceleration (no gravity, centre of mass on the axis) needs a torque + # of 2. With the tensor left unrotated it would wrongly be izz=3. + links, _, _ = URDF_file( + io.StringIO(TEMPLATE.format(xyz="0 0 0", rpy=f"{np.pi / 2} 0 0")) + ) + robot = rtb.Robot(links) + tau = robot.rne([0.0], [0.0], [1.0], gravity=[0, 0, 0]) + nt.assert_array_almost_equal(tau, [2.0]) + + +if __name__ == "__main__": + unittest.main()