Add basic support for Eigen matrices in Data

Plugin/src/SofaPython3/PythonFactory.cpp

@@ -19,6 +19,8 @@
 ******************************************************************************/
 
 #include <functional>
+#include <tuple>
+#include <utility>
 #include <SofaPython3/PythonFactory.h>
 #include <SofaPython3/DataHelper.h>
 
@@ -45,6 +47,13 @@ using sofa::core::objectmodel::Event;
 
 #include <sofa/defaulttype/DataTypeInfo.h>
 
+/// Declares the Data<Eigen::...> read()/getValueString() specializations.
+/// This include is required before instantiating Data<Eigen::...> (see the
+/// note in this header).
+#include <sofa/core/data/Data[Eigen].h>
+
+#include <pybind11/eigen.h>
+
 SOFAPYTHON3_BIND_ATTRIBUTE_ERROR()
 
 /// Makes an alias for the pybind11 namespace to increase readability.
@@ -181,6 +190,92 @@ py::object PythonFactory::toPython(sofa::core::objectmodel::BaseData* data)
     return py::cast(data);
 }
 
+namespace
+{
+/// Conversion helpers for the Eigen matrix types supported by
+/// sofa/core/data/Data[Eigen].h. A single dispatch table, built once at
+/// first use, maps the AbstractTypeInfo singleton of each matrix type to its
+/// conversion functions, so that no per-type if(dynamic_cast<>) chain is
+/// needed.
+
+/// Exposes the matrix as a read-only numpy view on its storage (no copy).
+/// The strides follow the Eigen storage order (column-major by default),
+/// like pybind11's own zero-copy Eigen conversion.
+template<typename MatrixType>
+py::object eigenMatrixToPython(BaseData* data)
+{
+    auto* d = static_cast<sofa::core::objectmodel::Data<MatrixType>*>(data);
+    const MatrixType& m = d->getValue();
+    using Scalar = typename MatrixType::Scalar;
+
+    /// Keep the owner of the data alive as long as the view exists.
+    auto capsule = py::capsule(new Base::SPtr(d->getOwner()),
+                               [](void* p){ delete static_cast<Base::SPtr*>(p); });
+
+    const void* ptr = (m.size() != 0) ? static_cast<const void*>(m.data()) : nullptr;
+    py::array a(py::dtype::of<Scalar>(),
+                { py::ssize_t(m.rows()), py::ssize_t(m.cols()) },
+                { py::ssize_t(sizeof(Scalar) * m.rowStride()),
+                  py::ssize_t(sizeof(Scalar) * m.colStride()) },
+                ptr, capsule);
+    a.attr("flags").attr("writeable") = false;
+    return a;
+}
+
+/// Sets the matrix from a numpy array or a (nested) sequence. pybind11's
+/// Eigen caster converts the scalar type and resizes dynamic matrices;
+/// fixed-size matrices require a matching shape.
+template<typename MatrixType>
+void eigenMatrixFromPython(BaseData* data, const py::object& o)
+{
+    auto* d = static_cast<sofa::core::objectmodel::Data<MatrixType>*>(data);
+    try
+    {
+        d->setValue(py::cast<MatrixType>(o));
+    }
+    catch(const py::cast_error& e)
+    {
+        std::stringstream s;
+        s << "Trying to set the value of the data " << d->getName()
+          << " from an incompatible python object (" << e.what() << ")";
+        throw std::runtime_error(s.str());
+    }
+}
+
+struct EigenMatrixCasters
+{
+    py::object (*toPython)(BaseData*);
+    void (*fromPython)(BaseData*, const py::object&);
+};
+
+/// The closed list of matrix types supported by sofa/core/data/Data[Eigen].h.
+/// Supporting an additional matrix type only requires adding it here.
+using EigenMatrixTypes = std::tuple<
+    Eigen::MatrixXf, Eigen::MatrixXd, Eigen::MatrixXi,
+    Eigen::Matrix2f, Eigen::Matrix2d,
+    Eigen::Matrix3f, Eigen::Matrix3d,
+    Eigen::Matrix4f, Eigen::Matrix4d>;
+
+template<typename Tuple, std::size_t... I>
+std::map<const AbstractTypeInfo*, EigenMatrixCasters>
+makeEigenMatrixCasters(std::index_sequence<I...>)
+{
+    return { { sofa::defaulttype::VirtualTypeInfo<std::tuple_element_t<I, Tuple>>::get(),
+               { &eigenMatrixToPython<std::tuple_element_t<I, Tuple>>,
+                 &eigenMatrixFromPython<std::tuple_element_t<I, Tuple>> } }... };
+}
+
+std::map<const AbstractTypeInfo*, EigenMatrixCasters>& eigenMatrixCasters()
+{
+    static auto table =
+        makeEigenMatrixCasters<EigenMatrixTypes>(
+            std::make_index_sequence<
+                std::tuple_size_v<EigenMatrixTypes>
+                >{});
+    return table;
+}
+} // anonymous namespace
+
 py::object PythonFactory::valueToPython_ro(sofa::core::objectmodel::BaseData* data)
 {
     const AbstractTypeInfo& nfo{ *(data->getValueTypeInfo()) };
@@ -197,6 +292,23 @@ py::object PythonFactory::valueToPython_ro(sofa::core::objectmodel::BaseData* da
         return std::move(a);
     }
 
+    /// Eigen matrices (see sofa/core/data/Data[Eigen].h) are exposed as
+    /// read-only numpy views on the matrix storage (no copy).
+    if(const auto it = eigenMatrixCasters().find(data->getValueTypeInfo());
+       it == eigenMatrixCasters().end())
+    {
+        /// If there is no caster registered for this AbstractTypeInfo we search if we are in the case
+        /// of a duplicated AbstractTypeInfo. If we find one we add it to the caster map.
+        for(auto& [k,v] : eigenMatrixCasters()){
+            if( *(k->type_info()) == *(nfo.type_info()) ){
+                eigenMatrixCasters()[&nfo] = v;
+                return v.toPython(data);
+            }
+        }
+    }else{
+        return it->second.toPython(data);
+    }
+
     /// If this is not the case we return the converted datas (copy)
     return convertToPython(data);
 }
