From b64d59bd9578212cb6c0eb4827149ad06878cbeb Mon Sep 17 00:00:00 2001 From: Gonzalo Casas Date: Tue, 8 Sep 2026 11:29:09 +0200 Subject: [PATCH] Fix ABB/RFL inertias and preserve PyBullet base-link placement Apply the mesh-derived inertias contributed in issue #486, document their approximation, and validate physical realizability across the robot library. Correct the PyBullet inertial-to-link frame conversion exposed by the new ABB base centre of mass. Co-authored-by: OmniLink <234818255+omnilink-tech@users.noreply.github.com> --- AUTHORS.md | 1 + CHANGELOG.md | 2 + src/compas_fab/backends/pybullet/client.py | 17 +- .../urdf/robot_description.urdf | 31 ++-- .../rfl/urdf/robot_description.urdf | 155 +++++++++--------- .../backends/pybullet/test_pybullet_client.py | 31 ++++ tests/robots/test_robot_library_inertias.py | 35 ++++ 7 files changed, 178 insertions(+), 94 deletions(-) create mode 100644 tests/robots/test_robot_library_inertias.py diff --git a/AUTHORS.md b/AUTHORS.md index 29b1952b62..21a015a831 100644 --- a/AUTHORS.md +++ b/AUTHORS.md @@ -17,3 +17,4 @@ - Edvard Bruun <> [@ebruun](https://github.com/ebruun) - Victor Pok Yin Leung <> [@yck011522](https://github.com/yck011522) - Begüm Saral <> [@begums](https://github.com/begums) +- OmniLink [@omnilink-tech](https://github.com/omnilink-tech) diff --git a/CHANGELOG.md b/CHANGELOG.md index 4081a5ccf7..6902ae4f2f 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -26,6 +26,8 @@ Requires `compas_robots >= 1.1`. ### Changed +* Corrected the invalid inertia tensors and centres of mass in the bundled ABB IRB4600-40/2.55 and RFL URDFs using the collision-mesh approximations contributed by @omnilink-tech in #486. These assume uniform density at the existing declared masses; they are not manufacturer-calibrated dynamic parameters. +* Fixed PyBullet base-frame reads and updates for robots and objects whose inertial frame differs from the base-link frame. This keeps the corrected ABB centre of mass from displacing the robot in planning and collision checks. * Refreshed the bundled UR5 and UR10e descriptions and meshes from the official Universal Robots ROS 2 description package. All seven supported UR cells now share one `ur_description` mesh package instead of duplicating model assets per cell. * Bundled UR and Panda visual meshes now use their original Collada files instead of derived OBJ copies, preserving source materials and eliminating redundant converted assets. The ROS package extraction script likewise keeps downloaded DAE files unchanged. * The tools in `ToolLibrary` now mount along the +Z axis of their base frame instead of +X. Every planning group in `RobotCellLibrary` ends at a link whose +Z points away from the arm (`tool0` for the industrial robots, `panda_hand_tcp` for the Panda), so with this the same tool attaches to any of them with an identity attachment frame — previously each cell carried a rotation to bridge the two conventions, and a tool authored for one robot did not necessarily fit another. Their TCF states the tool's working direction with its own Z axis too, so a `TargetMode.TOOL` target aligns the tool along the target's Z — previously the TCF's X axis ran along the tool, which put every tool-mode target 90 degrees out. The tools are still modelled along +X internally and re-framed on the way out via `ToolModel.reframe_base`. The beams held by the gripper cells are authored in TCF coordinates and were re-authored to match, so they stay put. Poses are unchanged: the attached tools and workpieces of every cell land exactly where they did, only the tool's base frame is now the end effector link's frame rather than a rotated version of it. Requires the `reframe_base` support of `compas_robots >= 1.1`. diff --git a/src/compas_fab/backends/pybullet/client.py b/src/compas_fab/backends/pybullet/client.py index bd49887aa8..73c409de7f 100644 --- a/src/compas_fab/backends/pybullet/client.py +++ b/src/compas_fab/backends/pybullet/client.py @@ -10,6 +10,7 @@ from compas.colors import Color from compas.datastructures import Mesh from compas.geometry import Frame +from compas.geometry import Transformation from compas_robots import Configuration from compas_robots import RobotModel from compas_robots import ToolModel @@ -652,7 +653,14 @@ def _build_pose_for_pybullet(self, configuration: Configuration) -> list[float]: def _get_base_frame(self, body_id): pose = pybullet.getBasePositionAndOrientation(body_id, physicsClientId=self.client_id) - return frame_from_pose(pose) + # PyBullet reports the base's inertial frame, not its URDF link frame. + t_world_inertial = Transformation.from_frame(frame_from_pose(pose)) + t_link_inertial = Transformation.from_frame(self._get_base_inertial_frame(body_id)) + return Frame.from_transformation(t_world_inertial * t_link_inertial.inverse()) + + def _get_base_inertial_frame(self, body_id): + dynamics = pybullet.getDynamicsInfo(body_id, const.BASE_LINK_ID, physicsClientId=self.client_id) + return frame_from_pose((dynamics[3], dynamics[4])) def _get_base_name(self, body_id): return self._get_body_info(body_id).base_name.decode(encoding="UTF-8") @@ -711,7 +719,9 @@ def _get_link_frame(self, link_id, body_id): return frame_from_pose(pose) def _set_base_frame(self, frame, body_id): - point, quaternion = pose_from_frame(frame) + # resetBasePositionAndOrientation expects the inertial frame in world coordinates. + inertial_frame = frame.to_world_coordinates(self._get_base_inertial_frame(body_id)) + point, quaternion = pose_from_frame(inertial_frame) pybullet.resetBasePositionAndOrientation(body_id, point, quaternion, physicsClientId=self.client_id) def _set_joint_position(self, joint_id, value, body_id): @@ -860,8 +870,7 @@ def _set_rigid_body_base_frame(self, rigid_body_name: str, frame: Frame): self._set_object_frame(body_id, frame) def _set_object_frame(self, body_id, frame): - (point, quat) = pose_from_frame(frame) - pybullet.resetBasePositionAndOrientation(body_id, point, quat, physicsClientId=self.client_id) + self._set_base_frame(frame, body_id) # ------------------------------------------------------------------------------------ # Helper functions for creating rigid bodies in PyBullet diff --git a/src/compas_fab/data/robot_library/abb_irb4600_40_255/urdf/robot_description.urdf b/src/compas_fab/data/robot_library/abb_irb4600_40_255/urdf/robot_description.urdf index c6326c9635..09c66ec710 100644 --- a/src/compas_fab/data/robot_library/abb_irb4600_40_255/urdf/robot_description.urdf +++ b/src/compas_fab/data/robot_library/abb_irb4600_40_255/urdf/robot_description.urdf @@ -1,5 +1,8 @@ + @@ -15,9 +18,9 @@ - + - + @@ -35,9 +38,9 @@ - + - + @@ -55,9 +58,9 @@ - + - + @@ -75,9 +78,9 @@ - + - + @@ -95,9 +98,9 @@ - + - + @@ -115,9 +118,9 @@ - + - + @@ -135,9 +138,9 @@ - + - + diff --git a/src/compas_fab/data/robot_library/rfl/urdf/robot_description.urdf b/src/compas_fab/data/robot_library/rfl/urdf/robot_description.urdf index 1624236ca8..169a866ed6 100644 --- a/src/compas_fab/data/robot_library/rfl/urdf/robot_description.urdf +++ b/src/compas_fab/data/robot_library/rfl/urdf/robot_description.urdf @@ -1,5 +1,8 @@ + @@ -48,9 +51,9 @@ - + - + @@ -70,9 +73,9 @@ - + - + @@ -92,9 +95,9 @@ - + - + @@ -112,9 +115,9 @@ - + - + @@ -132,9 +135,9 @@ - + - + @@ -152,9 +155,9 @@ - + - + @@ -172,9 +175,9 @@ - + - + @@ -192,9 +195,9 @@ - + - + @@ -212,9 +215,9 @@ - + - + @@ -232,9 +235,9 @@ - + - + @@ -255,9 +258,9 @@ - + - + @@ -277,9 +280,9 @@ - + - + @@ -297,9 +300,9 @@ - + - + @@ -317,9 +320,9 @@ - + - + @@ -337,9 +340,9 @@ - + - + @@ -357,9 +360,9 @@ - + - + @@ -377,9 +380,9 @@ - + - + @@ -397,9 +400,9 @@ - + - + @@ -417,9 +420,9 @@ - + - + @@ -440,9 +443,9 @@ - + - + @@ -462,9 +465,9 @@ - + - + @@ -484,9 +487,9 @@ - + - + @@ -504,9 +507,9 @@ - + - + @@ -524,9 +527,9 @@ - + - + @@ -544,9 +547,9 @@ - + - + @@ -564,9 +567,9 @@ - + - + @@ -584,9 +587,9 @@ - + - + @@ -604,9 +607,9 @@ - + - + @@ -624,9 +627,9 @@ - + - + @@ -647,9 +650,9 @@ - + - + @@ -669,9 +672,9 @@ - + - + @@ -689,9 +692,9 @@ - + - + @@ -709,9 +712,9 @@ - + - + @@ -729,9 +732,9 @@ - + - + @@ -749,9 +752,9 @@ - + - + @@ -769,9 +772,9 @@ - + - + @@ -789,9 +792,9 @@ - + - + @@ -809,9 +812,9 @@ - + - + diff --git a/tests/backends/pybullet/test_pybullet_client.py b/tests/backends/pybullet/test_pybullet_client.py index 27c8bfc8a4..69c8b1717b 100644 --- a/tests/backends/pybullet/test_pybullet_client.py +++ b/tests/backends/pybullet/test_pybullet_client.py @@ -1,4 +1,6 @@ import pytest +from compas.geometry import Frame +from compas.geometry import Transformation from compas_robots import RobotModel import compas_fab @@ -13,6 +15,35 @@ def test_pybullet_client_connection_direct(): assert client.is_connected +@pytest.mark.parametrize("setter", ["_set_base_frame", "_set_object_frame"]) +@pytest.mark.parametrize("base_frame", [Frame.worldXY(), Frame([1, 2, 3], [0, 1, 0], [0, 0, 1])]) +def test_base_link_frame_with_offset_and_rotated_inertia(setter, base_frame): + robot_cell, state = RobotCellLibrary.abb_irb4600_40_255(load_geometry=True) + model = robot_cell.robot_model + model.root.inertial.origin = Frame([0.2, -0.3, 0.4], [0, 0, 1], [1, 0, 0]) + tip_name = robot_cell.get_end_effector_link_name() + expected_tip = model.forward_kinematics(state.robot_configuration, tip_name) + expected_tip.transform(Transformation.from_frame(base_frame)) + + with PyBulletClient(connection_type="direct") as client: + client._set_robot(model, robot_cell.robot_semantics) + client._robot_cell = robot_cell + client._set_robot_configuration(state.robot_configuration) + if setter == "_set_base_frame": + client._set_base_frame(base_frame, client.robot_puid) + else: + client._set_object_frame(client.robot_puid, base_frame) + + actual_base = client._get_link_frame(-1, client.robot_puid) + actual_tip = client._get_link_frame(client.robot_link_puids[tip_name], client.robot_puid) + # Check the child link as well: matching errors in the setter/getter could + # otherwise make a round-trip pass while the whole robot is displaced. + for actual, expected in [(actual_base, base_frame), (actual_tip, expected_tip)]: + assert list(actual.point) == pytest.approx(list(expected.point), abs=1e-6) + assert list(actual.xaxis) == pytest.approx(list(expected.xaxis), abs=1e-6) + assert list(actual.yaxis) == pytest.approx(list(expected.yaxis), abs=1e-6) + + def test_pybullet_client_set_robot(): # Testing workflow of loading robot from URDF with PyBulletClient(connection_type="direct") as client: diff --git a/tests/robots/test_robot_library_inertias.py b/tests/robots/test_robot_library_inertias.py new file mode 100644 index 0000000000..2b52fb91f9 --- /dev/null +++ b/tests/robots/test_robot_library_inertias.py @@ -0,0 +1,35 @@ +from pathlib import Path +import xml.etree.ElementTree as ET + +import numpy as np +import pytest + +import compas_fab + + +ROBOT_LIBRARY = Path(compas_fab.get("robot_library")) +URDF_PATHS = sorted(ROBOT_LIBRARY.rglob("*.urdf")) + + +@pytest.mark.parametrize("urdf_path", URDF_PATHS, ids=lambda path: path.relative_to(ROBOT_LIBRARY).as_posix()) +def test_bundled_inertias_are_physically_realisable(urdf_path): + """Principal moments must be positive and satisfy the triangle inequality.""" + for link in ET.parse(urdf_path).getroot().findall("link"): + inertia = link.find("inertial/inertia") + if inertia is None: + continue + + values = {key: float(inertia.attrib[key]) for key in ("ixx", "ixy", "ixz", "iyy", "iyz", "izz")} + tensor = np.array( + [ + [values["ixx"], values["ixy"], values["ixz"]], + [values["ixy"], values["iyy"], values["iyz"]], + [values["ixz"], values["iyz"], values["izz"]], + ] + ) + assert np.isfinite(tensor).all(), link.attrib["name"] + moments = np.linalg.eigvalsh(tensor) + context = "{}: principal moments {}".format(link.attrib["name"], moments) + assert moments[0] > 0, context + # eigvalsh sorts the moments; checking the largest covers all three inequalities. + assert moments[0] + moments[1] >= moments[2] - 1e-9 * moments[2], context