diff --git a/CHANGELOG.md b/CHANGELOG.md index a0c6e744..23b871ea 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -6,6 +6,7 @@ The format is based on [Keep a Changelog](https://keepachangelog.com/en/1.0.0/). ## [Unreleased] +- Unit testing inertia validity and projected rotation inertial to the fully-physical consistent set in https://github.com/Gepetto/example-robot-data/pull/370 - Installing example-robot-data without hppfcl in https://github.com/Gepetto/example-robot-data/pull/338 - ROS: jrl_cmakemodules dependency ([#330](https://github.com/Gepetto/example-robot-data/pull/330)) - Added Centauro ([#346](https://github.com/Gepetto/example-robot-data/pull/346)) diff --git a/CMakeLists.txt b/CMakeLists.txt index ac1adbd6..0c7cb40b 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -54,7 +54,7 @@ include("${JRL_CMAKE_MODULES}/python.cmake") # Print initial message message(STATUS "${PROJECT_DESCRIPTION}, version ${PROJECT_VERSION}") -message(STATUS "Copyright (C) 2018-2023 LAAS-CNRS, University of Edinburgh") +message(STATUS "Copyright (C) 2018-2026 LAAS-CNRS, University of Edinburgh") message(STATUS " Heriot-Watt University, INRIA") message(STATUS "All rights reserved.") message(STATUS "Released under the BSD 3-Clause License.") diff --git a/robots/allegro_hand_description/urdf/allegro_left_hand.urdf b/robots/allegro_hand_description/urdf/allegro_left_hand.urdf index 7d55683a..9cc723a7 100644 --- a/robots/allegro_hand_description/urdf/allegro_left_hand.urdf +++ b/robots/allegro_hand_description/urdf/allegro_left_hand.urdf @@ -84,7 +84,7 @@ - + @@ -112,7 +112,7 @@ - + @@ -222,7 +222,7 @@ - + @@ -250,7 +250,7 @@ - + @@ -360,7 +360,7 @@ - + @@ -388,7 +388,7 @@ - + @@ -474,7 +474,7 @@ - + @@ -504,7 +504,7 @@ - + @@ -533,7 +533,7 @@ - + diff --git a/robots/allegro_hand_description/urdf/allegro_right_hand.urdf b/robots/allegro_hand_description/urdf/allegro_right_hand.urdf index 41d8f307..c2e916da 100644 --- a/robots/allegro_hand_description/urdf/allegro_right_hand.urdf +++ b/robots/allegro_hand_description/urdf/allegro_right_hand.urdf @@ -84,7 +84,7 @@ - + @@ -112,7 +112,7 @@ - + @@ -222,7 +222,7 @@ - + @@ -250,7 +250,7 @@ - + @@ -360,7 +360,7 @@ - + @@ -388,7 +388,7 @@ - + @@ -474,7 +474,7 @@ - + @@ -504,7 +504,7 @@ - + @@ -533,7 +533,7 @@ - + diff --git a/robots/human_description/robots/human.urdf b/robots/human_description/robots/human.urdf index b858994e..6fbfc64b 100644 --- a/robots/human_description/robots/human.urdf +++ b/robots/human_description/robots/human.urdf @@ -240,7 +240,7 @@ - + @@ -364,7 +364,7 @@ - + diff --git a/robots/icub_description/robots/icub.urdf b/robots/icub_description/robots/icub.urdf index e9179f93..7811b921 100644 --- a/robots/icub_description/robots/icub.urdf +++ b/robots/icub_description/robots/icub.urdf @@ -3,7 +3,7 @@ - + diff --git a/robots/icub_description/robots/icub_reduced.urdf b/robots/icub_description/robots/icub_reduced.urdf index e9207bb6..e9239a8d 100644 --- a/robots/icub_description/robots/icub_reduced.urdf +++ b/robots/icub_description/robots/icub_reduced.urdf @@ -3,7 +3,7 @@ - + diff --git a/robots/romeo_description/urdf/romeo.urdf b/robots/romeo_description/urdf/romeo.urdf index 865d44d4..7f8df201 100644 --- a/robots/romeo_description/urdf/romeo.urdf +++ b/robots/romeo_description/urdf/romeo.urdf @@ -575,7 +575,7 @@ - + @@ -613,7 +613,7 @@ - + diff --git a/robots/talos_data/robots/talos_left_arm.urdf b/robots/talos_data/robots/talos_left_arm.urdf index 913e97f3..b452b624 100644 --- a/robots/talos_data/robots/talos_left_arm.urdf +++ b/robots/talos_data/robots/talos_left_arm.urdf @@ -598,7 +598,7 @@ - + diff --git a/robots/talos_data/robots/talos_reduced.urdf b/robots/talos_data/robots/talos_reduced.urdf index b0d88136..5efe188a 100644 --- a/robots/talos_data/robots/talos_reduced.urdf +++ b/robots/talos_data/robots/talos_reduced.urdf @@ -1445,7 +1445,7 @@ - + @@ -1800,7 +1800,7 @@ - + diff --git a/robots/talos_data/robots/talos_reduced_box.urdf b/robots/talos_data/robots/talos_reduced_box.urdf index e1baf716..9262a91d 100644 --- a/robots/talos_data/robots/talos_reduced_box.urdf +++ b/robots/talos_data/robots/talos_reduced_box.urdf @@ -1486,7 +1486,7 @@ - + @@ -1841,7 +1841,7 @@ - + diff --git a/robots/talos_data/robots/talos_reduced_corrected.urdf b/robots/talos_data/robots/talos_reduced_corrected.urdf index e88f4920..db5a475c 100644 --- a/robots/talos_data/robots/talos_reduced_corrected.urdf +++ b/robots/talos_data/robots/talos_reduced_corrected.urdf @@ -1442,7 +1442,7 @@ - + @@ -1797,7 +1797,7 @@ - + diff --git a/robots/tiago_description/robots/tiago.urdf b/robots/tiago_description/robots/tiago.urdf index fb98ba0b..e4e88fbc 100644 --- a/robots/tiago_description/robots/tiago.urdf +++ b/robots/tiago_description/robots/tiago.urdf @@ -994,7 +994,7 @@ - + diff --git a/robots/tiago_description/robots/tiago_dual.urdf b/robots/tiago_description/robots/tiago_dual.urdf index d13016f7..f8836995 100644 --- a/robots/tiago_description/robots/tiago_dual.urdf +++ b/robots/tiago_description/robots/tiago_dual.urdf @@ -1076,7 +1076,7 @@ - + @@ -3278,7 +3278,7 @@ - + diff --git a/robots/tiago_description/robots/tiago_no_hand.urdf b/robots/tiago_description/robots/tiago_no_hand.urdf index c7278fba..430442f7 100644 --- a/robots/tiago_description/robots/tiago_no_hand.urdf +++ b/robots/tiago_description/robots/tiago_no_hand.urdf @@ -995,7 +995,7 @@ - + diff --git a/unittest/CMakeLists.txt b/unittest/CMakeLists.txt index 686b67f2..7df5f1bd 100644 --- a/unittest/CMakeLists.txt +++ b/unittest/CMakeLists.txt @@ -1,4 +1,4 @@ -set(${PROJECT_NAME}_PYTHON_TESTS load) +set(${PROJECT_NAME}_PYTHON_TESTS load inertia_validation) foreach(TEST ${${PROJECT_NAME}_PYTHON_TESTS}) add_python_unit_test("${PROJECT_NAME}-py-${TEST}" "unittest/test_${TEST}.py" diff --git a/unittest/test_inertia_validation.py b/unittest/test_inertia_validation.py new file mode 100644 index 00000000..544b37af --- /dev/null +++ b/unittest/test_inertia_validation.py @@ -0,0 +1,93 @@ +#!/usr/bin/env python + +import unittest + +import numpy as np +import pinocchio as pin + +from example_robot_data import ROBOTS, load + + +def _get_inertia_attribute(inertia, *names): + for name in names: + if hasattr(inertia, name): + value = getattr(inertia, name) + return value() if callable(value) else value + joined_names = ", ".join(names) + raise AttributeError(f"Unsupported inertia API, expected one of: {joined_names}") + + +def _validate_rigid_body_inertia(inertia, context, atol=1e-12): + """Validate one rigid-body inertia for trusted-model execution.""" + mass = float(_get_inertia_attribute(inertia, "mass")) + com = np.asarray(_get_inertia_attribute(inertia, "com", "lever"), dtype=float) + inertia_com = np.asarray( + _get_inertia_attribute(inertia, "inertiaCom", "inertia"), dtype=float + ) + + if not np.isfinite(mass): + raise ValueError(f"{context} has non-finite mass {mass}") + if not np.isfinite(com).all(): + raise ValueError(f"{context} has non-finite center of mass {com}") + if not np.isfinite(inertia_com).all(): + raise ValueError(f"{context} has non-finite inertia matrix entries") + if mass < -atol: + raise ValueError(f"{context} has negative mass {mass}") + + sym_inertia = 0.5 * (inertia_com + inertia_com.T) + if not np.allclose(inertia_com, sym_inertia, atol=atol, rtol=0.0): + raise ValueError(f"{context} inertia matrix must be symmetric") + + principal_moments = np.linalg.eigvalsh(sym_inertia) + if principal_moments[0] < -atol: + raise ValueError(f"{context} inertia matrix must be positive semidefinite") + + if mass <= atol: + if np.max(np.abs(sym_inertia)) > atol: + raise ValueError(f"{context} is massless but has non-zero inertia") + return + + i0, i1, i2 = principal_moments + if i0 + i1 < i2 - atol or i0 + i2 < i1 - atol or i1 + i2 < i0 - atol: + raise ValueError(f"{context} violates rigid-body inertia triangle inequalities") + + +def _pinocchio_version(): + return tuple(int(part) for part in pin.__version__.split(".")) + + +class InertiaValidationTestCase(unittest.TestCase): + def _check_robot(self, name): + robot = load(name, display=False, verbose=False) + model = robot.model + + self.assertEqual( + len(model.inertias), + len(model.names), + f"{name} exposes inconsistent joint/inertia metadata", + ) + + for joint_id, inertia in enumerate(model.inertias): + joint_name = model.names[joint_id] + context = f"{name}:{joint_name} (joint {joint_id})" + with self.subTest(robot=name, joint=joint_name, joint_id=joint_id): + _validate_rigid_body_inertia(inertia, context) + + def test_registered_robots_have_physically_valid_inertias(self): + for name in sorted(name for name in ROBOTS if name != "cassie"): + with self.subTest(robot=name): + self._check_robot(name) + + def test_cassie_has_physically_valid_inertias(self): + try: + self._check_robot("cassie") + except ImportError: + if _pinocchio_version() >= (2, 9, 1): + self.skipTest( + "Cassie requires Pinocchio SDF support in this environment." + ) + raise + + +if __name__ == "__main__": + unittest.main()