@@ -294,6 +406,19 @@ void PythonFactory::fromPython(BaseData* d, const py::object& o)
     // If no we fill it with direct access.
     if(!nfo.Container())
     {
+        /// Eigen matrices (see sofa/core/data/Data[Eigen].h) can be set from a
+        /// numpy array or a (nested) sequence. Other python objects (e.g. strings)
+        /// still go through the read() fallback below.
+        if(const auto it = eigenMatrixCasters().find(d->getValueTypeInfo());
+           it != eigenMatrixCasters().end())
+        {
+            if(py::isinstance<py::array>(o) || py::isinstance<py::list>(o) || py::isinstance<py::tuple>(o))
+            {
+                it->second.fromPython(d, o);
+                return;
+            }
+        }
+
         scoped_writeonly_access guard(d);
         if(nfo.Integer()) {
             nfo.setIntegerValue(guard.ptr, 0, py::cast<int>(o));

bindings/Sofa/tests/Binding_BaseObject_MockComponent.cpp

@@ -23,19 +23,31 @@
 
 
 Binding_BaseObject_MockComponent::Binding_BaseObject_MockComponent():
-d_test(initData(&d_test, std::string(""), "test", "This data should have the value of the last called method in the test."))
+d_test(initData(&d_test, std::string(""), "test", "This data should have the value of the last called method in the test.")),
+d_matrixXf(initData(&d_matrixXf, "matrixXf", "Dynamic 2x2 float Eigen matrix.")),
+d_matrixXd(initData(&d_matrixXd, "matrixXd", "Dynamic 2x3 double Eigen matrix.")),
+d_matrix3d(initData(&d_matrix3d, "matrix3d", "Fixed-size 3x3 double Eigen matrix.")),
+d_matrixXi(initData(&d_matrixXi, "matrixXi", "Dynamic 2x2 int Eigen matrix."))
 {
+    d_matrixXf.setValue(Eigen::MatrixXf::Constant(2, 2, 1.0f));
+    d_matrixXd.setValue(Eigen::MatrixXd::Constant(2, 3, 2.0));
+    d_matrix3d.setValue(Eigen::Matrix3d::Identity());
+    d_matrixXi.setValue(Eigen::MatrixXi::Constant(2, 2, 7));
 }
+
 Binding_BaseObject_MockComponent::~Binding_BaseObject_MockComponent()
 {
 
 }
+
 void Binding_BaseObject_MockComponent::bwdInit(){
     d_test.setValue("bwdInit");
 }
+
 void Binding_BaseObject_MockComponent::cleanup(){
     d_test.setValue("cleanup");
 }
+
 void Binding_BaseObject_MockComponent::computeBBox(const sofa::core::ExecParams* /* params */, bool /*onlyVisible*/){
     d_test.setValue("computeBBox");
 }
@@ -56,7 +68,5 @@ void Binding_BaseObject_MockComponent::reinit(){
     d_test.setValue("reinit");
 }
 
-
-
 static int Binding_BaseObject_MockComponentClass = sofa::core::RegisterObject("This component is used to test several functions of Binding_BaseObject")
         .add< Binding_BaseObject_MockComponent >();

bindings/Sofa/tests/Binding_BaseObject_MockComponent.h

@@ -21,6 +21,7 @@
 #pragma once
 
 #include <sofa/core/objectmodel/BaseObject.h>
+#include <sofa/core/data/Data[Eigen].h>
 
 
 
@@ -31,6 +32,12 @@ class Binding_BaseObject_MockComponent : public sofa::core::objectmodel::BaseObj
 SOFA_CLASS(Binding_BaseObject_MockComponent, sofa::core::objectmodel::BaseObject);
         sofa::core::objectmodel::Data <std::string> d_test;
 
+        /// Eigen matrices used to test the python bindings (see Core/EigenData.py)
+        sofa::core::objectmodel::Data<Eigen::MatrixXf> d_matrixXf;
+        sofa::core::objectmodel::Data<Eigen::MatrixXd> d_matrixXd;
+        sofa::core::objectmodel::Data<Eigen::Matrix3d> d_matrix3d;
+        sofa::core::objectmodel::Data<Eigen::MatrixXi> d_matrixXi;
+
         Binding_BaseObject_MockComponent ();
         virtual ~Binding_BaseObject_MockComponent () override;
         void bwdInit() override;

bindings/Sofa/tests/CMakeLists.txt

@@ -17,6 +17,7 @@ set(PYTHON_FILES
     ${CMAKE_CURRENT_SOURCE_DIR}/Core/BaseObject.py
     ${CMAKE_CURRENT_SOURCE_DIR}/Core/Controller.py
     ${CMAKE_CURRENT_SOURCE_DIR}/Core/DataEngine.py
+    ${CMAKE_CURRENT_SOURCE_DIR}/Core/EigenData.py
     ${CMAKE_CURRENT_SOURCE_DIR}/Core/Events.py
     ${CMAKE_CURRENT_SOURCE_DIR}/Core/ForceField.py
     ${CMAKE_CURRENT_SOURCE_DIR}/Core/Mass.py

bindings/Sofa/tests/Core/EigenData.py

@@ -0,0 +1,114 @@
+# coding: utf8
+
+import unittest
+import numpy
+import Sofa
+
+
+def create_scene(rootName="root"):
+    return Sofa.Core.Node(rootName)
+
+
+class Test(unittest.TestCase):
+
+    def test_read_dynamicFloat(self):
+        root = create_scene()
+        c = root.addObject("Binding_BaseObject_MockComponent", name="c")
+        m = c.matrixXf.value
+        self.assertIsInstance(m, numpy.ndarray)
+        self.assertEqual(m.shape, (2, 2))
+        self.assertEqual(m.dtype, numpy.dtype(numpy.float32))
+        numpy.testing.assert_array_equal(
+            m, numpy.ones((2, 2), dtype=numpy.float32))
+        self.assertFalse(m.flags.writeable)
+
+    def test_read_dynamicDouble(self):
+        root = create_scene()
+        c = root.addObject("Binding_BaseObject_MockComponent", name="c")
+        m = c.matrixXd.value
+        self.assertIsInstance(m, numpy.ndarray)
+        self.assertEqual(m.shape, (2, 3))
+        self.assertEqual(m.dtype, numpy.dtype(numpy.float64))
+        numpy.testing.assert_array_equal(m, 2.0 * numpy.ones((2, 3)))
+        self.assertFalse(m.flags.writeable)
+
+    def test_read_fixedSize(self):
+        root = create_scene()
+        c = root.addObject("Binding_BaseObject_MockComponent", name="c")
+        m = c.matrix3d.value
+        self.assertIsInstance(m, numpy.ndarray)
+        self.assertEqual(m.shape, (3, 3))
+        self.assertEqual(m.dtype, numpy.dtype(numpy.float64))
+        numpy.testing.assert_array_equal(m, numpy.identity(3))
+
+    def test_read_int(self):
+        root = create_scene()
+        c = root.addObject("Binding_BaseObject_MockComponent", name="c")
+        m = c.matrixXi.value
+        self.assertIsInstance(m, numpy.ndarray)
+        self.assertEqual(m.shape, (2, 2))
+        self.assertEqual(m.dtype, numpy.dtype(numpy.int32))
+        numpy.testing.assert_array_equal(
+            m, 7 * numpy.ones((2, 2), dtype=numpy.int32))
+
+    def test_zeroCopy(self):
+        # Two successive accesses must alias the same C++ storage:
+        # .value exposes a view, not a copy.
+        root = create_scene()
+        c = root.addObject("Binding_BaseObject_MockComponent", name="c")
+        v1 = c.matrixXd.value
+        v2 = c.matrixXd.value
+        self.assertTrue(numpy.shares_memory(v1, v2))
+
+    def test_viewIsLive(self):
+        # A view is not a snapshot: an in-place modification of the data
+        # (here through the string read() path on a fixed-size matrix)
+        # is visible through a previously obtained view.
+        root = create_scene()
+        c = root.addObject("Binding_BaseObject_MockComponent", name="c")
+        v = c.matrix3d.value
+        numpy.testing.assert_array_equal(v, numpy.identity(3))
+        c.matrix3d = "3 3 1 2 3 4 5 6 7 8 9"
+        numpy.testing.assert_array_equal(v, numpy.arange(1, 10).reshape(3, 3))
+
+    def test_write_fromArray(self):
+        root = create_scene()
+        c = root.addObject("Binding_BaseObject_MockComponent", name="c")
+        # a dynamic matrix is resized to the array shape
+        c.matrixXd = numpy.arange(6, dtype=numpy.float64).reshape(3, 2)
+        m = c.matrixXd.value
+        self.assertEqual(m.shape, (3, 2))
+        numpy.testing.assert_array_equal(m, numpy.arange(6).reshape(3, 2))
+        # scalar type conversion: float64 array into a float32 matrix
+        c.matrixXf = numpy.ones((4, 5), dtype=numpy.float64)
+        self.assertEqual(c.matrixXf.value.shape, (4, 5))
+        self.assertEqual(c.matrixXf.value.dtype, numpy.dtype(numpy.float32))
+        # int matrix from an int64 array
+        c.matrixXi = numpy.array([[1, 2], [3, 4]], dtype=numpy.int64)
+        self.assertEqual(c.matrixXi.value.dtype, numpy.dtype(numpy.int32))
+        numpy.testing.assert_array_equal(c.matrixXi.value, [[1, 2], [3, 4]])
+
+    def test_write_fromList(self):
+        root = create_scene()
+        c = root.addObject("Binding_BaseObject_MockComponent", name="c")
+        c.matrixXd = [[1.0, 2.0], [3.0, 4.0]]
+        numpy.testing.assert_array_equal(
+            c.matrixXd.value, [[1.0, 2.0], [3.0, 4.0]])
+
+    def test_write_fixedSizeShapeChecked(self):
+        root = create_scene()
+        c = root.addObject("Binding_BaseObject_MockComponent", name="c")
+        c.matrix3d = numpy.arange(9, dtype=numpy.float64).reshape(3, 3)
+        numpy.testing.assert_array_equal(
+            c.matrix3d.value, numpy.arange(9).reshape(3, 3))
+        # a wrong shape is rejected for a fixed-size matrix
+        with self.assertRaises(Exception):
+            c.matrix3d = numpy.ones((2, 2))
+
+    def test_write_fromStringStillWorks(self):
+        root = create_scene()
+        c = root.addObject("Binding_BaseObject_MockComponent", name="c")
+        c.matrixXd = "2 2 1 2 3 4"
+        m = c.matrixXd.value
+        self.assertEqual(m.shape, (2, 2))
+        numpy.testing.assert_array_equal(m, [[1.0, 2.0], [3.0, 4.0]])