From 8a40642ba9e39f8023e4dbf1cd7f77e559a8a16c Mon Sep 17 00:00:00 2001
From: Giulio Romualdi <giulio.romualdi@gmail.com>
Date: Sat, 6 Nov 2021 14:49:16 +0100
Subject: [PATCH 01/28] Implement raw buffer constructor for the Position class

---
 src/core/include/iDynTree/Core/Position.h | 5 +++++
 src/core/src/Position.cpp                 | 5 +++++
 2 files changed, 10 insertions(+)

diff --git a/src/core/include/iDynTree/Core/Position.h b/src/core/include/iDynTree/Core/Position.h
index 0906e5daed7..1233e4ac51e 100644
--- a/src/core/include/iDynTree/Core/Position.h
+++ b/src/core/include/iDynTree/Core/Position.h
@@ -67,6 +67,11 @@ namespace iDynTree
          */
         Position(iDynTree::Span<const double> other);
 
+        /**
+         * Constructor from a raw buffer of 3 doubles.
+         */
+        Position(const double* in_data, const unsigned int in_size);
+
         /**
          * Geometric operations
          */
diff --git a/src/core/src/Position.cpp b/src/core/src/Position.cpp
index e0bf0511ec3..35db5b9c44a 100644
--- a/src/core/src/Position.cpp
+++ b/src/core/src/Position.cpp
@@ -73,6 +73,11 @@ namespace iDynTree
     {
     }
 
