From 65a3663bb6584d3d11f8ca46af095310e4c99db2 Mon Sep 17 00:00:00 2001 From: plancher Date: Fri, 16 Oct 2026 19:03:04 -0400 Subject: [PATCH 1/2] Rotate a rotated inertial origin's tensor into the link frame; rotate a prismatic joint's translation through its origin; pinocchio oracle over every fixture --- Joint.py | 8 +- Link.py | 16 ++- tests/fixtures/rotated_inertial_arm.urdf | 54 +++++++++ tests/test_inertia_oracle.py | 144 +++++++++++++++++++++++ 4 files changed, 220 insertions(+), 2 deletions(-) create mode 100644 tests/fixtures/rotated_inertial_arm.urdf create mode 100644 tests/test_inertia_oracle.py diff --git a/Joint.py b/Joint.py index 2d68f9e..716e7fc 100644 --- a/Joint.py +++ b/Joint.py @@ -499,7 +499,13 @@ def set_type(self, jtype, axis = None, pitch = 0.0): # prismatic Y translation into -Y in the parent frame). self.Xmat_sp_hom = sp.eye(4) self.Xmat_sp_hom[:3,:3] = (self.Xmat_sp_hom_free[:3,:3] * self.origin.Xmat_sp_hom_fixed[:3,:3]).transpose() - self.Xmat_sp_hom[:3,3] = (self.origin.Xmat_sp_hom_fixed[:3,:3] * self.Xmat_sp_hom_free[:3,3] + # origin.Xmat_sp_hom_fixed[:3,:3] is Featherstone's E = R^T, so the + # joint-frame translation reaches the parent frame through E^T = R. + # With E itself the translation of a prismatic joint whose origin + # is rotated came out mirrored (2026-10-16 oracle: a yaw of 0.7 + # flipped the y offset; fr3_finger_joint2's yaw of pi hid it, since + # that R is its own transpose). + self.Xmat_sp_hom[:3,3] = (self.origin.Xmat_sp_hom_fixed[:3,:3].transpose() * self.Xmat_sp_hom_free[:3,3] + self.origin.Xmat_sp_hom_fixed[:3,3]) self.Xmat_sp_hom = _simplify_transform(self.Xmat_sp_hom) # and derivative diff --git a/Link.py b/Link.py index 5686884..3b466e5 100644 --- a/Link.py +++ b/Link.py @@ -57,13 +57,27 @@ def build_spatial_inertia(self): # I6x6 = I3x3 + mccT mc I3x3 = Ixx Ixy Ixz c = 0 -cz cy # mcT mI Ixy Iyy Iyz cz 0 -cx # Ixz Iyz Izz -cy cx 0 + # The URDF tensor is written in the frame. + # URDF's rpy gives R = Rz(y) Ry(p) Rx(r), the rotation taking inertial- + # frame coordinates to link-frame coordinates; Rotation.E is Featherstone's + # E = R^T, so the tensor about the centre of mass in LINK axes is + # E^T I E, and only then does the parallel-axis term move it to the + # link origin. Pinocchio applies the same rotation; a tensor given with a + # rotated inertial origin (UR5e shoulder and wrists, rpy = +-pi/2 about x) + # was previously taken unrotated and came out up to 10 % wrong. rx = sp.nsimplify(self.origin.translation.rx, tolerance=1e-12, rational=True) com_trans = np.reshape(np.array([expr.evalf() for expr in rx]),(3,3)) mc = self.mass*com_trans mccT = np.matmul(mc,com_trans.transpose()) - topLeft = self.inertia.to_matrix() + mccT + I_inertial = np.asarray(self.inertia.to_matrix(), dtype=float) + if self.origin.rotation is not None: + E = np.asarray(self.origin.rotation.E.evalf(), dtype=float) + I_link = E.transpose() @ I_inertial @ E + else: + I_link = I_inertial + topLeft = I_link + mccT top = np.hstack((topLeft,mc)) bottom = np.hstack((mc.transpose(),self.mass*np.eye(3))) self.spatial_ineratia = np.vstack((top,bottom)).astype(float) diff --git a/tests/fixtures/rotated_inertial_arm.urdf b/tests/fixtures/rotated_inertial_arm.urdf new file mode 100644 index 0000000..22f6db3 --- /dev/null +++ b/tests/fixtures/rotated_inertial_arm.urdf @@ -0,0 +1,54 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/tests/test_inertia_oracle.py b/tests/test_inertia_oracle.py new file mode 100644 index 0000000..d328aee --- /dev/null +++ b/tests/test_inertia_oracle.py @@ -0,0 +1,144 @@ +"""Pinocchio as the oracle for what the parser builds from a URDF. + +Two checks over every fixture pinocchio can also build: the spatial inertia of +every body (mass, first moment and the tensor about the joint origin, fixed-joint +children folded into their parent exactly as pinocchio folds them) and the +world placement of every joint frame at random configurations. The tensor check +is what the 2026-10-16 UR5e finding needed: an that is +not zero rotates the tensor, and the parser used to take it unrotated. +""" +import contextlib +import glob +import io +import os + +import numpy as np +import pytest + +pin = pytest.importorskip("pinocchio") +from URDFParser import URDFParser # noqa: E402 + +FIXTURES = os.path.join(os.path.dirname(__file__), "fixtures") +# Fixtures pinocchio's URDF reader cannot build: helical, spherical and +# translation joints are parser extensions with no URDF joint type (their +# equivalence lives in the sibling tests against hand-built pinocchio models). +NOT_IN_PINOCCHIO_URDF = {"helical_arm.urdf", "spherical_arm.urdf", "mixed_spherical_arm.urdf", + "translation_arm.urdf"} +# Joint types whose single scalar q the kinematics check can feed to both sides +# directly; the manifold-valued joints keep their own tests. +SCALAR_JOINTS = {"revolute", "continuous", "prismatic"} + +# pinocchio's URDF loader ignores a planar joint's , so a skew planar +# normal has no URDF oracle; test_planar_translation_decomposition checks that +# fixture's kinematics against a programmatic pinocchio model. Its inertias are +# still checked here. +NO_URDF_KINEMATICS_ORACLE = {"skew_planar_arm.urdf"} + +ALL = sorted(os.path.basename(p) for p in glob.glob(os.path.join(FIXTURES, "*.urdf"))) +ORACLE_FIXTURES = [f for f in ALL if f not in NOT_IN_PINOCCHIO_URDF] +KINEMATICS_FIXTURES = [f for f in ORACLE_FIXTURES if f not in NO_URDF_KINEMATICS_ORACLE] + + +def _parse(name): + with contextlib.redirect_stdout(io.StringIO()): + return URDFParser().parse(os.path.join(FIXTURES, name), floating_base=False) + + +def _pin_model(name): + return pin.buildModelFromUrdf(os.path.join(FIXTURES, name)) + + +def _pin_joint_of_body(model, link_name): + """The pinocchio joint carrying a URDF link (a fixed-joint child maps to the + joint its body was folded into; a parser-decomposed planar joint's real link + maps to pinocchio's single planar joint).""" + return model.frames[model.getFrameId(link_name)].parentJoint + + +@pytest.mark.parametrize("name", ORACLE_FIXTURES) +def test_every_body_inertia_matches_pinocchio(name): + robot = _parse(name) + model = _pin_model(name) + checked = 0 + for joint in robot.get_joints_ordered_by_id(): + link = robot.get_link_by_name(joint.child) + if link.is_dummy_link(): + continue # massless intermediate of a decomposed multi-DOF joint + S = np.asarray(link.get_spatial_inertia(), dtype=float) # angular first + P = model.inertias[_pin_joint_of_body(model, link.name)].matrix() # linear first + scale = max(np.abs(P).max(), 1.0) + np.testing.assert_allclose(S[3, 3], P[0, 0], rtol=0, atol=1e-12 * scale, err_msg=f"{name} {joint.name} mass") + np.testing.assert_allclose(S[:3, 3:], P[3:, :3], rtol=0, atol=1e-12 * scale, err_msg=f"{name} {joint.name} first moment") + np.testing.assert_allclose(S[:3, :3], P[3:, 3:], rtol=0, atol=1e-12 * scale, err_msg=f"{name} {joint.name} inertia about the joint origin") + np.testing.assert_allclose(S[3:, 3:], P[:3, :3], rtol=0, atol=1e-12 * scale, err_msg=f"{name} {joint.name} mass block") + checked += 1 + assert checked > 0 + + +def test_rotated_inertial_origin_is_rotated_into_the_link_frame(): + """The fixture's upper-arm tensor is given in a frame rotated by a quarter + turn about x plus two more angles; unrotated it is 3 % off the link-frame + tensor pinocchio reports. The folded tool plate behind the fixed joint has + its own rotated inertial and must land in the wrist exactly.""" + robot = _parse("rotated_inertial_arm.urdf") + model = _pin_model("rotated_inertial_arm.urdf") + link = robot.get_link_by_name("upper-arm") + S = np.asarray(link.get_spatial_inertia(), dtype=float) + P = model.inertias[model.getJointId("shoulder-pan")].matrix() + np.testing.assert_allclose(S[:3, :3], P[3:, 3:], rtol=0, atol=1e-14) + raw = np.asarray(link.inertia.to_matrix(), dtype=float) + c = np.array([float(link.origin.translation.x), float(link.origin.translation.y), float(link.origin.translation.z)]) + cx = np.array([[0, -c[2], c[1]], [c[2], 0, -c[0]], [-c[1], c[0], 0]]) + unrotated = raw + link.mass * cx @ cx.T + assert np.abs(unrotated - P[3:, 3:]).max() > 1e-3 + wrist = np.asarray(robot.get_link_by_name("wrist").get_spatial_inertia(), dtype=float) + assert robot.get_fixed_joint_by_name("tool0-flange") is not None + np.testing.assert_allclose(wrist[:3, :3], model.inertias[model.getJointId("wrist-slide")].matrix()[3:, 3:], rtol=0, atol=1e-14) + + +@pytest.mark.parametrize("name", KINEMATICS_FIXTURES) +def test_joint_placements_match_pinocchio_at_random_configurations(name): + """World placement of every real body's joint frame. A planar joint the + parser decomposes into x, y and yaw sub-joints is fed to pinocchio as its + native q = [x, y, cos(yaw), sin(yaw)]; the sub-joints' massless + intermediate links have no pinocchio counterpart and are composed through.""" + robot = _parse(name) + joints = robot.get_joints_ordered_by_id() + model = _pin_model(name) + data = model.createData() + assert all(j.jtype in SCALAR_JOINTS for j in joints), f"{name}: non-scalar parser joint" + rng = np.random.default_rng(20261016) + for _ in range(3): + q = rng.uniform(-1.0, 1.0, len(joints)) + # pinocchio configuration: scalar joints copy their value; a decomposed + # planar joint takes the parser's [x, y, yaw] as [x, y, cos, sin]. + q_pin = np.zeros(model.nq) + pending = [] + for k, joint in enumerate(joints): + if model.existJointName(joint.name): + jm = model.joints[model.getJointId(joint.name)] + assert jm.nq == 1, f"{name} {joint.name}: pinocchio nq {jm.nq}" + q_pin[jm.idx_q] = q[k] + continue + pending.append(q[k]) + link = robot.get_link_by_name(joint.child) + if link.is_dummy_link(): + continue + jm = model.joints[_pin_joint_of_body(model, link.name)] + assert jm.nq == 4 and len(pending) == 3, f"{name} {joint.name}: unexpected decomposition" + x, y, yaw = pending + q_pin[jm.idx_q:jm.idx_q + 4] = [x, y, np.cos(yaw), np.sin(yaw)] + pending = [] + pin.forwardKinematics(model, data, q_pin) + world = {-1: np.eye(4)} + checked = 0 + for k, joint in enumerate(joints): + X = np.asarray(joint.get_transformation_matrix_hom_function()(q[k]), dtype=float).reshape(4, 4) + world[joint.get_id()] = world[robot.get_parent_id(joint.get_id())] @ X + link = robot.get_link_by_name(joint.child) + if link.is_dummy_link(): + continue + expected = data.oMi[_pin_joint_of_body(model, link.name)].homogeneous + np.testing.assert_allclose(world[joint.get_id()], expected, rtol=0, atol=1e-12, err_msg=f"{name} {joint.name}") + checked += 1 + assert checked > 0 From fc5bf8c27afaecffb19a64979b636451ac7aed72 Mon Sep 17 00:00:00 2001 From: plancher Date: Fri, 16 Oct 2026 19:07:48 -0400 Subject: [PATCH 2/2] Oracle builds pinocchio from the quarter-turn-snapped URDF; continuous joints use the cos-sin configuration --- tests/test_inertia_oracle.py | 24 +++++++++++++++++++++--- 1 file changed, 21 insertions(+), 3 deletions(-) diff --git a/tests/test_inertia_oracle.py b/tests/test_inertia_oracle.py index d328aee..178f50a 100644 --- a/tests/test_inertia_oracle.py +++ b/tests/test_inertia_oracle.py @@ -45,7 +45,22 @@ def _parse(name): def _pin_model(name): - return pin.buildModelFromUrdf(os.path.join(FIXTURES, name)) + return _snapped_pin_model(os.path.join(FIXTURES, name)) + + +def _snapped_pin_model(path): + """pinocchio built from the URDF with every snapped to the + quarter-turn grid the parser snaps joint origins to (Joint._snap_to_pi_grid, + tolerance 1e-5): a URDF that writes 3.14159 describes a robot 2.7e-6 rad + away from the one the parser models, and the oracle must model the parser's + robot. RBDReference's pinocchio backend snaps the same way.""" + from bs4 import BeautifulSoup + from URDFParser.Joint import _snap_to_pi_grid + soup = BeautifulSoup(open(path).read(), "xml") + for origin in soup.find_all("origin"): + if origin.has_attr("rpy") and len(origin["rpy"].split()) == 3: + origin["rpy"] = " ".join(repr(float(_snap_to_pi_grid(float(v)))) for v in origin["rpy"].split()) + return pin.buildModelFromXML(str(soup)) def _pin_joint_of_body(model, link_name): @@ -117,8 +132,11 @@ def test_joint_placements_match_pinocchio_at_random_configurations(name): for k, joint in enumerate(joints): if model.existJointName(joint.name): jm = model.joints[model.getJointId(joint.name)] - assert jm.nq == 1, f"{name} {joint.name}: pinocchio nq {jm.nq}" - q_pin[jm.idx_q] = q[k] + if jm.nq == 1: + q_pin[jm.idx_q] = q[k] + else: # a continuous joint: pinocchio's unbounded revolute is (cos, sin) + assert jm.nq == 2 and joint.jtype == "continuous", f"{name} {joint.name}: pinocchio nq {jm.nq}" + q_pin[jm.idx_q:jm.idx_q + 2] = [np.cos(q[k]), np.sin(q[k])] continue pending.append(q[k]) link = robot.get_link_by_name(joint.child)