Merge pull request #206 from ManifoldFR/topic/restore-pin3-python-tests

[tests/python] Restore Pinocchio 3 tests, add check for Pinocchio features from Python

Author: ManifoldFR

CHANGELOG.md

@@ -10,11 +10,16 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0
 ### Added
 
 - Templated getters `getCost<U>()` and `getDynamics<U>()` in the StageModel class, and another `getDynamics<U>()` for integrator classes, to get the concrete types ([##205](https://github.com/Simple-Robotics/aligator/pull/205))
+- python: Add helper `aligator.has_pinocchio_features()` ([#206](https://github.com/Simple-Robotics/aligator/pull/206))
 
 ### Changed
 
 - All map types are now `boost::unordered_map` ([#203](https://github.com/Simple-Robotics/aligator/pull/203))
 
+### Fixed
+
+- Restore Pinocchio 3 Python tests ([#206](https://github.com/Simple-Robotics/aligator/pull/206))
+
 ## [0.8.0] - 2024-09-18
 
 ### Added

bindings/python/src/module.cpp

@@ -81,6 +81,19 @@ BOOST_PYTHON_MODULE(MODULE_NAME) {
   bp::import("warnings");
   bp::import("proxsuite_nlp");
 
+  bp::def(
+      "has_pinocchio_features",
+      +[]() constexpr -> bool {
+        return
+#ifdef ALIGATOR_WITH_PINOCCHIO
+            true;
+#else
+            false;
+#endif
+      },
+      "Whether Aligator (and its Python bindings) were compiled with support "
+      "for Pinocchio.");
+
   exposeContainers();
   exposeGAR();
   exposeEnums();

tests/python/CMakeLists.txt

@@ -10,22 +10,20 @@ file(
 if(NOT BUILD_CROCODDYL_COMPAT)
   list(REMOVE_ITEM PYTHON_TESTS test_compat_croc.py)
 endif()
-if(NOT BUILD_WITH_PINOCCHIO_PYTHON_BINDINGS)
-  list(REMOVE_ITEM PYTHON_TESTS test_constrained_dynamics.py)
+if(NOT BUILD_WITH_PINOCCHIO_SUPPORT)
+  list(REMOVE_ITEM PYTHON_TESTS test_center_of_mass.py)
   list(REMOVE_ITEM PYTHON_TESTS test_frames.py)
+  list(REMOVE_ITEM PYTHON_TESTS test_rollout.py)
+endif()
+if(NOT PINOCCHIO_V3)
+  list(REMOVE_ITEM PYTHON_TESTS test_constrained_dynamics.py)
 endif()
-# TODO Add tests when Pinocchio 3 is released
-list(REMOVE_ITEM PYTHON_TESTS test_center_of_mass.py)
-list(REMOVE_ITEM PYTHON_TESTS test_constrained_dynamics.py)
-list(REMOVE_ITEM PYTHON_TESTS test_finite_diff.py)
-list(REMOVE_ITEM PYTHON_TESTS test_frames.py)
-list(REMOVE_ITEM PYTHON_TESTS test_rollout.py)
 make_directory(${CMAKE_CURRENT_BINARY_DIR})
 
 foreach(pyfile ${PYTHON_TESTS})
   get_filename_component(test_name ${pyfile} NAME_WE)
   string(REGEX REPLACE "^test_" "${PROJECT_NAME}-test-py-" test_name ${test_name})
-  string(REGEX REPLACE "_" "-" test_name ${test_name})
+  string(REPLACE "_" "-" test_name ${test_name})
   execute_process(
     COMMAND ${CMAKE_COMMAND} -E create_symlink ${CMAKE_CURRENT_SOURCE_DIR}/${pyfile}
             ${CMAKE_CURRENT_BINARY_DIR}/${pyfile}

tests/python/test_center_of_mass.py

@@ -12,8 +12,8 @@
 model = pin.buildSampleModelHumanoid()
 rdata: pin.Data = model.createData()
 np.random.seed(0)
-FD_EPS = 1e-8
-THRESH = 2 * FD_EPS**0.5
+EPS = 1e-7
+ATOL = 2 * EPS**0.5
 
 nq = model.nq
 nv = model.nv
@@ -55,7 +55,7 @@ def test_com_placement():
     assert J.shape == realJ.shape
     assert np.allclose(fdata.Jx[:, :nv], realJ)
 
-    fun_fd = aligator.FiniteDifferenceHelper(space, fun, FD_EPS)
+    fun_fd = aligator.FiniteDifferenceHelper(space, fun, EPS)
     fdata2 = fun_fd.createData()
     fun_fd.evaluate(x0, u0, x0, fdata2)
     assert np.allclose(fdata.value, fdata2.value)
@@ -70,7 +70,7 @@ def test_com_placement():
         fun.computeJacobians(x0, u0, x0, fdata)
         fun_fd.evaluate(x0, u0, x0, fdata2)
         fun_fd.computeJacobians(x0, u0, x0, fdata2)
-        assert np.allclose(fdata.Jx, fdata2.Jx, THRESH)
+        assert np.allclose(fdata.Jx, fdata2.Jx, ATOL)
 
 
 def test_frame_velocity():
@@ -94,7 +94,7 @@ def test_frame_velocity():
 
     fun.computeJacobians(x0, fdata)
 
-    fun_fd = aligator.FiniteDifferenceHelper(space, fun, FD_EPS)
+    fun_fd = aligator.FiniteDifferenceHelper(space, fun, EPS)
     fdata2 = fun_fd.createData()
     fun_fd.evaluate(x0, u0, x0, fdata2)
     fun_fd.computeJacobians(x0, u0, x0, fdata2)
@@ -110,7 +110,7 @@ def test_frame_velocity():
         print(fdata.Jx)
         print(fdata2.Jx)
         print(fdata.Jx)
-        assert np.allclose(fdata.Jx, fdata2.Jx, THRESH)
+        assert np.allclose(fdata.Jx, fdata2.Jx, ATOL)
 
 
 if __name__ == "__main__":

tests/python/test_continuous_dynamics.py

@@ -6,12 +6,13 @@
 import numpy as np
 import aligator
 from aligator import dynamics, manifolds
-from pinocchio import Quaternion
+from eigenpy import Quaternion
 from utils import finite_diff, infNorm, create_multibody_ode
 
 epsilon = 1e-6
 aligator.seed(42)
 np.random.seed(42)
+HAS_PINOCCHIO = aligator.has_pinocchio_features()
 
 
 class MyODE(dynamics.ODEAbstract):
@@ -71,6 +72,9 @@ def test_custom_ode():
     assert len(xs) == 11
 
 
+@pytest.mark.skipif(
+    not HAS_PINOCCHIO, reason="Aligator was compiled without Pinocchio."
+)
 def test_multibody_free():
     ode = create_multibody_ode(True)
     if ode is None:
@@ -304,6 +308,9 @@ def test_continuous_centroidal_diff():
     assert np.allclose(Judiff, Ju0, epsilon), "err={}".format(infNorm(Judiff - Ju0))
 
 
+@pytest.mark.skipif(
+    not HAS_PINOCCHIO, reason="Aligator was compiled without Pinocchio."
+)
 def test_kinodynamics():
     import pinocchio as pin
 
@@ -337,6 +344,9 @@ def test_kinodynamics():
     ode.dForward(x0, u0, data)
 
 
+@pytest.mark.skipif(
+    not HAS_PINOCCHIO, reason="Aligator was compiled without Pinocchio."
+)
 def test_kinodynamics_diff():
     import pinocchio as pin
 

tests/python/test_finite_diff.py

@@ -5,8 +5,19 @@
     ControlBoxFunction,
     StateErrorResidual,
     manifolds,
+    has_pinocchio_features,
 )
 import numpy as np
+import pytest
+
+# Attempt importing Pinocchio at least. Then, check if the Python module
+# has Pinocchio features.
+try:
+    import pinocchio as pin
+
+    HAS_PINOCCHIO = has_pinocchio_features()
+except ImportError:
+    HAS_PINOCCHIO = False
 
 
 def test_compute_jac_vs():
@@ -31,32 +42,30 @@ def test_compute_jac_vs():
         assert np.allclose(fdata1.Jy, fdata1b.Jy, 1e-2)
 
 
+@pytest.mark.skipif(
+    not HAS_PINOCCHIO, reason="Aligator was compiled without Pinocchio."
+)
 def test_compute_jac_multibody():
-    try:
-        import pinocchio as pin
-
-        model = pin.buildSampleModelHumanoid()
-        space = manifolds.MultibodyConfiguration(model)
-        nu = 3
-        x_tar = space.neutral()
-        fun2 = StateErrorResidual(space, nu, x_tar)
-        fdata2 = fun2.createData()
-        fun2_fd = FiniteDifferenceHelper(space, fun2, 1e-6)
-        fdata2b = fun2_fd.createData()
-        for i in range(1000):
-            x0 = pin.randomConfiguration(model, -np.ones(model.nq), np.ones(model.nq))
-            u0 = 0.6 * np.ones(nu)
-            fun2.evaluate(x0, u0, x0, fdata2)
-            fun2_fd.evaluate(x0, u0, x0, fdata2b)
-            assert np.allclose(fdata2.value, fdata2b.value, 1e-2)
-            fun2.computeJacobians(x0, u0, x0, fdata2)
-            fun2_fd.computeJacobians(x0, u0, x0, fdata2b)
-            assert np.allclose(fdata2.Jx, fdata2b.Jx, 1e-2)
-            assert np.allclose(fdata2.Ju, fdata2b.Ju, 1e-2)
-            assert np.allclose(fdata2.Jy, fdata2b.Jy, 1e-2)
-        return
-    except ImportError:
-        pass
+    model = pin.buildSampleModelHumanoid()
+    space = manifolds.MultibodyConfiguration(model)
+    nu = 3
+    x_tar = space.neutral()
+    fun2 = StateErrorResidual(space, nu, x_tar)
+    fdata2 = fun2.createData()
+    fun2_fd = FiniteDifferenceHelper(space, fun2, 1e-6)
+    fdata2b = fun2_fd.createData()
+    for i in range(1000):
+        x0 = pin.randomConfiguration(model, -np.ones(model.nq), np.ones(model.nq))
+        u0 = 0.6 * np.ones(nu)
+        fun2.evaluate(x0, u0, x0, fdata2)
+        fun2_fd.evaluate(x0, u0, x0, fdata2b)
+        assert np.allclose(fdata2.value, fdata2b.value, 1e-2)
+        fun2.computeJacobians(x0, u0, x0, fdata2)
+        fun2_fd.computeJacobians(x0, u0, x0, fdata2b)
+        assert np.allclose(fdata2.Jx, fdata2b.Jx, 1e-2)
+        assert np.allclose(fdata2.Ju, fdata2b.Ju, 1e-2)
+        assert np.allclose(fdata2.Jy, fdata2b.Jy, 1e-2)
+    return
 
 
 def test_compute_cost_se3():
@@ -80,7 +89,6 @@ def test_compute_cost_se3():
 
 
 if __name__ == "__main__":
-    import pytest
     import sys
 
     sys.exit(pytest.main(sys.argv))

tests/python/test_frames.py

@@ -1,5 +1,6 @@
 """
-Test function related to frames.
+Test function related to Pinocchio frames.
+This test is not added to CTest when CMake does not detect Pinocchio.
 """
 
 import aligator
@@ -165,7 +166,7 @@ def test_fly_high():
     fr_name1 = "larm_shoulder2_body"
     fr_id1 = model.getFrameId(fr_name1)
     space = manifolds.MultibodyPhaseSpace(model)
-    fun = aligator.FlyHighResidual(space, fr_id1, 0.1, nu)
+    fun = aligator.FlyHighResidual(space.ndx, model, fr_id1, 0.1, nu)
     data = fun.createData()
     data2 = fun.createData()
     Jx_nd = data.Jx.copy()
@@ -194,4 +195,3 @@ def test_fly_high():
     import pytest
 
     sys.exit(pytest.main(sys.argv))
-    # test_frame_placement()