+    Position::Position(const double* in_data, const unsigned int in_size)
+        : PositionRaw(in_data, in_size)
+    {
+    }
+
     const Position& Position::changePoint(const Position& newPoint)
     {
         this->PositionRaw::changePoint(newPoint);

From f2f6237c3cd4417dabac29d3d42654080d8791e0 Mon Sep 17 00:00:00 2001
From: Giulio Romualdi <giulio.romualdi@gmail.com>
Date: Sat, 6 Nov 2021 14:50:26 +0100
Subject: [PATCH 02/28] Implement pybind11 type_caster for iDynTree core types

---
 bindings/pybind11/CMakeLists.txt         |   3 +-
 bindings/pybind11/idyntree_type_caster.h | 224 +++++++++++++++++++++++
 2 files changed, 226 insertions(+), 1 deletion(-)
 create mode 100644 bindings/pybind11/idyntree_type_caster.h

diff --git a/bindings/pybind11/CMakeLists.txt b/bindings/pybind11/CMakeLists.txt
index 898021d2364..ba500d7dec7 100644
--- a/bindings/pybind11/CMakeLists.txt
+++ b/bindings/pybind11/CMakeLists.txt
@@ -4,7 +4,8 @@ pybind11_add_module(pybind11_idyntree SYSTEM idyntree.cpp
                                     error_utilities.h error_utilities.cpp
                                     idyntree_model.h idyntree_model.cpp
                                     idyntree_sensors.h idyntree_sensors.cpp
-                                    idyntree_modelio_urdf.h idyntree_modelio_urdf.cpp)
+                                    idyntree_modelio_urdf.h idyntree_modelio_urdf.cpp
+                                    idyntree_type_caster.h)
 
 target_link_libraries(pybind11_idyntree PUBLIC idyntree-core
                                                idyntree-model
diff --git a/bindings/pybind11/idyntree_type_caster.h b/bindings/pybind11/idyntree_type_caster.h
new file mode 100644
index 00000000000..5be9cb1330a
--- /dev/null
+++ b/bindings/pybind11/idyntree_type_caster.h
@@ -0,0 +1,224 @@
+#ifndef IDYNTREE_PYBIND11_IDYNTREE_TYPE_CASTER_H
+#define IDYNTREE_PYBIND11_IDYNTREE_TYPE_CASTER_H
+
+#include <iostream>
+#include <string>
+#include <type_traits>
+
+#include <iDynTree/Core/MatrixDynSize.h>
+#include <iDynTree/Core/MatrixFixSize.h>
+#include <iDynTree/Core/Rotation.h>
+#include <iDynTree/Core/VectorDynSize.h>
+#include <iDynTree/Core/VectorFixSize.h>
+
+#include <pybind11/numpy.h>
+#include <pybind11/pybind11.h>
+
+namespace pybind11
+{
+namespace detail
+{
+template <template <unsigned int...> class base, typename derived>
+struct is_base_of_template_int_impl
+{
+    template <unsigned int... Ts> static constexpr std::true_type test(const base<Ts...>*);
+    static constexpr std::false_type test(...);
+    using type = decltype(test(std::declval<derived*>()));
+};
+
+template <template <unsigned int...> class base, typename derived>
+using is_base_of_template_int = typename is_base_of_template_int_impl<base, derived>::type;
+
+template <typename Type>
+using is_idyntree_vector_fix_size = is_base_of_template_int<iDynTree::VectorFixSize, Type>;
+template <typename Type>
+using is_idyntree_matrix_fix_size = is_base_of_template_int<iDynTree::MatrixFixSize, Type>;
+
+// Type caster for all the classes that inherits from VectorDynSize
+template <typename Type>
+struct type_caster<Type, enable_if_t<std::is_base_of<iDynTree::VectorDynSize, Type>::value>>
+{
+    PYBIND11_TYPE_CASTER(Type, _(PYBIND11_TOSTRING(Type)));
+
+    /**
+     * Conversion from Python to C++
+     */
+    bool load(handle src, bool convert)
+    {
+        if (!convert && array_t<double>::check_(src))
+        {
+            return false;
+        }
+
+        auto buf
+            = pybind11::array_t<double,
+                                pybind11::array::c_style | pybind11::array::forcecast>::ensure(src);
+        if (!buf)
+            return false;
+
+        // the number of dimension must be equal to 1 since it is a vector
+        const auto dims = buf.ndim();
+        if (dims != 1)
+            return false;
+
+        // store the data
+        value = Type(buf.data(), buf.size());
+        return true;
+    }
+
+    /**
+     * Conversion from C++ to Python
+     */
+    static handle cast(Type src, return_value_policy policy, handle parent)
+    {
+        pybind11::array a(src.size(), src.data());
+        return a.release();
+    }
+};
+
+// Type caster for all the classes that inherits from MatriDynSize
+template <typename Type>
+struct type_caster<Type, enable_if_t<std::is_base_of<iDynTree::MatrixDynSize, Type>::value>>
+{
+    PYBIND11_TYPE_CASTER(Type, _(PYBIND11_TOSTRING(Type)));
+
+    /**
+     * Conversion from Python to C++
+     */
+    bool load(handle src, bool convert)
+    {
+        if (!convert && array_t<double>::check_(src))
+        {
+            return false;
+        }
+
+        auto buf
+            = pybind11::array_t<double,
+                                pybind11::array::c_style | pybind11::array::forcecast>::ensure(src);
+        if (!buf)
+            return false;
+
+        // Since it is a matrix the buffer must have 2 dimensions
+        const auto dims = buf.ndim();
+        if (dims != 2)
+            return false;
+
+        // this can be done since numpy and iDynTree store matrices in Row-Major order
+        const auto rows = buf.shape(0);
+        const auto cols = buf.shape(1);
+        value = Type(buf.data(), rows, cols);
+
+        return true;
+    }
+
+    static handle cast(Type src, return_value_policy policy, handle parent)
+    {
+        // this can be done since numpy and iDynTree store matrices in Row-Major order
+        pybind11::array a({src.rows(), src.cols()}, src.data());
+        return a.release();
+    }
+};
+
+// Type caster for all the classes that inherits from VectorFixSize
+template <typename Type>
+struct type_caster<Type, enable_if_t<is_idyntree_vector_fix_size<Type>::value>>
+{
+    PYBIND11_TYPE_CASTER(Type, _(PYBIND11_TOSTRING(Type)));
+
+    /**
+     * Conversion from Python to C++
+     */
+    bool load(handle src, bool convert)
+    {
+        if (!convert && array_t<double>::check_(src))
+        {
+            return false;
+        }
+
+        auto buf
+            = pybind11::array_t<double,
+                                pybind11::array::c_style | pybind11::array::forcecast>::ensure(src);
+        if (!buf)
+            return false;
+
+        // It must be a vector
+        const auto dims = buf.ndim();
+        if (dims != 1)
+            return false;
+
+        // since the vector is fixed size it is not possible to resize it. This check if the size is
+        // correct
+        if (buf.size() != value.size())
+            return false;
+
+        value = Type(buf.data(), buf.size());
+
+        return true;
+    }
+
+    /**
+     * Conversion from C++ to Python
+     */
+    static handle cast(Type src, return_value_policy policy, handle parent)
+    {
+        pybind11::array a(src.size(), src.data());
+        return a.release();
+    }
+};
+
+// Type caster for all the classes that inherits from MatrixFixSize but that are not rotations
+template <typename Type>
+struct type_caster<Type,
+                   enable_if_t<is_idyntree_matrix_fix_size<Type>::value
+                               && !std::is_same<iDynTree::Rotation, Type>::value>>
+{
+    PYBIND11_TYPE_CASTER(Type, _(PYBIND11_TOSTRING(Type)));
+
+    /**
+     * Conversion from Python to C++
+     */
+    bool load(handle src, bool convert)
+    {
+        if (!convert && array_t<double>::check_(src))
+        {
+            return false;
+        }
+
+        auto buf
+            = pybind11::array_t<double,
+                                pybind11::array::c_style | pybind11::array::forcecast>::ensure(src);
+        if (!buf)
+            return false;
+
+        // since it is a matrix the number of dimension must be equal to 2
+        auto dims = buf.ndim();
+        if (dims != 2)
+            return false;
+
+        // the matrix is fix size so it is not possible to change it's size
+        if (buf.size() != value.rows() * value.cols())
+            return false;
+
+        // this can be done since numpy and iDynTree store matrices in Row-Major order
+        const auto rows = buf.shape(0);
+        const auto cols = buf.shape(1);
+        value = Type(buf.data(), rows, cols);
+
+        return true;
+    }
+
+    /**
+     * Conversion from C++ to Python
+     */
+    static handle cast(Type src, return_value_policy policy, handle parent)
+    {
+        // this can be done since numpy and iDynTree store matrices in Row-Major order
+        pybind11::array a({src.rows(), src.cols()}, src.data());
+        return a.release();
+    }
+};
+
+} // namespace detail
+} // namespace pybind11
+
+#endif /* end of include guard: IDYNTREE_PYBIND11_IDYNTREE_TYPE_CASTER_H */

From c46a4bf4188161bca91cb056d6c330ff8e2255b8 Mon Sep 17 00:00:00 2001
From: Giulio Romualdi <giulio.romualdi@gmail.com>
Date: Sat, 6 Nov 2021 14:53:28 +0100
Subject: [PATCH 03/28] Use pybidn11 typecaster in the already existing
 bindings

---
 bindings/pybind11/idyntree_core.cpp         | 270 +++++---------------
 bindings/pybind11/idyntree_model.cpp        |   1 +
 bindings/pybind11/idyntree_modelio_urdf.cpp |   2 +-
 bindings/pybind11/idyntree_sensors.cpp      |   1 +
 4 files changed, 61 insertions(+), 213 deletions(-)

diff --git a/bindings/pybind11/idyntree_core.cpp b/bindings/pybind11/idyntree_core.cpp
index ba4bf16e9d8..76b0aea65be 100644
--- a/bindings/pybind11/idyntree_core.cpp
+++ b/bindings/pybind11/idyntree_core.cpp
@@ -1,4 +1,5 @@
 #include "idyntree_core.h"
+#include "idyntree_type_caster.h"
 
 #include <iDynTree/Core/Axis.h>
 #include <iDynTree/Core/Direction.h>
@@ -24,136 +25,6 @@ namespace bindings {
 namespace {
 namespace py = ::pybind11;
 
-void vectorDynSizeClassDefinition(py::class_<VectorDynSize>& vector) {
-  vector.def(py::init())
-      .def(py::init<unsigned>())
-      .def("__getitem__", py::overload_cast<const std::size_t>(
-                              &VectorDynSize::operator(), py::const_))
-      .def("__setitem__",
-           [](VectorDynSize& the_vector, std::size_t index, double new_value) {
-             the_vector(index) = new_value;
-           })
-      .def(
-          "__iter__",
-          [](const VectorDynSize& s) {
-            return py::make_iterator(s.begin(), s.end());
-          },
-          py::keep_alive<
-              0, 1>() /* Essential: keep object alive while iterator exists */)
-      .def("__len__", &VectorDynSize::size)
-      .def("set_zero", &VectorDynSize::zero)
-      .def("resize", &VectorDynSize::resize)
-      .def("__repr__", &VectorDynSize::toString)
-      .def_buffer([](VectorDynSize& v) -> py::buffer_info {
-        return py::buffer_info(
-            v.data(),                                /* Pointer to buffer */
-            sizeof(double),                          /* Size of one scalar */
-            py::format_descriptor<double>::format(), /* Python struct-style
-                                                        format descriptor */
-            1,                                       /* Number of dimensions */
-            {v.size()},                              /* Buffer dimensions */
-            {sizeof(double)} /* Strides (in bytes) for each index */
-        );
-      });
-}
-
-template <unsigned size>
-void createFixSizeVector(pybind11::module& module,
-                         const std::string& class_name) {
-  py::class_<VectorFixSize<size>>(module, class_name.c_str(),
-                                  py::buffer_protocol())
-      .def(py::init())
-      .def("__getitem__", py::overload_cast<const std::size_t>(
-                              &VectorFixSize<size>::operator(), py::const_))
-      .def("__setitem__",
-           [](VectorFixSize<size>& the_vector, std::size_t index,
-              double new_value) { the_vector(index) = new_value; })
-      .def("__len__", &VectorFixSize<size>::size)
-      .def(
-          "__iter__",
-          [](const VectorFixSize<size>& s) {
-            return py::make_iterator(s.begin(), s.end());
-          },
-          py::keep_alive<
-              0, 1>() /* Essential: keep object alive while iterator exists */)
-      .def("set_zero", &VectorFixSize<size>::zero)
-      .def("__repr__", &VectorFixSize<size>::toString)
-      .def_buffer([](VectorFixSize<size>& v) -> py::buffer_info {
-        return py::buffer_info(
-            v.data(),                                /* Pointer to buffer */
-            sizeof(double),                          /* Size of one scalar */
-            py::format_descriptor<double>::format(), /* Python struct-style
-                                                        format descriptor */
-            1,                                       /* Number of dimensions */
-            {v.size()},                              /* Buffer dimensions */
-            {sizeof(double)} /* Strides (in bytes) for each index */
-        );
-      });
-}
-
-void matrixDynSizeClassDefinition(py::class_<MatrixDynSize>& matrix) {
-  matrix.def(py::init())
-      .def(py::init<unsigned, unsigned>())
-      .def("__getitem__",
-           [](MatrixDynSize& matrix,
-              std::pair<std::size_t, std::size_t> indices) {
-             return matrix.getVal(indices.first, indices.second);
-           })
-      .def("__setitem__",
-           [](MatrixDynSize& matrix,
-              std::pair<std::size_t, std::size_t> indices, double item) {
-             return matrix.setVal(indices.first, indices.second, item);
-           })
-      .def("rows", &MatrixDynSize::rows)
-      .def("cols", &MatrixDynSize::cols)
-      .def("set_zero", &MatrixDynSize::zero)
-      .def("resize", &MatrixDynSize::resize)
-      .def("__repr__", &MatrixDynSize::toString)
-      .def_buffer([](MatrixDynSize& m) -> py::buffer_info {
-        return py::buffer_info(
-            m.data(),                                /* Pointer to buffer */
-            sizeof(double),                          /* Size of one scalar */
-            py::format_descriptor<double>::format(), /* Python struct-style
-                                                        format descriptor */
-            2,                                       /* Number of dimensions */
-            {m.rows(), m.cols()},                    /* Buffer dimensions */
-            {sizeof(double) * m.cols(), /* Strides (in bytes) for each index */
-             sizeof(double)});
-      });
-}
-
-template <unsigned rows, unsigned cols>
-void createFixSizeMatrix(pybind11::module& module,
-                         const std::string& class_name) {
-  py::class_<MatrixFixSize<rows, cols>>(module, class_name.c_str(),
-                                        py::buffer_protocol())
-      .def(py::init())
-      .def("__getitem__",
-           [](MatrixFixSize<rows, cols>& matrix,
-              std::pair<std::size_t, std::size_t> indices) {
-             return matrix.getVal(indices.first, indices.second);
-           })
-      .def("__setitem__",
-           [](MatrixFixSize<rows, cols>& matrix,
-              std::pair<std::size_t, std::size_t> indices, double item) {
-             return matrix.setVal(indices.first, indices.second, item);
-           })
-      .def("rows", &MatrixFixSize<rows, cols>::rows)
-      .def("cols", &MatrixFixSize<rows, cols>::cols)
-      .def("set_zero", &MatrixFixSize<rows, cols>::zero)
-      .def("__repr__", &MatrixFixSize<rows, cols>::toString)
-      .def_buffer([](MatrixFixSize<rows, cols>& m) -> py::buffer_info {
-        return py::buffer_info(
-            m.data(),                                /* Pointer to buffer */
-            sizeof(double),                          /* Size of one scalar */
-            py::format_descriptor<double>::format(), /* Python struct-style
-                                                        format descriptor */
-            2,                                       /* Number of dimensions */
-            {m.rows(), m.cols()},                    /* Buffer dimensions */
-            {sizeof(double) * m.cols(), /* Strides (in bytes) for each index */
-             sizeof(double)});
-      });
-}
 
 void transformClassDefinition(py::class_<Transform>& transform) {
   transform.def(py::init())
@@ -179,89 +50,64 @@ void transformClassDefinition(py::class_<Transform>& transform) {
 }  // namespace
 
 void iDynTreeCoreBindings(pybind11::module& module) {
-  // Vectors and matrices.
-  py::class_<VectorDynSize> vector_dyn(module, "VectorDynSize",
-                                       py::buffer_protocol());
-  vectorDynSizeClassDefinition(vector_dyn);
-  createFixSizeVector<3>(module, "Vector3");
-  createFixSizeVector<4>(module, "Vector4");
-  createFixSizeVector<6>(module, "Vector6");
-
-  py::class_<MatrixDynSize> matrix_dyn(module, "MatrixDynSize",
-                                       py::buffer_protocol());
-  matrixDynSizeClassDefinition(matrix_dyn);
-  createFixSizeMatrix<3, 3>(module, "Matrix3x3");
-  createFixSizeMatrix<4, 4>(module, "Matrix4x4");
-  createFixSizeMatrix<6, 6>(module, "Matrix6x6");
-
-  // Positions, Rotations and Transforms.
-  py::class_<PositionRaw, VectorFixSize<3>>(module, "_PositionRaw")
-      // Do not expose constructor as we do not want users to use this class.
-      .def("__repr__", &PositionRaw::toString);
-
-  py::class_<Position, PositionRaw>(module, "Position")
-      .def(py::init())
-      .def(py::init<double, double, double>())
-      .def(py::self + py::self)
-      .def(py::self - py::self)
-      .def(-py::self)
-      .def_static("Zero", &Position::Zero);
-
-  py::class_<RotationRaw, MatrixFixSize<3, 3>>(module, "_RotationRaw")
-      // Do not expose constructor as we do not want users to use this class.
-      .def("__repr__", &RotationRaw::toString);
-
-  py::class_<Rotation, RotationRaw>(module, "Rotation")
-      .def(py::init())
-      .def(py::init<double, double, double,  //
-                    double, double, double,  //
-                    double, double, double>())
-      .def("inverse", &Rotation::inverse)
-      .def(py::self * py::self)
-      .def(
-          "__mul__",
-          [](const Rotation& r, const Position& p) -> Position {
-            return r * p;
-          },
-          py::is_operator())
-      .def_static("Identity", &Rotation::Identity);
-
-  py::class_<Transform> transform(module, "Transform");
-  transformClassDefinition(transform);
-
-  // Other classes.
-  py::class_<Direction, VectorFixSize<3>>(module, "Direction")
-      .def(py::init<double, double, double>());
-
-  py::class_<Axis>(module, "Axis")
-      .def(py::init<const Direction&, const Position&>())
-      .def_property("direction", &Axis::getDirection, &Axis::setDirection)
-      .def_property("origin", &Axis::getOrigin, &Axis::setOrigin)
-      .def("__repr__", &Axis::toString);
-
-  py::class_<RotationalInertiaRaw, MatrixFixSize<3, 3>>(module,
-                                                        "RotationalInertia")
-      .def(py::init());
-
-  py::class_<SpatialInertiaRaw>(module, "_SpatialInertiaRaw")
-      .def("from_rotational_inertia_wrt_center_of_mass",
-           &SpatialInertiaRaw::fromRotationalInertiaWrtCenterOfMass)
-      .def("get_mass", &SpatialInertiaRaw::getMass)
-      .def("get_center_of_mass", &SpatialInertiaRaw::getCenterOfMass)
-      .def("get_rotational_inertia_wrt_frame_origin",
-           &SpatialInertiaRaw::getRotationalInertiaWrtFrameOrigin)
-      .def("get_rotational_inertia_wrt_center_of_mass",
-           &SpatialInertiaRaw::getRotationalInertiaWrtCenterOfMass);
 
-  py::class_<SpatialInertia, SpatialInertiaRaw>(module, "SpatialInertia")
-      .def(py::init())
-      .def(py::init<double, const PositionRaw&, const RotationalInertiaRaw&>())
-      .def_static("Zero", &SpatialInertia::Zero)
-      .def("as_matrix", &SpatialInertia::asMatrix)
-      .def(py::self + py::self)
-      .def("__repr__", [](const SpatialInertia& inertia) {
-        return inertia.asMatrix().toString();
-      });
+    py::class_<Rotation>(module, "Rotation")
+        .def(py::init())
+        .def(py::init<double,
+                      double,
+                      double, //
+                      double,
+                      double,
+                      double, //
+                      double,
+                      double,
+                      double>())
+        .def("inverse", &Rotation::inverse)
+        .def(py::self * py::self)
+        .def(
+            "__mul__",
+            [](const Rotation& r, const Position& p) -> Position { return r * p; },
+            py::is_operator())
+        .def_static("Identity", &Rotation::Identity)
+        .def("__repr__", &Rotation::toString)
+        .def("to_numpy",
+             [](const Rotation& impl) {
+                 iDynTree::Matrix3x3 m(impl.data(), 3, 3);
+                 return m;
+             })
+        .def("__getitem__",
+             [](const Rotation& matrix, std::pair<std::size_t, std::size_t> indices) {
+                 return matrix.getVal(indices.first, indices.second);
+             });
+
+    py::class_<Transform> transform(module, "Transform");
+    transformClassDefinition(transform);
+
+    // Other classes.
+    py::class_<Axis>(module, "Axis")
+        .def(py::init<const Direction&, const Position&>())
+        .def_property("direction", &Axis::getDirection, &Axis::setDirection)
+        .def_property("origin", &Axis::getOrigin, &Axis::setOrigin)
+        .def("__repr__", &Axis::toString);
+
+    py::class_<SpatialInertiaRaw>(module, "_SpatialInertiaRaw")
+        .def("from_rotational_inertia_wrt_center_of_mass",
+             &SpatialInertiaRaw::fromRotationalInertiaWrtCenterOfMass)
+        .def("get_mass", &SpatialInertiaRaw::getMass)
+        .def("get_center_of_mass", &SpatialInertiaRaw::getCenterOfMass)
+        .def("get_rotational_inertia_wrt_frame_origin",
+             &SpatialInertiaRaw::getRotationalInertiaWrtFrameOrigin)
+        .def("get_rotational_inertia_wrt_center_of_mass",
+             &SpatialInertiaRaw::getRotationalInertiaWrtCenterOfMass);
+
+    py::class_<SpatialInertia, SpatialInertiaRaw>(module, "SpatialInertia")
+        .def(py::init())
+        .def(py::init<double, const PositionRaw&, const RotationalInertiaRaw&>())
+        .def_static("Zero", &SpatialInertia::Zero)
+        .def("as_matrix", &SpatialInertia::asMatrix)
+        .def(py::self + py::self)
+        .def("__repr__",
+             [](const SpatialInertia& inertia) { return inertia.asMatrix().toString(); });
 }
 }  // namespace bindings
 }  // namespace iDynTree
diff --git a/bindings/pybind11/idyntree_model.cpp b/bindings/pybind11/idyntree_model.cpp
index 65d47fa0846..7ab564f428a 100644
--- a/bindings/pybind11/idyntree_model.cpp
+++ b/bindings/pybind11/idyntree_model.cpp
@@ -1,4 +1,5 @@
 #include "idyntree_model.h"
+#include "idyntree_type_caster.h"
 
 #include "error_utilities.h"
 
diff --git a/bindings/pybind11/idyntree_modelio_urdf.cpp b/bindings/pybind11/idyntree_modelio_urdf.cpp
index e5d99bd1488..84efbb110e9 100644
--- a/bindings/pybind11/idyntree_modelio_urdf.cpp
+++ b/bindings/pybind11/idyntree_modelio_urdf.cpp
@@ -1,6 +1,6 @@
 #include "idyntree_modelio_urdf.h"
-
 #include "error_utilities.h"
+#include "idyntree_type_caster.h"
 
 #include <string>
 #include <iDynTree/ModelIO/ModelExporter.h>
diff --git a/bindings/pybind11/idyntree_sensors.cpp b/bindings/pybind11/idyntree_sensors.cpp
index 2ade2842315..13abeccc9e6 100644
--- a/bindings/pybind11/idyntree_sensors.cpp
+++ b/bindings/pybind11/idyntree_sensors.cpp
@@ -1,5 +1,6 @@
 #include "idyntree_sensors.h"
 #include "error_utilities.h"
+#include "idyntree_type_caster.h"
 
 #include <pybind11/operators.h>
 #include <pybind11/pybind11.h>

From 030066e6c00b42fd108312f061cbda28b4d1c942 Mon Sep 17 00:00:00 2001
From: Giulio Romualdi <giulio.romualdi@gmail.com>
Date: Sat, 6 Nov 2021 14:54:41 +0100
Subject: [PATCH 04/28] Make rotation and position property readable and
 writable in the pybind11 Transform bindings

---
 bindings/pybind11/idyntree_core.cpp | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/bindings/pybind11/idyntree_core.cpp b/bindings/pybind11/idyntree_core.cpp
index 76b0aea65be..59a0928fdb6 100644
--- a/bindings/pybind11/idyntree_core.cpp
+++ b/bindings/pybind11/idyntree_core.cpp
@@ -31,8 +31,8 @@ void transformClassDefinition(py::class_<Transform>& transform) {
       .def(py::init<const Rotation&, const Position&>())
       .def(py::init<const MatrixFixSize<4, 4>&>())
       .def_static("Identity", &Transform::Identity)
-      .def_property_readonly("rotation", &Transform::getRotation)
-      .def_property_readonly("position", &Transform::getPosition)
+      .def_property("rotation", &Transform::getRotation, &Transform::setRotation)
+      .def_property("position", &Transform::getPosition,  &Transform::setPosition)
       .def("inverse", &Transform::inverse)
       .def(py::self * py::self)
       .def(

From c95517531690aeb842cabcdc3718fc34dc7e928f Mon Sep 17 00:00:00 2001
From: Giulio Romualdi <giulio.romualdi@gmail.com>
Date: Mon, 8 Nov 2021 16:31:52 +0100
Subject: [PATCH 05/28] Update the pybind11 tests

---
 bindings/pybind11/tests/test_idyntree_core.py | 201 ++----------------
 .../pybind11/tests/test_idyntree_model.py     |  26 +--
 .../pybind11/tests/test_idyntree_sensors.py   |   2 +-
 3 files changed, 32 insertions(+), 197 deletions(-)

diff --git a/bindings/pybind11/tests/test_idyntree_core.py b/bindings/pybind11/tests/test_idyntree_core.py
index 3e3a3238ffa..e3e4864ebac 100644
--- a/bindings/pybind11/tests/test_idyntree_core.py
+++ b/bindings/pybind11/tests/test_idyntree_core.py
@@ -6,165 +6,6 @@
 import numpy as np
 
 
-class IDynTreeDynVectorTest(unittest.TestCase):
-
-  def test_dyn_vector_accessor(self):
-    vector = iDynTree.VectorDynSize(5)
-    vector[1] = 3.14
-    self.assertEqual(3.14, vector[1])
-
-  def test_dyn_vector_size(self):
-    vector = iDynTree.VectorDynSize(5)
-    self.assertEqual(len(vector), 5)
-
-  def test_set_zero(self):
-    vector = iDynTree.VectorDynSize(2)
-    vector[0] = 10
-    vector[1] = -6
-    vector.set_zero()
-    for element in vector:
-      self.assertEqual(element, 0)
-
-  def test_buffer(self):
-    vector = iDynTree.VectorDynSize(5)
-    np_array = np.array(vector, copy=False)
-    self.assertEqual(len(np_array), len(vector))
-    np_array[0] = 3.14
-    self.assertEqual(3.14, vector[0])
-
-
-class IDynTreeFixVectorTest(unittest.TestCase):
-
-  def test_fix_vector_accessor(self):
-    vector = iDynTree.Vector3()
-    vector[1] = 3.14
-    self.assertEqual(3.14, vector[1])
-
-  def test_fix_vector_size(self):
-    vector = iDynTree.Vector4()
-    self.assertEqual(len(vector), 4)
-
-  def test_set_zero(self):
-    vector = iDynTree.Vector3()
-    vector[0] = 10
-    vector[1] = -6
-    vector[2] = 1.4
-    vector.set_zero()
-    for element in vector:
-      self.assertEqual(element, 0)
-
-  def test_buffer(self):
-    vector = iDynTree.Vector3()
-    np_array = np.array(vector, copy=False)
-    self.assertEqual(len(np_array), len(vector))
-    np_array[0] = 3.14
-    self.assertEqual(3.14, vector[0])
-
-
-class IDynTreeDynMatrixTest(unittest.TestCase):
-
-  def test_dyn_matrix_accessor(self):
-    matrix = iDynTree.MatrixDynSize(3, 3)
-    matrix[1, 1] = 3.14
-    self.assertEqual(3.14, matrix[1, 1])
-
-  def test_dyn_matrix_size(self):
-    matrix = iDynTree.MatrixDynSize(3, 4)
-    self.assertEqual(3, matrix.rows())
-    self.assertEqual(4, matrix.cols())
-
-  def test_set_zero(self):
-    matrix = iDynTree.MatrixDynSize(3, 4)
-    matrix[0, 2] = 10
-    matrix.set_zero()
-    for r in range(matrix.rows()):
-      for c in range(matrix.cols()):
-        self.assertEqual(matrix[r, c], 0)
-
-  def test_buffer(self):
-    matrix = iDynTree.MatrixDynSize(3, 4)
-    np_mat = np.array(matrix, copy=False)
-    self.assertEqual(np_mat.shape[0], matrix.rows())
-    self.assertEqual(np_mat.shape[1], matrix.cols())
-    np_mat[0, 0] = 3.14
-    self.assertEqual(3.14, matrix[0, 0])
-
-
-class IDynTreeFixMatrixTest(unittest.TestCase):
-
-  def test_fix_matrix_accessor(self):
-    matrix = iDynTree.Matrix3x3()
-    matrix[1, 1] = 3.14
-    self.assertEqual(3.14, matrix[1, 1])
-
-  def test_fix_matrix_size(self):
-    matrix = iDynTree.Matrix4x4()
-    self.assertEqual(4, matrix.rows())
-    self.assertEqual(4, matrix.cols())
-
-  def test_set_zero(self):
-    matrix = iDynTree.Matrix4x4()
-    matrix[0, 1] = 10
-    matrix.set_zero()
-    for r in range(matrix.rows()):
-      for c in range(matrix.cols()):
-        self.assertEqual(matrix[r, c], 0)
-
-  def test_buffer(self):
-    matrix = iDynTree.Matrix4x4()
-    np_mat = np.array(matrix, copy=False)
-    self.assertEqual(np_mat.shape[0], matrix.rows())
-    self.assertEqual(np_mat.shape[1], matrix.cols())
-    np_mat[0, 0] = 3.14
-    self.assertEqual(3.14, matrix[0, 0])
-
-
-class IDynTreePositionTest(unittest.TestCase):
-
-  def test_zero_position(self):
-    pos = iDynTree.Position.Zero()
-    self.assertEqual(len(pos), 3)
-    for element in pos:
-      self.assertEqual(element, 0)
-
-  def test_non_zero_creation(self):
-    pos = iDynTree.Position(1, 2, 3)
-    self.assertEqual(pos[0], 1)
-    self.assertEqual(pos[1], 2)
-    self.assertEqual(pos[2], 3)
-
-  def test_negate_operator(self):
-    pos = iDynTree.Position(1, 2, 3)
-    neg_pos = -pos
-    self.assertEqual(neg_pos[0], -1)
-    self.assertEqual(neg_pos[1], -2)
-    self.assertEqual(neg_pos[2], -3)
-    # Original object is unmodified.
-    self.assertEqual(tuple(pos), (1, 2, 3))
-
-  def test_add_operation(self):
-    pos1 = iDynTree.Position(1, 2, 3)
-    pos2 = iDynTree.Position(4, 5, 6)
-    sum_pos = pos1 + pos2
-    self.assertEqual(sum_pos[0], 5)
-    self.assertEqual(sum_pos[1], 7)
-    self.assertEqual(sum_pos[2], 9)
-    # Original objects are unmodified.
-    self.assertEqual(tuple(pos1), (1, 2, 3))
-    self.assertEqual(tuple(pos2), (4, 5, 6))
-
-  def test_subtract_operation(self):
-    pos1 = iDynTree.Position(1, 2, 3)
-    pos2 = iDynTree.Position(4, 5, 6)
-    sum_pos = pos1 - pos2
-    self.assertEqual(sum_pos[0], -3)
-    self.assertEqual(sum_pos[1], -3)
-    self.assertEqual(sum_pos[2], -3)
-    # Original objects are unmodified.
-    self.assertEqual(tuple(pos1), (1, 2, 3))
-    self.assertEqual(tuple(pos2), (4, 5, 6))
-
-
 class IDynTreeRotationTest(unittest.TestCase):
 
   def test_create_identity(self):
@@ -214,9 +55,9 @@ def test_position_rotation(self):
     rotation = iDynTree.Rotation(0, 0, 1,
                                  1, 0, 0,
                                  0, 1, 0)
-    pos = iDynTree.Position(0, 0, 1)
+    pos =[0, 0, 1]
     pos_rotated = rotation * pos
-    expected_pos = iDynTree.Position(1, 0, 0)
+    expected_pos = [1, 0, 0]
     for i in range(3):
       self.assertAlmostEqual(pos_rotated[i], expected_pos[i])
 
@@ -224,7 +65,7 @@ def test_position_rotation(self):
 class IDynTreeTransformTest(unittest.TestCase):
 
   def test_creation(self):
-    position = iDynTree.Position(1, 2, 3)
+    position =[1, 2, 3]
     rotation = iDynTree.Rotation(0, 0, 1,
                                  1, 0, 0,
                                  0, 1, 0)
@@ -247,24 +88,24 @@ def test_identity(self):
         self.assertEqual(transform.rotation[r, c], 0)
 
   def test_position_transform(self):
-    position = iDynTree.Position(1, 2, 3)
+    position = [1, 2, 3]
     rotation = iDynTree.Rotation(0, 0, 1,
                                  1, 0, 0,
                                  0, 1, 0)
     transform = iDynTree.Transform(rotation, position)
-    pos = iDynTree.Position(0, 0, 1)
+    pos = [0, 0, 1]
     pos_transformed = transform * pos
-    expected_pos = iDynTree.Position(2, 2, 3)
+    expected_pos = [2, 2, 3]
     for i in range(3):
       self.assertAlmostEqual(pos_transformed[i], expected_pos[i])
 
   def test_transform_composition(self):
-    position = iDynTree.Position(1, 2, 3)
+    position = [1, 2, 3]
     rotation = iDynTree.Rotation(0, 0, 1,
                                  1, 0, 0,
                                  0, 1, 0)
     t1 = iDynTree.Transform(rotation, position)
-    position = iDynTree.Position(3, 4, 5)
+    position = [3, 4, 5]
     rotation = iDynTree.Rotation(0, -1,  0,
                                  0,  0, -1,
                                  1,  0,  0)
@@ -284,7 +125,7 @@ def test_creation(self):
     n_factor = np.linalg.norm([1, 2, 3])
     raw_dir = [1 / (n_factor), 2 / n_factor, 3 / n_factor]
 
-    direction = iDynTree.Direction(raw_dir[0], raw_dir[1], raw_dir[2])
+    direction = raw_dir
     self.assertEqual(list(direction), raw_dir)
 
 
@@ -293,8 +134,8 @@ class IDynTreeAxisTest(unittest.TestCase):
   def test_read_origin_and_direction_properties(self):
     n_factor = np.linalg.norm([1, 2, 3])
     raw_dir = [1 / (n_factor), 2 / n_factor, 3 / n_factor]
-    direction = iDynTree.Direction(raw_dir[0], raw_dir[1], raw_dir[2])
-    origin = iDynTree.Position(3, 4, 5)
+    direction = raw_dir
+    origin = [3, 4, 5]
     axis = iDynTree.Axis(direction, origin)
     self.assertEqual(list(axis.direction), raw_dir)
     self.assertEqual(list(axis.origin), [3, 4, 5])
@@ -304,8 +145,8 @@ class IDynTreeSpatialInertiaTest(unittest.TestCase):
 
   def test_creation(self):
     mass = 3.14
-    position = iDynTree.Position(1, 2, 3)
-    rotational_inertia = iDynTree.RotationalInertia()
+    position = [1, 2, 3]
+    rotational_inertia = np.zeros((3, 3))
     rotational_inertia[1, 1] = 1
 
     inertia = iDynTree.SpatialInertia(mass, position, rotational_inertia)
@@ -340,15 +181,15 @@ def r_vector_item(vector, row, col):
 
   def test_sum(self):
     mass = 3.14
-    position = iDynTree.Position(1, 2, 3)
-    rotational_inertia = iDynTree.RotationalInertia()
+    position = [1, 2, 3]
+    rotational_inertia = np.zeros((3, 3))
     rotational_inertia[1, 1] = 1
 
     inertia1 = iDynTree.SpatialInertia(mass, position, rotational_inertia)
 
     mass = 6.28
-    position = iDynTree.Position(4, 5, 6)
-    rotational_inertia = iDynTree.RotationalInertia()
+    position = [4, 5, 6]
+    rotational_inertia = np.zeros((3, 3))
     rotational_inertia[2, 2] = 1
 
     inertia2 = iDynTree.SpatialInertia(mass, position, rotational_inertia)
@@ -376,15 +217,15 @@ def test_sum_assignment(self):
     # We do not explicitly define this operation.
     # Check if Python does the correct thing by using the elementary + operator.
     mass = 3.14
-    position = iDynTree.Position(1, 2, 3)
-    rotational_inertia = iDynTree.RotationalInertia()
+    position = [1, 2, 3]
+    rotational_inertia = np.zeros((3, 3))
     rotational_inertia[1, 1] = 1
 
     inertia1 = iDynTree.SpatialInertia(mass, position, rotational_inertia)
 
     mass = 6.28
-    position = iDynTree.Position(4, 5, 6)
-    rotational_inertia = iDynTree.RotationalInertia()
+    position = [4, 5, 6]
+    rotational_inertia = np.zeros((3, 3))
     rotational_inertia[2, 2] = 1
 
     inertia_sum = iDynTree.SpatialInertia(mass, position, rotational_inertia)
diff --git a/bindings/pybind11/tests/test_idyntree_model.py b/bindings/pybind11/tests/test_idyntree_model.py
index 951cb317fc1..761d8ed95df 100644
--- a/bindings/pybind11/tests/test_idyntree_model.py
+++ b/bindings/pybind11/tests/test_idyntree_model.py
@@ -3,6 +3,7 @@
 import unittest
 
 import idyntree.pybind as iDynTree
+import numpy as np
 
 
 class IDynTreeMaterialTest(unittest.TestCase):
@@ -16,11 +17,7 @@ def test_creation_with_name(self):
 
   def test_color(self):
     material = iDynTree.Material()
-    color = iDynTree.Vector4()
-    color[0] = 1.0
-    color[1] = 0.0
-    color[2] = 0.0
-    color[3] = 1.0
+    color = [1, 0, 0, 1]
     material.color = color
     self.assertEqual(list(material.color), list(color))
 
@@ -38,7 +35,7 @@ def test_name(self):
     self.assertEqual(mesh.name, "a_name")
 
   def test_transform(self):
-    position = iDynTree.Position(1, 2, 3)
+    position = [1, 2, 3]
     rotation = iDynTree.Rotation(0, 0, 1,
                                  1, 0, 0,
                                  0, 1, 0)
@@ -70,10 +67,7 @@ def test_mesh_filename(self):
 
   def test_mesh_scale(self):
     mesh = iDynTree.ExternalMesh()
-    scale = iDynTree.Vector3()
-    scale[0] = 1
-    scale[1] = 2
-    scale[2] = 3
+    scale = [1, 2, 3]
     mesh.scale = scale
     self.assertEqual(list(mesh.scale), list(scale))
 
@@ -114,8 +108,8 @@ def test_index(self):
 
   def test_inertia(self):
     mass = 3.14
-    position = iDynTree.Position(1, 2, 3)
-    rotational_inertia = iDynTree.RotationalInertia()
+    position = [1, 2, 3]
+    rotational_inertia = np.zeros([3,3])
     rotational_inertia[1, 1] = 1
 
     inertia = iDynTree.SpatialInertia(mass, position, rotational_inertia)
@@ -133,7 +127,7 @@ def test_attached_links(self):
     self.assertEqual(joint.get_second_attached_link(), 2)
 
   def test_rest_transform(self):
-    position = iDynTree.Position(1, 2, 3)
+    position = (1, 2, 3)
     rotation = iDynTree.Rotation(0, 0, 1,
                                  1, 0, 0,
                                  0, 1, 0)
@@ -167,7 +161,7 @@ def test_attached_links(self):
     self.assertEqual(joint.get_second_attached_link(), 2)
 
   def test_rest_transform(self):
-    position = iDynTree.Position(1, 2, 3)
+    position = (1, 2, 3)
     rotation = iDynTree.Rotation(0, 0, 1,
                                  1, 0, 0,
                                  0, 1, 0)
@@ -201,7 +195,7 @@ def test_attached_links(self):
     self.assertEqual(joint.get_second_attached_link(), 2)
 
   def test_rest_transform(self):
-    position = iDynTree.Position(1, 2, 3)
+    position = (1, 2, 3)
     rotation = iDynTree.Rotation(0, 0, 1,
                                  1, 0, 0,
                                  0, 1, 0)
@@ -355,7 +349,7 @@ def test_model_one_joint(self):
     self.assertIsNotNone(model.get_joint(0))
 
   def test_model_frame(self):
-    position = iDynTree.Position(1, 2, 3)
+    position = (1, 2, 3)
     rotation = iDynTree.Rotation(0, 0, 1,
                                  1, 0, 0,
                                  0, 1, 0)
diff --git a/bindings/pybind11/tests/test_idyntree_sensors.py b/bindings/pybind11/tests/test_idyntree_sensors.py
index d4792cb1f07..d04b8f7b0ba 100644
--- a/bindings/pybind11/tests/test_idyntree_sensors.py
+++ b/bindings/pybind11/tests/test_idyntree_sensors.py
@@ -47,7 +47,7 @@ def test_link_sensor_link_index(self):
 
   def test_link_sensor_transform(self):
     sensor = iDynTree.AccelerometerSensor()
-    position = iDynTree.Position(1, 2, 3)
+    position = (1, 2, 3)
     rotation = iDynTree.Rotation(0, 0, 1,
                                  1, 0, 0,
                                  0, 1, 0)

From a6c3058c1e2a9969fa7f9f51336a11734be60617 Mon Sep 17 00:00:00 2001
From: Giulio Romualdi <giulio.romualdi@gmail.com>
Date: Mon, 8 Nov 2021 16:35:26 +0100
Subject: [PATCH 06/28] Update the CHANGELOG

---
 CHANGELOG.md | 1 +
 1 file changed, 1 insertion(+)

diff --git a/CHANGELOG.md b/CHANGELOG.md
index 1e325da9245..72260e1afa9 100644
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -8,6 +8,7 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0
 
 ### Added
 - Added the possibility to draw in different portions of the visualizer window and textures at the same time. Allow disabling the drawing on textures (https://github.com/robotology/idyntree/pull/903).
+- Implement iDynTree base type `type_caster` for pybind11 bindings. In other word, `VectorDynSize`,`VectorFixSize`, `MatrixDynSize` and `MatrixFixSize` are now automatically converted in `NumPy` objects (https://github.com/robotology/idyntree/pull/931)
 
 ### Deprecated
 - The `iDynTree::Visualizer::enviroment()` was deprecated. Please use the `iDynTree::Visualizer::environment()` method instead (https://github.com/robotology/idyntree/pull/903).

From 899643b7d4c4e938c9f99dd58b4420e70b6c5724 Mon Sep 17 00:00:00 2001
From: Giulio Romualdi <giulio.romualdi@gmail.com>
Date: Tue, 23 Nov 2021 10:17:01 +0100
Subject: [PATCH 07/28] Update CHANGELOG.md

Co-authored-by: Silvio Traversaro <silvio.traversaro@iit.it>
---
 CHANGELOG.md | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/CHANGELOG.md b/CHANGELOG.md
index 72260e1afa9..9d05c5bde5c 100644
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -8,7 +8,7 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0
 
 ### Added
 - Added the possibility to draw in different portions of the visualizer window and textures at the same time. Allow disabling the drawing on textures (https://github.com/robotology/idyntree/pull/903).
-- Implement iDynTree base type `type_caster` for pybind11 bindings. In other word, `VectorDynSize`,`VectorFixSize`, `MatrixDynSize` and `MatrixFixSize` are now automatically converted in `NumPy` objects (https://github.com/robotology/idyntree/pull/931)
+- Implement iDynTree base type `type_caster` for pybind11 bindings. In other word, `VectorDynSize`,`VectorFixSize`, `MatrixDynSize` and `MatrixFixSize` are now automatically converted in `NumPy` objects (https://github.com/robotology/idyntree/pull/931).
 
 ### Deprecated
 - The `iDynTree::Visualizer::enviroment()` was deprecated. Please use the `iDynTree::Visualizer::environment()` method instead (https://github.com/robotology/idyntree/pull/903).

From ecc64ad17c9f1018b8e9df5766673a3d66255c90 Mon Sep 17 00:00:00 2001
From: Giulio Romualdi <giulio.romualdi@gmail.com>
Date: Thu, 25 Nov 2021 15:05:15 +0100
Subject: [PATCH 08/28] Improve the idyntree_type_caster structs

---
 bindings/pybind11/idyntree_type_caster.h | 275 ++++++++++++++---------
 1 file changed, 163 insertions(+), 112 deletions(-)

diff --git a/bindings/pybind11/idyntree_type_caster.h b/bindings/pybind11/idyntree_type_caster.h
index 5be9cb1330a..163535d59cf 100644
--- a/bindings/pybind11/idyntree_type_caster.h
+++ b/bindings/pybind11/idyntree_type_caster.h
@@ -1,13 +1,10 @@
 #ifndef IDYNTREE_PYBIND11_IDYNTREE_TYPE_CASTER_H
 #define IDYNTREE_PYBIND11_IDYNTREE_TYPE_CASTER_H
 
-#include <iostream>
-#include <string>
 #include <type_traits>
 
 #include <iDynTree/Core/MatrixDynSize.h>
 #include <iDynTree/Core/MatrixFixSize.h>
-#include <iDynTree/Core/Rotation.h>
 #include <iDynTree/Core/VectorDynSize.h>
 #include <iDynTree/Core/VectorFixSize.h>
 
@@ -18,139 +15,149 @@ namespace pybind11
 {
 namespace detail
 {
-template <template <unsigned int...> class base, typename derived>
-struct is_base_of_template_int_impl
+
+/**
+ * is_vector_fixsize is used to build a type-dependent expression that check if an element
+ * has is an iDynTree::VectorFixSize. This specific implementation is used
+ * when the the object is not an iDynTree::VectorFixSize
+ */
+template<typename>
+struct is_vector_fixsize : std::false_type {};
+
+/**
+ * is_vector_fixsize is used to build a type-dependent expression that check if an element
+ * has is an iDynTree::VectorFixSize. This specific implementation is used
+ * when the the object not an iDynTree::VectorFixSize
+ */
+template<unsigned int N>
+struct is_vector_fixsize<::iDynTree::VectorFixSize<N>> : std::true_type {};
+
+/**
+ * is_matrix_fixsize is used to build a type-dependent expression that check if an element
+ * has is an iDynTree::MatrixFixSize. This specific implementation is used
+ * when the the object is not an iDynTree::MatrixFixSize
+ */
+template<typename>
+struct is_matrix_fixsize : std::false_type {};
+
+/**
+ * is_matrix_fixsize is used to build a type-dependent expression that check if an element
+ * has is an iDynTree::MatrixFixSize. This specific implementation is used
+ * when the the object is an iDynTree::MatrixFixSize
+ */
+template<unsigned int nRows, unsigned int nCols>
+struct is_matrix_fixsize<::iDynTree::MatrixFixSize<nRows, nCols>> : std::true_type {};
+
+/**
+ * vector_size_at_compile_time is a utility metafunction to get the compile-time size of an iDynTree
+ * vector. This specific implementation is used with VectorDynSize.
+ */
+template<typename>
+struct vector_size_at_compile_time
 {
-    template <unsigned int... Ts> static constexpr std::true_type test(const base<Ts...>*);
-    static constexpr std::false_type test(...);
-    using type = decltype(test(std::declval<derived*>()));
+    constexpr static int value = -1;
 };
 
-template <template <unsigned int...> class base, typename derived>
-using is_base_of_template_int = typename is_base_of_template_int_impl<base, derived>::type;
-
-template <typename Type>
-using is_idyntree_vector_fix_size = is_base_of_template_int<iDynTree::VectorFixSize, Type>;
-template <typename Type>
-using is_idyntree_matrix_fix_size = is_base_of_template_int<iDynTree::MatrixFixSize, Type>;
-
-// Type caster for all the classes that inherits from VectorDynSize
-template <typename Type>
-struct type_caster<Type, enable_if_t<std::is_base_of<iDynTree::VectorDynSize, Type>::value>>
+/**
+ * vector_size_at_compile_time is a utility metafunction to get the compile-time size of an iDynTree
+ * vector. This specific implementation is used with VectorFixSize.
+ */
+template<unsigned int N>
+struct vector_size_at_compile_time<::iDynTree::VectorFixSize<N>>
 {
-    PYBIND11_TYPE_CASTER(Type, _(PYBIND11_TOSTRING(Type)));
-
-    /**
-     * Conversion from Python to C++
-     */
-    bool load(handle src, bool convert)
-    {
-        if (!convert && array_t<double>::check_(src))
-        {
-            return false;
-        }
+    constexpr static int value = N;
+};
 
-        auto buf
-            = pybind11::array_t<double,
-                                pybind11::array::c_style | pybind11::array::forcecast>::ensure(src);
-        if (!buf)
-            return false;
+/**
+ * matrix_size_at_compile_time is a utility metafunction to get the compile-time rows and cols of
+ * an iDynTree matrix. This specific implementation is used with MatrixDynSize.
+ */
+template<typename>
+struct matrix_size_at_compile_time
+{
+    constexpr static int rows = -1;
+    constexpr static int cols = -1;
+};
 
-        // the number of dimension must be equal to 1 since it is a vector
-        const auto dims = buf.ndim();
-        if (dims != 1)
-            return false;
+/**
+ * matrix_size_at_compile_time is a utility metafunction to get the compile-time rows and cols of
+ * an iDynTree matrix. This specific implementation is used with MatrixFixSize.
+ */
+template<unsigned int nRows, unsigned int nCols>
+struct matrix_size_at_compile_time<::iDynTree::MatrixFixSize<nRows, nCols>>
+{
+    constexpr static int rows = nRows;
+    constexpr static int cols = nCols;
+};
 
-        // store the data
-        value = Type(buf.data(), buf.size());
-        return true;
-    }
 
-    /**
-     * Conversion from C++ to Python
-     */
-    static handle cast(Type src, return_value_policy policy, handle parent)
-    {
-        pybind11::array a(src.size(), src.data());
-        return a.release();
-    }
-};
 
-// Type caster for all the classes that inherits from MatriDynSize
+/**
+ * type_caster implementation for the iDynTree Vectors (both fixed and dynamical size)
+ */
 template <typename Type>
-struct type_caster<Type, enable_if_t<std::is_base_of<iDynTree::MatrixDynSize, Type>::value>>
+struct type_caster<Type,
+                   enable_if_t<is_vector_fixsize<Type>::value
+                               || std::is_same<Type, iDynTree::VectorDynSize>::value>>
 {
-    PYBIND11_TYPE_CASTER(Type, _(PYBIND11_TOSTRING(Type)));
+private:
 
-    /**
-     * Conversion from Python to C++
-     */
-    bool load(handle src, bool convert)
+    template <typename U>
+    static bool checkSize(const std::size_t size, const U& value)
     {
-        if (!convert && array_t<double>::check_(src))
-        {
-            return false;
-        }
-
-        auto buf
-            = pybind11::array_t<double,
-                                pybind11::array::c_style | pybind11::array::forcecast>::ensure(src);
-        if (!buf)
-            return false;
-
-        // Since it is a matrix the buffer must have 2 dimensions
-        const auto dims = buf.ndim();
-        if (dims != 2)
-            return false;
-
-        // this can be done since numpy and iDynTree store matrices in Row-Major order
-        const auto rows = buf.shape(0);
-        const auto cols = buf.shape(1);
-        value = Type(buf.data(), rows, cols);
-
         return true;
     }
 
-    static handle cast(Type src, return_value_policy policy, handle parent)
+    template <typename U,
+              typename std::enable_if_t<is_vector_fixsize<U>::value>>
+    static bool checkSize(const std::size_t size, const U& value)
     {
-        // this can be done since numpy and iDynTree store matrices in Row-Major order
-        pybind11::array a({src.rows(), src.cols()}, src.data());
-        return a.release();
+        return size == value.size();
     }
-};
 
-// Type caster for all the classes that inherits from VectorFixSize
-template <typename Type>
-struct type_caster<Type, enable_if_t<is_idyntree_vector_fix_size<Type>::value>>
-{
-    PYBIND11_TYPE_CASTER(Type, _(PYBIND11_TOSTRING(Type)));
+    /**
+     * Python name description
+     */
+    static constexpr auto descriptor = _("numpy.ndarray[")
+        + npy_format_descriptor<double>::name
+        + _("[")
+        + _<is_vector_fixsize<Type>::value>(_<(size_t) vector_size_at_compile_time<Type>::value>(),
+                                             _("m"))
+        + _(", 1]]");
+
+public:
+    PYBIND11_TYPE_CASTER(Type, descriptor);
 
     /**
      * Conversion from Python to C++
      */
     bool load(handle src, bool convert)
     {
+        namespace py = ::pybind11;
+
         if (!convert && array_t<double>::check_(src))
         {
             return false;
         }
 
-        auto buf
-            = pybind11::array_t<double,
-                                pybind11::array::c_style | pybind11::array::forcecast>::ensure(src);
+        const auto buf = py::array_t<double, py::array::c_style | py::array::forcecast>::ensure(src);
         if (!buf)
             return false;
 
         // It must be a vector
-        const auto dims = buf.ndim();
+        const std::size_t dims = buf.ndim();
         if (dims != 1)
+        {
             return false;
+        }
 
-        // since the vector is fixed size it is not possible to resize it. This check if the size is
-        // correct
-        if (buf.size() != value.size())
+        // Check if the size of the vector is correct.
+        if (!this->checkSize(buf.size(), value))
+        {
             return false;
+        }
 
+        // Copy the content of the python vector in to the iDynTree vector
         value = Type(buf.data(), buf.size());
 
         return true;
@@ -161,47 +168,88 @@ struct type_caster<Type, enable_if_t<is_idyntree_vector_fix_size<Type>::value>>
      */
     static handle cast(Type src, return_value_policy policy, handle parent)
     {
-        pybind11::array a(src.size(), src.data());
+        namespace py = ::pybind11;
+
+        // copy the content of the iDynTree vector in the python one
+        py::array a(src.size(), src.data());
         return a.release();
     }
 };
 
-// Type caster for all the classes that inherits from MatrixFixSize but that are not rotations
+/**
+ * type_caster implementation for the iDynTree Matrices (both fixed and dynamical size)
+ */
 template <typename Type>
 struct type_caster<Type,
-                   enable_if_t<is_idyntree_matrix_fix_size<Type>::value
-                               && !std::is_same<iDynTree::Rotation, Type>::value>>
+                   enable_if_t<is_matrix_fixsize<Type>::value
+                               || std::is_same<Type, iDynTree::MatrixDynSize>::value>>
 {
-    PYBIND11_TYPE_CASTER(Type, _(PYBIND11_TOSTRING(Type)));
+private:
+
+    template <typename U>
+    static bool checkSize(const std::size_t size, const U& value)
+    {
+        return true;
+    }
+
+    template <typename U,
+              typename std::enable_if_t<is_matrix_fixsize<U>::value>>
+    static bool checkSize(const std::size_t size, const U& value)
+    {
+        return size == value.rows() * value.cols();
+    }
+
+    /**
+     * Python name description
+     */
+    static constexpr auto descriptor = _("numpy.ndarray[")
+        + npy_format_descriptor<double>::name
+        + _("[")
+        + _<is_matrix_fixsize<Type>::value>(_<(size_t) matrix_size_at_compile_time<Type>::rows>(),
+                                             _("m"))
+        + _(", ")
+        + _<is_matrix_fixsize<Type>::value>(_<(size_t) matrix_size_at_compile_time<Type>::rows>(),
+                                            _("n"))
+        + _("]]");
+
+public:
+
+    PYBIND11_TYPE_CASTER(Type, descriptor);
 
     /**
      * Conversion from Python to C++
      */
     bool load(handle src, bool convert)
     {
+        namespace py = ::pybind11;
+
         if (!convert && array_t<double>::check_(src))
         {
             return false;
         }
 
-        auto buf
-            = pybind11::array_t<double,
-                                pybind11::array::c_style | pybind11::array::forcecast>::ensure(src);
+        const auto buf = py::array_t<double, py::array::c_style | py::array::forcecast>::ensure(src);
         if (!buf)
+        {
             return false;
+        }
 
         // since it is a matrix the number of dimension must be equal to 2
-        auto dims = buf.ndim();
+        const std::size_t dims = buf.ndim();
         if (dims != 2)
             return false;
 
-        // the matrix is fix size so it is not possible to change it's size
-        if (buf.size() != value.rows() * value.cols())
+        // Check if the size of the vector is correct.
+        if (!this->checkSize(buf.size(), value))
+        {
             return false;
+        }
 
-        // this can be done since numpy and iDynTree store matrices in Row-Major order
-        const auto rows = buf.shape(0);
-        const auto cols = buf.shape(1);
+        // Copy the content of the python matrix into an iDynTree Matrix
+        // Here there is the assumption that the iDynTree matrices are stored using row-major
+        // order
+        const std::size_t rows = buf.shape(0);
+        const std::size_t cols = buf.shape(1);
         value = Type(buf.data(), rows, cols);
 
         return true;
@@ -212,8 +260,11 @@ struct type_caster<Type,
      */
     static handle cast(Type src, return_value_policy policy, handle parent)
     {
+        namespace py = ::pybind11;
+
+        // Copy the content of the iDynTree matrix into the python one
         // this can be done since numpy and iDynTree store matrices in Row-Major order
-        pybind11::array a({src.rows(), src.cols()}, src.data());
+        py::array a({src.rows(), src.cols()}, src.data());
         return a.release();
     }
 };

From f60121c52c2fb8df026ca5539be82ddc35a8a46b Mon Sep 17 00:00:00 2001
From: Giulio Romualdi <giulio.romualdi@gmail.com>
Date: Thu, 25 Nov 2021 15:13:14 +0100
Subject: [PATCH 09/28] Revert "Use pybidn11 typecaster in the already existing
 bindings"

This reverts commit c46a4bf4188161bca91cb056d6c330ff8e2255b8.
---
 bindings/pybind11/idyntree_core.cpp         | 270 +++++++++++++++-----
 bindings/pybind11/idyntree_model.cpp        |   1 -
 bindings/pybind11/idyntree_modelio_urdf.cpp |   2 +-
 bindings/pybind11/idyntree_sensors.cpp      |   1 -
 4 files changed, 213 insertions(+), 61 deletions(-)

diff --git a/bindings/pybind11/idyntree_core.cpp b/bindings/pybind11/idyntree_core.cpp
index 59a0928fdb6..2e211a25788 100644
--- a/bindings/pybind11/idyntree_core.cpp
+++ b/bindings/pybind11/idyntree_core.cpp
@@ -1,5 +1,4 @@
 #include "idyntree_core.h"
-#include "idyntree_type_caster.h"
 
 #include <iDynTree/Core/Axis.h>
 #include <iDynTree/Core/Direction.h>
@@ -25,6 +24,136 @@ namespace bindings {
 namespace {
 namespace py = ::pybind11;
 
+void vectorDynSizeClassDefinition(py::class_<VectorDynSize>& vector) {
+  vector.def(py::init())
+      .def(py::init<unsigned>())
+      .def("__getitem__", py::overload_cast<const std::size_t>(
+                              &VectorDynSize::operator(), py::const_))
+      .def("__setitem__",
+           [](VectorDynSize& the_vector, std::size_t index, double new_value) {
+             the_vector(index) = new_value;
+           })
+      .def(
+          "__iter__",
+          [](const VectorDynSize& s) {
+            return py::make_iterator(s.begin(), s.end());
+          },
+          py::keep_alive<
+              0, 1>() /* Essential: keep object alive while iterator exists */)
+      .def("__len__", &VectorDynSize::size)
+      .def("set_zero", &VectorDynSize::zero)
+      .def("resize", &VectorDynSize::resize)
+      .def("__repr__", &VectorDynSize::toString)
+      .def_buffer([](VectorDynSize& v) -> py::buffer_info {
+        return py::buffer_info(
+            v.data(),                                /* Pointer to buffer */
+            sizeof(double),                          /* Size of one scalar */
+            py::format_descriptor<double>::format(), /* Python struct-style
+                                                        format descriptor */
+            1,                                       /* Number of dimensions */
+            {v.size()},                              /* Buffer dimensions */
+            {sizeof(double)} /* Strides (in bytes) for each index */
+        );
+      });
+}
+
+template <unsigned size>
+void createFixSizeVector(pybind11::module& module,
+                         const std::string& class_name) {
+  py::class_<VectorFixSize<size>>(module, class_name.c_str(),
+                                  py::buffer_protocol())
+      .def(py::init())
+      .def("__getitem__", py::overload_cast<const std::size_t>(
+                              &VectorFixSize<size>::operator(), py::const_))
+      .def("__setitem__",
+           [](VectorFixSize<size>& the_vector, std::size_t index,
+              double new_value) { the_vector(index) = new_value; })
+      .def("__len__", &VectorFixSize<size>::size)
+      .def(
+          "__iter__",
+          [](const VectorFixSize<size>& s) {
+            return py::make_iterator(s.begin(), s.end());
+          },
+          py::keep_alive<
+              0, 1>() /* Essential: keep object alive while iterator exists */)
+      .def("set_zero", &VectorFixSize<size>::zero)
+      .def("__repr__", &VectorFixSize<size>::toString)
+      .def_buffer([](VectorFixSize<size>& v) -> py::buffer_info {
+        return py::buffer_info(
+            v.data(),                                /* Pointer to buffer */
+            sizeof(double),                          /* Size of one scalar */
+            py::format_descriptor<double>::format(), /* Python struct-style
+                                                        format descriptor */
+            1,                                       /* Number of dimensions */
+            {v.size()},                              /* Buffer dimensions */
+            {sizeof(double)} /* Strides (in bytes) for each index */
+        );
+      });
+}
+
+void matrixDynSizeClassDefinition(py::class_<MatrixDynSize>& matrix) {
+  matrix.def(py::init())
+      .def(py::init<unsigned, unsigned>())
+      .def("__getitem__",
+           [](MatrixDynSize& matrix,
+              std::pair<std::size_t, std::size_t> indices) {
+             return matrix.getVal(indices.first, indices.second);
+           })
+      .def("__setitem__",
+           [](MatrixDynSize& matrix,
+              std::pair<std::size_t, std::size_t> indices, double item) {
+             return matrix.setVal(indices.first, indices.second, item);
+           })
+      .def("rows", &MatrixDynSize::rows)
+      .def("cols", &MatrixDynSize::cols)
+      .def("set_zero", &MatrixDynSize::zero)
+      .def("resize", &MatrixDynSize::resize)
+      .def("__repr__", &MatrixDynSize::toString)
+      .def_buffer([](MatrixDynSize& m) -> py::buffer_info {
+        return py::buffer_info(
+            m.data(),                                /* Pointer to buffer */
+            sizeof(double),                          /* Size of one scalar */
+            py::format_descriptor<double>::format(), /* Python struct-style
+                                                        format descriptor */
+            2,                                       /* Number of dimensions */
+            {m.rows(), m.cols()},                    /* Buffer dimensions */
+            {sizeof(double) * m.cols(), /* Strides (in bytes) for each index */
+             sizeof(double)});
+      });
+}
+
+template <unsigned rows, unsigned cols>
+void createFixSizeMatrix(pybind11::module& module,
+                         const std::string& class_name) {
+  py::class_<MatrixFixSize<rows, cols>>(module, class_name.c_str(),
+                                        py::buffer_protocol())
+      .def(py::init())
+      .def("__getitem__",
+           [](MatrixFixSize<rows, cols>& matrix,
+              std::pair<std::size_t, std::size_t> indices) {
+             return matrix.getVal(indices.first, indices.second);
+           })
+      .def("__setitem__",
+           [](MatrixFixSize<rows, cols>& matrix,
+              std::pair<std::size_t, std::size_t> indices, double item) {
+             return matrix.setVal(indices.first, indices.second, item);
+           })
+      .def("rows", &MatrixFixSize<rows, cols>::rows)
+      .def("cols", &MatrixFixSize<rows, cols>::cols)
+      .def("set_zero", &MatrixFixSize<rows, cols>::zero)
+      .def("__repr__", &MatrixFixSize<rows, cols>::toString)
+      .def_buffer([](MatrixFixSize<rows, cols>& m) -> py::buffer_info {
+        return py::buffer_info(
+            m.data(),                                /* Pointer to buffer */
+            sizeof(double),                          /* Size of one scalar */
+            py::format_descriptor<double>::format(), /* Python struct-style
+                                                        format descriptor */
+            2,                                       /* Number of dimensions */
+            {m.rows(), m.cols()},                    /* Buffer dimensions */
+            {sizeof(double) * m.cols(), /* Strides (in bytes) for each index */
+             sizeof(double)});
+      });
+}
 
 void transformClassDefinition(py::class_<Transform>& transform) {
   transform.def(py::init())
@@ -50,64 +179,89 @@ void transformClassDefinition(py::class_<Transform>& transform) {
 }  // namespace
 
 void iDynTreeCoreBindings(pybind11::module& module) {
+  // Vectors and matrices.
+  py::class_<VectorDynSize> vector_dyn(module, "VectorDynSize",
+                                       py::buffer_protocol());
+  vectorDynSizeClassDefinition(vector_dyn);
+  createFixSizeVector<3>(module, "Vector3");
+  createFixSizeVector<4>(module, "Vector4");
+  createFixSizeVector<6>(module, "Vector6");
+
+  py::class_<MatrixDynSize> matrix_dyn(module, "MatrixDynSize",
+                                       py::buffer_protocol());
+  matrixDynSizeClassDefinition(matrix_dyn);
+  createFixSizeMatrix<3, 3>(module, "Matrix3x3");
+  createFixSizeMatrix<4, 4>(module, "Matrix4x4");
+  createFixSizeMatrix<6, 6>(module, "Matrix6x6");
+
+  // Positions, Rotations and Transforms.
+  py::class_<PositionRaw, VectorFixSize<3>>(module, "_PositionRaw")
+      // Do not expose constructor as we do not want users to use this class.
+      .def("__repr__", &PositionRaw::toString);
+
+  py::class_<Position, PositionRaw>(module, "Position")
+      .def(py::init())
+      .def(py::init<double, double, double>())
+      .def(py::self + py::self)
+      .def(py::self - py::self)
+      .def(-py::self)
+      .def_static("Zero", &Position::Zero);
+
+  py::class_<RotationRaw, MatrixFixSize<3, 3>>(module, "_RotationRaw")
+      // Do not expose constructor as we do not want users to use this class.
+      .def("__repr__", &RotationRaw::toString);
+
+  py::class_<Rotation, RotationRaw>(module, "Rotation")
+      .def(py::init())
+      .def(py::init<double, double, double,  //
+                    double, double, double,  //
+                    double, double, double>())
+      .def("inverse", &Rotation::inverse)
+      .def(py::self * py::self)
+      .def(
+          "__mul__",
+          [](const Rotation& r, const Position& p) -> Position {
+            return r * p;
+          },
+          py::is_operator())
+      .def_static("Identity", &Rotation::Identity);
+
+  py::class_<Transform> transform(module, "Transform");
+  transformClassDefinition(transform);
+
+  // Other classes.
+  py::class_<Direction, VectorFixSize<3>>(module, "Direction")
+      .def(py::init<double, double, double>());
+
+  py::class_<Axis>(module, "Axis")
+      .def(py::init<const Direction&, const Position&>())
+      .def_property("direction", &Axis::getDirection, &Axis::setDirection)
+      .def_property("origin", &Axis::getOrigin, &Axis::setOrigin)
+      .def("__repr__", &Axis::toString);
+
+  py::class_<RotationalInertiaRaw, MatrixFixSize<3, 3>>(module,
+                                                        "RotationalInertia")
+      .def(py::init());
+
+  py::class_<SpatialInertiaRaw>(module, "_SpatialInertiaRaw")
+      .def("from_rotational_inertia_wrt_center_of_mass",
+           &SpatialInertiaRaw::fromRotationalInertiaWrtCenterOfMass)
+      .def("get_mass", &SpatialInertiaRaw::getMass)
+      .def("get_center_of_mass", &SpatialInertiaRaw::getCenterOfMass)
+      .def("get_rotational_inertia_wrt_frame_origin",
+           &SpatialInertiaRaw::getRotationalInertiaWrtFrameOrigin)
+      .def("get_rotational_inertia_wrt_center_of_mass",
+           &SpatialInertiaRaw::getRotationalInertiaWrtCenterOfMass);
 
-    py::class_<Rotation>(module, "Rotation")
-        .def(py::init())
-        .def(py::init<double,
-                      double,
-                      double, //
-                      double,
-                      double,
-                      double, //
-                      double,
-                      double,
-                      double>())
-        .def("inverse", &Rotation::inverse)
-        .def(py::self * py::self)
-        .def(
-            "__mul__",
-            [](const Rotation& r, const Position& p) -> Position { return r * p; },
-            py::is_operator())
-        .def_static("Identity", &Rotation::Identity)
-        .def("__repr__", &Rotation::toString)
-        .def("to_numpy",
-             [](const Rotation& impl) {
-                 iDynTree::Matrix3x3 m(impl.data(), 3, 3);
-                 return m;
-             })
-        .def("__getitem__",
-             [](const Rotation& matrix, std::pair<std::size_t, std::size_t> indices) {
-                 return matrix.getVal(indices.first, indices.second);
-             });
-
-    py::class_<Transform> transform(module, "Transform");
-    transformClassDefinition(transform);
-
-    // Other classes.
-    py::class_<Axis>(module, "Axis")
-        .def(py::init<const Direction&, const Position&>())
-        .def_property("direction", &Axis::getDirection, &Axis::setDirection)
-        .def_property("origin", &Axis::getOrigin, &Axis::setOrigin)
-        .def("__repr__", &Axis::toString);
-
-    py::class_<SpatialInertiaRaw>(module, "_SpatialInertiaRaw")
-        .def("from_rotational_inertia_wrt_center_of_mass",
-             &SpatialInertiaRaw::fromRotationalInertiaWrtCenterOfMass)
-        .def("get_mass", &SpatialInertiaRaw::getMass)
-        .def("get_center_of_mass", &SpatialInertiaRaw::getCenterOfMass)
-        .def("get_rotational_inertia_wrt_frame_origin",
-             &SpatialInertiaRaw::getRotationalInertiaWrtFrameOrigin)
-        .def("get_rotational_inertia_wrt_center_of_mass",
-             &SpatialInertiaRaw::getRotationalInertiaWrtCenterOfMass);
-
-    py::class_<SpatialInertia, SpatialInertiaRaw>(module, "SpatialInertia")
-        .def(py::init())
-        .def(py::init<double, const PositionRaw&, const RotationalInertiaRaw&>())
-        .def_static("Zero", &SpatialInertia::Zero)
-        .def("as_matrix", &SpatialInertia::asMatrix)
-        .def(py::self + py::self)
-        .def("__repr__",
-             [](const SpatialInertia& inertia) { return inertia.asMatrix().toString(); });
+  py::class_<SpatialInertia, SpatialInertiaRaw>(module, "SpatialInertia")
+      .def(py::init())
+      .def(py::init<double, const PositionRaw&, const RotationalInertiaRaw&>())
+      .def_static("Zero", &SpatialInertia::Zero)
+      .def("as_matrix", &SpatialInertia::asMatrix)
+      .def(py::self + py::self)
+      .def("__repr__", [](const SpatialInertia& inertia) {
+        return inertia.asMatrix().toString();
+      });
 }
 }  // namespace bindings
 }  // namespace iDynTree
diff --git a/bindings/pybind11/idyntree_model.cpp b/bindings/pybind11/idyntree_model.cpp
index 7ab564f428a..65d47fa0846 100644
--- a/bindings/pybind11/idyntree_model.cpp
+++ b/bindings/pybind11/idyntree_model.cpp
@@ -1,5 +1,4 @@
 #include "idyntree_model.h"
-#include "idyntree_type_caster.h"
 
 #include "error_utilities.h"
 
diff --git a/bindings/pybind11/idyntree_modelio_urdf.cpp b/bindings/pybind11/idyntree_modelio_urdf.cpp
index 84efbb110e9..e5d99bd1488 100644
--- a/bindings/pybind11/idyntree_modelio_urdf.cpp
+++ b/bindings/pybind11/idyntree_modelio_urdf.cpp
@@ -1,6 +1,6 @@
 #include "idyntree_modelio_urdf.h"
+
 #include "error_utilities.h"
-#include "idyntree_type_caster.h"
 
 #include <string>
 #include <iDynTree/ModelIO/ModelExporter.h>
diff --git a/bindings/pybind11/idyntree_sensors.cpp b/bindings/pybind11/idyntree_sensors.cpp
index 13abeccc9e6..2ade2842315 100644
--- a/bindings/pybind11/idyntree_sensors.cpp
+++ b/bindings/pybind11/idyntree_sensors.cpp
@@ -1,6 +1,5 @@
 #include "idyntree_sensors.h"
 #include "error_utilities.h"
-#include "idyntree_type_caster.h"
 
 #include <pybind11/operators.h>
 #include <pybind11/pybind11.h>

From 97faee10a057765488902bd36ee3e2a9c39f253a Mon Sep 17 00:00:00 2001
From: Giulio Romualdi <giulio.romualdi@gmail.com>
Date: Thu, 25 Nov 2021 16:06:45 +0100
Subject: [PATCH 10/28] Use pybind11 typecaster in the already existing
 bindings

---
 bindings/pybind11/idyntree_core.cpp | 166 +++-------------------------
 1 file changed, 16 insertions(+), 150 deletions(-)

diff --git a/bindings/pybind11/idyntree_core.cpp b/bindings/pybind11/idyntree_core.cpp
index 2e211a25788..bb09fac9fef 100644
--- a/bindings/pybind11/idyntree_core.cpp
+++ b/bindings/pybind11/idyntree_core.cpp
@@ -1,4 +1,5 @@
 #include "idyntree_core.h"
+#include "idyntree_type_caster.h"
 
 #include <iDynTree/Core/Axis.h>
 #include <iDynTree/Core/Direction.h>
@@ -24,137 +25,6 @@ namespace bindings {
 namespace {
 namespace py = ::pybind11;
 
-void vectorDynSizeClassDefinition(py::class_<VectorDynSize>& vector) {
-  vector.def(py::init())
-      .def(py::init<unsigned>())
-      .def("__getitem__", py::overload_cast<const std::size_t>(
-                              &VectorDynSize::operator(), py::const_))
-      .def("__setitem__",
-           [](VectorDynSize& the_vector, std::size_t index, double new_value) {
-             the_vector(index) = new_value;
-           })
-      .def(
-          "__iter__",
-          [](const VectorDynSize& s) {
-            return py::make_iterator(s.begin(), s.end());
-          },
-          py::keep_alive<
-              0, 1>() /* Essential: keep object alive while iterator exists */)
-      .def("__len__", &VectorDynSize::size)
-      .def("set_zero", &VectorDynSize::zero)
-      .def("resize", &VectorDynSize::resize)
-      .def("__repr__", &VectorDynSize::toString)
-      .def_buffer([](VectorDynSize& v) -> py::buffer_info {
-        return py::buffer_info(
-            v.data(),                                /* Pointer to buffer */
-            sizeof(double),                          /* Size of one scalar */
-            py::format_descriptor<double>::format(), /* Python struct-style
-                                                        format descriptor */
-            1,                                       /* Number of dimensions */
-            {v.size()},                              /* Buffer dimensions */
-            {sizeof(double)} /* Strides (in bytes) for each index */
-        );
-      });
-}
-
-template <unsigned size>
-void createFixSizeVector(pybind11::module& module,
-                         const std::string& class_name) {
-  py::class_<VectorFixSize<size>>(module, class_name.c_str(),
-                                  py::buffer_protocol())
-      .def(py::init())
-      .def("__getitem__", py::overload_cast<const std::size_t>(
-                              &VectorFixSize<size>::operator(), py::const_))
-      .def("__setitem__",
-           [](VectorFixSize<size>& the_vector, std::size_t index,
-              double new_value) { the_vector(index) = new_value; })
-      .def("__len__", &VectorFixSize<size>::size)
-      .def(
-          "__iter__",
-          [](const VectorFixSize<size>& s) {
-            return py::make_iterator(s.begin(), s.end());
-          },
-          py::keep_alive<
-              0, 1>() /* Essential: keep object alive while iterator exists */)
-      .def("set_zero", &VectorFixSize<size>::zero)
-      .def("__repr__", &VectorFixSize<size>::toString)
-      .def_buffer([](VectorFixSize<size>& v) -> py::buffer_info {
-        return py::buffer_info(
-            v.data(),                                /* Pointer to buffer */
-            sizeof(double),                          /* Size of one scalar */
-            py::format_descriptor<double>::format(), /* Python struct-style
-                                                        format descriptor */
-            1,                                       /* Number of dimensions */
-            {v.size()},                              /* Buffer dimensions */
-            {sizeof(double)} /* Strides (in bytes) for each index */
-        );
-      });
-}
-
-void matrixDynSizeClassDefinition(py::class_<MatrixDynSize>& matrix) {
-  matrix.def(py::init())
-      .def(py::init<unsigned, unsigned>())
-      .def("__getitem__",
-           [](MatrixDynSize& matrix,
-              std::pair<std::size_t, std::size_t> indices) {
-             return matrix.getVal(indices.first, indices.second);
-           })
-      .def("__setitem__",
-           [](MatrixDynSize& matrix,
-              std::pair<std::size_t, std::size_t> indices, double item) {
-             return matrix.setVal(indices.first, indices.second, item);
-           })
-      .def("rows", &MatrixDynSize::rows)
-      .def("cols", &MatrixDynSize::cols)
-      .def("set_zero", &MatrixDynSize::zero)
-      .def("resize", &MatrixDynSize::resize)
-      .def("__repr__", &MatrixDynSize::toString)
-      .def_buffer([](MatrixDynSize& m) -> py::buffer_info {
-        return py::buffer_info(
-            m.data(),                                /* Pointer to buffer */
-            sizeof(double),                          /* Size of one scalar */
-            py::format_descriptor<double>::format(), /* Python struct-style
-                                                        format descriptor */
-            2,                                       /* Number of dimensions */
-            {m.rows(), m.cols()},                    /* Buffer dimensions */
-            {sizeof(double) * m.cols(), /* Strides (in bytes) for each index */
-             sizeof(double)});
-      });
-}
-
-template <unsigned rows, unsigned cols>
-void createFixSizeMatrix(pybind11::module& module,
-                         const std::string& class_name) {
-  py::class_<MatrixFixSize<rows, cols>>(module, class_name.c_str(),
-                                        py::buffer_protocol())
-      .def(py::init())
-      .def("__getitem__",
-           [](MatrixFixSize<rows, cols>& matrix,
-              std::pair<std::size_t, std::size_t> indices) {
-             return matrix.getVal(indices.first, indices.second);
-           })
-      .def("__setitem__",
-           [](MatrixFixSize<rows, cols>& matrix,
-              std::pair<std::size_t, std::size_t> indices, double item) {
-             return matrix.setVal(indices.first, indices.second, item);
-           })
-      .def("rows", &MatrixFixSize<rows, cols>::rows)
-      .def("cols", &MatrixFixSize<rows, cols>::cols)
-      .def("set_zero", &MatrixFixSize<rows, cols>::zero)
-      .def("__repr__", &MatrixFixSize<rows, cols>::toString)
-      .def_buffer([](MatrixFixSize<rows, cols>& m) -> py::buffer_info {
-        return py::buffer_info(
-            m.data(),                                /* Pointer to buffer */
-            sizeof(double),                          /* Size of one scalar */
-            py::format_descriptor<double>::format(), /* Python struct-style
-                                                        format descriptor */
-            2,                                       /* Number of dimensions */
-            {m.rows(), m.cols()},                    /* Buffer dimensions */
-            {sizeof(double) * m.cols(), /* Strides (in bytes) for each index */
-             sizeof(double)});
-      });
-}
-
 void transformClassDefinition(py::class_<Transform>& transform) {
   transform.def(py::init())
       .def(py::init<const Rotation&, const Position&>())
@@ -179,35 +49,27 @@ void transformClassDefinition(py::class_<Transform>& transform) {
 }  // namespace
 
 void iDynTreeCoreBindings(pybind11::module& module) {
-  // Vectors and matrices.
-  py::class_<VectorDynSize> vector_dyn(module, "VectorDynSize",
-                                       py::buffer_protocol());
-  vectorDynSizeClassDefinition(vector_dyn);
-  createFixSizeVector<3>(module, "Vector3");
-  createFixSizeVector<4>(module, "Vector4");
-  createFixSizeVector<6>(module, "Vector6");
-
-  py::class_<MatrixDynSize> matrix_dyn(module, "MatrixDynSize",
-                                       py::buffer_protocol());
-  matrixDynSizeClassDefinition(matrix_dyn);
-  createFixSizeMatrix<3, 3>(module, "Matrix3x3");
-  createFixSizeMatrix<4, 4>(module, "Matrix4x4");
-  createFixSizeMatrix<6, 6>(module, "Matrix6x6");
 
   // Positions, Rotations and Transforms.
-  py::class_<PositionRaw, VectorFixSize<3>>(module, "_PositionRaw")
+  py::class_<PositionRaw>(module, "_PositionRaw")
       // Do not expose constructor as we do not want users to use this class.
       .def("__repr__", &PositionRaw::toString);
 
+
   py::class_<Position, PositionRaw>(module, "Position")
       .def(py::init())
       .def(py::init<double, double, double>())
+      .def(py::init([](const VectorFixSize<3>& position)
+                    {
+                        return Position(position(0), position(1), position(2));
+                    }))
       .def(py::self + py::self)
       .def(py::self - py::self)
       .def(-py::self)
       .def_static("Zero", &Position::Zero);
+  py::implicitly_convertible<iDynTree::VectorFixSize<3>, iDynTree::Position>();
 
-  py::class_<RotationRaw, MatrixFixSize<3, 3>>(module, "_RotationRaw")
+  py::class_<RotationRaw>(module, "_RotationRaw")
       // Do not expose constructor as we do not want users to use this class.
       .def("__repr__", &RotationRaw::toString);
 
@@ -216,6 +78,10 @@ void iDynTreeCoreBindings(pybind11::module& module) {
       .def(py::init<double, double, double,  //
                     double, double, double,  //
                     double, double, double>())
+      .def(py::init([](const MatrixFixSize<3,3>& rotation)
+                    {
+                        return Rotation(rotation);
+                    }))
       .def("inverse", &Rotation::inverse)
       .def(py::self * py::self)
       .def(
@@ -225,12 +91,13 @@ void iDynTreeCoreBindings(pybind11::module& module) {
           },
           py::is_operator())
       .def_static("Identity", &Rotation::Identity);
+  py::implicitly_convertible<iDynTree::MatrixFixSize<3,3>, iDynTree::Rotation>();
 
   py::class_<Transform> transform(module, "Transform");
   transformClassDefinition(transform);
 
   // Other classes.
-  py::class_<Direction, VectorFixSize<3>>(module, "Direction")
+  py::class_<Direction>(module, "Direction")
       .def(py::init<double, double, double>());
 
   py::class_<Axis>(module, "Axis")
@@ -239,8 +106,7 @@ void iDynTreeCoreBindings(pybind11::module& module) {
       .def_property("origin", &Axis::getOrigin, &Axis::setOrigin)
       .def("__repr__", &Axis::toString);
 
-  py::class_<RotationalInertiaRaw, MatrixFixSize<3, 3>>(module,
-                                                        "RotationalInertia")
+  py::class_<RotationalInertiaRaw>(module, "RotationalInertia")
       .def(py::init());
 
   py::class_<SpatialInertiaRaw>(module, "_SpatialInertiaRaw")

From 4a5509a1c946ac776242220e709b9e81b42f4a01 Mon Sep 17 00:00:00 2001
From: Giulio Romualdi <giulio.romualdi@gmail.com>
Date: Thu, 25 Nov 2021 16:09:38 +0100
Subject: [PATCH 11/28] Revert "Implement raw buffer constructor for the
 Position class"

This reverts commit 8a40642ba9e39f8023e4dbf1cd7f77e559a8a16c.
---
 src/core/include/iDynTree/Core/Position.h | 5 -----
 src/core/src/Position.cpp                 | 5 -----
 2 files changed, 10 deletions(-)

diff --git a/src/core/include/iDynTree/Core/Position.h b/src/core/include/iDynTree/Core/Position.h
index 1233e4ac51e..0906e5daed7 100644
--- a/src/core/include/iDynTree/Core/Position.h
+++ b/src/core/include/iDynTree/Core/Position.h
@@ -67,11 +67,6 @@ namespace iDynTree
          */
         Position(iDynTree::Span<const double> other);
 
-        /**
-         * Constructor from a raw buffer of 3 doubles.
-         */
-        Position(const double* in_data, const unsigned int in_size);
-
         /**
          * Geometric operations
          */
diff --git a/src/core/src/Position.cpp b/src/core/src/Position.cpp
index 35db5b9c44a..e0bf0511ec3 100644
--- a/src/core/src/Position.cpp
+++ b/src/core/src/Position.cpp
@@ -73,11 +73,6 @@ namespace iDynTree
     {
     }
 
-    Position::Position(const double* in_data, const unsigned int in_size)
-        : PositionRaw(in_data, in_size)
-    {
-    }
-
     const Position& Position::changePoint(const Position& newPoint)
     {
         this->PositionRaw::changePoint(newPoint);

From 2f7fa5be4fa4c5f8f101c09ad7080304d4b09f58 Mon Sep 17 00:00:00 2001
From: Giulio Romualdi <giulio.romualdi@gmail.com>
Date: Thu, 25 Nov 2021 16:21:18 +0100
Subject: [PATCH 12/28] Fix type 'struct type_caster' violates one definition
 rule [-Wodr] bug in the bindings generation

---
 bindings/pybind11/idyntree_model.cpp        | 1 +
 bindings/pybind11/idyntree_modelio_urdf.cpp | 1 +
 bindings/pybind11/idyntree_sensors.cpp      | 1 +
 3 files changed, 3 insertions(+)

diff --git a/bindings/pybind11/idyntree_model.cpp b/bindings/pybind11/idyntree_model.cpp
index 65d47fa0846..7ab564f428a 100644
--- a/bindings/pybind11/idyntree_model.cpp
+++ b/bindings/pybind11/idyntree_model.cpp
@@ -1,4 +1,5 @@
 #include "idyntree_model.h"
+#include "idyntree_type_caster.h"
 
 #include "error_utilities.h"
 
diff --git a/bindings/pybind11/idyntree_modelio_urdf.cpp b/bindings/pybind11/idyntree_modelio_urdf.cpp
index e5d99bd1488..c992554a4b3 100644
--- a/bindings/pybind11/idyntree_modelio_urdf.cpp
+++ b/bindings/pybind11/idyntree_modelio_urdf.cpp
@@ -1,4 +1,5 @@
 #include "idyntree_modelio_urdf.h"
+#include "idyntree_type_caster.h"
 
 #include "error_utilities.h"
 
diff --git a/bindings/pybind11/idyntree_sensors.cpp b/bindings/pybind11/idyntree_sensors.cpp
index 2ade2842315..09730754eab 100644
--- a/bindings/pybind11/idyntree_sensors.cpp
+++ b/bindings/pybind11/idyntree_sensors.cpp
@@ -1,3 +1,4 @@
+#include "idyntree_type_caster.h"
 #include "idyntree_sensors.h"
 #include "error_utilities.h"
 

From 70377673fab95dc4f5393d39af17d17e0ef0640a Mon Sep 17 00:00:00 2001
From: Giulio Romualdi <giulio.romualdi@gmail.com>
Date: Thu, 25 Nov 2021 16:25:20 +0100
Subject: [PATCH 13/28] Rename idyntree_type_caster.h into
 idyntree_vector_casters.h

---
 bindings/pybind11/CMakeLists.txt                            | 2 +-
 bindings/pybind11/idyntree_core.cpp                         | 2 +-
 bindings/pybind11/idyntree_model.cpp                        | 2 +-
 bindings/pybind11/idyntree_modelio_urdf.cpp                 | 2 +-
 bindings/pybind11/idyntree_sensors.cpp                      | 3 ++-
 .../{idyntree_type_caster.h => idyntree_vector_casters.h}   | 6 +++---
 6 files changed, 9 insertions(+), 8 deletions(-)
 rename bindings/pybind11/{idyntree_type_caster.h => idyntree_vector_casters.h} (97%)

diff --git a/bindings/pybind11/CMakeLists.txt b/bindings/pybind11/CMakeLists.txt
index ba500d7dec7..94b0cae5648 100644
--- a/bindings/pybind11/CMakeLists.txt
+++ b/bindings/pybind11/CMakeLists.txt
@@ -5,7 +5,7 @@ pybind11_add_module(pybind11_idyntree SYSTEM idyntree.cpp
                                     idyntree_model.h idyntree_model.cpp
                                     idyntree_sensors.h idyntree_sensors.cpp
                                     idyntree_modelio_urdf.h idyntree_modelio_urdf.cpp
-                                    idyntree_type_caster.h)
+                                    idyntree_vector_casters.h)
 
 target_link_libraries(pybind11_idyntree PUBLIC idyntree-core
                                                idyntree-model
diff --git a/bindings/pybind11/idyntree_core.cpp b/bindings/pybind11/idyntree_core.cpp
index bb09fac9fef..ec7fe8e8a30 100644
--- a/bindings/pybind11/idyntree_core.cpp
+++ b/bindings/pybind11/idyntree_core.cpp
@@ -1,5 +1,5 @@
 #include "idyntree_core.h"
-#include "idyntree_type_caster.h"
+#include "idyntree_vector_casters.h"
 
 #include <iDynTree/Core/Axis.h>
 #include <iDynTree/Core/Direction.h>
diff --git a/bindings/pybind11/idyntree_model.cpp b/bindings/pybind11/idyntree_model.cpp
index 7ab564f428a..fad4c994772 100644
--- a/bindings/pybind11/idyntree_model.cpp
+++ b/bindings/pybind11/idyntree_model.cpp
@@ -1,5 +1,5 @@
 #include "idyntree_model.h"
-#include "idyntree_type_caster.h"
+#include "idyntree_vector_casters.h"
 
 #include "error_utilities.h"
 
diff --git a/bindings/pybind11/idyntree_modelio_urdf.cpp b/bindings/pybind11/idyntree_modelio_urdf.cpp
index c992554a4b3..7a25441e305 100644
--- a/bindings/pybind11/idyntree_modelio_urdf.cpp
+++ b/bindings/pybind11/idyntree_modelio_urdf.cpp
@@ -1,5 +1,5 @@
 #include "idyntree_modelio_urdf.h"
-#include "idyntree_type_caster.h"
+#include "idyntree_vector_casters.h"
 
 #include "error_utilities.h"
 
diff --git a/bindings/pybind11/idyntree_sensors.cpp b/bindings/pybind11/idyntree_sensors.cpp
index 09730754eab..c771e2aee66 100644
--- a/bindings/pybind11/idyntree_sensors.cpp
+++ b/bindings/pybind11/idyntree_sensors.cpp
@@ -1,4 +1,5 @@
-#include "idyntree_type_caster.h"
+
+#include "idyntree_vector_casters.h"
 #include "idyntree_sensors.h"
 #include "error_utilities.h"
 
diff --git a/bindings/pybind11/idyntree_type_caster.h b/bindings/pybind11/idyntree_vector_casters.h
similarity index 97%
rename from bindings/pybind11/idyntree_type_caster.h
rename to bindings/pybind11/idyntree_vector_casters.h
index 163535d59cf..d0783a7013d 100644
--- a/bindings/pybind11/idyntree_type_caster.h
+++ b/bindings/pybind11/idyntree_vector_casters.h
@@ -1,5 +1,5 @@
-#ifndef IDYNTREE_PYBIND11_IDYNTREE_TYPE_CASTER_H
-#define IDYNTREE_PYBIND11_IDYNTREE_TYPE_CASTER_H
+#ifndef IDYNTREE_PYBIND11_IDYNTREE_VECTOR_CASTERS_H
+#define IDYNTREE_PYBIND11_IDYNTREE_VECTOR_CASTERS_H
 
 #include <type_traits>
 
@@ -272,4 +272,4 @@ struct type_caster<Type,
 } // namespace detail
 } // namespace pybind11
 
-#endif /* end of include guard: IDYNTREE_PYBIND11_IDYNTREE_TYPE_CASTER_H */
+#endif /* end of include guard: IDYNTREE_PYBIND11_IDYNTREE_VECTOR_CASTERS_H */

From 97236d6c87ed904cdaf771937838816d7b528fc1 Mon Sep 17 00:00:00 2001
From: Giulio Romualdi <giulio.romualdi@gmail.com>
Date: Thu, 25 Nov 2021 16:32:12 +0100
Subject: [PATCH 14/28] Bugfix th the idyntree_vector_casters load functions

---
 bindings/pybind11/idyntree_vector_casters.h | 6 ++++--
 1 file changed, 4 insertions(+), 2 deletions(-)

diff --git a/bindings/pybind11/idyntree_vector_casters.h b/bindings/pybind11/idyntree_vector_casters.h
index d0783a7013d..9f7a885e7b2 100644
--- a/bindings/pybind11/idyntree_vector_casters.h
+++ b/bindings/pybind11/idyntree_vector_casters.h
@@ -135,7 +135,8 @@ struct type_caster<Type,
     {
         namespace py = ::pybind11;
 
-        if (!convert && array_t<double>::check_(src))
+        // If we're in no-convert mode, only load if given an array of the correct type
+        if (!convert && !py::isinstance<array_t<double>>(src))
         {
             return false;
         }
@@ -223,7 +224,8 @@ struct type_caster<Type,
     {
         namespace py = ::pybind11;
 
-        if (!convert && array_t<double>::check_(src))
+        // If we're in no-convert mode, only load if given an array of the correct type
+        if (!convert && !py::isinstance<array_t<double>>(src))
         {
             return false;
         }

From b3df484018648f6b0fde42e71eace4a5443c81c4 Mon Sep 17 00:00:00 2001
From: Giulio Romualdi <giulio.romualdi@gmail.com>
Date: Thu, 25 Nov 2021 16:36:38 +0100
Subject: [PATCH 15/28] Use a single namespace conversion in the
 idyntree_vector_casters.h

---
 bindings/pybind11/idyntree_vector_casters.h | 8 ++++----
 1 file changed, 4 insertions(+), 4 deletions(-)

diff --git a/bindings/pybind11/idyntree_vector_casters.h b/bindings/pybind11/idyntree_vector_casters.h
index 9f7a885e7b2..ee7a5d38b53 100644
--- a/bindings/pybind11/idyntree_vector_casters.h
+++ b/bindings/pybind11/idyntree_vector_casters.h
@@ -131,7 +131,7 @@ struct type_caster<Type,
     /**
      * Conversion from Python to C++
      */
-    bool load(handle src, bool convert)
+    bool load(pybind11::handle src, bool convert)
     {
         namespace py = ::pybind11;
 
@@ -167,7 +167,7 @@ struct type_caster<Type,
     /**
      * Conversion from C++ to Python
      */
-    static handle cast(Type src, return_value_policy policy, handle parent)
+    static handle cast(Type src, pybind11::return_value_policy policy, pybind11::handle parent)
     {
         namespace py = ::pybind11;
 
@@ -220,7 +220,7 @@ struct type_caster<Type,
     /**
      * Conversion from Python to C++
      */
-    bool load(handle src, bool convert)
+    bool load(pybind11::handle src, bool convert)
     {
         namespace py = ::pybind11;
 
@@ -260,7 +260,7 @@ struct type_caster<Type,
     /**
      * Conversion from C++ to Python
      */
-    static handle cast(Type src, return_value_policy policy, handle parent)
+    static handle cast(Type src, pybind11::return_value_policy policy, pybind11::handle parent)
     {
         namespace py = ::pybind11;
 

From 6a0db6244aa04cea9a1b02e455a62e723ee55cef Mon Sep 17 00:00:00 2001
From: Giulio Romualdi <giulio.romualdi@gmail.com>
Date: Thu, 25 Nov 2021 18:30:35 +0100
Subject: [PATCH 16/28] Create idyntree-pybind11 library

---
 CMakeLists.txt                                |  4 +--
 bindings/pybind11/CMakeLists.txt              | 28 +++++++++++++++++--
 bindings/pybind11/idyntree_core.cpp           |  2 +-
 bindings/pybind11/idyntree_model.cpp          |  2 +-
 bindings/pybind11/idyntree_modelio_urdf.cpp   |  3 +-
 bindings/pybind11/idyntree_sensors.cpp        |  2 +-
 .../iDynTree/pybind11/VectorCasters.h}        |  0
 7 files changed, 33 insertions(+), 8 deletions(-)
 rename bindings/pybind11/{idyntree_vector_casters.h => include/iDynTree/pybind11/VectorCasters.h} (100%)

diff --git a/CMakeLists.txt b/CMakeLists.txt
index df85a8b77da..68781cc3f0d 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -67,6 +67,8 @@ if(NOT IDYNTREE_ONLY_DOCS)
         list(APPEND _IDYNTREE_EXPORTED_DEPENDENCIES_ONLY_STATIC assimp)
     endif()
 
+    add_subdirectory(bindings)
+
     include(InstallBasicPackageFiles)
     install_basic_package_files(iDynTree VARS_PREFIX ${VARS_PREFIX}
                                          VERSION ${${VARS_PREFIX}_VERSION}
@@ -78,8 +80,6 @@ if(NOT IDYNTREE_ONLY_DOCS)
                                          PRIVATE_DEPENDENCIES ${_IDYNTREE_EXPORTED_DEPENDENCIES_ONLY_STATIC})
 
     include(AddUninstallTarget)
-
-    add_subdirectory(bindings)
 endif()
 
 # add a dox target to generate doxygen documentation
diff --git a/bindings/pybind11/CMakeLists.txt b/bindings/pybind11/CMakeLists.txt
index 94b0cae5648..c7c5a060537 100644
--- a/bindings/pybind11/CMakeLists.txt
+++ b/bindings/pybind11/CMakeLists.txt
@@ -1,3 +1,26 @@
+set(libraryname idyntree-pybind11)
+
+add_library(${libraryname} INTERFACE)
+target_link_libraries(${libraryname} INTERFACE idyntree-core)
+
+# Specify include directories for both compilation and installation process.
+# The $<INSTALL_PREFIX> generator expression is useful to ensure to create
+# relocatable configuration files, see https://cmake.org/cmake/help/latest/manual/cmake-packages.7.html
+# creating-relocatable-packages
+target_include_directories(${libraryname} INTERFACE
+  "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
+  "$<INSTALL_INTERFACE:<INSTALL_PREFIX>/${CMAKE_INSTALL_INCLUDEDIR}>")
+
+# Specify installation targets, typology and destination folders.
+install(TARGETS        ${libraryname}
+  EXPORT               ${PROJECT_NAME})
+
+install(FILES "${CMAKE_CURRENT_SOURCE_DIR}/include/iDynTree/pybind11/VectorCasters.h"
+  DESTINATION "${CMAKE_INSTALL_INCLUDEDIR}/iDynTree/pybind11")
+
+add_library(iDynTree::${libraryname} ALIAS ${libraryname})
+set_property(GLOBAL APPEND PROPERTY ${VARS_PREFIX}_TARGETS ${libraryname})
+
 pybind11_add_module(pybind11_idyntree SYSTEM idyntree.cpp
                                     error_utilities.h error_utilities.cpp
                                     idyntree_core.h idyntree_core.cpp
@@ -5,12 +28,13 @@ pybind11_add_module(pybind11_idyntree SYSTEM idyntree.cpp
                                     idyntree_model.h idyntree_model.cpp
                                     idyntree_sensors.h idyntree_sensors.cpp
                                     idyntree_modelio_urdf.h idyntree_modelio_urdf.cpp
-                                    idyntree_vector_casters.h)
+                                    )
 
 target_link_libraries(pybind11_idyntree PUBLIC idyntree-core
                                                idyntree-model
                                                idyntree-sensors
-                                               idyntree-modelio)
+                                               idyntree-modelio
+                                               iDynTree::idyntree-pybind11)
 
 # The generated Python dynamic module must have the same name as the pybind11
 # module, i.e. `bindings`.
diff --git a/bindings/pybind11/idyntree_core.cpp b/bindings/pybind11/idyntree_core.cpp
index ec7fe8e8a30..35185798ff6 100644
--- a/bindings/pybind11/idyntree_core.cpp
+++ b/bindings/pybind11/idyntree_core.cpp
@@ -1,6 +1,6 @@
 #include "idyntree_core.h"
-#include "idyntree_vector_casters.h"
 
+#include <iDynTree/pybind11/VectorCasters.h>
 #include <iDynTree/Core/Axis.h>
 #include <iDynTree/Core/Direction.h>
 #include <iDynTree/Core/MatrixDynSize.h>
diff --git a/bindings/pybind11/idyntree_model.cpp b/bindings/pybind11/idyntree_model.cpp
index fad4c994772..3e0face3c7f 100644
--- a/bindings/pybind11/idyntree_model.cpp
+++ b/bindings/pybind11/idyntree_model.cpp
@@ -1,5 +1,5 @@
 #include "idyntree_model.h"
-#include "idyntree_vector_casters.h"
+#include <iDynTree/pybind11/VectorCasters.h>
 
 #include "error_utilities.h"
 
diff --git a/bindings/pybind11/idyntree_modelio_urdf.cpp b/bindings/pybind11/idyntree_modelio_urdf.cpp
index 7a25441e305..be71a0b1bcd 100644
--- a/bindings/pybind11/idyntree_modelio_urdf.cpp
+++ b/bindings/pybind11/idyntree_modelio_urdf.cpp
@@ -1,5 +1,6 @@
+#include <iDynTree/pybind11/VectorCasters.h>
+
 #include "idyntree_modelio_urdf.h"
-#include "idyntree_vector_casters.h"
 
 #include "error_utilities.h"
 
diff --git a/bindings/pybind11/idyntree_sensors.cpp b/bindings/pybind11/idyntree_sensors.cpp
index c771e2aee66..189d787895d 100644
--- a/bindings/pybind11/idyntree_sensors.cpp
+++ b/bindings/pybind11/idyntree_sensors.cpp
@@ -1,5 +1,5 @@
 
-#include "idyntree_vector_casters.h"
+#include <iDynTree/pybind11/VectorCasters.h>
 #include "idyntree_sensors.h"
 #include "error_utilities.h"
 
diff --git a/bindings/pybind11/idyntree_vector_casters.h b/bindings/pybind11/include/iDynTree/pybind11/VectorCasters.h
similarity index 100%
rename from bindings/pybind11/idyntree_vector_casters.h
rename to bindings/pybind11/include/iDynTree/pybind11/VectorCasters.h

From 0b3b3498574397ee1f58acda5d0312c61f32a168 Mon Sep 17 00:00:00 2001
From: Giulio Romualdi <giulio.romualdi@gmail.com>
Date: Thu, 25 Nov 2021 18:31:12 +0100
Subject: [PATCH 17/28] Define the iDynTreeBindings project only if
 IDYNTREE_BINDINGS_BUILD_STANDALONE is ON

---
 bindings/CMakeLists.txt | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/bindings/CMakeLists.txt b/bindings/CMakeLists.txt
index cddc9a72e97..9031b4b62d9 100644
--- a/bindings/CMakeLists.txt
+++ b/bindings/CMakeLists.txt
@@ -1,5 +1,4 @@
 cmake_minimum_required(VERSION 3.16)
-project(iDynTreeBindings)
 
 # Detect if we are doing a standalone build of the bindings, using an external iDynTree
 if(CMAKE_SOURCE_DIR STREQUAL CMAKE_CURRENT_SOURCE_DIR)
@@ -9,6 +8,7 @@ else()
 endif()
 
 if(IDYNTREE_BINDINGS_BUILD_STANDALONE)
+  project(iDynTreeBindings)
   # Add CMake helpers
   list(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/../cmake)
   find_package(iDynTree REQUIRED)

From 7552d12ff13be2d1e75c8b8e3484e5b265d821d5 Mon Sep 17 00:00:00 2001
From: Giulio Romualdi <giulio.romualdi@gmail.com>
Date: Thu, 25 Nov 2021 18:45:07 +0100
Subject: [PATCH 18/28] Fix style in VectorCasters.h file

---
 .../include/iDynTree/pybind11/VectorCasters.h | 106 ++++++------------
 1 file changed, 37 insertions(+), 69 deletions(-)

diff --git a/bindings/pybind11/include/iDynTree/pybind11/VectorCasters.h b/bindings/pybind11/include/iDynTree/pybind11/VectorCasters.h
index ee7a5d38b53..08f7d6597a2 100644
--- a/bindings/pybind11/include/iDynTree/pybind11/VectorCasters.h
+++ b/bindings/pybind11/include/iDynTree/pybind11/VectorCasters.h
@@ -18,39 +18,27 @@ namespace detail
 
 /**
  * is_vector_fixsize is used to build a type-dependent expression that check if an element
- * has is an iDynTree::VectorFixSize. This specific implementation is used
- * when the the object is not an iDynTree::VectorFixSize
+ * is an iDynTree::VectorFixSize.
  */
 template<typename>
 struct is_vector_fixsize : std::false_type {};
 
-/**
- * is_vector_fixsize is used to build a type-dependent expression that check if an element
- * has is an iDynTree::VectorFixSize. This specific implementation is used
- * when the the object not an iDynTree::VectorFixSize
- */
 template<unsigned int N>
 struct is_vector_fixsize<::iDynTree::VectorFixSize<N>> : std::true_type {};
 
 /**
  * is_matrix_fixsize is used to build a type-dependent expression that check if an element
- * has is an iDynTree::MatrixFixSize. This specific implementation is used
- * when the the object is not an iDynTree::MatrixFixSize
+ * is an iDynTree::MatrixFixSize.
  */
 template<typename>
 struct is_matrix_fixsize : std::false_type {};
 
-/**
- * is_matrix_fixsize is used to build a type-dependent expression that check if an element
- * has is an iDynTree::MatrixFixSize. This specific implementation is used
- * when the the object is an iDynTree::MatrixFixSize
- */
 template<unsigned int nRows, unsigned int nCols>
 struct is_matrix_fixsize<::iDynTree::MatrixFixSize<nRows, nCols>> : std::true_type {};
 
 /**
  * vector_size_at_compile_time is a utility metafunction to get the compile-time size of an iDynTree
- * vector. This specific implementation is used with VectorDynSize.
+ * vector.
  */
 template<typename>
 struct vector_size_at_compile_time
@@ -58,10 +46,6 @@ struct vector_size_at_compile_time
     constexpr static int value = -1;
 };
 
-/**
- * vector_size_at_compile_time is a utility metafunction to get the compile-time size of an iDynTree
- * vector. This specific implementation is used with VectorFixSize.
- */
 template<unsigned int N>
 struct vector_size_at_compile_time<::iDynTree::VectorFixSize<N>>
 {
@@ -70,7 +54,7 @@ struct vector_size_at_compile_time<::iDynTree::VectorFixSize<N>>
 
 /**
  * matrix_size_at_compile_time is a utility metafunction to get the compile-time rows and cols of
- * an iDynTree matrix. This specific implementation is used with MatrixDynSize.
+ * an iDynTree matrix.
  */
 template<typename>
 struct matrix_size_at_compile_time
@@ -79,10 +63,6 @@ struct matrix_size_at_compile_time
     constexpr static int cols = -1;
 };
 
-/**
- * matrix_size_at_compile_time is a utility metafunction to get the compile-time rows and cols of
- * an iDynTree matrix. This specific implementation is used with MatrixFixSize.
- */
 template<unsigned int nRows, unsigned int nCols>
 struct matrix_size_at_compile_time<::iDynTree::MatrixFixSize<nRows, nCols>>
 {
@@ -95,23 +75,21 @@ struct matrix_size_at_compile_time<::iDynTree::MatrixFixSize<nRows, nCols>>
 /**
  * type_caster implementation for the iDynTree Vectors (both fixed and dynamical size)
  */
-template <typename Type>
-struct type_caster<Type,
-                   enable_if_t<is_vector_fixsize<Type>::value
-                               || std::is_same<Type, iDynTree::VectorDynSize>::value>>
+template <typename VectorType>
+struct type_caster<VectorType,
+                   std::enable_if_t<is_vector_fixsize<VectorType>::value
+                                    || std::is_same<VectorType, iDynTree::VectorDynSize>::value>>
 {
 private:
 
     template <typename U>
-    static bool checkSize(const std::size_t size, const U& value)
-    {
+    static bool checkSize(const std::size_t size, const U& value) {
         return true;
     }
 
     template <typename U,
               typename std::enable_if_t<is_vector_fixsize<U>::value>>
-    static bool checkSize(const std::size_t size, const U& value)
-    {
+    static bool checkSize(const std::size_t size, const U& value) {
         return size == value.size();
     }
 
@@ -121,45 +99,42 @@ struct type_caster<Type,
     static constexpr auto descriptor = _("numpy.ndarray[")
         + npy_format_descriptor<double>::name
         + _("[")
-        + _<is_vector_fixsize<Type>::value>(_<(size_t) vector_size_at_compile_time<Type>::value>(),
+        + _<is_vector_fixsize<VectorType>::value>(_<(size_t) vector_size_at_compile_time<VectorType>::value>(),
                                              _("m"))
         + _(", 1]]");
 
 public:
-    PYBIND11_TYPE_CASTER(Type, descriptor);
+    PYBIND11_TYPE_CASTER(VectorType, descriptor);
 
     /**
      * Conversion from Python to C++
      */
-    bool load(pybind11::handle src, bool convert)
-    {
+    bool load(pybind11::handle src, bool convert) {
         namespace py = ::pybind11;
 
         // If we're in no-convert mode, only load if given an array of the correct type
-        if (!convert && !py::isinstance<array_t<double>>(src))
-        {
+        if (!convert && !py::isinstance<array_t<double>>(src)) {
             return false;
         }
 
         const auto buf = py::array_t<double, py::array::c_style | py::array::forcecast>::ensure(src);
-        if (!buf)
+        if (!buf) {
             return false;
+	}
 
         // It must be a vector
         const std::size_t dims = buf.ndim();
-        if (dims != 1)
-        {
+        if (dims != 1) {
             return false;
         }
 
         // Check if the size of the vector is correct.
-        if (!this->checkSize(buf.size(), value))
-        {
+        if (!this->checkSize(buf.size(), value)) {
             return false;
         }
 
         // Copy the content of the python vector in to the iDynTree vector
-        value = Type(buf.data(), buf.size());
+        value = VectorType(buf.data(), buf.size());
 
         return true;
     }
@@ -167,8 +142,7 @@ struct type_caster<Type,
     /**
      * Conversion from C++ to Python
      */
-    static handle cast(Type src, pybind11::return_value_policy policy, pybind11::handle parent)
-    {
+    static handle cast(VectorType src, pybind11::return_value_policy policy, pybind11::handle parent) {
         namespace py = ::pybind11;
 
         // copy the content of the iDynTree vector in the python one
@@ -180,23 +154,21 @@ struct type_caster<Type,
 /**
  * type_caster implementation for the iDynTree Matrices (both fixed and dynamical size)
  */
-template <typename Type>
-struct type_caster<Type,
-                   enable_if_t<is_matrix_fixsize<Type>::value
-                               || std::is_same<Type, iDynTree::MatrixDynSize>::value>>
+template <typename MatrixType>
+struct type_caster<MatrixType,
+                   enable_if_t<is_matrix_fixsize<MatrixType>::value
+                               || std::is_same<MatrixType, iDynTree::MatrixDynSize>::value>>
 {
 private:
 
     template <typename U>
-    static bool checkSize(const std::size_t size, const U& value)
-    {
+    static bool checkSize(const std::size_t size, const U& value) {
         return true;
     }
 
     template <typename U,
               typename std::enable_if_t<is_matrix_fixsize<U>::value>>
-    static bool checkSize(const std::size_t size, const U& value)
-    {
+    static bool checkSize(const std::size_t size, const U& value) {
         return size == value.rows() * value.cols();
     }
 
@@ -206,44 +178,41 @@ struct type_caster<Type,
     static constexpr auto descriptor = _("numpy.ndarray[")
         + npy_format_descriptor<double>::name
         + _("[")
-        + _<is_matrix_fixsize<Type>::value>(_<(size_t) matrix_size_at_compile_time<Type>::rows>(),
+        + _<is_matrix_fixsize<MatrixType>::value>(_<(size_t) matrix_size_at_compile_time<MatrixType>::rows>(),
                                              _("m"))
         + _(", ")
-        + _<is_matrix_fixsize<Type>::value>(_<(size_t) matrix_size_at_compile_time<Type>::rows>(),
+        + _<is_matrix_fixsize<MatrixType>::value>(_<(size_t) matrix_size_at_compile_time<MatrixType>::rows>(),
                                             _("n"))
         + _("]]");
 
 public:
 
-    PYBIND11_TYPE_CASTER(Type, descriptor);
+    PYBIND11_TYPE_CASTER(MatrixType, descriptor);
 
     /**
      * Conversion from Python to C++
      */
-    bool load(pybind11::handle src, bool convert)
-    {
+    bool load(pybind11::handle src, bool convert) {
         namespace py = ::pybind11;
 
         // If we're in no-convert mode, only load if given an array of the correct type
-        if (!convert && !py::isinstance<array_t<double>>(src))
-        {
+        if (!convert && !py::isinstance<array_t<double>>(src)) {
             return false;
         }
 
         const auto buf = py::array_t<double, py::array::c_style | py::array::forcecast>::ensure(src);
-        if (!buf)
-        {
+        if (!buf) {
             return false;
         }
 
         // since it is a matrix the number of dimension must be equal to 2
         const std::size_t dims = buf.ndim();
-        if (dims != 2)
+        if (dims != 2) {
             return false;
+	}
 
         // Check if the size of the vector is correct.
-        if (!this->checkSize(buf.size(), value))
-        {
+        if (!this->checkSize(buf.size(), value)) {
             return false;
         }
 
@@ -252,7 +221,7 @@ struct type_caster<Type,
         // order
         const std::size_t rows = buf.shape(0);
         const std::size_t cols = buf.shape(1);
-        value = Type(buf.data(), rows, cols);
+        value = MatrixType(buf.data(), rows, cols);
 
         return true;
     }
@@ -260,8 +229,7 @@ struct type_caster<Type,
     /**
      * Conversion from C++ to Python
      */
-    static handle cast(Type src, pybind11::return_value_policy policy, pybind11::handle parent)
-    {
+    static handle cast(MatrixType src, pybind11::return_value_policy policy, pybind11::handle parent) {
         namespace py = ::pybind11;
 
         // Copy the content of the iDynTree matrix into the python one

From c746633955e07c1c83ff241e5b624084901c2f50 Mon Sep 17 00:00:00 2001
From: Giulio Romualdi <giulio.romualdi@gmail.com>
Date: Thu, 25 Nov 2021 18:49:53 +0100
Subject: [PATCH 19/28] Fix style in bindings/pybind11/idyntree_core.cpp file

---
 bindings/pybind11/idyntree_core.cpp | 9 +++------
 1 file changed, 3 insertions(+), 6 deletions(-)

diff --git a/bindings/pybind11/idyntree_core.cpp b/bindings/pybind11/idyntree_core.cpp
index 35185798ff6..71f4b941e9f 100644
--- a/bindings/pybind11/idyntree_core.cpp
+++ b/bindings/pybind11/idyntree_core.cpp
@@ -31,7 +31,7 @@ void transformClassDefinition(py::class_<Transform>& transform) {
       .def(py::init<const MatrixFixSize<4, 4>&>())
       .def_static("Identity", &Transform::Identity)
       .def_property("rotation", &Transform::getRotation, &Transform::setRotation)
-      .def_property("position", &Transform::getPosition,  &Transform::setPosition)
+      .def_property("position", &Transform::getPosition, &Transform::setPosition)
       .def("inverse", &Transform::inverse)
       .def(py::self * py::self)
       .def(
@@ -55,12 +55,10 @@ void iDynTreeCoreBindings(pybind11::module& module) {
       // Do not expose constructor as we do not want users to use this class.
       .def("__repr__", &PositionRaw::toString);
 
-
   py::class_<Position, PositionRaw>(module, "Position")
       .def(py::init())
       .def(py::init<double, double, double>())
-      .def(py::init([](const VectorFixSize<3>& position)
-                    {
+      .def(py::init([](const VectorFixSize<3>& position) {
                         return Position(position(0), position(1), position(2));
                     }))
       .def(py::self + py::self)
@@ -78,8 +76,7 @@ void iDynTreeCoreBindings(pybind11::module& module) {
       .def(py::init<double, double, double,  //
                     double, double, double,  //
                     double, double, double>())
-      .def(py::init([](const MatrixFixSize<3,3>& rotation)
-                    {
+      .def(py::init([](const MatrixFixSize<3,3>& rotation) {
                         return Rotation(rotation);
                     }))
       .def("inverse", &Rotation::inverse)

From 376690f98aca45449641770ec6d7fa9788a7be03 Mon Sep 17 00:00:00 2001
From: Giulio Romualdi <giulio.romualdi@gmail.com>
Date: Thu, 25 Nov 2021 18:53:09 +0100
Subject: [PATCH 20/28] Revert "Update the pybind11 tests"

This reverts commit c95517531690aeb842cabcdc3718fc34dc7e928f.
---
 bindings/pybind11/tests/test_idyntree_core.py | 201 ++++++++++++++++--
 .../pybind11/tests/test_idyntree_model.py     |  26 ++-
 .../pybind11/tests/test_idyntree_sensors.py   |   2 +-
 3 files changed, 197 insertions(+), 32 deletions(-)

diff --git a/bindings/pybind11/tests/test_idyntree_core.py b/bindings/pybind11/tests/test_idyntree_core.py
index e3e4864ebac..3e3a3238ffa 100644
--- a/bindings/pybind11/tests/test_idyntree_core.py
+++ b/bindings/pybind11/tests/test_idyntree_core.py
@@ -6,6 +6,165 @@
 import numpy as np
 
 
+class IDynTreeDynVectorTest(unittest.TestCase):
+
+  def test_dyn_vector_accessor(self):
+    vector = iDynTree.VectorDynSize(5)
+    vector[1] = 3.14
+    self.assertEqual(3.14, vector[1])
+
+  def test_dyn_vector_size(self):
+    vector = iDynTree.VectorDynSize(5)
+    self.assertEqual(len(vector), 5)
+
+  def test_set_zero(self):
+    vector = iDynTree.VectorDynSize(2)
+    vector[0] = 10
+    vector[1] = -6
+    vector.set_zero()
+    for element in vector:
+      self.assertEqual(element, 0)
+
+  def test_buffer(self):
+    vector = iDynTree.VectorDynSize(5)
+    np_array = np.array(vector, copy=False)
+    self.assertEqual(len(np_array), len(vector))
+    np_array[0] = 3.14
+    self.assertEqual(3.14, vector[0])
+
+
+class IDynTreeFixVectorTest(unittest.TestCase):
+
+  def test_fix_vector_accessor(self):
+    vector = iDynTree.Vector3()
+    vector[1] = 3.14
+    self.assertEqual(3.14, vector[1])
+
+  def test_fix_vector_size(self):
+    vector = iDynTree.Vector4()
+    self.assertEqual(len(vector), 4)
+
+  def test_set_zero(self):
+    vector = iDynTree.Vector3()
+    vector[0] = 10
+    vector[1] = -6
+    vector[2] = 1.4
+    vector.set_zero()
+    for element in vector:
+      self.assertEqual(element, 0)
+
+  def test_buffer(self):
+    vector = iDynTree.Vector3()
+    np_array = np.array(vector, copy=False)
+    self.assertEqual(len(np_array), len(vector))
+    np_array[0] = 3.14
+    self.assertEqual(3.14, vector[0])
+
+
+class IDynTreeDynMatrixTest(unittest.TestCase):
+
+  def test_dyn_matrix_accessor(self):
+    matrix = iDynTree.MatrixDynSize(3, 3)
+    matrix[1, 1] = 3.14
+    self.assertEqual(3.14, matrix[1, 1])
+
+  def test_dyn_matrix_size(self):
+    matrix = iDynTree.MatrixDynSize(3, 4)
+    self.assertEqual(3, matrix.rows())
+    self.assertEqual(4, matrix.cols())
+
+  def test_set_zero(self):
+    matrix = iDynTree.MatrixDynSize(3, 4)
+    matrix[0, 2] = 10
+    matrix.set_zero()
+    for r in range(matrix.rows()):
+      for c in range(matrix.cols()):
+        self.assertEqual(matrix[r, c], 0)
+
+  def test_buffer(self):
+    matrix = iDynTree.MatrixDynSize(3, 4)
+    np_mat = np.array(matrix, copy=False)
+    self.assertEqual(np_mat.shape[0], matrix.rows())
+    self.assertEqual(np_mat.shape[1], matrix.cols())
+    np_mat[0, 0] = 3.14
+    self.assertEqual(3.14, matrix[0, 0])
+
+
+class IDynTreeFixMatrixTest(unittest.TestCase):
+
+  def test_fix_matrix_accessor(self):
+    matrix = iDynTree.Matrix3x3()
+    matrix[1, 1] = 3.14
+    self.assertEqual(3.14, matrix[1, 1])
+
+  def test_fix_matrix_size(self):
+    matrix = iDynTree.Matrix4x4()
+    self.assertEqual(4, matrix.rows())
+    self.assertEqual(4, matrix.cols())
+
+  def test_set_zero(self):
+    matrix = iDynTree.Matrix4x4()
+    matrix[0, 1] = 10
+    matrix.set_zero()
+    for r in range(matrix.rows()):
+      for c in range(matrix.cols()):
+        self.assertEqual(matrix[r, c], 0)
+
+  def test_buffer(self):
+    matrix = iDynTree.Matrix4x4()
+    np_mat = np.array(matrix, copy=False)
+    self.assertEqual(np_mat.shape[0], matrix.rows())
+    self.assertEqual(np_mat.shape[1], matrix.cols())
+    np_mat[0, 0] = 3.14
+    self.assertEqual(3.14, matrix[0, 0])
+
+
+class IDynTreePositionTest(unittest.TestCase):
+
+  def test_zero_position(self):
+    pos = iDynTree.Position.Zero()
+    self.assertEqual(len(pos), 3)
+    for element in pos:
+      self.assertEqual(element, 0)
+
+  def test_non_zero_creation(self):
+    pos = iDynTree.Position(1, 2, 3)
+    self.assertEqual(pos[0], 1)
+    self.assertEqual(pos[1], 2)
+    self.assertEqual(pos[2], 3)
+
+  def test_negate_operator(self):
+    pos = iDynTree.Position(1, 2, 3)
+    neg_pos = -pos
+    self.assertEqual(neg_pos[0], -1)
+    self.assertEqual(neg_pos[1], -2)
+    self.assertEqual(neg_pos[2], -3)
+    # Original object is unmodified.
+    self.assertEqual(tuple(pos), (1, 2, 3))
+
+  def test_add_operation(self):
+    pos1 = iDynTree.Position(1, 2, 3)
+    pos2 = iDynTree.Position(4, 5, 6)
+    sum_pos = pos1 + pos2
+    self.assertEqual(sum_pos[0], 5)
+    self.assertEqual(sum_pos[1], 7)
+    self.assertEqual(sum_pos[2], 9)
+    # Original objects are unmodified.
+    self.assertEqual(tuple(pos1), (1, 2, 3))
+    self.assertEqual(tuple(pos2), (4, 5, 6))
+
+  def test_subtract_operation(self):
+    pos1 = iDynTree.Position(1, 2, 3)
+    pos2 = iDynTree.Position(4, 5, 6)
+    sum_pos = pos1 - pos2
+    self.assertEqual(sum_pos[0], -3)
+    self.assertEqual(sum_pos[1], -3)
+    self.assertEqual(sum_pos[2], -3)
+    # Original objects are unmodified.
+    self.assertEqual(tuple(pos1), (1, 2, 3))
+    self.assertEqual(tuple(pos2), (4, 5, 6))
+
+
 class IDynTreeRotationTest(unittest.TestCase):
 
   def test_create_identity(self):
@@ -55,9 +214,9 @@ def test_position_rotation(self):
     rotation = iDynTree.Rotation(0, 0, 1,
                                  1, 0, 0,
                                  0, 1, 0)
-    pos =[0, 0, 1]
+    pos = iDynTree.Position(0, 0, 1)
     pos_rotated = rotation * pos
-    expected_pos = [1, 0, 0]
+    expected_pos = iDynTree.Position(1, 0, 0)
     for i in range(3):
       self.assertAlmostEqual(pos_rotated[i], expected_pos[i])
 
@@ -65,7 +224,7 @@ def test_position_rotation(self):
 class IDynTreeTransformTest(unittest.TestCase):
 
   def test_creation(self):
-    position =[1, 2, 3]
+    position = iDynTree.Position(1, 2, 3)
     rotation = iDynTree.Rotation(0, 0, 1,
                                  1, 0, 0,
                                  0, 1, 0)
@@ -88,24 +247,24 @@ def test_identity(self):
         self.assertEqual(transform.rotation[r, c], 0)
 
   def test_position_transform(self):
-    position = [1, 2, 3]
+    position = iDynTree.Position(1, 2, 3)
     rotation = iDynTree.Rotation(0, 0, 1,
                                  1, 0, 0,
                                  0, 1, 0)
     transform = iDynTree.Transform(rotation, position)
-    pos = [0, 0, 1]
+    pos = iDynTree.Position(0, 0, 1)
     pos_transformed = transform * pos
-    expected_pos = [2, 2, 3]
+    expected_pos = iDynTree.Position(2, 2, 3)
     for i in range(3):
       self.assertAlmostEqual(pos_transformed[i], expected_pos[i])
 
   def test_transform_composition(self):
-    position = [1, 2, 3]
+    position = iDynTree.Position(1, 2, 3)
     rotation = iDynTree.Rotation(0, 0, 1,
                                  1, 0, 0,
                                  0, 1, 0)
     t1 = iDynTree.Transform(rotation, position)
-    position = [3, 4, 5]
+    position = iDynTree.Position(3, 4, 5)
     rotation = iDynTree.Rotation(0, -1,  0,
                                  0,  0, -1,
                                  1,  0,  0)
@@ -125,7 +284,7 @@ def test_creation(self):
     n_factor = np.linalg.norm([1, 2, 3])
     raw_dir = [1 / (n_factor), 2 / n_factor, 3 / n_factor]
 
-    direction = raw_dir
+    direction = iDynTree.Direction(raw_dir[0], raw_dir[1], raw_dir[2])
     self.assertEqual(list(direction), raw_dir)
 
 
@@ -134,8 +293,8 @@ class IDynTreeAxisTest(unittest.TestCase):
   def test_read_origin_and_direction_properties(self):
     n_factor = np.linalg.norm([1, 2, 3])
     raw_dir = [1 / (n_factor), 2 / n_factor, 3 / n_factor]
-    direction = raw_dir
-    origin = [3, 4, 5]
+    direction = iDynTree.Direction(raw_dir[0], raw_dir[1], raw_dir[2])
+    origin = iDynTree.Position(3, 4, 5)
     axis = iDynTree.Axis(direction, origin)
     self.assertEqual(list(axis.direction), raw_dir)
     self.assertEqual(list(axis.origin), [3, 4, 5])
@@ -145,8 +304,8 @@ class IDynTreeSpatialInertiaTest(unittest.TestCase):
 
   def test_creation(self):
     mass = 3.14
-    position = [1, 2, 3]
-    rotational_inertia = np.zeros((3, 3))
+    position = iDynTree.Position(1, 2, 3)
+    rotational_inertia = iDynTree.RotationalInertia()
     rotational_inertia[1, 1] = 1
 
     inertia = iDynTree.SpatialInertia(mass, position, rotational_inertia)
@@ -181,15 +340,15 @@ def r_vector_item(vector, row, col):
 
   def test_sum(self):
     mass = 3.14
-    position = [1, 2, 3]
-    rotational_inertia = np.zeros((3, 3))
+    position = iDynTree.Position(1, 2, 3)
+    rotational_inertia = iDynTree.RotationalInertia()
     rotational_inertia[1, 1] = 1
 
     inertia1 = iDynTree.SpatialInertia(mass, position, rotational_inertia)
 
     mass = 6.28
-    position = [4, 5, 6]
-    rotational_inertia = np.zeros((3, 3))
+    position = iDynTree.Position(4, 5, 6)
+    rotational_inertia = iDynTree.RotationalInertia()
     rotational_inertia[2, 2] = 1
 
     inertia2 = iDynTree.SpatialInertia(mass, position, rotational_inertia)
@@ -217,15 +376,15 @@ def test_sum_assignment(self):
     # We do not explicitly define this operation.
     # Check if Python does the correct thing by using the elementary + operator.
     mass = 3.14
-    position = [1, 2, 3]
-    rotational_inertia = np.zeros((3, 3))
+    position = iDynTree.Position(1, 2, 3)
+    rotational_inertia = iDynTree.RotationalInertia()
     rotational_inertia[1, 1] = 1
 
     inertia1 = iDynTree.SpatialInertia(mass, position, rotational_inertia)
 
     mass = 6.28
-    position = [4, 5, 6]
-    rotational_inertia = np.zeros((3, 3))
+    position = iDynTree.Position(4, 5, 6)
+    rotational_inertia = iDynTree.RotationalInertia()
     rotational_inertia[2, 2] = 1
 
     inertia_sum = iDynTree.SpatialInertia(mass, position, rotational_inertia)
diff --git a/bindings/pybind11/tests/test_idyntree_model.py b/bindings/pybind11/tests/test_idyntree_model.py
index 761d8ed95df..951cb317fc1 100644
--- a/bindings/pybind11/tests/test_idyntree_model.py
+++ b/bindings/pybind11/tests/test_idyntree_model.py
@@ -3,7 +3,6 @@
 import unittest
 
 import idyntree.pybind as iDynTree
-import numpy as np
 
 
 class IDynTreeMaterialTest(unittest.TestCase):
@@ -17,7 +16,11 @@ def test_creation_with_name(self):
 
   def test_color(self):
     material = iDynTree.Material()
-    color = [1, 0, 0, 1]
+    color = iDynTree.Vector4()
+    color[0] = 1.0
+    color[1] = 0.0
+    color[2] = 0.0
+    color[3] = 1.0
     material.color = color
     self.assertEqual(list(material.color), list(color))
 
@@ -35,7 +38,7 @@ def test_name(self):
     self.assertEqual(mesh.name, "a_name")
 
   def test_transform(self):
-    position = [1, 2, 3]
+    position = iDynTree.Position(1, 2, 3)
     rotation = iDynTree.Rotation(0, 0, 1,
                                  1, 0, 0,
                                  0, 1, 0)
@@ -67,7 +70,10 @@ def test_mesh_filename(self):
 
   def test_mesh_scale(self):
     mesh = iDynTree.ExternalMesh()
-    scale = [1, 2, 3]
+    scale = iDynTree.Vector3()
+    scale[0] = 1
+    scale[1] = 2
+    scale[2] = 3
     mesh.scale = scale
     self.assertEqual(list(mesh.scale), list(scale))
 
@@ -108,8 +114,8 @@ def test_index(self):
 
   def test_inertia(self):
     mass = 3.14
-    position = [1, 2, 3]
-    rotational_inertia = np.zeros([3,3])
+    position = iDynTree.Position(1, 2, 3)
+    rotational_inertia = iDynTree.RotationalInertia()
     rotational_inertia[1, 1] = 1
 
     inertia = iDynTree.SpatialInertia(mass, position, rotational_inertia)
@@ -127,7 +133,7 @@ def test_attached_links(self):
     self.assertEqual(joint.get_second_attached_link(), 2)
 
   def test_rest_transform(self):
-    position = (1, 2, 3)
+    position = iDynTree.Position(1, 2, 3)
     rotation = iDynTree.Rotation(0, 0, 1,
                                  1, 0, 0,
                                  0, 1, 0)
@@ -161,7 +167,7 @@ def test_attached_links(self):
     self.assertEqual(joint.get_second_attached_link(), 2)
 
   def test_rest_transform(self):
-    position = (1, 2, 3)
+    position = iDynTree.Position(1, 2, 3)
     rotation = iDynTree.Rotation(0, 0, 1,
                                  1, 0, 0,
                                  0, 1, 0)
@@ -195,7 +201,7 @@ def test_attached_links(self):
     self.assertEqual(joint.get_second_attached_link(), 2)
 
   def test_rest_transform(self):
-    position = (1, 2, 3)
+    position = iDynTree.Position(1, 2, 3)
     rotation = iDynTree.Rotation(0, 0, 1,
                                  1, 0, 0,
                                  0, 1, 0)
@@ -349,7 +355,7 @@ def test_model_one_joint(self):
     self.assertIsNotNone(model.get_joint(0))
 
   def test_model_frame(self):
-    position = (1, 2, 3)
+    position = iDynTree.Position(1, 2, 3)
     rotation = iDynTree.Rotation(0, 0, 1,
                                  1, 0, 0,
                                  0, 1, 0)
diff --git a/bindings/pybind11/tests/test_idyntree_sensors.py b/bindings/pybind11/tests/test_idyntree_sensors.py
index d04b8f7b0ba..d4792cb1f07 100644
--- a/bindings/pybind11/tests/test_idyntree_sensors.py
+++ b/bindings/pybind11/tests/test_idyntree_sensors.py
@@ -47,7 +47,7 @@ def test_link_sensor_link_index(self):
 
   def test_link_sensor_transform(self):
     sensor = iDynTree.AccelerometerSensor()
-    position = (1, 2, 3)
+    position = iDynTree.Position(1, 2, 3)
     rotation = iDynTree.Rotation(0, 0, 1,
                                  1, 0, 0,
                                  0, 1, 0)

From a7522fe28ab3ee6a7a8e3056cdfc51e1d82d0828 Mon Sep 17 00:00:00 2001
From: Giulio Romualdi <giulio.romualdi@gmail.com>
Date: Thu, 25 Nov 2021 19:02:33 +0100
Subject: [PATCH 21/28] Update of the python tests

---
 bindings/pybind11/tests/test_idyntree_core.py | 124 ++----------------
 .../pybind11/tests/test_idyntree_model.py     |  11 +-
 2 files changed, 13 insertions(+), 122 deletions(-)

diff --git a/bindings/pybind11/tests/test_idyntree_core.py b/bindings/pybind11/tests/test_idyntree_core.py
index 3e3a3238ffa..178359c2a14 100644
--- a/bindings/pybind11/tests/test_idyntree_core.py
+++ b/bindings/pybind11/tests/test_idyntree_core.py
@@ -6,119 +6,6 @@
 import numpy as np
 
 
-class IDynTreeDynVectorTest(unittest.TestCase):
-
-  def test_dyn_vector_accessor(self):
-    vector = iDynTree.VectorDynSize(5)
-    vector[1] = 3.14
-    self.assertEqual(3.14, vector[1])
-
-  def test_dyn_vector_size(self):
-    vector = iDynTree.VectorDynSize(5)
-    self.assertEqual(len(vector), 5)
-
-  def test_set_zero(self):
-    vector = iDynTree.VectorDynSize(2)
-    vector[0] = 10
-    vector[1] = -6
-    vector.set_zero()
-    for element in vector:
-      self.assertEqual(element, 0)
-
-  def test_buffer(self):
-    vector = iDynTree.VectorDynSize(5)
-    np_array = np.array(vector, copy=False)
-    self.assertEqual(len(np_array), len(vector))
-    np_array[0] = 3.14
-    self.assertEqual(3.14, vector[0])
-
-
-class IDynTreeFixVectorTest(unittest.TestCase):
-
-  def test_fix_vector_accessor(self):
-    vector = iDynTree.Vector3()
-    vector[1] = 3.14
-    self.assertEqual(3.14, vector[1])
-
-  def test_fix_vector_size(self):
-    vector = iDynTree.Vector4()
-    self.assertEqual(len(vector), 4)
-
-  def test_set_zero(self):
-    vector = iDynTree.Vector3()
-    vector[0] = 10
-    vector[1] = -6
-    vector[2] = 1.4
-    vector.set_zero()
-    for element in vector:
-      self.assertEqual(element, 0)
-
-  def test_buffer(self):
-    vector = iDynTree.Vector3()
-    np_array = np.array(vector, copy=False)
-    self.assertEqual(len(np_array), len(vector))
-    np_array[0] = 3.14
-    self.assertEqual(3.14, vector[0])
-
-
-class IDynTreeDynMatrixTest(unittest.TestCase):
-
-  def test_dyn_matrix_accessor(self):
-    matrix = iDynTree.MatrixDynSize(3, 3)
-    matrix[1, 1] = 3.14
-    self.assertEqual(3.14, matrix[1, 1])
-
-  def test_dyn_matrix_size(self):
-    matrix = iDynTree.MatrixDynSize(3, 4)
-    self.assertEqual(3, matrix.rows())
-    self.assertEqual(4, matrix.cols())
-
-  def test_set_zero(self):
-    matrix = iDynTree.MatrixDynSize(3, 4)
-    matrix[0, 2] = 10
-    matrix.set_zero()
-    for r in range(matrix.rows()):
-      for c in range(matrix.cols()):
-        self.assertEqual(matrix[r, c], 0)
-
-  def test_buffer(self):
-    matrix = iDynTree.MatrixDynSize(3, 4)
-    np_mat = np.array(matrix, copy=False)
-    self.assertEqual(np_mat.shape[0], matrix.rows())
-    self.assertEqual(np_mat.shape[1], matrix.cols())
-    np_mat[0, 0] = 3.14
-    self.assertEqual(3.14, matrix[0, 0])
-
-
-class IDynTreeFixMatrixTest(unittest.TestCase):
-
-  def test_fix_matrix_accessor(self):
-    matrix = iDynTree.Matrix3x3()
-    matrix[1, 1] = 3.14
-    self.assertEqual(3.14, matrix[1, 1])
-
-  def test_fix_matrix_size(self):
-    matrix = iDynTree.Matrix4x4()
-    self.assertEqual(4, matrix.rows())
-    self.assertEqual(4, matrix.cols())
-
-  def test_set_zero(self):
-    matrix = iDynTree.Matrix4x4()
-    matrix[0, 1] = 10
-    matrix.set_zero()
-    for r in range(matrix.rows()):
-      for c in range(matrix.cols()):
-        self.assertEqual(matrix[r, c], 0)
-
-  def test_buffer(self):
-    matrix = iDynTree.Matrix4x4()
-    np_mat = np.array(matrix, copy=False)
-    self.assertEqual(np_mat.shape[0], matrix.rows())
-    self.assertEqual(np_mat.shape[1], matrix.cols())
-    np_mat[0, 0] = 3.14
-    self.assertEqual(3.14, matrix[0, 0])
-
-
 class IDynTreePositionTest(unittest.TestCase):
 
   def test_zero_position(self):
@@ -236,6 +123,17 @@ def test_creation(self):
     for i in range(3):
       self.assertEqual(transform.position[i], position[i])
 
+  def test_creation_from_numpy(self):
+    position = np.array([1., 2., 3.])
+    rotation = np.array([[0., 0., 1.], [1., 0., 0.], [0., 1., 0.]])
+    transform = iDynTree.Transform(rotation, position)
+
+    # Assert the content is the same.
+    for r, c in itertools.product(range(3), range(3)):
+      self.assertEqual(transform.rotation[r, c], rotation[r, c])
+    for i in range(3):
+      self.assertEqual(transform.position[i], position[i])
+
   def test_identity(self):
     transform = iDynTree.Transform.Identity()
     for i in range(3):
diff --git a/bindings/pybind11/tests/test_idyntree_model.py b/bindings/pybind11/tests/test_idyntree_model.py
index 951cb317fc1..d292123776f 100644
--- a/bindings/pybind11/tests/test_idyntree_model.py
+++ b/bindings/pybind11/tests/test_idyntree_model.py
@@ -16,11 +16,7 @@ def test_creation_with_name(self):
 
   def test_color(self):
     material = iDynTree.Material()
-    color = iDynTree.Vector4()
-    color[0] = 1.0
-    color[1] = 0.0
-    color[2] = 0.0
-    color[3] = 1.0
+    color = [1., 0., 0., 1.]
     material.color = color
     self.assertEqual(list(material.color), list(color))
 
@@ -70,10 +66,7 @@ def test_mesh_filename(self):
 
   def test_mesh_scale(self):
     mesh = iDynTree.ExternalMesh()
-    scale = iDynTree.Vector3()
-    scale[0] = 1
-    scale[1] = 2
-    scale[2] = 3
+    scale = [1, 2, 3]
     mesh.scale = scale
     self.assertEqual(list(mesh.scale), list(scale))
 

From f04e844802f585d05db557db335dc107b5c26b38 Mon Sep 17 00:00:00 2001
From: Giulio Romualdi <giulio.romualdi@gmail.com>
Date: Thu, 25 Nov 2021 21:16:09 +0100
Subject: [PATCH 22/28] Implement baseVectorDefinition and baseMatrixDefinition
 in bindings/pybind11/idyntree_core.cpp

---
 bindings/pybind11/idyntree_core.cpp | 82 +++++++++++++++++++++++++----
 1 file changed, 72 insertions(+), 10 deletions(-)

diff --git a/bindings/pybind11/idyntree_core.cpp b/bindings/pybind11/idyntree_core.cpp
index 71f4b941e9f..ed45997298b 100644
--- a/bindings/pybind11/idyntree_core.cpp
+++ b/bindings/pybind11/idyntree_core.cpp
@@ -25,6 +25,66 @@ namespace bindings {
 namespace {
 namespace py = ::pybind11;
 
+template <typename VectorType>
+void baseVectorDefinition(py::class_<VectorType>& vector) {
+  vector
+      .def("__getitem__", py::overload_cast<const std::size_t>(
+                              &VectorType::operator(), py::const_))
+      .def("__setitem__",
+           [](VectorType& the_vector, std::size_t index,
+              double new_value) { the_vector(index) = new_value; })
+      .def("__len__", &VectorType::size)
+      .def("__iter__",
+          [](const VectorType& s) {
+            return py::make_iterator(s.begin(), s.end());
+          },
+          py::keep_alive<
+              0, 1>() /* Essential: keep object alive while iterator exists */)
+      .def("set_zero", &VectorType::zero)
+      .def("__repr__", &VectorType::toString)
+      .def_buffer([](VectorType& v) -> py::buffer_info {
+        return py::buffer_info(
+            v.data(),                                /* Pointer to buffer */
+            sizeof(double),                          /* Size of one scalar */
+            py::format_descriptor<double>::format(), /* Python struct-style
+                                                        format descriptor */
+            1,                                       /* Number of dimensions */
+            {v.size()},                              /* Buffer dimensions */
+            {sizeof(double)} /* Strides (in bytes) for each index */
+        );
+      });
+}
+
+template <typename MatrixType>
+void baseMatrixDefinition(py::class_<MatrixType>& matrix) {
+  matrix
+      .def("__getitem__",
+           [](MatrixType& matrix,
+              std::pair<std::size_t, std::size_t> indices) {
+             return matrix.getVal(indices.first, indices.second);
+           })
+      .def("__setitem__",
+           [](MatrixType& matrix,
+              std::pair<std::size_t, std::size_t> indices, double item) {
+             return matrix.setVal(indices.first, indices.second, item);
+           })
+      .def("rows", &MatrixType::rows)
+      .def("cols", &MatrixType::cols)
+      .def("set_zero", &MatrixType::zero)
+      .def("__repr__", &MatrixType::toString)
+      .def_buffer([](MatrixType& m) -> py::buffer_info {
+        return py::buffer_info(
+            m.data(),                                /* Pointer to buffer */
+            sizeof(double),                          /* Size of one scalar */
+            py::format_descriptor<double>::format(), /* Python struct-style
+                                                        format descriptor */
+            2,                                       /* Number of dimensions */
+            {m.rows(), m.cols()},                    /* Buffer dimensions */
+            {sizeof(double) * m.cols(), /* Strides (in bytes) for each index */
+             sizeof(double)});
+      });
+}
+
 void transformClassDefinition(py::class_<Transform>& transform) {
   transform.def(py::init())
       .def(py::init<const Rotation&, const Position&>())
@@ -48,12 +108,12 @@ void transformClassDefinition(py::class_<Transform>& transform) {
 }
 }  // namespace
 
+
 void iDynTreeCoreBindings(pybind11::module& module) {
 
   // Positions, Rotations and Transforms.
-  py::class_<PositionRaw>(module, "_PositionRaw")
-      // Do not expose constructor as we do not want users to use this class.
-      .def("__repr__", &PositionRaw::toString);
+  py::class_<PositionRaw> positionRaw(module, "_PositionRaw",  py::buffer_protocol());
+  baseVectorDefinition(positionRaw);
 
   py::class_<Position, PositionRaw>(module, "Position")
       .def(py::init())
@@ -67,9 +127,8 @@ void iDynTreeCoreBindings(pybind11::module& module) {
       .def_static("Zero", &Position::Zero);
   py::implicitly_convertible<iDynTree::VectorFixSize<3>, iDynTree::Position>();
 
-  py::class_<RotationRaw>(module, "_RotationRaw")
-      // Do not expose constructor as we do not want users to use this class.
-      .def("__repr__", &RotationRaw::toString);
+  py::class_<RotationRaw> rotationRaw(module, "_RotationRaw", py::buffer_protocol());
+  baseMatrixDefinition(rotationRaw);
 
   py::class_<Rotation, RotationRaw>(module, "Rotation")
       .def(py::init())
@@ -94,8 +153,9 @@ void iDynTreeCoreBindings(pybind11::module& module) {
   transformClassDefinition(transform);
 
   // Other classes.
-  py::class_<Direction>(module, "Direction")
-      .def(py::init<double, double, double>());
+  py::class_<Direction> direction(module, "Direction", py::buffer_protocol());
+  direction.def(py::init<double, double, double>());
+  baseVectorDefinition(direction);
 
   py::class_<Axis>(module, "Axis")
       .def(py::init<const Direction&, const Position&>())
@@ -103,8 +163,10 @@ void iDynTreeCoreBindings(pybind11::module& module) {
       .def_property("origin", &Axis::getOrigin, &Axis::setOrigin)
       .def("__repr__", &Axis::toString);
 
-  py::class_<RotationalInertiaRaw>(module, "RotationalInertia")
-      .def(py::init());
+  py::class_<RotationalInertiaRaw> rotationalInertia(module, "RotationalInertia",
+                                                     py::buffer_protocol());
+  rotationalInertia.def(py::init());
+  baseMatrixDefinition(rotationalInertia);
 
   py::class_<SpatialInertiaRaw>(module, "_SpatialInertiaRaw")
       .def("from_rotational_inertia_wrt_center_of_mass",

From c16496f7e12a12e84e2606e78643397ab292af38 Mon Sep 17 00:00:00 2001
From: Giulio Romualdi <giulio.romualdi@gmail.com>
Date: Thu, 25 Nov 2021 23:53:03 +0100
Subject: [PATCH 23/28] Bugfix in VectorCasters.h

---
 .../pybind11/include/iDynTree/pybind11/VectorCasters.h | 10 ++++++----
 1 file changed, 6 insertions(+), 4 deletions(-)

diff --git a/bindings/pybind11/include/iDynTree/pybind11/VectorCasters.h b/bindings/pybind11/include/iDynTree/pybind11/VectorCasters.h
index 08f7d6597a2..7312b74ea67 100644
--- a/bindings/pybind11/include/iDynTree/pybind11/VectorCasters.h
+++ b/bindings/pybind11/include/iDynTree/pybind11/VectorCasters.h
@@ -82,13 +82,14 @@ struct type_caster<VectorType,
 {
 private:
 
-    template <typename U>
+    template <typename U,
+	      typename std::enable_if_t<std::is_same<U, iDynTree::VectorDynSize>::value>* = nullptr>
     static bool checkSize(const std::size_t size, const U& value) {
         return true;
     }
 
     template <typename U,
-              typename std::enable_if_t<is_vector_fixsize<U>::value>>
+              typename std::enable_if_t<is_vector_fixsize<U>::value>* = nullptr>
     static bool checkSize(const std::size_t size, const U& value) {
         return size == value.size();
     }
@@ -161,13 +162,14 @@ struct type_caster<MatrixType,
 {
 private:
 
-    template <typename U>
+    template <typename U,
+              typename std::enable_if_t<std::is_same<U, iDynTree::MatrixDynSize>::value>* = nullptr>
     static bool checkSize(const std::size_t size, const U& value) {
         return true;
     }
 
     template <typename U,
-              typename std::enable_if_t<is_matrix_fixsize<U>::value>>
+              typename std::enable_if_t<is_matrix_fixsize<U>::value>* = nullptr>
     static bool checkSize(const std::size_t size, const U& value) {
         return size == value.rows() * value.cols();
     }

From 609923db4ba91cc2181f842fb9f91e10d5e4aa85 Mon Sep 17 00:00:00 2001
From: Giulio Romualdi <giulio.romualdi@gmail.com>
Date: Thu, 25 Nov 2021 23:53:30 +0100
Subject: [PATCH 24/28] Uniform pybind11 target_link_libraries

---
 bindings/pybind11/CMakeLists.txt | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/bindings/pybind11/CMakeLists.txt b/bindings/pybind11/CMakeLists.txt
index c7c5a060537..c8364eb046f 100644
--- a/bindings/pybind11/CMakeLists.txt
+++ b/bindings/pybind11/CMakeLists.txt
@@ -34,7 +34,7 @@ target_link_libraries(pybind11_idyntree PUBLIC idyntree-core
                                                idyntree-model
                                                idyntree-sensors
                                                idyntree-modelio
-                                               iDynTree::idyntree-pybind11)
+                                               idyntree-pybind11)
 
 # The generated Python dynamic module must have the same name as the pybind11
 # module, i.e. `bindings`.

From 25d57285f9a9cbeaac8cdcb59fb2181eec619af4 Mon Sep 17 00:00:00 2001
From: Giulio Romualdi <giulio.romualdi@gmail.com>
Date: Thu, 25 Nov 2021 23:54:41 +0100
Subject: [PATCH 25/28] Implement the test for pybind11 type_caster

---
 bindings/pybind11/CMakeLists.txt              |  2 +-
 bindings/pybind11/tests/CMakeLists.txt        | 10 +++
 .../tests/test_idyntree_vector_casters.cpp    | 61 +++++++++++++++++++
 .../tests/test_idyntree_vector_casters.py     | 44 +++++++++++++
 4 files changed, 116 insertions(+), 1 deletion(-)
 create mode 100644 bindings/pybind11/tests/test_idyntree_vector_casters.cpp
 create mode 100644 bindings/pybind11/tests/test_idyntree_vector_casters.py

diff --git a/bindings/pybind11/CMakeLists.txt b/bindings/pybind11/CMakeLists.txt
index c8364eb046f..c81fca341cd 100644
--- a/bindings/pybind11/CMakeLists.txt
+++ b/bindings/pybind11/CMakeLists.txt
@@ -44,7 +44,7 @@ set_target_properties(pybind11_idyntree PROPERTIES
 
 # if compile tests execute also python tests
 if(IDYNTREE_COMPILE_TESTS)
-    add_subdirectory(tests)
+    add_subdirectory(tests idyntree_pybind11_test)
 endif()
 
 # Output package is:
diff --git a/bindings/pybind11/tests/CMakeLists.txt b/bindings/pybind11/tests/CMakeLists.txt
index 45963d4b4ba..33913443d0e 100644
--- a/bindings/pybind11/tests/CMakeLists.txt
+++ b/bindings/pybind11/tests/CMakeLists.txt
@@ -1,3 +1,13 @@
+pybind11_add_module(test_idyntree_vector_casters test_idyntree_vector_casters.cpp)
+target_link_libraries(test_idyntree_vector_casters PUBLIC idyntree-pybind11)
+
+# The generated Python dynamic module must have the same name as the pybind11
+# module, i.e. `bindings`.
+set_target_properties (test_idyntree_vector_casters PROPERTIES
+  LIBRARY_OUTPUT_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}
+  OUTPUT_NAME "vector_casters"
+)
+
 add_test (NAME pybind11_idyntree_test
   COMMAND ${Python3_EXECUTABLE} -m unittest discover -s ${CMAKE_CURRENT_SOURCE_DIR}
   WORKING_DIRECTORY ${PROJECT_BINARY_DIR}/bindings/pybind11
diff --git a/bindings/pybind11/tests/test_idyntree_vector_casters.cpp b/bindings/pybind11/tests/test_idyntree_vector_casters.cpp
new file mode 100644
index 00000000000..7aa3c5cc424
--- /dev/null
+++ b/bindings/pybind11/tests/test_idyntree_vector_casters.cpp
@@ -0,0 +1,61 @@
+#include <pybind11/pybind11.h>
+#include <iDynTree/pybind11/VectorCasters.h>
+
+#include <iDynTree/Core/VectorDynSize.h>
+#include <iDynTree/Core/VectorFixSize.h>
+#include <iDynTree/Core/MatrixDynSize.h>
+#include <iDynTree/Core/MatrixFixSize.h>
+
+
+class TestClass
+{
+    iDynTree::VectorDynSize vectorDyn;
+    iDynTree::VectorFixSize<3> vectorFix;
+    iDynTree::MatrixDynSize matrixDyn;
+    iDynTree::MatrixFixSize<4,5> matrixFix;
+
+public:
+
+    void setVectorDyn(const iDynTree::VectorDynSize& vector) {
+        vectorDyn = vector;
+    }
+
+    const iDynTree::VectorDynSize& getVectorDyn() const {
+        return vectorDyn;
+    }
+
+    void setMatrixDyn(const iDynTree::MatrixDynSize& matrix) {
+        matrixDyn = matrix;
+    }
+
+    const iDynTree::MatrixDynSize& getMatrixDyn() const {
+        return matrixDyn;
+    }
+
+    void setVectorFix(const iDynTree::VectorFixSize<3>& vector) {
+        vectorFix = vector;
+    }
+
+    const iDynTree::VectorFixSize<3>& getVectorFix() const {
+        return vectorFix;
+    }
+
+    void setMatrixFix(const iDynTree::MatrixFixSize<4,5>& matrix) {
+        matrixFix = matrix;
+    }
+
+    const iDynTree::MatrixFixSize<4,5>& getMatrixFix() const {
+        return matrixFix;
+    }
+};
+
+
+namespace py = ::pybind11;
+PYBIND11_MODULE(vector_casters, m) {
+  py::class_<TestClass>(m, "TestClass")
+    .def(py::init())
+    .def_property("vector_dyn", &TestClass::getVectorDyn, &TestClass::setVectorDyn)
+    .def_property("vector_fix", &TestClass::getVectorFix, &TestClass::setVectorFix)
+    .def_property("matrix_dyn", &TestClass::getMatrixDyn, &TestClass::setMatrixDyn)
+    .def_property("matrix_fix", &TestClass::getMatrixFix, &TestClass::setMatrixFix);
+}
diff --git a/bindings/pybind11/tests/test_idyntree_vector_casters.py b/bindings/pybind11/tests/test_idyntree_vector_casters.py
new file mode 100644
index 00000000000..f790062a75e
--- /dev/null
+++ b/bindings/pybind11/tests/test_idyntree_vector_casters.py
@@ -0,0 +1,44 @@
+"""Tests for idyntree-vector-casters."""
+import unittest
+
+from idyntree_pybind11_test.vector_casters import TestClass
+import numpy as np
+
+
+class TestClassTest(unittest.TestCase):
+
+  def test_vector_fix(self):
+    obj = TestClass()
+    vector = [1., 2., 3.]
+    obj.vector_fix = vector
+    self.assertEqual(len(obj.vector_fix), 3)
+    self.assertEqual(list(obj.vector_fix), list(vector))
+
+  def test_vector_dyn(self):
+    obj = TestClass()
+    vector = [1., 2., 3., 5., 6., 7.]
+    obj.vector_dyn = vector
+    self.assertEqual(len(obj.vector_dyn), len(vector))
+    self.assertEqual(list(obj.vector_dyn), list(vector))
+
+  def test_matrix_fix(self):
+    obj = TestClass()
+    matrix = np.random.rand(4, 5)
+    obj.matrix_fix = matrix
+    self.assertEqual(obj.matrix_fix.shape, matrix.shape)
+    for i in range(4):
+      for j in range(3):
+        self.assertEqual(obj.matrix_fix[i,j], matrix[i,j])
+
+  def test_matrix_dyn(self):
+    obj = TestClass()
+    matrix = np.random.rand(10, 10)
+    obj.matrix_dyn = matrix
+    self.assertEqual(obj.matrix_dyn.shape, matrix.shape)
+    for i in range(10):
+      for j in range(10):
+        self.assertEqual(obj.matrix_dyn[i,j], matrix[i,j])
+
+if __name__ == "__main__":
+  unittest.main()
+

From 9258e6bd0eca0587cfa3af2355aab44ea916ab6c Mon Sep 17 00:00:00 2001
From: Giulio Romualdi <giulio.romualdi@gmail.com>
Date: Fri, 26 Nov 2021 21:47:38 +0100
Subject: [PATCH 26/28] Fix typo in pybind11/CMakeLists.txt

---
 bindings/pybind11/CMakeLists.txt | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/bindings/pybind11/CMakeLists.txt b/bindings/pybind11/CMakeLists.txt
index 5a1b48606ac..f0f5ea3c4c2 100644
--- a/bindings/pybind11/CMakeLists.txt
+++ b/bindings/pybind11/CMakeLists.txt
@@ -9,7 +9,7 @@ target_link_libraries(${libraryname} INTERFACE idyntree-core)
 # creating-relocatable-packages
 target_include_directories(${libraryname} INTERFACE
   "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
-  "$<INSTALL_INTERFACE:<INSTALL_PREFIX>/${CMAKE_INSTALL_INCLUDEDIR}>")
+  "$<INSTALL_INTERFACE:$<INSTALL_PREFIX>/${CMAKE_INSTALL_INCLUDEDIR}>")
 
 # Specify installation targets, typology and destination folders.
 install(TARGETS        ${libraryname}

From 58f5fca5bd1c637b7253b83f6e6a943a9a46006b Mon Sep 17 00:00:00 2001
From: Giulio Romualdi <giulio.romualdi@gmail.com>
Date: Sun, 28 Nov 2021 15:21:17 +0100
Subject: [PATCH 27/28] Fix style in VectorCasters.h

---
 .../include/iDynTree/pybind11/VectorCasters.h        | 12 +++++++-----
 1 file changed, 7 insertions(+), 5 deletions(-)

diff --git a/bindings/pybind11/include/iDynTree/pybind11/VectorCasters.h b/bindings/pybind11/include/iDynTree/pybind11/VectorCasters.h
index 7312b74ea67..542ffefa61b 100644
--- a/bindings/pybind11/include/iDynTree/pybind11/VectorCasters.h
+++ b/bindings/pybind11/include/iDynTree/pybind11/VectorCasters.h
@@ -83,7 +83,7 @@ struct type_caster<VectorType,
 private:
 
     template <typename U,
-	      typename std::enable_if_t<std::is_same<U, iDynTree::VectorDynSize>::value>* = nullptr>
+              typename std::enable_if_t<std::is_same<U, iDynTree::VectorDynSize>::value>* = nullptr>
     static bool checkSize(const std::size_t size, const U& value) {
         return true;
     }
@@ -121,7 +121,7 @@ struct type_caster<VectorType,
         const auto buf = py::array_t<double, py::array::c_style | py::array::forcecast>::ensure(src);
         if (!buf) {
             return false;
-	}
+        }
 
         // It must be a vector
         const std::size_t dims = buf.ndim();
@@ -143,7 +143,8 @@ struct type_caster<VectorType,
     /**
      * Conversion from C++ to Python
      */
-    static handle cast(VectorType src, pybind11::return_value_policy policy, pybind11::handle parent) {
+    static handle cast(const VectorType& src, pybind11::return_value_policy policy,
+                       pybind11::handle parent) {
         namespace py = ::pybind11;
 
         // copy the content of the iDynTree vector in the python one
@@ -211,7 +212,7 @@ struct type_caster<MatrixType,
         const std::size_t dims = buf.ndim();
         if (dims != 2) {
             return false;
-	}
+        }
 
         // Check if the size of the vector is correct.
         if (!this->checkSize(buf.size(), value)) {
@@ -231,7 +232,8 @@ struct type_caster<MatrixType,
     /**
      * Conversion from C++ to Python
      */
-    static handle cast(MatrixType src, pybind11::return_value_policy policy, pybind11::handle parent) {
+    static handle cast(const MatrixType& src, pybind11::return_value_policy policy,
+                       pybind11::handle parent) {
         namespace py = ::pybind11;
 
         // Copy the content of the iDynTree matrix into the python one

From 10658f3a0d46959b2b833503932787fc74792ab0 Mon Sep 17 00:00:00 2001
From: Giulio Romualdi <giulio.romualdi@gmail.com>
Date: Sun, 28 Nov 2021 15:56:35 +0100
Subject: [PATCH 28/28] Fix WORKING_DIRECTORY of pybind11 tests

---
 bindings/pybind11/tests/CMakeLists.txt | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/bindings/pybind11/tests/CMakeLists.txt b/bindings/pybind11/tests/CMakeLists.txt
index 5a248f58286..e02d6a5575e 100644
--- a/bindings/pybind11/tests/CMakeLists.txt
+++ b/bindings/pybind11/tests/CMakeLists.txt
@@ -10,5 +10,5 @@ set_target_properties (test_idyntree_vector_casters PROPERTIES
 
 add_test (NAME pybind11_idyntree_test
   COMMAND ${Python3_EXECUTABLE} -m unittest discover -s ${CMAKE_CURRENT_SOURCE_DIR}
-  WORKING_DIRECTORY ${PROJECT_BINARY_DIR}/pybind11
+  WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}/.."
 )