Use MatrixView and Span in KinDynComputations class

CHANGELOG.md

@@ -15,6 +15,7 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0
   and existing objects can be converted to NumPy arrays (`*.toNumPy`) (https://github.com/robotology/idyntree/pull/726).
 - iDynTree Python bindings can now be installed with `pip3 install git+https://github.com/robotology/idyntree.git` (https://github.com/robotology/idyntree/pull/733).
 - Implement the MatrixView class (https://github.com/robotology/idyntree/pull/734)
+- Add the possibility to use `MatrixView` and `Span` as input/output objects for `KinDynComputations` class (https://github.com/robotology/idyntree/pull/736).
 
 ### Fixed
 - Fixed bug in `yarprobotstatepublisher` that caused segmentation fault each time an unknown joint name was read from the input joint states topic (https://github.com/robotology/idyntree/pull/719)
@@ -26,6 +27,7 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0
 - The Python method `*.fromPyList` is replaced by `*.FromPython` (https://github.com/robotology/idyntree/pull/726).
 - The minimum required CMake version to configure and compile iDynTree is now 3.16 (https://github.com/robotology/idyntree/pull/732).
 - The Python package name of the SWIG bindings changed from `iDynTree` to `idyntree.bindings` (https://github.com/robotology/idyntree/pull/733, https://github.com/robotology/idyntree/pull/735). To continue referring to iDynTree classes as `iDynTree.<ClassName>`, you can change your `import iDynTree` statements to `import idyntree.bindings as iDynTree`. Otherwise, you can use `import idyntree.bindings` to refer them as `idyntree.bindings.<ClassName>`.
+- Improve the use of `const` keyword  in `KinDynComputations`(https://github.com/robotology/idyntree/pull/736).
 
 ### Removed 
 - Remove the CMake option IDYNTREE_USES_KDL and all the classes available when enabling it. They were deprecated in iDynTree 1.0 .

bindings/iDynTree.i

@@ -220,8 +220,10 @@ namespace std {
 %include "iDynTree/Core/Transform.h"
 %include "iDynTree/Core/TransformDerivative.h"
 %include "iDynTree/Core/Span.h"
+%include "iDynTree/Core/MatrixView.h"
 
 %template(DynamicSpan) iDynTree::Span<double, iDynTree::dynamic_extent>;
+%template(DynamicMatrixView) iDynTree::MatrixView<double>;
 
 // Model related data structures
 %include "iDynTree/Model/Indices.h"

src/core/include/iDynTree/Core/MatrixDynSize.h

@@ -98,6 +98,16 @@ namespace iDynTree
          */
         MatrixDynSize(const MatrixDynSize& other);
 
+
+        /**
+         * Constructor from MatrixView object
+         *
+         * @param other MatrixView to copy
+         * \warning performs dynamic memory allocation operations
+         *
+         */
+        MatrixDynSize(iDynTree::MatrixView<const double> other);
+
         /**
          * Assignment operator
          *
@@ -114,7 +124,7 @@ namespace iDynTree
          *
          * @return *this
          */
-        MatrixDynSize& operator=(const MatrixView<const double>& other);
+        MatrixDynSize& operator=(iDynTree::MatrixView<const double> other);
 
         /**
          * Denstructor

src/core/include/iDynTree/Core/MatrixFixSize.h

@@ -13,6 +13,7 @@
 
 
 #include <iDynTree/Core/Utils.h>
+#include <iDynTree/Core/MatrixView.h>
 
 #include <cassert>
 #include <cstring>
@@ -76,6 +77,13 @@ namespace iDynTree
          */
         MatrixFixSize(const double * in_data, const unsigned int in_rows, const unsigned int in_cols);
 
+        /**
+         * Constructor from a MatrixView
+         *
+         * \warning this class stores data using the row major order
+         */
+        MatrixFixSize(iDynTree::MatrixView<const double> other);
+
         /**
          * @name Matrix interface methods.
          * Methods exposing a matrix-like interface to MatrixFixSize.
@@ -90,6 +98,8 @@ namespace iDynTree
         unsigned int cols() const;
         ///@}
 
+        MatrixFixSize & operator=(iDynTree::MatrixView<const double> mat);
+
         /**
          * Raw data accessor
          *
@@ -185,6 +195,27 @@ namespace iDynTree
         }
     }
 
+    template<unsigned int nRows, unsigned int nCols>
+    MatrixFixSize<nRows,nCols>::MatrixFixSize(iDynTree::MatrixView<const double> other)
+    {
+        if( other.rows() != nRows ||
+            other.cols() != nCols )
+        {
+            reportError("MatrixFixSize","constructor","input matrix does not have the right size");
+            this->zero();
+        }
+        else
+        {
+            for(unsigned int row=0; row < nRows; row++ )
+            {
+                for(unsigned int col=0; col < nCols; col++ )
+                {
+                    this->m_data[rawIndexRowMajor(row,col)] = other(row, col);
+                }
+            }
+        }
+    }
+
     template<unsigned int nRows, unsigned int nCols>
     void MatrixFixSize<nRows,nCols>::zero()
     {
@@ -221,6 +252,21 @@ namespace iDynTree
         return this->m_data;
     }
 
+    template<unsigned int nRows, unsigned int nCols>
+    MatrixFixSize<nRows,nCols> & MatrixFixSize<nRows,nCols>::operator=(iDynTree::MatrixView<const double> mat) {
+        assert(nCols == mat.cols());
+        assert(nRows == mat.rows());
+
+        for(unsigned int i = 0; i < nRows; i++)
+        {
+            for(unsigned int j = 0; j < nCols; j++)
+            {
+                this->m_data[this->rawIndexRowMajor(i,j)] = mat(i, j);
+            }
+        }
+        return *this;
+    }
+
     template<unsigned int nRows, unsigned int nCols>
     double& MatrixFixSize<nRows,nCols>::operator()(const unsigned int row, const unsigned int col)
     {

src/core/include/iDynTree/Core/MatrixView.h

@@ -31,6 +31,7 @@ namespace iDynTree
 
     namespace MatrixViewInternal
     {
+#ifndef SWIG
         // this is required to be compatible with c++17
         template <typename... Ts> struct make_void { typedef void type; };
         template <typename... Ts> using void_t = typename make_void<Ts...>::type;
@@ -51,6 +52,7 @@ namespace iDynTree
          */
         template <typename T>
         struct has_IsRowMajor<T, void_t<decltype(T::IsRowMajor)>> : std::true_type {};
+#endif
 
     } // namespace MatrixViewIntenal
 
@@ -109,6 +111,7 @@ namespace iDynTree
         {
         }
 
+#ifndef SWIG
         template <
             class OtherElementType,
             class = std::enable_if_t<
@@ -174,6 +177,8 @@ namespace iDynTree
         {
         }
 
+#endif // SWIG
+
         MatrixView(pointer in_data,
                    index_type in_rows,
                    index_type in_cols,

src/core/include/iDynTree/Core/Position.h

@@ -62,6 +62,11 @@ namespace iDynTree
          */
         Position(const PositionRaw & other);
 
+        /**
+         * Create a Position from a span
+         */
+        Position(iDynTree::Span<const double> other);
+
         /**
          * Geometric operations
          */

src/core/include/iDynTree/Core/PositionRaw.h

@@ -51,6 +51,12 @@ namespace iDynTree
          */
         PositionRaw(const PositionRaw & other);
 
+        /**
+         * Construct from a span
+         * @warning if the Span size is different from 3 an assert is thrown at run-time.
+         */
+        PositionRaw(iDynTree::Span<const double> other);
+
         /**
          * Geometric operations
          */

src/core/include/iDynTree/Core/Rotation.h

@@ -79,6 +79,11 @@ namespace iDynTree
          */
         Rotation(const Rotation & other);
 
+        /**
+         * Create a Rotation from a MatrixView.
+         */
+        Rotation(iDynTree::MatrixView<const double> other);
+
         /**
          * Geometric operations.
          * For the inverse2() operation, both the forward and the inverse geometric relations have to

src/core/include/iDynTree/Core/RotationRaw.h

@@ -21,6 +21,8 @@ namespace iDynTree
     class SpatialForceVector;
     class ClassicalAcc;
     class RotationalInertiaRaw;
+    template<class>
+    class MatrixView;
 
     /**
      * Class providing the raw coordinates for iDynTree::Rotation class.
@@ -63,6 +65,8 @@ namespace iDynTree
                     const unsigned int in_rows,
                     const unsigned int in_cols);
 
+        RotationRaw(iDynTree::MatrixView<const double> other);
+
         /**
          * Copy constructor: create a RotationRaw from another RotationRaw.
          */

src/core/include/iDynTree/Core/VectorDynSize.h

@@ -80,6 +80,18 @@ namespace iDynTree
          */
         VectorDynSize(const VectorDynSize& vec);
 
+#if !defined(SWIG_VERSION) || SWIG_VERSION >= 0x030000
+
+        /**
+         * Constructor from an iDynTree::Span
+         *
+         * @param vec span representing a vector
+         *
+         * \warning performs dynamic memory allocation operations
+         */
+        VectorDynSize(iDynTree::Span<const double> vec);
+#endif
+
         /**
          * Denstructor
          *
@@ -108,7 +120,7 @@ namespace iDynTree
          *
          * \warning performs dynamic memory allocation operations
          */
-        VectorDynSize & operator=(const Span<const double>& vec);
+        VectorDynSize & operator=(iDynTree::Span<const double> vec);
 #endif
         /**
          * @name Vector interface methods.

src/core/include/iDynTree/Core/VectorFixSize.h

@@ -54,6 +54,15 @@ namespace iDynTree
          */
         VectorFixSize(const double * in_data, const unsigned int in_size);
 
+#if !defined(SWIG_VERSION) || SWIG_VERSION >= 0x030000
+        /**
+         * Constructor from an iDynTree::Span
+         *
+         * Print an error an build a vector full of zeros if in_size is not size().
+         */
+        VectorFixSize(iDynTree::Span<const double> vec);
+#endif
+
         /**
          * @name Vector interface methods.
          * Methods exposing a vector-like interface to VectorFixSize.
@@ -124,7 +133,7 @@ namespace iDynTree
          * Checks that dimensions are matching through an assert.
          *
          */
-        VectorFixSize & operator=(const Span<const double>& vec);
+        VectorFixSize & operator=(iDynTree::Span<const double> vec);
 #endif
 
         /**
@@ -209,6 +218,15 @@ namespace iDynTree
         }
     }
 
+#if !defined(SWIG_VERSION) || SWIG_VERSION >= 0x030000
+
+    template<unsigned int VecSize>
+    VectorFixSize<VecSize>::VectorFixSize(iDynTree::Span<const double> vec)
+        : VectorFixSize<VecSize>::VectorFixSize(vec.data(), vec.size())
+        {}
+
+#endif
+
     template<unsigned int VecSize>
     void VectorFixSize<VecSize>::zero()
     {
@@ -275,7 +293,7 @@ namespace iDynTree
 
 #if !defined(SWIG_VERSION) || SWIG_VERSION >= 0x030000
     template<unsigned int VecSize>
-    VectorFixSize<VecSize> & VectorFixSize<VecSize>::operator=(const Span<const double>& vec) {
+    VectorFixSize<VecSize> & VectorFixSize<VecSize>::operator=(iDynTree::Span<const double> vec) {
         assert(VecSize == vec.size());
         std::memcpy(this->m_data, vec.data(), VecSize*sizeof(double));
         return *this;

src/core/src/MatrixDynSize.cpp

@@ -42,6 +42,29 @@ MatrixDynSize::MatrixDynSize(unsigned int _rows,
     zero();
 }
 
+MatrixDynSize::MatrixDynSize(MatrixView<const double> other) : m_rows(other.rows()),
+                                                                      m_cols(other.cols())
+{
+    if( this->m_rows*this->m_cols == 0 )
+    {
+        this->m_capacity = 0;
+        this->m_data = 0;
+    }
+    else
+    {
+        this->m_capacity = this->m_rows*this->m_cols;
+        this->m_data = new double[this->m_capacity];
+
+        // copy the matrix
+        for(unsigned int i = 0; i < m_rows; i++)
+        {
+            for(unsigned int j = 0; j < m_cols; j++)
+            {
+                this->m_data[this->rawIndexRowMajor(i,j)] = other(i, j);
+            }
+        }
+    }
+}
 
 MatrixDynSize::MatrixDynSize(const double* in_data,
                              const unsigned int in_rows,
@@ -107,7 +130,7 @@ MatrixDynSize& MatrixDynSize::operator=(const MatrixDynSize& other)
     return *this;
 }
 
-MatrixDynSize& MatrixDynSize::operator=(const MatrixView<const double>& other)
+MatrixDynSize& MatrixDynSize::operator=(MatrixView<const double> other)
 {
     m_rows = other.rows();
     m_cols = other.cols();

src/core/src/Position.cpp

@@ -69,6 +69,10 @@ namespace iDynTree
 
     }
 
+    Position::Position(Span<const double> other): PositionRaw(other)
+    {
+    }
+
     const Position& Position::changePoint(const Position& newPoint)
     {
         this->PositionRaw::changePoint(newPoint);

src/core/src/PositionRaw.cpp

@@ -52,6 +52,12 @@ namespace iDynTree
 
     }
 
+    PositionRaw::PositionRaw(Span<const double> other):
+                 VectorFixSize< 3 >(other)
+    {
+
+    }
+
     const PositionRaw& PositionRaw::changePoint(const PositionRaw& newPoint)
     {
         this->m_data[0] += newPoint(0);

src/core/src/Rotation.cpp

@@ -71,6 +71,12 @@ namespace iDynTree
     {
 
     }
+
+    Rotation::Rotation(MatrixView<const double> other): RotationRaw(other)
+    {
+
+    }
+
     const Rotation& Rotation::changeOrientFrame(const Rotation& newOrientFrame)
     {
         this->RotationRaw::changeOrientFrame(newOrientFrame);

src/core/src/RotationRaw.cpp

@@ -48,6 +48,10 @@ namespace iDynTree
         this->m_data[8] = zz;
     }
 
+    RotationRaw::RotationRaw(MatrixView<const double> other):
+                             MatrixFixSize<3, 3>(other)
+    {}
+
     RotationRaw::RotationRaw(const double* in_data, const unsigned int in_rows, const unsigned int in_cols):
                              MatrixFixSize<3, 3>(in_data,in_rows,in_cols)
     {