frame-collision.hpp : throw out of range error

tests/python/test_frames.py : ask for the corresponding error type

Author: ManifoldFR

include/aligator/modelling/multibody/frame-collision.hpp

@@ -23,7 +23,6 @@ struct FrameCollisionResidualTpl : UnaryFunctionTpl<_Scalar>, frame_api {
   ALIGATOR_UNARY_FUNCTION_INTERFACE(Scalar);
   using BaseData = typename Base::Data;
   using Model = pinocchio::ModelTpl<Scalar>;
-  using ManifoldPtr = xyz::polymorphic<ManifoldAbstractTpl<Scalar>>;
   using SE3 = pinocchio::SE3Tpl<Scalar>;
   using Data = FrameCollisionDataTpl<Scalar>;
   using GeometryModel = pinocchio::GeometryModel;
@@ -36,6 +35,12 @@ struct FrameCollisionResidualTpl : UnaryFunctionTpl<_Scalar>, frame_api {
                             const pinocchio::PairIndex frame_pair_id)
       : Base(ndx, nu, 1), pin_model_(model), geom_model_(geom_model),
         frame_pair_id_(frame_pair_id) {
+    if (frame_pair_id >= geom_model_.collisionPairs.size()) {
+      ALIGATOR_OUT_OF_RANGE_ERROR(
+          "Provided collision pair index {:d} is not valid "
+          "(geom model has {:d} pairs).",
+          frame_pair_id, geom_model.collisionPairs.size());
+    }
     frame_id1_ =
         geom_model
             .geometryObjects[geom_model.collisionPairs[frame_pair_id_].first]

tests/python/test_frames.py

@@ -4,10 +4,11 @@
 """
 
 import aligator
+from aligator import manifolds
+
 import numpy as np
 import pinocchio as pin
-
-from aligator import manifolds
+import pytest
 
 
 model = pin.buildSampleModelHumanoid()
@@ -274,11 +275,11 @@ def test_frame_collision_no_collision_pairs():
     x0 = space.integrate(x0, d)
 
     geometry = pin.buildSampleGeometryModelHumanoid(model)
-    aligator.FrameCollisionResidual(ndx, nu, model, geometry, 0)
+    with pytest.raises(IndexError, match="Provided collision pair index"):
+        aligator.FrameCollisionResidual(ndx, nu, model, geometry, 0)
 
 
 if __name__ == "__main__":
     import sys
-    import pytest
 
     sys.exit(pytest.main(sys.argv))