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()