From 10de0a5105175ca5b12be57dedace8a2c9036908 Mon Sep 17 00:00:00 2001 From: Joris Vaillant Date: Thu, 2 Apr 2026 11:59:29 +0200 Subject: [PATCH 01/14] container: Turn MatrixStackTpl internal --- CHANGELOG.md | 1 + .../constraints/joint-limit-constraint.hxx | 2 +- .../pinocchio/src/container/matrix-stack.hxx | 1826 +++++++++-------- .../src/math/block-diagonal-matrix.hxx | 2 +- include/pinocchio/src/multibody/data.hxx | 5 +- .../src/serialization/matrix-stack.hxx | 9 +- unittest/double-entry-container.cpp | 2 +- unittest/matrix-stack.cpp | 6 +- unittest/serialization-math.cpp | 6 +- unittest/serialization.cpp | 4 +- 10 files changed, 939 insertions(+), 924 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 22c8d3d78a..a0cb298248 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -24,6 +24,7 @@ The format is based on [Keep a Changelog](https://keepachangelog.com/en/1.0.0/). - Add `PINOCCHIO_BUILD_MPFR_TESTING` CMake option to build MPFR tests - Add `pinocchio/utils/alloca.hpp`: Helpers for mapping stack allocation for Eigen::Map - Add `pinochio/container/eigen-storage.hpp`: Introduce `EigenStorageTpl` +- Add `pinochio/container/matrix-stack.hpp`: Introduce `internal::MatrixStackTpl` ### Changed - Clean delassus API: DelassusOperatorBase define the main delassus API and each method calls `derived().[name-of-method]Impl` diff --git a/include/pinocchio/src/constraints/joint-limit-constraint.hxx b/include/pinocchio/src/constraints/joint-limit-constraint.hxx index ba413fb3c3..ebfb93e183 100644 --- a/include/pinocchio/src/constraints/joint-limit-constraint.hxx +++ b/include/pinocchio/src/constraints/joint-limit-constraint.hxx @@ -900,7 +900,7 @@ namespace pinocchio using Base::classname; // Useful types ------------------------------------------------ - typedef MatrixStackTpl RowVectorStack; + typedef internal::MatrixStackTpl RowVectorStack; // ------------------------------- // METHODS SPECIFIC TO CLASS diff --git a/include/pinocchio/src/container/matrix-stack.hxx b/include/pinocchio/src/container/matrix-stack.hxx index 5a188f574c..922be71917 100644 --- a/include/pinocchio/src/container/matrix-stack.hxx +++ b/include/pinocchio/src/container/matrix-stack.hxx @@ -13,495 +13,450 @@ namespace pinocchio { - template< - typename MatrixLike, - std::size_t Alignment = alignof(std::max_align_t), - typename Enable = void> - struct MatrixStackTpl; + namespace internal + { + template< + typename MatrixLike, + std::size_t Alignment = alignof(std::max_align_t), + typename Enable = void> + struct MatrixStackTpl; + } template - struct CastType> + struct CastType> { typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(MatrixLike) PlainMatrixType; typedef typename PlainMatrixType::template CastXpr::Type NewPlainMatrixExpression; typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(std::decay_t) NewPlainMatrixType; - typedef MatrixStackTpl type; + typedef internal::MatrixStackTpl type; }; - template - struct MatrixStackTpl< - MatrixLike, - _Alignment, - std::enable_if_t>> + namespace internal { - typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(MatrixLike) PlainMatrixType; - typedef typename PlainMatrixType::Scalar Scalar; - typedef typename Eigen::Index Index; + template + struct MatrixStackTpl< + MatrixLike, + _Alignment, + std::enable_if_t>> + { + typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(MatrixLike) PlainMatrixType; + typedef typename PlainMatrixType::Scalar Scalar; + typedef typename Eigen::Index Index; - static constexpr std::size_t Alignment = _Alignment; + static constexpr std::size_t Alignment = _Alignment; - typedef Eigen::Map MapType; - typedef MapType value_type; - typedef MapType & RefMapType; - typedef const MapType & ConstRefMapType; + typedef Eigen::Map MapType; + typedef MapType value_type; + typedef MapType & RefMapType; + typedef const MapType & ConstRefMapType; - typedef const Eigen::Map ConstMapType; - typedef ConstMapType & RefConstMapType; + typedef const Eigen::Map ConstMapType; + typedef ConstMapType & RefConstMapType; - typedef std::vector MapVector; + typedef std::vector MapVector; - typedef typename MapVector::iterator iterator; - typedef typename MapVector::const_iterator const_iterator; + typedef typename MapVector::iterator iterator; + typedef typename MapVector::const_iterator const_iterator; - /// \brief Default constructor - MatrixStackTpl() - : m_data_ptr(nullptr) - , m_memory_capacity(0) - { - } - - /// \brief Constructor - /// - /// \param[in] max_elts Maximum number of matrices contained in the stack - /// \param[in] max_elt_size Maximal size of each matrices (rows() x cols()) if known at - /// construction time. Default value to 0. - /// - explicit MatrixStackTpl(const std::size_t max_elts, const std::size_t max_elt_size = 0) - : m_data_ptr(nullptr) - , m_memory_capacity(0) - { - if (max_elts > 0) + /// \brief Default constructor + MatrixStackTpl() + : m_data_ptr(nullptr) + , m_memory_capacity(0) { - m_offsets.reserve(max_elts); - m_matrix_maps.reserve(max_elts); + } - // Allocate the full memory if max_elt_size is given - if (max_elt_size > 0) + /// \brief Constructor + /// + /// \param[in] max_elts Maximum number of matrices contained in the stack + /// \param[in] max_elt_size Maximal size of each matrices (rows() x cols()) if known at + /// construction time. Default value to 0. + /// + explicit MatrixStackTpl(const std::size_t max_elts, const std::size_t max_elt_size = 0) + : m_data_ptr(nullptr) + , m_memory_capacity(0) + { + if (max_elts > 0) { - const std::size_t max_chunck_size = max_elt_size * sizeof(Scalar); - const std::size_t max_total_size = - max_elts * max_chunck_size + (max_elts - 1) * Alignment; - - m_data_ptr = - MatrixStackTpl::malloc(max_total_size); // the first element is for sure aligned - m_memory_capacity = m_data_ptr != nullptr ? max_total_size : 0; + m_offsets.reserve(max_elts); + m_matrix_maps.reserve(max_elts); + + // Allocate the full memory if max_elt_size is given + if (max_elt_size > 0) + { + const std::size_t max_chunck_size = max_elt_size * sizeof(Scalar); + const std::size_t max_total_size = + max_elts * max_chunck_size + (max_elts - 1) * Alignment; + + m_data_ptr = + MatrixStackTpl::malloc(max_total_size); // the first element is for sure aligned + m_memory_capacity = m_data_ptr != nullptr ? max_total_size : 0; + } } } - } - - /// \brief Constructor from a vector of matrix information. - /// - /// \details Initializes the stack by allocating memory for all matrices described - /// in the provided vector and creating aligned memory maps for each one. - /// - /// \param[in] matrix_infos Vector of MatrixInfo describing the dimensions of each matrix - /// to be stored in the stack. - /// - explicit MatrixStackTpl(const std::vector & matrix_infos) - : m_data_ptr(nullptr) - , m_memory_capacity(0) - { - init_or_rebuild(matrix_infos); - } - - /// \brief Rebuilds the matrix stack from a vector of matrix information. - /// - /// \details Clears the current stack and reinitializes it with matrices - /// defined by the provided matrix_infos. Memory is reallocated if needed - /// to accommodate the new matrices, and existing data is discarded. - /// - /// \param[in] matrix_infos Vector of MatrixInfo describing the dimensions - /// of each matrix to be stored in the stack. - /// - void rebuild(const std::vector & matrix_infos) - { - init_or_rebuild(matrix_infos.data(), matrix_infos.size()); - } - - /// \brief Rebuilds the stack from a C-style array of matrix information. - /// - /// \details Clears the current stack and reinitializes it by allocating memory - /// for all matrices described in the provided array. Creates aligned memory maps - /// for each matrix according to the stack's alignment requirements. - /// - /// \param[in] matrix_infos Pointer to an array of MatrixInfo describing the dimensions - /// of each matrix to be stored in the stack. - /// \param[in] size Number of elements in the matrix_infos array. - /// - void rebuild(const MatrixInfo * matrix_infos, const size_t size) - { - init_or_rebuild(matrix_infos, size); - } - - protected: - /// \brief Allocates memory if the current capacity is insufficient. - /// - /// \details Checks if the current memory capacity can accommodate the requested size. - /// If not, frees the existing memory and allocates a new aligned block of the - /// requested size. The first element of the newly allocated memory is guaranteed - /// to be properly aligned according to the stack's Alignment parameter. - /// - /// \param[in] new_memory_size The required memory size in bytes. - /// - /// \returns true if a new allocation was performed, false if existing capacity was sufficient. - /// - /// \warning If allocation fails, m_data_ptr will be nullptr and m_memory_capacity will be 0. - /// - bool malloc_if_needed(const std::size_t new_memory_size) - { - bool new_malloc = false; - if (m_memory_capacity < new_memory_size) - { - free(m_data_ptr); - m_data_ptr = - MatrixStackTpl::malloc(new_memory_size); // the first element is for sure aligned - m_memory_capacity = m_data_ptr != nullptr ? new_memory_size : 0; - new_malloc = true; - } - - return new_malloc; - } - - /// \brief Initializes or rebuilds the matrix stack from a vector of matrix information. - /// - /// \details Clears any existing matrices and reinitializes the stack by allocating - /// contiguous memory for all matrices described in the provided vector. Each matrix - /// is placed at an aligned memory address according to the stack's Alignment parameter. - /// If the current memory capacity is insufficient, new memory is allocated. - /// - /// \param[in] matrix_infos Vector of MatrixInfo describing the dimensions (rows, cols) - /// of each matrix to be stored in the stack. - /// - /// \note This is an internal helper method called by constructors and rebuild() methods. - /// \note Existing data is discarded when this method is called. - /// - void init_or_rebuild(const std::vector & matrix_infos) - { - init_or_rebuild(matrix_infos.data(), matrix_infos.size()); - } - - /// \brief Initializes or rebuilds the stack from a C-style array of matrix information. - /// - /// \details Clears the current stack and reinitializes it by allocating memory - /// for all matrices described in the provided array. Creates aligned memory maps - /// for each matrix according to the stack's alignment requirements. If the current - /// memory capacity is insufficient, new memory is allocated. - /// - /// \param[in] matrix_infos Pointer to an array of MatrixInfo describing the dimensions - /// of each matrix to be stored in the stack. - /// \param[in] size Number of elements in the matrix_infos array. - /// - void init_or_rebuild(const MatrixInfo * matrix_infos, const size_t size) - { - clear(); - - if (size == 0) - return; - std::size_t max_total_size = 0; - for (std::size_t i = 0; i < size; ++i) + /// \brief Constructor from a vector of matrix information. + /// + /// \details Initializes the stack by allocating memory for all matrices described + /// in the provided vector and creating aligned memory maps for each one. + /// + /// \param[in] matrix_infos Vector of MatrixInfo describing the dimensions of each matrix + /// to be stored in the stack. + /// + explicit MatrixStackTpl(const std::vector & matrix_infos) + : m_data_ptr(nullptr) + , m_memory_capacity(0) { - const auto & block_info = matrix_infos[i]; - const auto elt_size = block_info.size(); - max_total_size += std::size_t(elt_size) * sizeof(Scalar) + Alignment; + init_or_rebuild(matrix_infos); } - malloc_if_needed(max_total_size); + /// \brief Rebuilds the matrix stack from a vector of matrix information. + /// + /// \details Clears the current stack and reinitializes it with matrices + /// defined by the provided matrix_infos. Memory is reallocated if needed + /// to accommodate the new matrices, and existing data is discarded. + /// + /// \param[in] matrix_infos Vector of MatrixInfo describing the dimensions + /// of each matrix to be stored in the stack. + /// + void rebuild(const std::vector & matrix_infos) + { + init_or_rebuild(matrix_infos.data(), matrix_infos.size()); + } - if (m_data_ptr == nullptr) - return; // potential malloc issue + /// \brief Rebuilds the stack from a C-style array of matrix information. + /// + /// \details Clears the current stack and reinitializes it by allocating memory + /// for all matrices described in the provided array. Creates aligned memory maps + /// for each matrix according to the stack's alignment requirements. + /// + /// \param[in] matrix_infos Pointer to an array of MatrixInfo describing the dimensions + /// of each matrix to be stored in the stack. + /// \param[in] size Number of elements in the matrix_infos array. + /// + void rebuild(const MatrixInfo * matrix_infos, const size_t size) + { + init_or_rebuild(matrix_infos, size); + } - // Allocate individual block - const auto original_data_ptr = m_data_ptr; - PINOCCHIO_ONLY_USED_FOR_DEBUG(original_data_ptr); + protected: + /// \brief Allocates memory if the current capacity is insufficient. + /// + /// \details Checks if the current memory capacity can accommodate the requested size. + /// If not, frees the existing memory and allocates a new aligned block of the + /// requested size. The first element of the newly allocated memory is guaranteed + /// to be properly aligned according to the stack's Alignment parameter. + /// + /// \param[in] new_memory_size The required memory size in bytes. + /// + /// \returns true if a new allocation was performed, false if existing capacity was + /// sufficient. + /// + /// \warning If allocation fails, m_data_ptr will be nullptr and m_memory_capacity will be 0. + /// + bool malloc_if_needed(const std::size_t new_memory_size) + { + bool new_malloc = false; + if (m_memory_capacity < new_memory_size) + { + free(m_data_ptr); + m_data_ptr = + MatrixStackTpl::malloc(new_memory_size); // the first element is for sure aligned + m_memory_capacity = m_data_ptr != nullptr ? new_memory_size : 0; + new_malloc = true; + } - m_offsets.reserve(size); - m_matrix_maps.reserve(size); + return new_malloc; + } - for (std::size_t i = 0; i < size; ++i) + /// \brief Initializes or rebuilds the matrix stack from a vector of matrix information. + /// + /// \details Clears any existing matrices and reinitializes the stack by allocating + /// contiguous memory for all matrices described in the provided vector. Each matrix + /// is placed at an aligned memory address according to the stack's Alignment parameter. + /// If the current memory capacity is insufficient, new memory is allocated. + /// + /// \param[in] matrix_infos Vector of MatrixInfo describing the dimensions (rows, cols) + /// of each matrix to be stored in the stack. + /// + /// \note This is an internal helper method called by constructors and rebuild() methods. + /// \note Existing data is discarded when this method is called. + /// + void init_or_rebuild(const std::vector & matrix_infos) { - this->push_back(matrix_infos[i]); + init_or_rebuild(matrix_infos.data(), matrix_infos.size()); } - assert(m_data_ptr == original_data_ptr && "m_data_ptr should not change."); - } - public: - /// - /// @brief Copy constructor. Creates a deep copy of *this. - /// @param other MatrixStackTpl to copy - /// - MatrixStackTpl(const MatrixStackTpl & other) - : m_data_ptr(nullptr) - { - *this = other; - } - - /// - /// @brief Move constructor. - /// @param other MatrixStackTpl to copy - /// - MatrixStackTpl(MatrixStackTpl && other) - : m_offsets(std::move(other.m_offsets)) - , m_matrix_maps(std::move(other.m_matrix_maps)) - , m_data_ptr(other.m_data_ptr) - , m_memory_capacity(other.m_memory_capacity) - { - other.m_data_ptr = nullptr; - other.m_memory_capacity = 0; - } - - /// - /// @brief Move assignment operator - /// @param other MatrixStackTpl to copy - /// @returns a reference to this. - /// - MatrixStackTpl & operator=(MatrixStackTpl && other) - { - free(m_data_ptr); - m_data_ptr = other.m_data_ptr; - m_memory_capacity = other.m_memory_capacity; - m_matrix_maps = std::move(other.m_matrix_maps); - m_offsets = std::move(other.m_offsets); - - other.m_data_ptr = nullptr; - other.m_memory_capacity = 0; - - return *this; - } - - /// - /// @brief Copy operator - /// @param other MatrixStackTpl to copy - /// @returns a reference to this. - /// - MatrixStackTpl & operator=(const MatrixStackTpl & other) - { - if (this == &other) - return *this; + /// \brief Initializes or rebuilds the stack from a C-style array of matrix information. + /// + /// \details Clears the current stack and reinitializes it by allocating memory + /// for all matrices described in the provided array. Creates aligned memory maps + /// for each matrix according to the stack's alignment requirements. If the current + /// memory capacity is insufficient, new memory is allocated. + /// + /// \param[in] matrix_infos Pointer to an array of MatrixInfo describing the dimensions + /// of each matrix to be stored in the stack. + /// \param[in] size Number of elements in the matrix_infos array. + /// + void init_or_rebuild(const MatrixInfo * matrix_infos, const size_t size) + { + clear(); - free(m_data_ptr); + if (size == 0) + return; + + std::size_t max_total_size = 0; + for (std::size_t i = 0; i < size; ++i) + { + const auto & block_info = matrix_infos[i]; + const auto elt_size = block_info.size(); + max_total_size += std::size_t(elt_size) * sizeof(Scalar) + Alignment; + } - m_memory_capacity = other.raw_size(); + malloc_if_needed(max_total_size); - if (m_memory_capacity > 0) - { - m_data_ptr = MatrixStackTpl::malloc(m_memory_capacity); if (m_data_ptr == nullptr) + return; // potential malloc issue + + // Allocate individual block + const auto original_data_ptr = m_data_ptr; + PINOCCHIO_ONLY_USED_FOR_DEBUG(original_data_ptr); + + m_offsets.reserve(size); + m_matrix_maps.reserve(size); + + for (std::size_t i = 0; i < size; ++i) { - m_memory_capacity = 0; - m_matrix_maps.clear(); - m_offsets.clear(); - return *this; + this->push_back(matrix_infos[i]); } + assert(m_data_ptr == original_data_ptr && "m_data_ptr should not change."); + } - // Copy raw data - std::memcpy(m_data_ptr, other.m_data_ptr, m_memory_capacity); + public: + /// + /// @brief Copy constructor. Creates a deep copy of *this. + /// @param other MatrixStackTpl to copy + /// + MatrixStackTpl(const MatrixStackTpl & other) + : m_data_ptr(nullptr) + { + *this = other; } - else + + /// + /// @brief Move constructor. + /// @param other MatrixStackTpl to copy + /// + MatrixStackTpl(MatrixStackTpl && other) + : m_offsets(std::move(other.m_offsets)) + , m_matrix_maps(std::move(other.m_matrix_maps)) + , m_data_ptr(other.m_data_ptr) + , m_memory_capacity(other.m_memory_capacity) { - m_data_ptr = nullptr; + other.m_data_ptr = nullptr; + other.m_memory_capacity = 0; } - // Add aligned map - m_matrix_maps.clear(); - m_matrix_maps.reserve(other.m_matrix_maps.size()); - m_offsets = other.m_offsets; - for (std::size_t i = 0; i < other.m_matrix_maps.size(); ++i) + /// + /// @brief Move assignment operator + /// @param other MatrixStackTpl to copy + /// @returns a reference to this. + /// + MatrixStackTpl & operator=(MatrixStackTpl && other) { - const auto offset_value = m_offsets[i]; - const auto & other_matrix_map = other.m_matrix_maps[i]; + free(m_data_ptr); + m_data_ptr = other.m_data_ptr; + m_memory_capacity = other.m_memory_capacity; + m_matrix_maps = std::move(other.m_matrix_maps); + m_offsets = std::move(other.m_offsets); - // Note: the matrix stack can contain empty maps even if there is no data - // in the stack. - // For example, if the matrix stack has a 0 x 0 matrix or 0 x 1 vector. - void * aligned_data = m_data_ptr ? incr_ptr(m_data_ptr, offset_value) : nullptr; - if (aligned_data != nullptr) - { - assert( - reinterpret_cast(aligned_data) % Alignment == 0 - && "aligned_data is not properly aligned."); - } + other.m_data_ptr = nullptr; + other.m_memory_capacity = 0; - MapType aligned_map = MapType( - reinterpret_cast(aligned_data), other_matrix_map.rows(), - other_matrix_map.cols()); - // aligned_map = other_matrix_map; // copy data - m_matrix_maps.push_back(aligned_map); + return *this; } - return *this; - } - - /// @brief Equality comparison operator. - /// @param other MatrixStackTpl to compare with. - /// @returns true if the underlying maps are equal. - bool operator==(const MatrixStackTpl & other) const - { - if (this == &other) - return true; - if (m_matrix_maps.size() != other.m_matrix_maps.size()) - return false; - - for (std::size_t i = 0; i < m_matrix_maps.size(); ++i) + /// + /// @brief Copy operator + /// @param other MatrixStackTpl to copy + /// @returns a reference to this. + /// + MatrixStackTpl & operator=(const MatrixStackTpl & other) { - const auto & map = m_matrix_maps[i]; - const auto & other_map = other.m_matrix_maps[i]; - const bool res = compare_maps(map, other_map); - if (!res) - return false; - } - return true; - } + if (this == &other) + return *this; - /// @brief Inequality comparison operator. - /// @param other MatrixStackTpl to compare with. - /// @return true if the underlying maps are not equal. - bool operator!=(const MatrixStackTpl & other) const - { - return !(*this == other); - } - - /// \brief Appends a matrix to the stack by copying its contents. - /// - /// \details Allocates space for a new matrix with the same dimensions as the input, - /// adds it to the stack, and copies the input matrix data into the newly allocated space. - /// Memory is reallocated if the current capacity is insufficient. - /// - /// \tparam Matrix Eigen matrix expression type (automatically deduced). - /// \param[in] matrix The matrix to copy and append to the stack. - /// - template - void push_back(const Eigen::MatrixBase & matrix) - { - this->push_back(matrix.rows(), matrix.cols()); - this->back() = matrix; - } - - /// \brief Adds a diagonal matrix to the stack by storing its diagonal elements. - /// - /// \details Stores only the diagonal elements of the input diagonal matrix as a column vector. - /// This is a space-efficient representation since diagonal matrices only have non-zero - /// elements on the main diagonal. - /// - /// \tparam Matrix The derived type of the Eigen diagonal expression. - /// \param[in] diagonal_matrix The diagonal matrix whose diagonal elements will be stored. - /// - /// \note The stored matrix will have dimensions (n, 1) where n is the size of the diagonal. - /// - template - void push_back(const Eigen::DiagonalBase & diagonal_matrix) - { - this->push_back(diagonal_matrix.rows(), 1); - this->back() = diagonal_matrix.diagonal(); - } - - /// \brief Constructs a matrix in-place and adds it to the stack. - /// - /// \details Forwards the provided arguments to construct a PlainMatrixType, - /// then pushes the constructed matrix onto the stack. This avoids unnecessary - /// copies when the matrix can be constructed directly from the arguments. - /// - /// \tparam Args Variadic template parameter pack for constructor arguments. - /// \param[in] args Arguments to forward to the PlainMatrixType constructor. - /// - template - void emplace_back(Args &&... args) - { - PlainMatrixType matrix(std::forward(args)...); - push_back(matrix); - } - - /// \brief Adds a matrix with given dimensions to the stack using MatrixInfo. - /// - /// \details Creates a new matrix entry with dimensions specified by the MatrixInfo - /// structure and optionally initializes it using the provided initialization function. - /// - /// \param[in] matrix_info Structure containing the row and column dimensions for the new - /// matrix. - /// \param[in] init_func Optional initialization function called with the newly created map. - /// If provided, this function is invoked to initialize the matrix - /// contents. - /// - void - push_back(const MatrixInfo & matrix_info, const std::function init_func = {}) - { - this->push_back(matrix_info.rows(), matrix_info.cols(), init_func); - } - - /// \brief Adds a matrix with specified dimensions to the stack. - /// - /// \details Allocates aligned memory for a new matrix with the given dimensions, - /// adds it to the stack, and optionally initializes it. If the current memory - /// capacity is insufficient, the internal buffer is reallocated with doubled capacity - /// and all existing matrix maps are updated to point to the new memory locations. - /// - /// \param[in] rows Number of rows for the new matrix. - /// \param[in] cols Number of columns for the new matrix. - /// \param[in] init_func Optional initialization function called with the newly created map. - /// If provided, this function is invoked to initialize the matrix - /// contents. - /// - void - push_back(const Index rows, const Index cols, const std::function init_func = {}) - { - void * next_data_ptr = - m_matrix_maps.size() == 0 - ? m_data_ptr - : incr_ptr(m_matrix_maps.back().data(), raw_map_size(m_matrix_maps.back())); - void * aligned_data = - reinterpret_cast(next_data_ptr) % Alignment == 0 - ? /* next_data_ptr is aligned */ - next_data_ptr - : reinterpret_cast( - (reinterpret_cast(next_data_ptr) & ~(std::size_t(Alignment - 1))) - + Alignment); - assert( - reinterpret_cast(aligned_data) % Alignment == 0 - && "aligned_data is not properly aligned."); - - const std::size_t matrix_raw_map_size = std::size_t(rows * cols) * sizeof(Scalar); - const std::size_t loss_bits = - (reinterpret_cast(aligned_data) - - reinterpret_cast(next_data_ptr)); - const std::size_t new_memory_chunck_size = matrix_raw_map_size + loss_bits; - - const std::size_t current_memory_size = - reinterpret_cast(next_data_ptr) - reinterpret_cast(m_data_ptr); - if (current_memory_size + new_memory_chunck_size > m_memory_capacity) - { // We need to proceed to a new allocation - const std::size_t new_size = - 2 * (current_memory_size + new_memory_chunck_size); // we double the allocated chunck + free(m_data_ptr); - if (m_data_ptr == nullptr) + m_memory_capacity = other.raw_size(); + + if (m_memory_capacity > 0) { - m_data_ptr = MatrixStackTpl::malloc(new_size); + m_data_ptr = MatrixStackTpl::malloc(m_memory_capacity); + if (m_data_ptr == nullptr) + { + m_memory_capacity = 0; + m_matrix_maps.clear(); + m_offsets.clear(); + return *this; + } + + // Copy raw data + std::memcpy(m_data_ptr, other.m_data_ptr, m_memory_capacity); } else { - m_data_ptr = MatrixStackTpl::realloc(m_data_ptr, new_size, m_memory_capacity); + m_data_ptr = nullptr; } - assert(m_data_ptr != nullptr); - m_memory_capacity = new_size; - // We need to realign all the existing Eigen maps - for (std::size_t i = 0; i < m_matrix_maps.size(); ++i) + // Add aligned map + m_matrix_maps.clear(); + m_matrix_maps.reserve(other.m_matrix_maps.size()); + m_offsets = other.m_offsets; + for (std::size_t i = 0; i < other.m_matrix_maps.size(); ++i) { - auto & matrix_map = m_matrix_maps[i]; const auto offset_value = m_offsets[i]; + const auto & other_matrix_map = other.m_matrix_maps[i]; + + // Note: the matrix stack can contain empty maps even if there is no data + // in the stack. + // For example, if the matrix stack has a 0 x 0 matrix or 0 x 1 vector. + void * aligned_data = m_data_ptr ? incr_ptr(m_data_ptr, offset_value) : nullptr; + if (aligned_data != nullptr) + { + assert( + reinterpret_cast(aligned_data) % Alignment == 0 + && "aligned_data is not properly aligned."); + } + + MapType aligned_map = MapType( + reinterpret_cast(aligned_data), other_matrix_map.rows(), + other_matrix_map.cols()); + // aligned_map = other_matrix_map; // copy data + m_matrix_maps.push_back(aligned_map); + } - void * new_map_data_ptr = incr_ptr(m_data_ptr, offset_value); + return *this; + } + + /// @brief Equality comparison operator. + /// @param other MatrixStackTpl to compare with. + /// @returns true if the underlying maps are equal. + bool operator==(const MatrixStackTpl & other) const + { + if (this == &other) + return true; + if (m_matrix_maps.size() != other.m_matrix_maps.size()) + return false; - new (&matrix_map) MapType( - reinterpret_cast(new_map_data_ptr), matrix_map.rows(), matrix_map.cols()); + for (std::size_t i = 0; i < m_matrix_maps.size(); ++i) + { + const auto & map = m_matrix_maps[i]; + const auto & other_map = other.m_matrix_maps[i]; + const bool res = compare_maps(map, other_map); + if (!res) + return false; } + return true; + } + /// @brief Inequality comparison operator. + /// @param other MatrixStackTpl to compare with. + /// @return true if the underlying maps are not equal. + bool operator!=(const MatrixStackTpl & other) const + { + return !(*this == other); + } + + /// \brief Appends a matrix to the stack by copying its contents. + /// + /// \details Allocates space for a new matrix with the same dimensions as the input, + /// adds it to the stack, and copies the input matrix data into the newly allocated space. + /// Memory is reallocated if the current capacity is insufficient. + /// + /// \tparam Matrix Eigen matrix expression type (automatically deduced). + /// \param[in] matrix The matrix to copy and append to the stack. + /// + template + void push_back(const Eigen::MatrixBase & matrix) + { + this->push_back(matrix.rows(), matrix.cols()); + this->back() = matrix; + } + + /// \brief Adds a diagonal matrix to the stack by storing its diagonal elements. + /// + /// \details Stores only the diagonal elements of the input diagonal matrix as a column + /// vector. This is a space-efficient representation since diagonal matrices only have + /// non-zero elements on the main diagonal. + /// + /// \tparam Matrix The derived type of the Eigen diagonal expression. + /// \param[in] diagonal_matrix The diagonal matrix whose diagonal elements will be stored. + /// + /// \note The stored matrix will have dimensions (n, 1) where n is the size of the diagonal. + /// + template + void push_back(const Eigen::DiagonalBase & diagonal_matrix) + { + this->push_back(diagonal_matrix.rows(), 1); + this->back() = diagonal_matrix.diagonal(); + } + + /// \brief Constructs a matrix in-place and adds it to the stack. + /// + /// \details Forwards the provided arguments to construct a PlainMatrixType, + /// then pushes the constructed matrix onto the stack. This avoids unnecessary + /// copies when the matrix can be constructed directly from the arguments. + /// + /// \tparam Args Variadic template parameter pack for constructor arguments. + /// \param[in] args Arguments to forward to the PlainMatrixType constructor. + /// + template + void emplace_back(Args &&... args) + { + PlainMatrixType matrix(std::forward(args)...); + push_back(matrix); + } + + /// \brief Adds a matrix with given dimensions to the stack using MatrixInfo. + /// + /// \details Creates a new matrix entry with dimensions specified by the MatrixInfo + /// structure and optionally initializes it using the provided initialization function. + /// + /// \param[in] matrix_info Structure containing the row and column dimensions for the new + /// matrix. + /// \param[in] init_func Optional initialization function called with the newly created map. + /// If provided, this function is invoked to initialize the matrix + /// contents. + /// + void + push_back(const MatrixInfo & matrix_info, const std::function init_func = {}) + { + this->push_back(matrix_info.rows(), matrix_info.cols(), init_func); + } + + /// \brief Adds a matrix with specified dimensions to the stack. + /// + /// \details Allocates aligned memory for a new matrix with the given dimensions, + /// adds it to the stack, and optionally initializes it. If the current memory + /// capacity is insufficient, the internal buffer is reallocated with doubled capacity + /// and all existing matrix maps are updated to point to the new memory locations. + /// + /// \param[in] rows Number of rows for the new matrix. + /// \param[in] cols Number of columns for the new matrix. + /// \param[in] init_func Optional initialization function called with the newly created map. + /// If provided, this function is invoked to initialize the matrix + /// contents. + /// + void push_back( + const Index rows, const Index cols, const std::function init_func = {}) + { void * next_data_ptr = m_matrix_maps.size() == 0 ? m_data_ptr : incr_ptr(m_matrix_maps.back().data(), raw_map_size(m_matrix_maps.back())); - aligned_data = + void * aligned_data = reinterpret_cast(next_data_ptr) % Alignment == 0 ? /* next_data_ptr is aligned */ next_data_ptr @@ -511,566 +466,623 @@ namespace pinocchio assert( reinterpret_cast(aligned_data) % Alignment == 0 && "aligned_data is not properly aligned."); + + const std::size_t matrix_raw_map_size = std::size_t(rows * cols) * sizeof(Scalar); + const std::size_t loss_bits = + (reinterpret_cast(aligned_data) + - reinterpret_cast(next_data_ptr)); + const std::size_t new_memory_chunck_size = matrix_raw_map_size + loss_bits; + + const std::size_t current_memory_size = + reinterpret_cast(next_data_ptr) - reinterpret_cast(m_data_ptr); + if (current_memory_size + new_memory_chunck_size > m_memory_capacity) + { // We need to proceed to a new allocation + const std::size_t new_size = + 2 * (current_memory_size + new_memory_chunck_size); // we double the allocated chunck + + if (m_data_ptr == nullptr) + { + m_data_ptr = MatrixStackTpl::malloc(new_size); + } + else + { + m_data_ptr = MatrixStackTpl::realloc(m_data_ptr, new_size, m_memory_capacity); + } + assert(m_data_ptr != nullptr); + m_memory_capacity = new_size; + + // We need to realign all the existing Eigen maps + for (std::size_t i = 0; i < m_matrix_maps.size(); ++i) + { + auto & matrix_map = m_matrix_maps[i]; + const auto offset_value = m_offsets[i]; + + void * new_map_data_ptr = incr_ptr(m_data_ptr, offset_value); + + new (&matrix_map) MapType( + reinterpret_cast(new_map_data_ptr), matrix_map.rows(), matrix_map.cols()); + } + + void * next_data_ptr = + m_matrix_maps.size() == 0 + ? m_data_ptr + : incr_ptr(m_matrix_maps.back().data(), raw_map_size(m_matrix_maps.back())); + aligned_data = + reinterpret_cast(next_data_ptr) % Alignment == 0 + ? /* next_data_ptr is aligned */ + next_data_ptr + : reinterpret_cast( + (reinterpret_cast(next_data_ptr) & ~(std::size_t(Alignment - 1))) + + Alignment); + assert( + reinterpret_cast(aligned_data) % Alignment == 0 + && "aligned_data is not properly aligned."); + } + + MapType aligned_map = MapType(reinterpret_cast(aligned_data), rows, cols); + m_matrix_maps.push_back(aligned_map); + if (init_func) + init_func(m_matrix_maps.back()); + + m_offsets.push_back( + reinterpret_cast(aligned_data) - reinterpret_cast(m_data_ptr)); } - MapType aligned_map = MapType(reinterpret_cast(aligned_data), rows, cols); - m_matrix_maps.push_back(aligned_map); - if (init_func) - init_func(m_matrix_maps.back()); + /// \brief Returns a reference to the last element in the container. + RefMapType back() + { + return m_matrix_maps.back(); + } + /// \brief Returns a reference to the last element in the container. + ConstRefMapType back() const + { + return m_matrix_maps.back(); + } - m_offsets.push_back( - reinterpret_cast(aligned_data) - reinterpret_cast(m_data_ptr)); - } + ///  \brief Checks if the container has no elements. + /// + /// \returns true if the container is empty, false otherwise. + bool empty() const + { + return m_matrix_maps.empty(); + } - /// \brief Returns a reference to the last element in the container. - RefMapType back() - { - return m_matrix_maps.back(); - } - /// \brief Returns a reference to the last element in the container. - ConstRefMapType back() const - { - return m_matrix_maps.back(); - } + /// \brief Increase the capacity of the vector of matrix maps. + void reserve(std::size_t new_cap) + { + m_matrix_maps.reserve(new_cap); + } - ///  \brief Checks if the container has no elements. - /// - /// \returns true if the container is empty, false otherwise. - bool empty() const - { - return m_matrix_maps.empty(); - } + /// \brief Returns the capacity of the matrix stack. + std::size_t capacity() const + { + return m_matrix_maps.capacity(); + } - /// \brief Increase the capacity of the vector of matrix maps. - void reserve(std::size_t new_cap) - { - m_matrix_maps.reserve(new_cap); - } + /// \brief Returns a reference to the element at specified location pos. + RefMapType operator[](const std::size_t pos) + { + return m_matrix_maps[pos]; + } + /// \brief Returns a reference to the element at specified location pos. + ConstRefMapType operator[](const std::size_t pos) const + { + return m_matrix_maps[pos]; + } - /// \brief Returns the capacity of the matrix stack. - std::size_t capacity() const - { - return m_matrix_maps.capacity(); - } + /// \brief Returns a typed reference to the element at specified location pos. + /// \tparam TargetMatrixType The desired matrix type (e.g., Eigen::Matrix) + /// \param pos The position of the element + /// \returns An Eigen::Map with the correct static dimensions + template + Eigen::Map get(const std::size_t pos) + { + auto & map = m_matrix_maps[pos]; + assert( + map.rows() == TargetMatrixType::RowsAtCompileTime + || TargetMatrixType::RowsAtCompileTime == Eigen::Dynamic); + assert( + map.cols() == TargetMatrixType::ColsAtCompileTime + || TargetMatrixType::ColsAtCompileTime == Eigen::Dynamic); + return Eigen::Map(map.data(), map.rows(), map.cols()); + } - /// \brief Returns a reference to the element at specified location pos. - RefMapType operator[](const std::size_t pos) - { - return m_matrix_maps[pos]; - } - /// \brief Returns a reference to the element at specified location pos. - ConstRefMapType operator[](const std::size_t pos) const - { - return m_matrix_maps[pos]; - } - - /// \brief Returns a typed reference to the element at specified location pos. - /// \tparam TargetMatrixType The desired matrix type (e.g., Eigen::Matrix) - /// \param pos The position of the element - /// \returns An Eigen::Map with the correct static dimensions - template - Eigen::Map get(const std::size_t pos) - { - auto & map = m_matrix_maps[pos]; - assert( - map.rows() == TargetMatrixType::RowsAtCompileTime - || TargetMatrixType::RowsAtCompileTime == Eigen::Dynamic); - assert( - map.cols() == TargetMatrixType::ColsAtCompileTime - || TargetMatrixType::ColsAtCompileTime == Eigen::Dynamic); - return Eigen::Map(map.data(), map.rows(), map.cols()); - } - - /// \brief Returns a const typed reference to the element at specified location pos. - /// \tparam TargetMatrixType The desired matrix type (e.g., Eigen::Matrix) - /// \param pos The position of the element - /// \returns A const Eigen::Map with the correct static dimensions - template - Eigen::Map get(const std::size_t pos) const - { - const auto & map = m_matrix_maps[pos]; - assert( - map.rows() == TargetMatrixType::RowsAtCompileTime - || TargetMatrixType::RowsAtCompileTime == Eigen::Dynamic); - assert( - map.cols() == TargetMatrixType::ColsAtCompileTime - || TargetMatrixType::ColsAtCompileTime == Eigen::Dynamic); - return Eigen::Map(map.data(), map.rows(), map.cols()); - } - - /// \brief Returns the number of elements in the container. - std::size_t size() const - { - return m_matrix_maps.size(); - } + /// \brief Returns a const typed reference to the element at specified location pos. + /// \tparam TargetMatrixType The desired matrix type (e.g., Eigen::Matrix) + /// \param pos The position of the element + /// \returns A const Eigen::Map with the correct static dimensions + template + Eigen::Map get(const std::size_t pos) const + { + const auto & map = m_matrix_maps[pos]; + assert( + map.rows() == TargetMatrixType::RowsAtCompileTime + || TargetMatrixType::RowsAtCompileTime == Eigen::Dynamic); + assert( + map.cols() == TargetMatrixType::ColsAtCompileTime + || TargetMatrixType::ColsAtCompileTime == Eigen::Dynamic); + return Eigen::Map(map.data(), map.rows(), map.cols()); + } - /// \brief Returns a pointer to the underlying array serving as element storage. - void * data() - { - return m_data_ptr; - } - /// \brief Returns a pointer to the underlying array serving as element storage. - const void * data() const - { - return m_data_ptr; - } + /// \brief Returns the number of elements in the container. + std::size_t size() const + { + return m_matrix_maps.size(); + } - /// \brief Erases the specified elements from the container. - /// \remarks The data associated with the pos element is not reused after erasing. - iterator erase(iterator pos) - { - return m_matrix_maps.erase(pos); - } + /// \brief Returns a pointer to the underlying array serving as element storage. + void * data() + { + return m_data_ptr; + } + /// \brief Returns a pointer to the underlying array serving as element storage. + const void * data() const + { + return m_data_ptr; + } - /// \brief Erases the specified elements from the container. - /// \remarks The data associated with the pos element is not reused after erasing. - iterator erase(const_iterator pos) - { - return m_matrix_maps.erase(pos); - } + /// \brief Erases the specified elements from the container. + /// \remarks The data associated with the pos element is not reused after erasing. + iterator erase(iterator pos) + { + return m_matrix_maps.erase(pos); + } - /// \brief Empties the matrix stack. - /// Does not deallocate memory, hence the matrix stack retains the same capacity. - void clear() - { - m_offsets.clear(); - m_matrix_maps.clear(); - } + /// \brief Erases the specified elements from the container. + /// \remarks The data associated with the pos element is not reused after erasing. + iterator erase(const_iterator pos) + { + return m_matrix_maps.erase(pos); + } - iterator begin() - { - return m_matrix_maps.begin(); - } + /// \brief Empties the matrix stack. + /// Does not deallocate memory, hence the matrix stack retains the same capacity. + void clear() + { + m_offsets.clear(); + m_matrix_maps.clear(); + } - iterator end() - { - return m_matrix_maps.end(); - } + iterator begin() + { + return m_matrix_maps.begin(); + } - const_iterator begin() const - { - return m_matrix_maps.begin(); - } + iterator end() + { + return m_matrix_maps.end(); + } - const_iterator end() const - { - return m_matrix_maps.end(); - } + const_iterator begin() const + { + return m_matrix_maps.begin(); + } - iterator rbegin() - { - return m_matrix_maps.cbegin(); - } + const_iterator end() const + { + return m_matrix_maps.end(); + } - iterator rend() - { - return m_matrix_maps.cend(); - } + iterator rbegin() + { + return m_matrix_maps.cbegin(); + } - const_iterator rbegin() const - { - return m_matrix_maps.cbegin(); - } + iterator rend() + { + return m_matrix_maps.cend(); + } - const_iterator rend() const - { - return m_matrix_maps.cend(); - } - - /// \brief Applies a function to each matrix in the stack. - /// - /// \details Iterates through all matrices in the stack and invokes the provided - /// function on each one. This allows for bulk operations on all stored matrices. - /// - /// \param[in] func A callable that takes a MapType and performs an operation on it. - /// - void apply(const std::function & func) - { - std::for_each(begin(), end(), func); - } - - /// \brief Applies a function to each matrix in the stack (const version). - /// - /// \details Iterates over all matrices in the stack and invokes the provided - /// function on each one. This is the const version that operates on immutable matrices. - /// - /// \param[in] func A function or callable object that takes a const MapType and - /// performs some operation on it. - /// - void apply(const std::function & func) const - { - std::for_each(begin(), end(), func); - } + const_iterator rbegin() const + { + return m_matrix_maps.cbegin(); + } - /// \brief Destructor of this matrix stack. - ~MatrixStackTpl() - { - MatrixStackTpl::free(m_data_ptr); - } + const_iterator rend() const + { + return m_matrix_maps.cend(); + } - /// \brief Returns the current memory footprint of this object in bytes. - /// \details Sums up the sizes of all internal data members. - std::size_t sizeInBytes() const - { - return raw_size(); - } + /// \brief Applies a function to each matrix in the stack. + /// + /// \details Iterates through all matrices in the stack and invokes the provided + /// function on each one. This allows for bulk operations on all stored matrices. + /// + /// \param[in] func A callable that takes a MapType and performs an operation on it. + /// + void apply(const std::function & func) + { + std::for_each(begin(), end(), func); + } - /// \brief Returns the current memory capacity of the stack in bytes. - std::size_t memoryCapacityInBytes() const - { - return m_memory_capacity; - } + /// \brief Applies a function to each matrix in the stack (const version). + /// + /// \details Iterates over all matrices in the stack and invokes the provided + /// function on each one. This is the const version that operates on immutable matrices. + /// + /// \param[in] func A function or callable object that takes a const MapType and + /// performs some operation on it. + /// + void apply(const std::function & func) const + { + std::for_each(begin(), end(), func); + } - protected: - static void * malloc(std::size_t size, std::size_t alignment = Alignment) - { - assert(size > 0 && "size should be greater than 0."); - // return Eigen::internal::handmade_aligned_malloc(size, alignment); - - eigen_assert( - alignment >= sizeof(void *) && alignment <= 256 && (alignment & (alignment - 1)) == 0 - && "Alignment must be at least sizeof(void*), less than or equal to 256, and a power of 2"); - - EIGEN_USING_STD(malloc) - void * original = malloc(size + alignment); - if (original == nullptr) - return nullptr; - std::size_t offset = alignment - (reinterpret_cast(original) & (alignment - 1)); - void * aligned = static_cast(static_cast(original) + offset); - // Store offset - 1, since it is guaranteed to be at least 1. - *(static_cast(aligned) - 1) = static_cast(offset - 1); - - return aligned; - } - - static void free(void * ptr) - { - // Eigen::internal::handmade_aligned_free(ptr); - if (ptr != nullptr) + /// \brief Destructor of this matrix stack. + ~MatrixStackTpl() { - std::size_t offset = static_cast(*(static_cast(ptr) - 1)) + 1; - void * original = static_cast(static_cast(ptr) - offset); + MatrixStackTpl::free(m_data_ptr); + } - EIGEN_USING_STD(free) - free(original); + /// \brief Returns the current memory footprint of this object in bytes. + /// \details Sums up the sizes of all internal data members. + std::size_t sizeInBytes() const + { + return raw_size(); } - } - static void * realloc( - void * ptr, std::size_t new_size, std::size_t old_size, std::size_t alignment = Alignment) - { - // #if EIGEN_VERSION_AT_LEAST(3, 4, 90) - // return Eigen::internal::handmade_aligned_realloc(ptr, new_size, old_size, alignment); - // #else - // return Eigen::internal::handmade_aligned_realloc(ptr, new_size, old_size); - // PINOCCHIO_UNUSED_VARIABLE(alignment); - // #endif - - if (ptr == nullptr) - return MatrixStackTpl::malloc(new_size, alignment); - std::size_t old_offset = static_cast(*(static_cast(ptr) - 1)) + 1; - void * old_original = static_cast(ptr) - old_offset; - - EIGEN_USING_STD(realloc) - void * original = realloc(old_original, new_size + alignment); - if (original == nullptr) - return nullptr; - if (original == old_original) - return ptr; - std::size_t offset = alignment - (reinterpret_cast(original) & (alignment - 1)); - void * aligned = static_cast(static_cast(original) + offset); - if (offset != old_offset) - { - const void * src = static_cast(static_cast(original) + old_offset); - std::size_t count = (std::min)(new_size, old_size); - std::memmove(aligned, src, count); - } - // Store offset - 1, since it is guaranteed to be at least 1. - *(static_cast(aligned) - 1) = static_cast(offset - 1); - return aligned; - } - - static void * incr_ptr(void * ptr, std::size_t inc_value) - { - return reinterpret_cast(reinterpret_cast(ptr) + inc_value); - } + /// \brief Returns the current memory capacity of the stack in bytes. + std::size_t memoryCapacityInBytes() const + { + return m_memory_capacity; + } - /// \brief Returns the total size in bytes of the map. - static std::size_t raw_map_size(const MapType & map) - { - return sizeof(Scalar) * std::size_t(map.rows() * map.cols()); - } + protected: + static void * malloc(std::size_t size, std::size_t alignment = Alignment) + { + assert(size > 0 && "size should be greater than 0."); + // return Eigen::internal::handmade_aligned_malloc(size, alignment); + + eigen_assert( + alignment >= sizeof(void *) && alignment <= 256 && (alignment & (alignment - 1)) == 0 + && "Alignment must be at least sizeof(void*), less than or equal to 256, and a power of " + "2"); + + EIGEN_USING_STD(malloc) + void * original = malloc(size + alignment); + if (original == nullptr) + return nullptr; + std::size_t offset = + alignment - (reinterpret_cast(original) & (alignment - 1)); + void * aligned = static_cast(static_cast(original) + offset); + // Store offset - 1, since it is guaranteed to be at least 1. + *(static_cast(aligned) - 1) = static_cast(offset - 1); + + return aligned; + } - /// \brief Returns the total size in bytes of the raw matrix data stored in the stack. - /// \details Computes the byte offset to the end of the last matrix's data. - /// \returns 0 if the stack is empty, otherwise the offset of the last matrix plus its size. - std::size_t raw_size() const - { - return m_matrix_maps.size() == 0 - ? 0 - : m_offsets.back() + raw_map_size(m_matrix_maps.back()); // + Alignment; - } - - std::vector m_offsets; - MapVector m_matrix_maps; - void * m_data_ptr; - std::size_t m_memory_capacity; - }; // struct MatrixStackTpl - - /// MatrixStackTpl specialization for non primitive scalar type. - /// This implementation doesn't take care of alignment because - /// Custom scalar like casadi or cppad doesn't need it. - template - struct MatrixStackTpl< - MatrixLike, - _Alignment, - std::enable_if_t>> - { - typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(MatrixLike) PlainMatrixType; - typedef typename PlainMatrixType::Scalar Scalar; - typedef typename Eigen::Index Index; + static void free(void * ptr) + { + // Eigen::internal::handmade_aligned_free(ptr); + if (ptr != nullptr) + { + std::size_t offset = static_cast(*(static_cast(ptr) - 1)) + 1; + void * original = static_cast(static_cast(ptr) - offset); - typedef Eigen::Map MapType; - typedef MapType value_type; + EIGEN_USING_STD(free) + free(original); + } + } - typedef const Eigen::Map ConstMapType; + static void * realloc( + void * ptr, std::size_t new_size, std::size_t old_size, std::size_t alignment = Alignment) + { + // #if EIGEN_VERSION_AT_LEAST(3, 4, 90) + // return Eigen::internal::handmade_aligned_realloc(ptr, new_size, old_size, + // alignment); + // #else + // return Eigen::internal::handmade_aligned_realloc(ptr, new_size, old_size); + // PINOCCHIO_UNUSED_VARIABLE(alignment); + // #endif + + if (ptr == nullptr) + return MatrixStackTpl::malloc(new_size, alignment); + std::size_t old_offset = static_cast(*(static_cast(ptr) - 1)) + 1; + void * old_original = static_cast(ptr) - old_offset; + + EIGEN_USING_STD(realloc) + void * original = realloc(old_original, new_size + alignment); + if (original == nullptr) + return nullptr; + if (original == old_original) + return ptr; + std::size_t offset = + alignment - (reinterpret_cast(original) & (alignment - 1)); + void * aligned = static_cast(static_cast(original) + offset); + if (offset != old_offset) + { + const void * src = + static_cast(static_cast(original) + old_offset); + std::size_t count = (std::min)(new_size, old_size); + std::memmove(aligned, src, count); + } + // Store offset - 1, since it is guaranteed to be at least 1. + *(static_cast(aligned) - 1) = static_cast(offset - 1); + return aligned; + } - typedef std::vector MapVector; + static void * incr_ptr(void * ptr, std::size_t inc_value) + { + return reinterpret_cast(reinterpret_cast(ptr) + inc_value); + } - typedef typename MapVector::iterator iterator; - typedef typename MapVector::const_iterator const_iterator; + /// \brief Returns the total size in bytes of the map. + static std::size_t raw_map_size(const MapType & map) + { + return sizeof(Scalar) * std::size_t(map.rows() * map.cols()); + } - /// \brief Default constructor - MatrixStackTpl() = default; + /// \brief Returns the total size in bytes of the raw matrix data stored in the stack. + /// \details Computes the byte offset to the end of the last matrix's data. + /// \returns 0 if the stack is empty, otherwise the offset of the last matrix plus its size. + std::size_t raw_size() const + { + return m_matrix_maps.size() == 0 + ? 0 + : m_offsets.back() + raw_map_size(m_matrix_maps.back()); // + Alignment; + } - /// \brief Constructor - /// - /// \param[in] max_elts Maximum number of matrices contained in the stack - /// \param[in] max_elt_size Maximal size of each matrices (rows() x cols()) if known at - /// construction time. Default value to 0. - /// - explicit MatrixStackTpl(const std::size_t max_elts, const std::size_t max_elt_size = 0) - { - PINOCCHIO_UNUSED_VARIABLE(max_elt_size); - if (max_elts > 0) + std::vector m_offsets; + MapVector m_matrix_maps; + void * m_data_ptr; + std::size_t m_memory_capacity; + }; // struct MatrixStackTpl + + /// MatrixStackTpl specialization for non primitive scalar type. + /// This implementation doesn't take care of alignment because + /// Custom scalar like casadi or cppad doesn't need it. + template + struct MatrixStackTpl< + MatrixLike, + _Alignment, + std::enable_if_t>> + { + typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(MatrixLike) PlainMatrixType; + typedef typename PlainMatrixType::Scalar Scalar; + typedef typename Eigen::Index Index; + + typedef Eigen::Map MapType; + typedef MapType value_type; + + typedef const Eigen::Map ConstMapType; + + typedef std::vector MapVector; + + typedef typename MapVector::iterator iterator; + typedef typename MapVector::const_iterator const_iterator; + + /// \brief Default constructor + MatrixStackTpl() = default; + + /// \brief Constructor + /// + /// \param[in] max_elts Maximum number of matrices contained in the stack + /// \param[in] max_elt_size Maximal size of each matrices (rows() x cols()) if known at + /// construction time. Default value to 0. + /// + explicit MatrixStackTpl(const std::size_t max_elts, const std::size_t max_elt_size = 0) { - m_matrix_maps.reserve(max_elts); + PINOCCHIO_UNUSED_VARIABLE(max_elt_size); + if (max_elts > 0) + { + m_matrix_maps.reserve(max_elts); + } } - } - /// \brief Copy constructor - MatrixStackTpl(const MatrixStackTpl & other) = default; + /// \brief Copy constructor + MatrixStackTpl(const MatrixStackTpl & other) = default; - /// \brief Move constructor - MatrixStackTpl(MatrixStackTpl && other) = default; + /// \brief Move constructor + MatrixStackTpl(MatrixStackTpl && other) = default; - /// @brief Equality comparison operator. - /// @param other MatrixStackTpl to compare with. - /// @returns true if the underlying maps are equal. - bool operator==(const MatrixStackTpl & other) const - { - if (this == &other) - return true; - return m_matrix_maps == other.m_matrix_maps; - } + /// @brief Equality comparison operator. + /// @param other MatrixStackTpl to compare with. + /// @returns true if the underlying maps are equal. + bool operator==(const MatrixStackTpl & other) const + { + if (this == &other) + return true; + return m_matrix_maps == other.m_matrix_maps; + } - /// @brief Inequality comparison operator. - /// @param other MatrixStackTpl to compare with. - /// @return true if the underlying maps are not equal. - bool operator!=(const MatrixStackTpl & other) const - { - return !(*this == other); - } + /// @brief Inequality comparison operator. + /// @param other MatrixStackTpl to compare with. + /// @return true if the underlying maps are not equal. + bool operator!=(const MatrixStackTpl & other) const + { + return !(*this == other); + } - MatrixStackTpl & operator=(const MatrixStackTpl & other) = default; - MatrixStackTpl & operator=(MatrixStackTpl && other) = default; + MatrixStackTpl & operator=(const MatrixStackTpl & other) = default; + MatrixStackTpl & operator=(MatrixStackTpl && other) = default; - void rebuild(const std::vector & matrix_infos) - { - rebuild(matrix_infos.data(), matrix_infos.size()); - } + void rebuild(const std::vector & matrix_infos) + { + rebuild(matrix_infos.data(), matrix_infos.size()); + } - void rebuild(const MatrixInfo * matrix_infos, const size_t size) - { - clear(); - for (std::size_t i = 0; i < size; ++i) + void rebuild(const MatrixInfo * matrix_infos, const size_t size) { - push_back(matrix_infos[i]); + clear(); + for (std::size_t i = 0; i < size; ++i) + { + push_back(matrix_infos[i]); + } } - } - template - void push_back(const Eigen::MatrixBase & matrix) - { - m_matrix_maps.push_back(matrix); - } + template + void push_back(const Eigen::MatrixBase & matrix) + { + m_matrix_maps.push_back(matrix); + } - template - void emplace_back(Args &&... args) - { - m_matrix_maps.emplace_back(std::forward(args)...); - } + template + void emplace_back(Args &&... args) + { + m_matrix_maps.emplace_back(std::forward(args)...); + } - void - push_back(const MatrixInfo & matrix_info, const std::function init_func = {}) - { - this->push_back(matrix_info.rows(), matrix_info.cols(), init_func); - } + void + push_back(const MatrixInfo & matrix_info, const std::function init_func = {}) + { + this->push_back(matrix_info.rows(), matrix_info.cols(), init_func); + } - void - push_back(const Index rows, const Index cols, const std::function init_func = {}) - { - m_matrix_maps.emplace_back(rows, cols); - if (init_func) - init_func(back()); - } + void push_back( + const Index rows, const Index cols, const std::function init_func = {}) + { + m_matrix_maps.emplace_back(rows, cols); + if (init_func) + init_func(back()); + } - /// \brief Returns a reference to the last element in the container. - MapType back() - { - auto & m = m_matrix_maps.back(); - return MapType(m.data(), m.rows(), m.cols()); - } + /// \brief Returns a reference to the last element in the container. + MapType back() + { + auto & m = m_matrix_maps.back(); + return MapType(m.data(), m.rows(), m.cols()); + } - /// \brief Returns a reference to the last element in the container. - ConstMapType back() const - { - const auto & m = m_matrix_maps.back(); - return ConstMapType(m.data(), m.rows(), m.cols()); - } - - ///  \brief Checks if the container has no elements. - /// - /// \returns true if the container is empty, false otherwise. - bool empty() const - { - return m_matrix_maps.empty(); - } + /// \brief Returns a reference to the last element in the container. + ConstMapType back() const + { + const auto & m = m_matrix_maps.back(); + return ConstMapType(m.data(), m.rows(), m.cols()); + } - /// \brief Increase the capacity of the vector of matrix maps. - void reserve(size_t new_cap) - { - m_matrix_maps.reserve(new_cap); - } + ///  \brief Checks if the container has no elements. + /// + /// \returns true if the container is empty, false otherwise. + bool empty() const + { + return m_matrix_maps.empty(); + } - std::size_t capacity() const - { - return m_matrix_maps.capacity(); - } + /// \brief Increase the capacity of the vector of matrix maps. + void reserve(size_t new_cap) + { + m_matrix_maps.reserve(new_cap); + } - /// \brief Returns a reference to the element at specified location pos. - MapType operator[](const std::size_t pos) - { - auto & m = m_matrix_maps[pos]; - return MapType(m.data(), m.rows(), m.cols()); - } - /// \brief Returns a reference to the element at specified location pos. - ConstMapType operator[](const std::size_t pos) const - { - const auto & m = m_matrix_maps[pos]; - return ConstMapType(m.data(), m.rows(), m.cols()); - } + std::size_t capacity() const + { + return m_matrix_maps.capacity(); + } - /// \brief Returns the number of elements in the container. - std::size_t size() const - { - return m_matrix_maps.size(); - } + /// \brief Returns a reference to the element at specified location pos. + MapType operator[](const std::size_t pos) + { + auto & m = m_matrix_maps[pos]; + return MapType(m.data(), m.rows(), m.cols()); + } + /// \brief Returns a reference to the element at specified location pos. + ConstMapType operator[](const std::size_t pos) const + { + const auto & m = m_matrix_maps[pos]; + return ConstMapType(m.data(), m.rows(), m.cols()); + } - /// \brief Returns a pointer to the underlying array serving as element storage. - void * data() - { - return m_matrix_maps.data(); - } - /// \brief Returns a pointer to the underlying array serving as element storage. - const void * data() const - { - return m_matrix_maps.data(); - } + /// \brief Returns the number of elements in the container. + std::size_t size() const + { + return m_matrix_maps.size(); + } - /// \brief Erases the specified elements from the container. - /// \remarks The data associated with the pos element is not reused after erasing. - iterator erase(iterator pos) - { - return m_matrix_maps.erase(pos); - } + /// \brief Returns a pointer to the underlying array serving as element storage. + void * data() + { + return m_matrix_maps.data(); + } + /// \brief Returns a pointer to the underlying array serving as element storage. + const void * data() const + { + return m_matrix_maps.data(); + } - /// \brief Erases the specified elements from the container. - /// \remarks The data associated with the pos element is not reused after erasing. - iterator erase(const_iterator pos) - { - return m_matrix_maps.erase(pos); - } + /// \brief Erases the specified elements from the container. + /// \remarks The data associated with the pos element is not reused after erasing. + iterator erase(iterator pos) + { + return m_matrix_maps.erase(pos); + } - void clear() - { - m_matrix_maps.clear(); - } + /// \brief Erases the specified elements from the container. + /// \remarks The data associated with the pos element is not reused after erasing. + iterator erase(const_iterator pos) + { + return m_matrix_maps.erase(pos); + } - iterator begin() - { - return m_matrix_maps.begin(); - } + void clear() + { + m_matrix_maps.clear(); + } - iterator end() - { - return m_matrix_maps.end(); - } + iterator begin() + { + return m_matrix_maps.begin(); + } - const_iterator begin() const - { - return m_matrix_maps.begin(); - } + iterator end() + { + return m_matrix_maps.end(); + } - const_iterator end() const - { - return m_matrix_maps.end(); - } + const_iterator begin() const + { + return m_matrix_maps.begin(); + } - iterator rbegin() - { - return m_matrix_maps.cbegin(); - } + const_iterator end() const + { + return m_matrix_maps.end(); + } - iterator rend() - { - return m_matrix_maps.cend(); - } + iterator rbegin() + { + return m_matrix_maps.cbegin(); + } - const_iterator rbegin() const - { - return m_matrix_maps.cbegin(); - } + iterator rend() + { + return m_matrix_maps.cend(); + } - const_iterator rend() const - { - return m_matrix_maps.cend(); - } + const_iterator rbegin() const + { + return m_matrix_maps.cbegin(); + } - void apply(const std::function & func) - { - std::for_each(begin(), end(), func); - } + const_iterator rend() const + { + return m_matrix_maps.cend(); + } - void apply(const std::function & func) const - { - std::for_each(begin(), end(), func); - } + void apply(const std::function & func) + { + std::for_each(begin(), end(), func); + } - /// \brief Returns the current memory footprint of this object in bytes. - /// \details Sums up the sizes of all internal data members. - std::size_t sizeInBytes() const - { - std::size_t size = 0; - for (const auto & m : m_matrix_maps) + void apply(const std::function & func) const { - size += sizeof(Scalar) * std::size_t(m.rows() * m.cols()); + std::for_each(begin(), end(), func); + } + + /// \brief Returns the current memory footprint of this object in bytes. + /// \details Sums up the sizes of all internal data members. + std::size_t sizeInBytes() const + { + std::size_t size = 0; + for (const auto & m : m_matrix_maps) + { + size += sizeof(Scalar) * std::size_t(m.rows() * m.cols()); + } + return size; } - return size; - } - protected: - MapVector m_matrix_maps; - }; // struct MatrixStackTpl + protected: + MapVector m_matrix_maps; + }; // struct MatrixStackTpl + } // namespace internal } // namespace pinocchio diff --git a/include/pinocchio/src/math/block-diagonal-matrix.hxx b/include/pinocchio/src/math/block-diagonal-matrix.hxx index b4a3f04eec..3cee1cf2ef 100644 --- a/include/pinocchio/src/math/block-diagonal-matrix.hxx +++ b/include/pinocchio/src/math/block-diagonal-matrix.hxx @@ -46,7 +46,7 @@ namespace pinocchio typedef Eigen::Map VectorMap; typedef Eigen::Map ConstVectorMap; - typedef MatrixStackTpl MatrixStack; + typedef internal::MatrixStackTpl MatrixStack; typedef MatrixBlockElementTpl MatrixBlockElement; typedef MatrixBlockElementTpl ConstMatrixBlockElement; /** diff --git a/include/pinocchio/src/multibody/data.hxx b/include/pinocchio/src/multibody/data.hxx index 64e6c47f26..be4f97d287 100644 --- a/include/pinocchio/src/multibody/data.hxx +++ b/include/pinocchio/src/multibody/data.hxx @@ -86,7 +86,7 @@ namespace pinocchio ///  \brief The type of Tensor for Kinematics and Dynamics second order derivatives typedef Tensor Tensor3x; - typedef MatrixStackTpl DynamicMatrixStack; + typedef internal::MatrixStackTpl DynamicMatrixStack; typedef ConstraintCholeskyDecompositionTpl ConstraintCholeskyDecomposition; @@ -604,7 +604,8 @@ namespace pinocchio /// \brief Stores the projected cross-coupling between links as /// `projected_joint_cross_coupling(j,i) = cross_coupling(j,i) * J_i`. - container::DoubleEntryContainer> projected_joint_cross_coupling; + container::DoubleEntryContainer> + projected_joint_cross_coupling; /// \brief Stores the elimination ordering of LC-ABA std::vector joint_elimination_order; diff --git a/include/pinocchio/src/serialization/matrix-stack.hxx b/include/pinocchio/src/serialization/matrix-stack.hxx index a8997b6059..67fdcef737 100644 --- a/include/pinocchio/src/serialization/matrix-stack.hxx +++ b/include/pinocchio/src/serialization/matrix-stack.hxx @@ -19,9 +19,10 @@ namespace boost namespace internal { template - struct MatrixStackAccessor : public ::pinocchio::MatrixStackTpl + struct MatrixStackAccessor + : public ::pinocchio::internal::MatrixStackTpl { - typedef ::pinocchio::MatrixStackTpl Base; + typedef ::pinocchio::internal::MatrixStackTpl Base; using Base::m_data_ptr; using Base::m_matrix_maps; using Base::m_memory_capacity; @@ -37,10 +38,10 @@ namespace boost template void serialize( Archive & ar, - ::pinocchio::MatrixStackTpl & matrix_stack, + ::pinocchio::internal::MatrixStackTpl & matrix_stack, const unsigned int /*version*/) { - typedef ::pinocchio::MatrixStackTpl MatrixStack; + typedef ::pinocchio::internal::MatrixStackTpl MatrixStack; typedef typename MatrixStack::MapType MapType; typedef typename MatrixStack::Scalar Scalar; typedef internal::MatrixStackAccessor Accessor; diff --git a/unittest/double-entry-container.cpp b/unittest/double-entry-container.cpp index 55b6bd558a..dc7b738ea0 100644 --- a/unittest/double-entry-container.cpp +++ b/unittest/double-entry-container.cpp @@ -119,7 +119,7 @@ BOOST_AUTO_TEST_CASE(test_all_matrix_stack) { typedef Eigen::Matrix Matrix6; - typedef MatrixStackTpl Vector; + typedef internal::MatrixStackTpl Vector; typedef container::DoubleEntryContainer Container; const Eigen::Index nrows = 20, ncols = 20; diff --git a/unittest/matrix-stack.cpp b/unittest/matrix-stack.cpp index d35bf1e51f..123a9955a3 100644 --- a/unittest/matrix-stack.cpp +++ b/unittest/matrix-stack.cpp @@ -14,8 +14,8 @@ using namespace pinocchio; typedef Eigen::MatrixXf MatrixXs; typedef MatrixXs::Scalar Scalar; -typedef MatrixStackTpl MatrixXsStack; -typedef MatrixStackTpl +typedef internal::MatrixStackTpl MatrixXsStack; +typedef internal::MatrixStackTpl RowMatrixXsStack; // typedef EigenStorageTpl EigenStorageVector; @@ -390,7 +390,7 @@ BOOST_AUTO_TEST_CASE(matrix_stack_product) using VectorXd = Eigen::Matrix; using Matrix3d = Eigen::Matrix; using Vector3d = Eigen::Matrix; - using MatrixStack = pinocchio::MatrixStackTpl; + using MatrixStack = pinocchio::internal::MatrixStackTpl; const std::size_t N = static_cast(std::rand() % 10); diff --git a/unittest/serialization-math.cpp b/unittest/serialization-math.cpp index 70cccfa639..32a7389132 100644 --- a/unittest/serialization-math.cpp +++ b/unittest/serialization-math.cpp @@ -12,9 +12,9 @@ #include "serialization.hpp" template -struct empty_contructor_algo> +struct empty_contructor_algo> { - typedef pinocchio::MatrixStackTpl Self; + typedef pinocchio::internal::MatrixStackTpl Self; static Self * run() { return new Self(0); @@ -25,7 +25,7 @@ BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE) BOOST_AUTO_TEST_CASE(matrix_stack) { - typedef pinocchio::MatrixStackTpl MatrixStack; + typedef pinocchio::internal::MatrixStackTpl MatrixStack; { MatrixStack matrix_stack(20); diff --git a/unittest/serialization.cpp b/unittest/serialization.cpp index 67564c8169..ce3e6044ca 100644 --- a/unittest/serialization.cpp +++ b/unittest/serialization.cpp @@ -30,9 +30,9 @@ // }; template -struct empty_contructor_algo> +struct empty_contructor_algo> { - typedef pinocchio::MatrixStackTpl Self; + typedef pinocchio::internal::MatrixStackTpl Self; static Self * run() { return new Self(0); From 22ff27203ffc3e09ef64c1fcaf0a623d6cd1b62f Mon Sep 17 00:00:00 2001 From: Joris Vaillant Date: Thu, 2 Apr 2026 12:07:07 +0200 Subject: [PATCH 02/14] container: Turn EigenStorage internal --- .../algorithm/constraint-cholesky-decl.hpp | 2 + .../algorithm/solvers/admm-solver.hpp | 4 +- .../algorithm/solvers/pgs-solver.hpp | 6 +- .../algorithm/constraint-cholesky-decl.hxx | 6 +- .../src/algorithm/delassus-operator-dense.hxx | 4 +- .../delassus-operator-rigid-body.hxx | 2 +- .../constraints/joint-limit-constraint.hxx | 2 +- .../pinocchio/src/container/eigen-storage.hxx | 590 +++++++++--------- .../src/serialization/eigen-storage.hxx | 6 +- unittest/eigen-storage.cpp | 6 +- unittest/matrix-stack.cpp | 2 - unittest/serialization-math.cpp | 2 +- 12 files changed, 319 insertions(+), 313 deletions(-) diff --git a/include/pinocchio/algorithm/constraint-cholesky-decl.hpp b/include/pinocchio/algorithm/constraint-cholesky-decl.hpp index 7cbcd41a07..3db9701abb 100644 --- a/include/pinocchio/algorithm/constraint-cholesky-decl.hpp +++ b/include/pinocchio/algorithm/constraint-cholesky-decl.hpp @@ -16,6 +16,8 @@ #include "pinocchio/eigen-common.hpp" #include "pinocchio/context.hpp" +#include "pinocchio/math.hpp" + #include "pinocchio/container/eigen-storage.hpp" #include "pinocchio/multibody/fwd.hpp" diff --git a/include/pinocchio/algorithm/solvers/admm-solver.hpp b/include/pinocchio/algorithm/solvers/admm-solver.hpp index 66bc1a62b2..acfe089ac3 100644 --- a/include/pinocchio/algorithm/solvers/admm-solver.hpp +++ b/include/pinocchio/algorithm/solvers/admm-solver.hpp @@ -466,7 +466,7 @@ namespace pinocchio typedef ConstraintSolverResultBase Base; typedef Eigen::Matrix VectorXs; - typedef EigenStorageTpl VectorXsStorage; + typedef internal::EigenStorageTpl VectorXsStorage; typedef Eigen::Ref RefConstVectorXs; using Base::constraintSize; @@ -818,7 +818,7 @@ namespace pinocchio static constexpr int Options = _Options; typedef Eigen::Matrix VectorXs; typedef Eigen::Matrix MatrixXs; - typedef EigenStorageTpl VectorXsStorage; + typedef internal::EigenStorageTpl VectorXsStorage; typedef LanczosDecompositionTpl LanczosDecomposition; typedef AndersonAccelerationTpl AndersonAcceleration; diff --git a/include/pinocchio/algorithm/solvers/pgs-solver.hpp b/include/pinocchio/algorithm/solvers/pgs-solver.hpp index 9ee33f28cd..c8c7c4ac35 100644 --- a/include/pinocchio/algorithm/solvers/pgs-solver.hpp +++ b/include/pinocchio/algorithm/solvers/pgs-solver.hpp @@ -238,7 +238,7 @@ namespace pinocchio typedef Eigen::Matrix VectorXs; typedef Eigen::Ref RefConstVectorXs; - typedef EigenStorageTpl VectorXsStorage; + typedef internal::EigenStorageTpl VectorXsStorage; using Base::constraintSize; using Base::setConstraintImpulseGuess; @@ -477,9 +477,9 @@ namespace pinocchio typedef _Scalar Scalar; static constexpr int Options = _Options; typedef Eigen::Matrix VectorXs; - typedef EigenStorageTpl VectorXsStorage; + typedef internal::EigenStorageTpl VectorXsStorage; typedef Eigen::Matrix MatrixXs; - typedef EigenStorageTpl MatrixXsStorage; + typedef internal::EigenStorageTpl MatrixXsStorage; /// \brief Constructor given problem_size. PGSSolverWorkspaceTpl(std::size_t problem_size = 0) diff --git a/include/pinocchio/src/algorithm/constraint-cholesky-decl.hxx b/include/pinocchio/src/algorithm/constraint-cholesky-decl.hxx index acbca747ce..87842226c3 100644 --- a/include/pinocchio/src/algorithm/constraint-cholesky-decl.hxx +++ b/include/pinocchio/src/algorithm/constraint-cholesky-decl.hxx @@ -65,9 +65,9 @@ namespace pinocchio typedef Eigen::Matrix Matrix; typedef typename PINOCCHIO_EIGEN_PLAIN_ROW_MAJOR_TYPE(Matrix) RowMatrix; - typedef EigenStorageTpl EigenStorageVector; - typedef EigenStorageTpl EigenStorageMatrix; - typedef EigenStorageTpl EigenStorageRowMatrix; + typedef internal::EigenStorageTpl EigenStorageVector; + typedef internal::EigenStorageTpl EigenStorageMatrix; + typedef internal::EigenStorageTpl EigenStorageRowMatrix; typedef BlockDiagonalMatrixTpl BlockDiagonalMatrix; typedef BlockDiagonalMatrix DampingType; diff --git a/include/pinocchio/src/algorithm/delassus-operator-dense.hxx b/include/pinocchio/src/algorithm/delassus-operator-dense.hxx index 424ff6ff64..8967c83451 100644 --- a/include/pinocchio/src/algorithm/delassus-operator-dense.hxx +++ b/include/pinocchio/src/algorithm/delassus-operator-dense.hxx @@ -87,8 +87,8 @@ namespace pinocchio typedef typename traits::VectorXs VectorXs; typedef typename traits::getDampingReturnType getDampingReturnType; typedef typename traits::getComplianceReturnType getComplianceReturnType; - typedef EigenStorageTpl MatrixStorage; - typedef EigenStorageTpl VectorStorage; + typedef internal::EigenStorageTpl MatrixStorage; + typedef internal::EigenStorageTpl VectorStorage; typedef typename MatrixStorage::RefMapType MatrixStorageRefMapType; typedef typename VectorStorage::RefMapType VectorStorageRefMapType; typedef typename traits::DampingType DampingType; diff --git a/include/pinocchio/src/algorithm/delassus-operator-rigid-body.hxx b/include/pinocchio/src/algorithm/delassus-operator-rigid-body.hxx index 522eb192e1..a508b35d99 100644 --- a/include/pinocchio/src/algorithm/delassus-operator-rigid-body.hxx +++ b/include/pinocchio/src/algorithm/delassus-operator-rigid-body.hxx @@ -37,7 +37,7 @@ namespace pinocchio typedef Eigen::Matrix MatrixXs; typedef MatrixXs Matrix; - typedef EigenStorageTpl EigenStorageVector; + typedef internal::EigenStorageTpl EigenStorageVector; typedef BlockDiagonalMatrixTpl BlockDiagonalMatrix; typedef ModelTpl Model; diff --git a/include/pinocchio/src/constraints/joint-limit-constraint.hxx b/include/pinocchio/src/constraints/joint-limit-constraint.hxx index ebfb93e183..46338bdac6 100644 --- a/include/pinocchio/src/constraints/joint-limit-constraint.hxx +++ b/include/pinocchio/src/constraints/joint-limit-constraint.hxx @@ -189,7 +189,7 @@ namespace pinocchio // Useful types ------------------------------------------------ typedef Eigen::Matrix VectorXs; - typedef EigenStorageTpl EigenStorageVector; + typedef internal::EigenStorageTpl EigenStorageVector; typedef Eigen::Matrix CompactTangentMap; typedef std::vector VectorOfSize; diff --git a/include/pinocchio/src/container/eigen-storage.hxx b/include/pinocchio/src/container/eigen-storage.hxx index 0cc5265075..fd57a9aece 100644 --- a/include/pinocchio/src/container/eigen-storage.hxx +++ b/include/pinocchio/src/container/eigen-storage.hxx @@ -13,345 +13,351 @@ namespace pinocchio { - - template - struct EigenStorageTpl; + namespace internal + { + template + struct EigenStorageTpl; + } template - struct CastType> + struct CastType> { typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(MatrixLike) PlainMatrixType; typedef typename PlainMatrixType::template CastXpr::Type NewPlainMatrixExpression; typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(std::decay_t) NewPlainMatrixType; - typedef EigenStorageTpl type; + typedef internal::EigenStorageTpl type; }; - template - struct EigenStorageTpl + namespace internal { - typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(MatrixLike) PlainMatrixType; - typedef typename MatrixLike::Scalar Scalar; - - typedef Eigen::Map MapType; - typedef Eigen::Map & RefMapType; - typedef const Eigen::Map & ConstRefMapType; - - typedef Eigen::Map ConstMapType; - typedef Eigen::Map & RefConstMapType; - typedef const Eigen::Map & ConstRefConstMapType; - - typedef Eigen::Index Index; - - static constexpr int MaxResidualSizeAtCompileTime = - ((PlainMatrixType::MaxRowsAtCompileTime != Eigen::Dynamic) - && (PlainMatrixType::MaxRowsAtCompileTime != Eigen::Dynamic)) - ? PlainMatrixType::MaxRowsAtCompileTime * PlainMatrixType::MaxColsAtCompileTime - : Eigen::Dynamic; - static constexpr bool IsVectorAtCompileTime = MatrixLike::IsVectorAtCompileTime; - static constexpr int Options = PlainMatrixType::Options & ~Eigen::RowMajorBit; - - typedef Eigen::Matrix StorageVector; - - /// \brief Default constructor from given matrix dimension (rows, cols) and maximum rows and - /// columns - /// - /// \param[in] rows Number of rows - /// \param[in] cols Number of columns - /// \param[in] max_rows Maximum number of rows - /// \param[in] max_cols Maximum number of columns - /// - EigenStorageTpl(const Index rows, const Index cols, const Index max_rows, const Index max_cols) - : m_storage(max_rows * max_cols) - , m_map({m_storage.data(), rows, cols}) - , m_const_map({m_storage.data(), rows, cols}) - { - } + template + struct EigenStorageTpl + { + typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(MatrixLike) PlainMatrixType; + typedef typename MatrixLike::Scalar Scalar; + + typedef Eigen::Map MapType; + typedef Eigen::Map & RefMapType; + typedef const Eigen::Map & ConstRefMapType; + + typedef Eigen::Map ConstMapType; + typedef Eigen::Map & RefConstMapType; + typedef const Eigen::Map & ConstRefConstMapType; + + typedef Eigen::Index Index; + + static constexpr int MaxResidualSizeAtCompileTime = + ((PlainMatrixType::MaxRowsAtCompileTime != Eigen::Dynamic) + && (PlainMatrixType::MaxRowsAtCompileTime != Eigen::Dynamic)) + ? PlainMatrixType::MaxRowsAtCompileTime * PlainMatrixType::MaxColsAtCompileTime + : Eigen::Dynamic; + static constexpr bool IsVectorAtCompileTime = MatrixLike::IsVectorAtCompileTime; + static constexpr int Options = PlainMatrixType::Options & ~Eigen::RowMajorBit; + + typedef Eigen::Matrix StorageVector; + + /// \brief Default constructor from given matrix dimension (rows, cols) and maximum rows and + /// columns + /// + /// \param[in] rows Number of rows + /// \param[in] cols Number of columns + /// \param[in] max_rows Maximum number of rows + /// \param[in] max_cols Maximum number of columns + /// + EigenStorageTpl( + const Index rows, const Index cols, const Index max_rows, const Index max_cols) + : m_storage(max_rows * max_cols) + , m_map({m_storage.data(), rows, cols}) + , m_const_map({m_storage.data(), rows, cols}) + { + } #ifdef PINOCCHIO_PARSED_BY_DOXYGEN - /// \brief Default constructor from given matrix dimension (rows, cols). - /// - /// \param[in] rows Number of rows. - /// \param[in] cols Number of columns. - /// - EigenStorageTpl(const Index rows, const Index cols) - : m_map(NULL, rows, cols) - , m_const_map(NULL, rows, cols) - { - _init2(rows, cols); - } - - /// \brief Default constructor from given matrix dimension (rows, cols). - /// - /// \param[in] rows Number of rows. - /// \param[in] cols Number of columns. - /// - EigenStorageTpl(const Index size, const Index max_size) - : m_map(NULL, size) - , m_const_map(NULL, size) - { - _init2(rows, cols); - } + /// \brief Default constructor from given matrix dimension (rows, cols). + /// + /// \param[in] rows Number of rows. + /// \param[in] cols Number of columns. + /// + EigenStorageTpl(const Index rows, const Index cols) + : m_map(NULL, rows, cols) + , m_const_map(NULL, rows, cols) + { + _init2(rows, cols); + } + + /// \brief Default constructor from given matrix dimension (rows, cols). + /// + /// \param[in] rows Number of rows. + /// \param[in] cols Number of columns. + /// + EigenStorageTpl(const Index size, const Index max_size) + : m_map(NULL, size) + , m_const_map(NULL, size) + { + _init2(rows, cols); + } #else - EigenStorageTpl(const Index arg0, const Index arg1) - : m_map(NULL, arg0, arg1) - , m_const_map(NULL, arg0, arg1) - { - _init2(arg0, arg1); - } + EigenStorageTpl(const Index arg0, const Index arg1) + : m_map(NULL, arg0, arg1) + , m_const_map(NULL, arg0, arg1) + { + _init2(arg0, arg1); + } #endif - /// \brief Default constructor - EigenStorageTpl() - : m_map(NULL, 0, IsVectorAtCompileTime ? 1 : 0) - , m_const_map(NULL, 0, IsVectorAtCompileTime ? 1 : 0) - { - } + /// \brief Default constructor + EigenStorageTpl() + : m_map(NULL, 0, IsVectorAtCompileTime ? 1 : 0) + , m_const_map(NULL, 0, IsVectorAtCompileTime ? 1 : 0) + { + } - /// \brief Constructor from a given size. For vector only. - explicit EigenStorageTpl(const Index size) - : m_storage(size) - , m_map(m_storage.data(), size) - , m_const_map(m_storage.data(), size) - { - } + /// \brief Constructor from a given size. For vector only. + explicit EigenStorageTpl(const Index size) + : m_storage(size) + , m_map(m_storage.data(), size) + , m_const_map(m_storage.data(), size) + { + } - /// \brief Copy constructor (only consider the active part of storage) - EigenStorageTpl(const EigenStorageTpl & other) - : m_storage(other.m_storage.head(other.m_map.size())) - , m_map(m_storage.data(), other.m_map.rows(), other.m_map.cols()) - , m_const_map(m_storage.data(), other.m_map.rows(), other.m_map.cols()) - { - } + /// \brief Copy constructor (only consider the active part of storage) + EigenStorageTpl(const EigenStorageTpl & other) + : m_storage(other.m_storage.head(other.m_map.size())) + , m_map(m_storage.data(), other.m_map.rows(), other.m_map.cols()) + , m_const_map(m_storage.data(), other.m_map.rows(), other.m_map.cols()) + { + } - /// \brief Move constructor. - EigenStorageTpl(EigenStorageTpl && other) - : m_storage(std::move(other.m_storage)) - , m_map(m_storage.data(), other.m_map.rows(), other.m_map.cols()) - , m_const_map(m_storage.data(), other.m_map.rows(), other.m_map.cols()) - { - } + /// \brief Move constructor. + EigenStorageTpl(EigenStorageTpl && other) + : m_storage(std::move(other.m_storage)) + , m_map(m_storage.data(), other.m_map.rows(), other.m_map.cols()) + , m_const_map(m_storage.data(), other.m_map.rows(), other.m_map.cols()) + { + } - /// \brief Copy assignment operator. - EigenStorageTpl & operator=(const EigenStorageTpl & other) - { - m_storage = other.m_storage.head(other.m_map.size()); - new (&m_map) MapType(m_storage.data(), other.m_map.rows(), other.m_map.cols()); - new (&m_const_map) MapType(m_storage.data(), other.m_map.rows(), other.m_map.cols()); + /// \brief Copy assignment operator. + EigenStorageTpl & operator=(const EigenStorageTpl & other) + { + m_storage = other.m_storage.head(other.m_map.size()); + new (&m_map) MapType(m_storage.data(), other.m_map.rows(), other.m_map.cols()); + new (&m_const_map) MapType(m_storage.data(), other.m_map.rows(), other.m_map.cols()); - return *this; - } + return *this; + } - /// \brief Move assignment operator. - EigenStorageTpl & operator=(EigenStorageTpl && other) - { - m_storage = std::move(other.m_storage); - new (&m_map) MapType(m_storage.data(), other.m_map.rows(), other.m_map.cols()); - new (&m_const_map) MapType(m_storage.data(), other.m_map.rows(), other.m_map.cols()); + /// \brief Move assignment operator. + EigenStorageTpl & operator=(EigenStorageTpl && other) + { + m_storage = std::move(other.m_storage); + new (&m_map) MapType(m_storage.data(), other.m_map.rows(), other.m_map.cols()); + new (&m_const_map) MapType(m_storage.data(), other.m_map.rows(), other.m_map.cols()); - return *this; - } + return *this; + } - /// \brief Cast operator - template - typename CastType::type cast() const - { - typedef typename CastType::type ReturnType; - ReturnType res = ReturnType(rows(), cols()); - res.m_storage.head(size()) = m_storage.head(size()).template cast(); - return res; - } - - /// \brief Resize the current capacity of the internal storage. - /// - /// \remarks The resizing only happens when the new_size is greater than the current capacity - void resize(const Index rows, const Index cols) - { - const Index new_size = rows * cols; - if (new_size > capacity()) - m_storage.resize(2 * new_size); // Double the size of the storage - new (&m_map) MapType(m_storage.data(), rows, cols); - new (&m_const_map) MapType(m_storage.data(), rows, cols); - } - - void resize(const Index new_size) - { - EIGEN_STATIC_ASSERT_VECTOR_ONLY(MatrixLike) - if (new_size > capacity()) - m_storage.resize(2 * new_size); // Double the size of the storage - new (&m_map) MapType(m_storage.data(), new_size); - new (&m_const_map) MapType(m_storage.data(), new_size); - } - - /// \brief Reserve some place if the capacity is not enough. - /// - /// \remarks This is not data conservative - void reserve(const Index rows, const Index cols) - { - const Index new_size = rows * cols; - if (new_size > capacity()) + /// \brief Cast operator + template + typename CastType::type cast() const { - m_storage.resize(new_size); - new (&m_map) MapType(m_storage.data(), m_map.rows(), m_map.cols()); - new (&m_const_map) MapType(m_storage.data(), m_map.rows(), m_map.cols()); + typedef typename CastType::type ReturnType; + ReturnType res = ReturnType(rows(), cols()); + res.m_storage.head(size()) = m_storage.head(size()).template cast(); + return res; } - } - void reserve(const Index new_size) - { - EIGEN_STATIC_ASSERT_VECTOR_ONLY(MatrixLike) - if (new_size > capacity()) + /// \brief Resize the current capacity of the internal storage. + /// + /// \remarks The resizing only happens when the new_size is greater than the current capacity + void resize(const Index rows, const Index cols) { - m_storage.resize(new_size); - new (&m_map) MapType(m_storage.data(), m_map.size()); - new (&m_const_map) MapType(m_storage.data(), m_map.size()); + const Index new_size = rows * cols; + if (new_size > capacity()) + m_storage.resize(2 * new_size); // Double the size of the storage + new (&m_map) MapType(m_storage.data(), rows, cols); + new (&m_const_map) MapType(m_storage.data(), rows, cols); } - } - /// \brief Conservative resize of the current capacity of the internal storage. The data are - /// kepts in memory. - /// - /// \remarks The resizing only happens when the new_size is greater than the current capacity - void conservativeResize(const Index rows, const Index cols) - { - const Index old_rows = this->rows(), old_cols = this->cols(); - const PlainMatrixType copy(map()); // save current value in the storage - this->resize(rows, cols); - - // copy back values - const Index min_rows = (std::min)(rows, old_rows), min_cols = (std::min)(cols, old_cols); - map().topLeftCorner(min_rows, min_cols) = copy.topLeftCorner(min_rows, min_cols); - } - - /// \brief Conservative resize of the current capacity of the internal storage. The data are - /// kepts in memory. - /// - /// \remarks The resizing only happens when the new_size is greater than the current capacity - void conservativeResize(const Index new_size) - { - EIGEN_STATIC_ASSERT_VECTOR_ONLY(MatrixLike) - const Index old_size = this->size(); - const PlainMatrixType copy(map()); // save current value in the storage - this->resize(new_size); - - // copy back values - const Index min_size = (std::min)(new_size, old_size); - map().head(min_size) = copy.head(min_size); - } - - ///  \brief Returns the size of the storage space currently allocated. - Index capacity() const - { - return m_storage.size(); - } + void resize(const Index new_size) + { + EIGEN_STATIC_ASSERT_VECTOR_ONLY(MatrixLike) + if (new_size > capacity()) + m_storage.resize(2 * new_size); // Double the size of the storage + new (&m_map) MapType(m_storage.data(), new_size); + new (&m_const_map) MapType(m_storage.data(), new_size); + } - /// \brief Returns a const reference of the internal storage. - const StorageVector & storage() const - { - return m_storage; - } + /// \brief Reserve some place if the capacity is not enough. + /// + /// \remarks This is not data conservative + void reserve(const Index rows, const Index cols) + { + const Index new_size = rows * cols; + if (new_size > capacity()) + { + m_storage.resize(new_size); + new (&m_map) MapType(m_storage.data(), m_map.rows(), m_map.cols()); + new (&m_const_map) MapType(m_storage.data(), m_map.rows(), m_map.cols()); + } + } - /// \brief Returns the internal pointer to the data. - const Scalar * data() const - { - return m_storage.data(); - } + void reserve(const Index new_size) + { + EIGEN_STATIC_ASSERT_VECTOR_ONLY(MatrixLike) + if (new_size > capacity()) + { + m_storage.resize(new_size); + new (&m_map) MapType(m_storage.data(), m_map.size()); + new (&m_const_map) MapType(m_storage.data(), m_map.size()); + } + } - /// \brief Returns a map toward the internal matrix. - ConstRefMapType map() const - { - return m_map; - } + /// \brief Conservative resize of the current capacity of the internal storage. The data are + /// kepts in memory. + /// + /// \remarks The resizing only happens when the new_size is greater than the current capacity + void conservativeResize(const Index rows, const Index cols) + { + const Index old_rows = this->rows(), old_cols = this->cols(); + const PlainMatrixType copy(map()); // save current value in the storage + this->resize(rows, cols); - /// \brief Returns a map toward the internal matrix. - RefMapType map() - { - return m_map; - } + // copy back values + const Index min_rows = (std::min)(rows, old_rows), min_cols = (std::min)(cols, old_cols); + map().topLeftCorner(min_rows, min_cols) = copy.topLeftCorner(min_rows, min_cols); + } - /// \brief Returns a const map toward the internal matrix. - ConstRefConstMapType const_map() const - { - return m_const_map; - } + /// \brief Conservative resize of the current capacity of the internal storage. The data are + /// kepts in memory. + /// + /// \remarks The resizing only happens when the new_size is greater than the current capacity + void conservativeResize(const Index new_size) + { + EIGEN_STATIC_ASSERT_VECTOR_ONLY(MatrixLike) + const Index old_size = this->size(); + const PlainMatrixType copy(map()); // save current value in the storage + this->resize(new_size); + + // copy back values + const Index min_size = (std::min)(new_size, old_size); + map().head(min_size) = copy.head(min_size); + } - /// \brief Returns a const map toward the internal matrix. - RefConstMapType const_map() - { - return m_const_map; - } + ///  \brief Returns the size of the storage space currently allocated. + Index capacity() const + { + return m_storage.size(); + } - /// \brief Returns the number of rows - Index rows() const - { - return map().rows(); - } + /// \brief Returns a const reference of the internal storage. + const StorageVector & storage() const + { + return m_storage; + } - /// \brief Returns the number of columns - Index cols() const - { - return map().cols(); - } + /// \brief Returns the internal pointer to the data. + const Scalar * data() const + { + return m_storage.data(); + } - ///  \brief Returns the size of the underlying matrix or vector. - Index size() const - { - return map().size(); - } + /// \brief Returns a map toward the internal matrix. + ConstRefMapType map() const + { + return m_map; + } - /// \brief Whether the internal map points towards a valid data. - bool isValid() const - { - return data() != nullptr; - } + /// \brief Returns a map toward the internal matrix. + RefMapType map() + { + return m_map; + } - template - friend struct EigenStorageTpl; + /// \brief Returns a const map toward the internal matrix. + ConstRefConstMapType const_map() const + { + return m_const_map; + } - /// \brief Comparison operator. - template - bool operator==(const EigenStorageTpl & other) const - { - return rows() == other.rows() && cols() == other.cols() - && m_storage.head(size()) == other.m_storage.head(size()); - } + /// \brief Returns a const map toward the internal matrix. + RefConstMapType const_map() + { + return m_const_map; + } - /// \brief Returns the storage capacity in bytes (i.e. sizeof(Scalar) * capacity()). - std::size_t capacityInBytes() const - { - return sizeof(Scalar) * std::size_t(capacity()); - } + /// \brief Returns the number of rows + Index rows() const + { + return map().rows(); + } - /// \brief Returns the current data size in bytes (i.e. sizeof(Scalar) * size()). - std::size_t sizeInBytes() const - { - return sizeof(Scalar) * std::size_t(size()); - } + /// \brief Returns the number of columns + Index cols() const + { + return map().cols(); + } - protected: - /// \brief Internal vector containing the stored quantities - StorageVector m_storage; + ///  \brief Returns the size of the underlying matrix or vector. + Index size() const + { + return map().size(); + } - /// \brief Map - MapType m_map; - ConstMapType m_const_map; + /// \brief Whether the internal map points towards a valid data. + bool isValid() const + { + return data() != nullptr; + } - template - void _init2(const T rows, const T cols, std::enable_if_t * = 0) - { - m_storage = StorageVector(Eigen::Index(rows * cols)); - new (&m_map) MapType(m_storage.data(), rows, cols); - new (&m_const_map) ConstMapType(m_storage.data(), rows, cols); - } + template + friend struct EigenStorageTpl; - template - void _init2(const T size, const T max_size, std::enable_if_t * = 0) - { - m_storage = StorageVector(max_size); - new (&m_map) MapType(m_storage.data(), size); - new (&m_const_map) ConstMapType(m_storage.data(), size); - } - }; // struct EigenStorageTpl + /// \brief Comparison operator. + template + bool operator==(const EigenStorageTpl & other) const + { + return rows() == other.rows() && cols() == other.cols() + && m_storage.head(size()) == other.m_storage.head(size()); + } + + /// \brief Returns the storage capacity in bytes (i.e. sizeof(Scalar) * capacity()). + std::size_t capacityInBytes() const + { + return sizeof(Scalar) * std::size_t(capacity()); + } + + /// \brief Returns the current data size in bytes (i.e. sizeof(Scalar) * size()). + std::size_t sizeInBytes() const + { + return sizeof(Scalar) * std::size_t(size()); + } + + protected: + /// \brief Internal vector containing the stored quantities + StorageVector m_storage; + + /// \brief Map + MapType m_map; + ConstMapType m_const_map; + + template + void _init2(const T rows, const T cols, std::enable_if_t * = 0) + { + m_storage = StorageVector(Eigen::Index(rows * cols)); + new (&m_map) MapType(m_storage.data(), rows, cols); + new (&m_const_map) ConstMapType(m_storage.data(), rows, cols); + } + + template + void _init2(const T size, const T max_size, std::enable_if_t * = 0) + { + m_storage = StorageVector(max_size); + new (&m_map) MapType(m_storage.data(), size); + new (&m_const_map) ConstMapType(m_storage.data(), size); + } + }; // struct EigenStorageTpl + } // namespace internal } // namespace pinocchio diff --git a/include/pinocchio/src/serialization/eigen-storage.hxx b/include/pinocchio/src/serialization/eigen-storage.hxx index d5e6186e27..86174973bf 100644 --- a/include/pinocchio/src/serialization/eigen-storage.hxx +++ b/include/pinocchio/src/serialization/eigen-storage.hxx @@ -19,9 +19,9 @@ namespace boost namespace internal { template - struct EigenStorageAccessor : public ::pinocchio::EigenStorageTpl + struct EigenStorageAccessor : public ::pinocchio::internal::EigenStorageTpl { - typedef ::pinocchio::EigenStorageTpl Base; + typedef ::pinocchio::internal::EigenStorageTpl Base; using Base::m_map; using Base::m_storage; }; @@ -30,7 +30,7 @@ namespace boost template void serialize( Archive & ar, - ::pinocchio::EigenStorageTpl & storage, + ::pinocchio::internal::EigenStorageTpl & storage, const unsigned int /*version*/) { Eigen::Index rows = storage.rows(); diff --git a/unittest/eigen-storage.cpp b/unittest/eigen-storage.cpp index c64e632831..f1c677196a 100644 --- a/unittest/eigen-storage.cpp +++ b/unittest/eigen-storage.cpp @@ -8,10 +8,10 @@ #include using namespace pinocchio; -typedef EigenStorageTpl EigenStorageMatrix; -typedef EigenStorageTpl +typedef internal::EigenStorageTpl EigenStorageMatrix; +typedef internal::EigenStorageTpl EigenStorageRowMatrix; -typedef EigenStorageTpl EigenStorageVector; +typedef internal::EigenStorageTpl EigenStorageVector; BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE) diff --git a/unittest/matrix-stack.cpp b/unittest/matrix-stack.cpp index 123a9955a3..7a7756d882 100644 --- a/unittest/matrix-stack.cpp +++ b/unittest/matrix-stack.cpp @@ -18,8 +18,6 @@ typedef internal::MatrixStackTpl MatrixXsStack; typedef internal::MatrixStackTpl RowMatrixXsStack; -// typedef EigenStorageTpl EigenStorageVector; - bool is_aligned(const void * ptr, const std::size_t alignment) { assert( diff --git a/unittest/serialization-math.cpp b/unittest/serialization-math.cpp index 32a7389132..2fbb7681a5 100644 --- a/unittest/serialization-math.cpp +++ b/unittest/serialization-math.cpp @@ -51,7 +51,7 @@ BOOST_AUTO_TEST_CASE(matrix_stack) BOOST_AUTO_TEST_CASE(eigen_storage) { - typedef pinocchio::EigenStorageTpl EigenStorage; + typedef pinocchio::internal::EigenStorageTpl EigenStorage; { EigenStorage storage(15, 8, 15, 8); From 41c04f4532d604b302d734ebcff369a20dadbc5e Mon Sep 17 00:00:00 2001 From: Joris Vaillant Date: Thu, 2 Apr 2026 14:24:10 +0200 Subject: [PATCH 03/14] container: Turn DoubleEntryContainer internal --- include/pinocchio/src/container/double-entry-container.hxx | 4 ++-- include/pinocchio/src/multibody/data.hxx | 4 ++-- .../pinocchio/src/serialization/double-entry-container.hxx | 6 +++--- unittest/double-entry-container.cpp | 4 ++-- unittest/serialization-math.cpp | 2 +- 5 files changed, 10 insertions(+), 10 deletions(-) diff --git a/include/pinocchio/src/container/double-entry-container.hxx b/include/pinocchio/src/container/double-entry-container.hxx index e508079513..a1beb50d31 100644 --- a/include/pinocchio/src/container/double-entry-container.hxx +++ b/include/pinocchio/src/container/double-entry-container.hxx @@ -13,7 +13,7 @@ namespace pinocchio { - namespace container + namespace internal { template struct DoubleEntryContainer; @@ -305,6 +305,6 @@ namespace pinocchio Vector m_values; }; - } // namespace container + } // namespace internal } // namespace pinocchio diff --git a/include/pinocchio/src/multibody/data.hxx b/include/pinocchio/src/multibody/data.hxx index be4f97d287..d0d873dc00 100644 --- a/include/pinocchio/src/multibody/data.hxx +++ b/include/pinocchio/src/multibody/data.hxx @@ -597,14 +597,14 @@ namespace pinocchio typedef std::pair JointIndexPair; /// \brief Stores the cross-coupling inertias between links in LC-ABA - container::DoubleEntryContainer> joint_cross_coupling; + internal::DoubleEntryContainer> joint_cross_coupling; /// \brief Coupling relation between joints in the presence of coupling constraints. MatrixXb joint_coupling_info; /// \brief Stores the projected cross-coupling between links as /// `projected_joint_cross_coupling(j,i) = cross_coupling(j,i) * J_i`. - container::DoubleEntryContainer> + internal::DoubleEntryContainer> projected_joint_cross_coupling; /// \brief Stores the elimination ordering of LC-ABA diff --git a/include/pinocchio/src/serialization/double-entry-container.hxx b/include/pinocchio/src/serialization/double-entry-container.hxx index d756804120..f24d6f621d 100644 --- a/include/pinocchio/src/serialization/double-entry-container.hxx +++ b/include/pinocchio/src/serialization/double-entry-container.hxx @@ -20,9 +20,9 @@ namespace boost { template struct DoubleEntryContainerAccessor - : public ::pinocchio::container::DoubleEntryContainer + : public ::pinocchio::internal::DoubleEntryContainer { - typedef ::pinocchio::container::DoubleEntryContainer Base; + typedef ::pinocchio::internal::DoubleEntryContainer Base; using Base::m_keys; using Base::m_values; }; @@ -31,7 +31,7 @@ namespace boost template void serialize( Archive & ar, - ::pinocchio::container::DoubleEntryContainer & container, + ::pinocchio::internal::DoubleEntryContainer & container, const unsigned int /*version*/) { typedef internal::DoubleEntryContainerAccessor Accessor; diff --git a/unittest/double-entry-container.cpp b/unittest/double-entry-container.cpp index dc7b738ea0..d76a5814d5 100644 --- a/unittest/double-entry-container.cpp +++ b/unittest/double-entry-container.cpp @@ -17,7 +17,7 @@ BOOST_AUTO_TEST_CASE(test_all_std_vector) typedef Eigen::Matrix Matrix6; typedef std::vector Vector; - typedef container::DoubleEntryContainer Container; + typedef internal::DoubleEntryContainer Container; const Eigen::Index nrows = 20, ncols = 20; @@ -120,7 +120,7 @@ BOOST_AUTO_TEST_CASE(test_all_matrix_stack) typedef Eigen::Matrix Matrix6; typedef internal::MatrixStackTpl Vector; - typedef container::DoubleEntryContainer Container; + typedef internal::DoubleEntryContainer Container; const Eigen::Index nrows = 20, ncols = 20; diff --git a/unittest/serialization-math.cpp b/unittest/serialization-math.cpp index 2fbb7681a5..e90e2e4d9e 100644 --- a/unittest/serialization-math.cpp +++ b/unittest/serialization-math.cpp @@ -73,7 +73,7 @@ BOOST_AUTO_TEST_CASE(eigen_storage) BOOST_AUTO_TEST_CASE(double_entry_container) { typedef pinocchio::Inertia::Matrix6 Matrix6; - typedef pinocchio::container::DoubleEntryContainer> DoubleEntryContainer; + typedef pinocchio::internal::DoubleEntryContainer> DoubleEntryContainer; DoubleEntryContainer container(10, 20); for (Eigen::Index k = 0; k < 10; ++k) From a0bec5dc36aba2ee8610a298b00fb0d5122ad051 Mon Sep 17 00:00:00 2001 From: Joris Vaillant Date: Thu, 2 Apr 2026 15:24:50 +0200 Subject: [PATCH 04/14] math: Turn MatrixBlock, BlockDiagonalMatrix, matrix_inversion and matrix_product internal --- benchmark/timings-eigen.cpp | 2 +- benchmark/timings-linalg-inverse.cpp | 4 +- .../src/algorithm/aba-derivatives.hxx | 2 +- include/pinocchio/src/algorithm/aba.hxx | 4 +- .../src/algorithm/constrained-dynamics.hxx | 2 +- .../algorithm/constraint-cholesky-decl.hxx | 8 +- .../src/algorithm/constraint-cholesky-def.hxx | 4 +- .../delassus-operator-cholesky-expression.hxx | 6 +- .../src/algorithm/delassus-operator-dense.hxx | 9 +- .../delassus-operator-rigid-body-visitors.hxx | 2 +- .../delassus-operator-rigid-body.hxx | 4 +- include/pinocchio/src/algorithm/delassus.hxx | 4 +- .../src/algorithm/loop-constrained-aba.hxx | 2 +- .../src/constraints/constraint-model-base.hxx | 2 +- .../constraints/constraint-model-generic.hxx | 2 +- .../frame-constraint-model-base.hxx | 12 +- .../constraints/joint-friction-constraint.hxx | 14 +- .../constraints/joint-limit-constraint.hxx | 14 +- .../point-constraint-model-base.hxx | 12 +- include/pinocchio/src/constraints/utils.hxx | 6 +- .../visitors/constraint-model-visitor.hxx | 7 +- include/pinocchio/src/fwd.hxx | 2 +- .../src/math/block-diagonal-matrix-base.hxx | 66 +- .../math/block-diagonal-matrix-expression.hxx | 24 +- .../math/block-diagonal-matrix-inverse.hxx | 398 +-- .../src/math/block-diagonal-matrix-sum.hxx | 481 ++-- .../src/math/block-diagonal-matrix.hxx | 2389 +++++++++-------- include/pinocchio/src/math/fwd.hxx | 55 +- .../src/math/matrix-block-element-base.hxx | 157 +- .../math/matrix-block-element-operation.hxx | 29 +- .../math/matrix-block-element-operations.hxx | 184 +- .../src/math/matrix-block-element-plain.hxx | 1396 +++++----- .../src/math/matrix-block-element.hxx | 1006 +++---- .../pinocchio/src/math/matrix-block-type.hxx | 134 +- .../math/matrix-inverse-code-generated.hxx | 20 +- include/pinocchio/src/math/matrix-inverse.hxx | 14 +- include/pinocchio/src/math/matrix-product.hxx | 33 +- .../src/multibody/joint/joint-composite.hxx | 2 +- .../src/multibody/joint/joint-ellipsoid.hxx | 2 +- .../src/multibody/joint/joint-free-flyer.hxx | 2 +- .../src/multibody/joint/joint-planar.hxx | 2 +- .../multibody/joint/joint-spherical-ZYX.hxx | 2 +- .../src/multibody/joint/joint-spherical.hxx | 2 +- .../src/multibody/joint/joint-translation.hxx | 2 +- .../src/multibody/joint/joint-universal.hxx | 2 +- .../serialization/block-diagonal-matrix.hxx | 10 +- .../serialization/matrix-block-element.hxx | 14 +- unittest/block-diagonal-matrix.cpp | 120 +- unittest/delassus-operations.cpp | 8 +- unittest/matrix-block-element.cpp | 88 +- unittest/matrix-inverse.cpp | 4 +- unittest/matrix-product.cpp | 2 +- unittest/serialization-math.cpp | 27 +- 53 files changed, 3432 insertions(+), 3367 deletions(-) diff --git a/benchmark/timings-eigen.cpp b/benchmark/timings-eigen.cpp index 1b89124b60..538eeb3cb1 100644 --- a/benchmark/timings-eigen.cpp +++ b/benchmark/timings-eigen.cpp @@ -112,7 +112,7 @@ void matrix_mult_matrix_call( if constexpr (evaluation_mode == EvaluationMode::STATIC_OP) pinocchio::promote_static_eval<10>(lhs.const_cast_derived().noalias()) = m * rhs; else if constexpr (evaluation_mode == EvaluationMode::MANUAL) - pinocchio::matrix_product( + pinocchio::internal::matrix_product( m.derived(), rhs.derived(), lhs.const_cast_derived()); else lhs.const_cast_derived().noalias() = m * rhs; diff --git a/benchmark/timings-linalg-inverse.cpp b/benchmark/timings-linalg-inverse.cpp index d62aed8341..644d460b6e 100644 --- a/benchmark/timings-linalg-inverse.cpp +++ b/benchmark/timings-linalg-inverse.cpp @@ -84,7 +84,7 @@ struct MatrixInversePinocchio PINOCCHIO_DONT_INLINE static void run(const Eigen::MatrixBase & mat, const Eigen::MatrixBase & mat_inv) { - ::pinocchio::matrix_inversion(mat, mat_inv.const_cast_derived()); + ::pinocchio::internal::matrix_inversion(mat, mat_inv.const_cast_derived()); } }; @@ -94,7 +94,7 @@ struct MatrixInverseCodeGenerated PINOCCHIO_DONT_INLINE static void run(const Eigen::MatrixBase & mat, const Eigen::MatrixBase & mat_inv) { - ::pinocchio::matrix_inversion_code_generated(mat, mat_inv.const_cast_derived()); + ::pinocchio::internal::matrix_inversion_code_generated(mat, mat_inv.const_cast_derived()); } }; diff --git a/include/pinocchio/src/algorithm/aba-derivatives.hxx b/include/pinocchio/src/algorithm/aba-derivatives.hxx index 91bc61e42b..e3b34ed4db 100644 --- a/include/pinocchio/src/algorithm/aba-derivatives.hxx +++ b/include/pinocchio/src/algorithm/aba-derivatives.hxx @@ -131,7 +131,7 @@ namespace pinocchio jdata.StU().diagonal() += jmodel.jointVelocitySelector(model.armature); - ::pinocchio::matrix_inversion(jdata.StU(), jdata.Dinv()); + ::pinocchio::internal::matrix_inversion(jdata.StU(), jdata.Dinv()); jdata.UDinv().noalias() = jdata.U() * jdata.Dinv(); MatrixType & Minv_ = PINOCCHIO_EIGEN_CONST_CAST(MatrixType, Minv); diff --git a/include/pinocchio/src/algorithm/aba.hxx b/include/pinocchio/src/algorithm/aba.hxx index c573b5abdf..37845d9435 100644 --- a/include/pinocchio/src/algorithm/aba.hxx +++ b/include/pinocchio/src/algorithm/aba.hxx @@ -178,7 +178,7 @@ namespace pinocchio // Account for the rotor inertia contribution jdata.StU().diagonal() += jmodel.jointVelocitySelector(model.armature); - ::pinocchio::matrix_inversion(jdata.StU(), jdata.Dinv()); + ::pinocchio::internal::matrix_inversion(jdata.StU(), jdata.Dinv()); jdata.UDinv().noalias() = jdata.U() * jdata.Dinv(); if (parent > 0) @@ -692,7 +692,7 @@ namespace pinocchio // Account for the rotor inertia contribution jdata.StU().diagonal() += jmodel.jointVelocitySelector(model.armature); - ::pinocchio::matrix_inversion(jdata.StU(), jdata.Dinv()); + ::pinocchio::internal::matrix_inversion(jdata.StU(), jdata.Dinv()); jdata.UDinv().noalias() = jdata.U() * jdata.Dinv(); Minv.block(jmodel.idx_v(), jmodel.idx_v(), jmodel.nv(), jmodel.nv()) = jdata.Dinv(); diff --git a/include/pinocchio/src/algorithm/constrained-dynamics.hxx b/include/pinocchio/src/algorithm/constrained-dynamics.hxx index 152303ea58..ab578af449 100644 --- a/include/pinocchio/src/algorithm/constrained-dynamics.hxx +++ b/include/pinocchio/src/algorithm/constrained-dynamics.hxx @@ -623,7 +623,7 @@ namespace pinocchio // Account for the rotor inertia contribution jdata.StU().diagonal() += jmodel.jointVelocitySelector(model.armature); - matrix_inversion(jdata.StU(), jdata.Dinv()); + internal::matrix_inversion(jdata.StU(), jdata.Dinv()); jdata.UDinv().noalias() = jdata.U() * jdata.Dinv(); if (parent > 0) diff --git a/include/pinocchio/src/algorithm/constraint-cholesky-decl.hxx b/include/pinocchio/src/algorithm/constraint-cholesky-decl.hxx index 87842226c3..5869eedf6d 100644 --- a/include/pinocchio/src/algorithm/constraint-cholesky-decl.hxx +++ b/include/pinocchio/src/algorithm/constraint-cholesky-decl.hxx @@ -68,7 +68,7 @@ namespace pinocchio typedef internal::EigenStorageTpl EigenStorageVector; typedef internal::EigenStorageTpl EigenStorageMatrix; typedef internal::EigenStorageTpl EigenStorageRowMatrix; - typedef BlockDiagonalMatrixTpl BlockDiagonalMatrix; + typedef internal::BlockDiagonalMatrixTpl BlockDiagonalMatrix; typedef BlockDiagonalMatrix DampingType; typedef Eigen::Matrix EigenIndexVector; @@ -427,14 +427,14 @@ namespace pinocchio /// template void updateDamping( - const BlockDiagonalMatrixTpl & block_damping); + const internal::BlockDiagonalMatrixTpl & block_damping); /// /// \brief Update the damping from a block diagonal matrix (move overload). /// template - void - updateDamping(BlockDiagonalMatrixTpl && block_damping); + void updateDamping( + internal::BlockDiagonalMatrixTpl && block_damping); /// /// \brief Returns the current damping as a block diagonal matrix. diff --git a/include/pinocchio/src/algorithm/constraint-cholesky-def.hxx b/include/pinocchio/src/algorithm/constraint-cholesky-def.hxx index 300e6ac5eb..86d786d7a2 100644 --- a/include/pinocchio/src/algorithm/constraint-cholesky-def.hxx +++ b/include/pinocchio/src/algorithm/constraint-cholesky-def.hxx @@ -425,7 +425,7 @@ namespace pinocchio template template void ConstraintCholeskyDecompositionTpl::updateDamping( - const BlockDiagonalMatrixTpl & block_damping) + const internal::BlockDiagonalMatrixTpl & block_damping) { if (&block_damping == &m_damping) return; @@ -436,7 +436,7 @@ namespace pinocchio template template void ConstraintCholeskyDecompositionTpl::updateDamping( - BlockDiagonalMatrixTpl && block_damping) + internal::BlockDiagonalMatrixTpl && block_damping) { if (&block_damping == &m_damping) return; diff --git a/include/pinocchio/src/algorithm/delassus-operator-cholesky-expression.hxx b/include/pinocchio/src/algorithm/delassus-operator-cholesky-expression.hxx index 15812ddb61..7937a79c3a 100644 --- a/include/pinocchio/src/algorithm/delassus-operator-cholesky-expression.hxx +++ b/include/pinocchio/src/algorithm/delassus-operator-cholesky-expression.hxx @@ -258,14 +258,14 @@ namespace pinocchio template void updateDampingImpl( - const BlockDiagonalMatrixTpl & block_damping) + const internal::BlockDiagonalMatrixTpl & block_damping) { const_cast(self).updateDamping(block_damping); } template - void - updateDampingImpl(BlockDiagonalMatrixTpl && block_damping) + void updateDampingImpl( + internal::BlockDiagonalMatrixTpl && block_damping) { const_cast(self).updateDamping(std::move(block_damping)); } diff --git a/include/pinocchio/src/algorithm/delassus-operator-dense.hxx b/include/pinocchio/src/algorithm/delassus-operator-dense.hxx index 8967c83451..2547762d98 100644 --- a/include/pinocchio/src/algorithm/delassus-operator-dense.hxx +++ b/include/pinocchio/src/algorithm/delassus-operator-dense.hxx @@ -28,11 +28,11 @@ namespace pinocchio typedef MatrixXs Matrix; // for eigen lazy evaluation typedef Eigen::Matrix VectorXs; - typedef BlockDiagonalMatrixTpl BlockDiagonalMatrix; + typedef internal::BlockDiagonalMatrixTpl BlockDiagonalMatrix; typedef BlockDiagonalMatrix DampingType; typedef const DampingType & getDampingReturnType; - typedef EigenStorageTpl VectorStorage; + typedef internal::EigenStorageTpl VectorStorage; typedef const typename VectorStorage::ConstMapType getComplianceReturnType; }; @@ -329,7 +329,7 @@ namespace pinocchio template void updateDampingImpl( - const BlockDiagonalMatrixTpl & + const internal::BlockDiagonalMatrixTpl & block_diagonal_damping_matrix) { if (&block_diagonal_damping_matrix == &m_damping) @@ -341,7 +341,8 @@ namespace pinocchio template void updateDampingImpl( - BlockDiagonalMatrixTpl && block_diagonal_damping_matrix) + internal::BlockDiagonalMatrixTpl && + block_diagonal_damping_matrix) { if (&block_diagonal_damping_matrix == &m_damping) return; diff --git a/include/pinocchio/src/algorithm/delassus-operator-rigid-body-visitors.hxx b/include/pinocchio/src/algorithm/delassus-operator-rigid-body-visitors.hxx index 88fe964ddb..e8d780a10e 100644 --- a/include/pinocchio/src/algorithm/delassus-operator-rigid-body-visitors.hxx +++ b/include/pinocchio/src/algorithm/delassus-operator-rigid-body-visitors.hxx @@ -76,7 +76,7 @@ namespace pinocchio jdata_augmented.StU() += data.joint_apparent_inertia[joint_i]; enforceSymmetry(jdata_augmented.StU()); - ::pinocchio::matrix_inversion(jdata_augmented.StU(), jdata_augmented.Dinv()); + ::pinocchio::internal::matrix_inversion(jdata_augmented.StU(), jdata_augmented.Dinv()); DO_NOT_PROMOTE_STATIC_EVAL(jdata_augmented.UDinv().noalias()) = jdata_augmented.U() * jdata_augmented.Dinv(); diff --git a/include/pinocchio/src/algorithm/delassus-operator-rigid-body.hxx b/include/pinocchio/src/algorithm/delassus-operator-rigid-body.hxx index a508b35d99..5ed4612fe1 100644 --- a/include/pinocchio/src/algorithm/delassus-operator-rigid-body.hxx +++ b/include/pinocchio/src/algorithm/delassus-operator-rigid-body.hxx @@ -38,7 +38,7 @@ namespace pinocchio typedef MatrixXs Matrix; typedef internal::EigenStorageTpl EigenStorageVector; - typedef BlockDiagonalMatrixTpl BlockDiagonalMatrix; + typedef internal::BlockDiagonalMatrixTpl BlockDiagonalMatrix; typedef ModelTpl Model; typedef typename Model::Data Data; @@ -659,7 +659,7 @@ namespace pinocchio std::runtime_error, "The sum of sizes of the blocks should be the same as the total residual size of the " "constraints vector."); - if (blocks.size() == 1 && blocks[0].type() == MatrixBlockType::Diagonal) + if (blocks.size() == 1 && blocks[0].type() == internal::MatrixBlockType::Diagonal) { // we assume we have a single diagonal block to dispatch on all the contraints typedef typename BlockDiagonalMatrix::ConstVectorMap ConstVectorMap; diff --git a/include/pinocchio/src/algorithm/delassus.hxx b/include/pinocchio/src/algorithm/delassus.hxx index 5bda4a5c32..777f3e3bb8 100644 --- a/include/pinocchio/src/algorithm/delassus.hxx +++ b/include/pinocchio/src/algorithm/delassus.hxx @@ -90,7 +90,7 @@ namespace pinocchio // Account for the rotor inertia contribution jdata.StU().diagonal() += jmodel.jointVelocitySelector(model.armature); - matrix_inversion(jdata.StU(), jdata.Dinv()); + internal::matrix_inversion(jdata.StU(), jdata.Dinv()); jdata.UDinv().noalias() = Jcols @@ -448,7 +448,7 @@ namespace pinocchio // Account for the rotor inertia contribution jdata.StU().diagonal() += jmodel.jointVelocitySelector(model.armature); - matrix_inversion(jdata.StU(), jdata.Dinv()); + internal::matrix_inversion(jdata.StU(), jdata.Dinv()); jdata.UDinv().noalias() = Jcols * jdata.Dinv().transpose(); data.oL[i].setIdentity(); diff --git a/include/pinocchio/src/algorithm/loop-constrained-aba.hxx b/include/pinocchio/src/algorithm/loop-constrained-aba.hxx index 7f0191ac25..63862e5770 100644 --- a/include/pinocchio/src/algorithm/loop-constrained-aba.hxx +++ b/include/pinocchio/src/algorithm/loop-constrained-aba.hxx @@ -113,7 +113,7 @@ namespace pinocchio // Account for the rotor inertia contribution jdata.StU().diagonal() += jmodel.jointVelocitySelector(model.armature); - ::pinocchio::matrix_inversion(jdata.StU(), jdata.Dinv()); + ::pinocchio::internal::matrix_inversion(jdata.StU(), jdata.Dinv()); jdata.UDinv().noalias() = jdata.U() * jdata.Dinv(); // TODO:check where its used when parent == 0 diff --git a/include/pinocchio/src/constraints/constraint-model-base.hxx b/include/pinocchio/src/constraints/constraint-model-base.hxx index 3310e06e53..5201b64c1a 100644 --- a/include/pinocchio/src/constraints/constraint-model-base.hxx +++ b/include/pinocchio/src/constraints/constraint-model-base.hxx @@ -631,7 +631,7 @@ namespace pinocchio const ModelTpl & model, DataTpl & data, const ConstraintData & cdata, - const MatrixBlockElementTpl & constraint_inertia, + const internal::MatrixBlockElementTpl & constraint_inertia, const ReferenceFrameTag reference_frame) const { derived().appendCouplingConstraintInertiasImpl( diff --git a/include/pinocchio/src/constraints/constraint-model-generic.hxx b/include/pinocchio/src/constraints/constraint-model-generic.hxx index 17048a2942..5d0278780c 100644 --- a/include/pinocchio/src/constraints/constraint-model-generic.hxx +++ b/include/pinocchio/src/constraints/constraint-model-generic.hxx @@ -520,7 +520,7 @@ namespace pinocchio const ModelTpl & model, DataTpl & data, const ConstraintData & cdata, - const MatrixBlockElementTpl & constraint_inertia, + const internal::MatrixBlockElementTpl & constraint_inertia, const ReferenceFrameTag reference_frame) const { ::pinocchio::visitors::appendCouplingConstraintInertias( diff --git a/include/pinocchio/src/constraints/frame-constraint-model-base.hxx b/include/pinocchio/src/constraints/frame-constraint-model-base.hxx index 65ff8469bd..967e952ff8 100644 --- a/include/pinocchio/src/constraints/frame-constraint-model-base.hxx +++ b/include/pinocchio/src/constraints/frame-constraint-model-base.hxx @@ -752,35 +752,35 @@ namespace pinocchio const ModelTpl & model, DataTpl & data, const ConstraintData & cdata, - const MatrixBlockElementTpl & constraint_inertia, + const internal::MatrixBlockElementTpl & constraint_inertia, const ReferenceFrameTag reference_frame) const { assert(constraint_inertia.size() == 6); switch (constraint_inertia.type()) { - case MatrixBlockType::Zero: { + case internal::MatrixBlockType::Zero: { break; } - case MatrixBlockType::Identity: { + case internal::MatrixBlockType::Identity: { appendFrameContactConstraintInertias( model, data, cdata, Matrix6::Identity(), reference_frame); break; } - case MatrixBlockType::ScalarIdentity: { + case internal::MatrixBlockType::ScalarIdentity: { const Scalar inertia_val = constraint_inertia.container()(0, 0); const auto cinertia = Vector6::Constant(inertia_val); appendFrameContactConstraintInertias( model, data, cdata, cinertia.asDiagonal(), reference_frame); break; } - case MatrixBlockType::Diagonal: { + case internal::MatrixBlockType::Diagonal: { Vector6 cinertia; constraint_inertia.diagonal(cinertia); appendFrameContactConstraintInertias( model, data, cdata, cinertia.asDiagonal(), reference_frame); break; } - case MatrixBlockType::Plain: { + case internal::MatrixBlockType::Plain: { Matrix6 cinertia; constraint_inertia.matrix(cinertia); appendFrameContactConstraintInertias(model, data, cdata, cinertia, reference_frame); diff --git a/include/pinocchio/src/constraints/joint-friction-constraint.hxx b/include/pinocchio/src/constraints/joint-friction-constraint.hxx index 405989b294..95040a8873 100644 --- a/include/pinocchio/src/constraints/joint-friction-constraint.hxx +++ b/include/pinocchio/src/constraints/joint-friction-constraint.hxx @@ -624,7 +624,7 @@ namespace pinocchio const ModelTpl & model, DataTpl & data, const ConstraintData & cdata, - const MatrixBlockElementTpl & constraint_inertia, + const internal::MatrixBlockElementTpl & constraint_inertia, const ReferenceFrameTag reference_frame) const; protected: @@ -1022,32 +1022,32 @@ namespace pinocchio const ModelTpl & model, DataTpl & data, const ConstraintData & cdata, - const MatrixBlockElementTpl & constraint_inertia, + const internal::MatrixBlockElementTpl & constraint_inertia, const ReferenceFrameTag reference_frame) const { assert(constraint_inertia.size() == residualSize()); switch (constraint_inertia.type()) { - case MatrixBlockType::Zero: { + case internal::MatrixBlockType::Zero: { break; } - case MatrixBlockType::Identity: { + case internal::MatrixBlockType::Identity: { appendCouplingConstraintInertiasImpl( model, data, cdata, VectorXs::Ones(residualSize()), reference_frame); break; } - case MatrixBlockType::ScalarIdentity: { + case internal::MatrixBlockType::ScalarIdentity: { const Scalar val = constraint_inertia.container()(0, 0); appendCouplingConstraintInertiasImpl( model, data, cdata, VectorXs::Constant(residualSize(), val), reference_frame); break; } - case MatrixBlockType::Diagonal: { + case internal::MatrixBlockType::Diagonal: { appendCouplingConstraintInertiasImpl( model, data, cdata, constraint_inertia.container().col(0), reference_frame); break; } - case MatrixBlockType::Plain: { + case internal::MatrixBlockType::Plain: { PINOCCHIO_THROW_PRETTY( std::invalid_argument, "JointFrictionConstraintModel does not support Plain inertia blocks."); diff --git a/include/pinocchio/src/constraints/joint-limit-constraint.hxx b/include/pinocchio/src/constraints/joint-limit-constraint.hxx index 46338bdac6..89ae361876 100644 --- a/include/pinocchio/src/constraints/joint-limit-constraint.hxx +++ b/include/pinocchio/src/constraints/joint-limit-constraint.hxx @@ -766,7 +766,7 @@ namespace pinocchio const ModelTpl & model, DataTpl & data, const ConstraintData & cdata, - const MatrixBlockElementTpl & constraint_inertia, + const internal::MatrixBlockElementTpl & constraint_inertia, const ReferenceFrameTag reference_frame) const; protected: @@ -1615,32 +1615,32 @@ namespace pinocchio const ModelTpl & model, DataTpl & data, const ConstraintData & cdata, - const MatrixBlockElementTpl & constraint_inertia, + const internal::MatrixBlockElementTpl & constraint_inertia, const ReferenceFrameTag reference_frame) const { assert(constraint_inertia.size() == residualSize()); switch (constraint_inertia.type()) { - case MatrixBlockType::Zero: { + case internal::MatrixBlockType::Zero: { break; } - case MatrixBlockType::Identity: { + case internal::MatrixBlockType::Identity: { appendCouplingConstraintInertiasImpl( model, data, cdata, VectorXs::Ones(residualSize()), reference_frame); break; } - case MatrixBlockType::ScalarIdentity: { + case internal::MatrixBlockType::ScalarIdentity: { const Scalar val = constraint_inertia.container()(0, 0); appendCouplingConstraintInertiasImpl( model, data, cdata, VectorXs::Constant(residualSize(), val), reference_frame); break; } - case MatrixBlockType::Diagonal: { + case internal::MatrixBlockType::Diagonal: { appendCouplingConstraintInertiasImpl( model, data, cdata, constraint_inertia.container().col(0), reference_frame); break; } - case MatrixBlockType::Plain: { + case internal::MatrixBlockType::Plain: { PINOCCHIO_THROW_PRETTY( std::invalid_argument, "JointLimitConstraintModel does not support Plain inertia blocks."); break; diff --git a/include/pinocchio/src/constraints/point-constraint-model-base.hxx b/include/pinocchio/src/constraints/point-constraint-model-base.hxx index e8235767d5..6e8919ca58 100644 --- a/include/pinocchio/src/constraints/point-constraint-model-base.hxx +++ b/include/pinocchio/src/constraints/point-constraint-model-base.hxx @@ -804,35 +804,35 @@ namespace pinocchio const ModelTpl & model, DataTpl & data, const ConstraintData & cdata, - const MatrixBlockElementTpl & constraint_inertia, + const internal::MatrixBlockElementTpl & constraint_inertia, const ReferenceFrameTag reference_frame) const { assert(constraint_inertia.size() == 3); switch (constraint_inertia.type()) { - case MatrixBlockType::Zero: { + case internal::MatrixBlockType::Zero: { break; } - case MatrixBlockType::Identity: { + case internal::MatrixBlockType::Identity: { appendPointContactConstraintInertias( model, data, cdata, Matrix3::Identity(), reference_frame); break; } - case MatrixBlockType::ScalarIdentity: { + case internal::MatrixBlockType::ScalarIdentity: { const Scalar inertia_val = constraint_inertia.container()(0, 0); const auto cinertia = Vector3::Constant(inertia_val); appendPointContactConstraintInertias( model, data, cdata, cinertia.asDiagonal(), reference_frame); break; } - case MatrixBlockType::Diagonal: { + case internal::MatrixBlockType::Diagonal: { Vector3 cinertia; constraint_inertia.diagonal(cinertia); appendPointContactConstraintInertias( model, data, cdata, cinertia.asDiagonal(), reference_frame); break; } - case MatrixBlockType::Plain: { + case internal::MatrixBlockType::Plain: { Matrix3 cinertia; constraint_inertia.matrix(cinertia); appendPointContactConstraintInertias(model, data, cdata, cinertia, reference_frame); diff --git a/include/pinocchio/src/constraints/utils.hxx b/include/pinocchio/src/constraints/utils.hxx index a6724d2a7f..9a3d61bd28 100644 --- a/include/pinocchio/src/constraints/utils.hxx +++ b/include/pinocchio/src/constraints/utils.hxx @@ -543,7 +543,7 @@ namespace pinocchio std::size_t Alignment> void constructPositiveDefiniteBlockDiagonalMatrix( const std::vector & constraint_models, - BlockDiagonalMatrixTpl & block_diagonal_matrix); + internal::BlockDiagonalMatrixTpl & block_diagonal_matrix); template< typename Scalar, @@ -1153,9 +1153,9 @@ namespace pinocchio std::size_t Alignment> void constructPositiveDefiniteBlockDiagonalMatrix( const std::vector & constraint_models, - BlockDiagonalMatrixTpl & block_diagonal_matrix) + internal::BlockDiagonalMatrixTpl & block_diagonal_matrix) { - typedef BlockDiagonalMatrixTpl BlockDiagonalMatrix; + typedef internal::BlockDiagonalMatrixTpl BlockDiagonalMatrix; typedef typename BlockDiagonalMatrix::MatrixBlockElement MatrixBlockElement; std::vector block_diagonal_infos; computeBlockDiagonalPattern(constraint_models, block_diagonal_infos, BlockDiagonalDispatcher()); diff --git a/include/pinocchio/src/constraints/visitors/constraint-model-visitor.hxx b/include/pinocchio/src/constraints/visitors/constraint-model-visitor.hxx index f94fb11755..ef3babd700 100644 --- a/include/pinocchio/src/constraints/visitors/constraint-model-visitor.hxx +++ b/include/pinocchio/src/constraints/visitors/constraint-model-visitor.hxx @@ -1417,7 +1417,7 @@ namespace pinocchio typedef boost::fusion::vector< const Model &, Data &, - const MatrixBlockElementTpl &, + const pinocchio::internal::MatrixBlockElementTpl &, ReferenceFrameTag> ArgsType; @@ -1427,7 +1427,8 @@ namespace pinocchio const typename ConstraintModel::ConstraintData & cdata, const Model & model, Data & data, - const MatrixBlockElementTpl & constraint_inertia, + const pinocchio::internal::MatrixBlockElementTpl & + constraint_inertia, const ReferenceFrameTag reference_frame) { cmodel.appendCouplingConstraintInertias( @@ -1449,7 +1450,7 @@ namespace pinocchio const ModelTpl & model, DataTpl & data, const ConstraintDataTpl & cdata, - const MatrixBlockElementTpl & constraint_inertia, + const pinocchio::internal::MatrixBlockElementTpl & constraint_inertia, const ReferenceFrameTag reference_frame) { typedef ConstraintModelAppendCouplingConstraintBlockInertiasVisitor< diff --git a/include/pinocchio/src/fwd.hxx b/include/pinocchio/src/fwd.hxx index 9a7b560f1d..c566cd0d6e 100644 --- a/include/pinocchio/src/fwd.hxx +++ b/include/pinocchio/src/fwd.hxx @@ -173,7 +173,7 @@ namespace pinocchio template class Template> inline constexpr bool is_specialization_of_v = is_specialization_of::value; - template + template struct traits { }; diff --git a/include/pinocchio/src/math/block-diagonal-matrix-base.hxx b/include/pinocchio/src/math/block-diagonal-matrix-base.hxx index 3815da6260..84a937c681 100644 --- a/include/pinocchio/src/math/block-diagonal-matrix-base.hxx +++ b/include/pinocchio/src/math/block-diagonal-matrix-base.hxx @@ -13,38 +13,42 @@ namespace pinocchio { - template - struct BlockDiagonalMatrixBase + namespace internal { - /// @brief Cast to Derived. - Derived & derived() - { - return *static_cast(this); - } - - /// @brief Const cast to Derived. - const Derived & derived() const - { - return *static_cast(this); - } - - /// @brief Returns the total number of rows of the full matrix. - Eigen::Index rows() const - { - return derived().rows(); - } - - /// @brief Returns the total number of cols of the full matrix. - Eigen::Index cols() const - { - return derived().cols(); - } - /// @brief Returns the total number of elements in the full matrix (rows * cols). - Eigen::Index size() const + template + struct BlockDiagonalMatrixBase { - return derived().size(); - } - }; - + /// @brief Cast to Derived. + Derived & derived() + { + return *static_cast(this); + } + + /// @brief Const cast to Derived. + const Derived & derived() const + { + return *static_cast(this); + } + + /// @brief Returns the total number of rows of the full matrix. + Eigen::Index rows() const + { + return derived().rows(); + } + + /// @brief Returns the total number of cols of the full matrix. + Eigen::Index cols() const + { + return derived().cols(); + } + + /// @brief Returns the total number of elements in the full matrix (rows * cols). + Eigen::Index size() const + { + return derived().size(); + } + }; + + } // namespace internal } // namespace pinocchio diff --git a/include/pinocchio/src/math/block-diagonal-matrix-expression.hxx b/include/pinocchio/src/math/block-diagonal-matrix-expression.hxx index ffc8366d31..810759799f 100644 --- a/include/pinocchio/src/math/block-diagonal-matrix-expression.hxx +++ b/include/pinocchio/src/math/block-diagonal-matrix-expression.hxx @@ -13,18 +13,22 @@ namespace pinocchio { - template - struct BlockDiagonalMatrixExpression : BlockDiagonalMatrixBase + namespace internal { - typedef BlockDiagonalMatrixBase Base; - using Base::derived; - /// @brief Evaluates this expression and stores it in res. - template - void evalTo(BlockDiagonalMatrixTpl & res) const + template + struct BlockDiagonalMatrixExpression : BlockDiagonalMatrixBase { - derived().evalTo(res.derived()); - } - }; // struct BlockDiagonalMatrixExpression + typedef BlockDiagonalMatrixBase Base; + using Base::derived; + /// @brief Evaluates this expression and stores it in res. + template + void evalTo(BlockDiagonalMatrixTpl & res) const + { + derived().evalTo(res.derived()); + } + }; // struct BlockDiagonalMatrixExpression + + } // namespace internal } // namespace pinocchio diff --git a/include/pinocchio/src/math/block-diagonal-matrix-inverse.hxx b/include/pinocchio/src/math/block-diagonal-matrix-inverse.hxx index 66e7d6fa4b..33ece61847 100644 --- a/include/pinocchio/src/math/block-diagonal-matrix-inverse.hxx +++ b/include/pinocchio/src/math/block-diagonal-matrix-inverse.hxx @@ -13,262 +13,268 @@ namespace pinocchio { - template - struct Inverse> - : BlockDiagonalMatrixExpression>> + namespace internal { - typedef BlockDiagonalMatrixTpl Derived; - typedef BlockDiagonalMatrixExpression Base; - Inverse(const Derived & matrix) - : m_matrix(matrix) + template + struct Inverse> + : BlockDiagonalMatrixExpression>> { - } + typedef BlockDiagonalMatrixTpl Derived; + typedef BlockDiagonalMatrixExpression Base; - /// @brief \copydoc Base::evalTo - template - void evalTo(BlockDiagonalMatrixTpl & res) const - { - typedef BlockDiagonalMatrixTpl ResType; - - // Check if rebuild is needed - bool need_rebuild = false; - if ( - res.rows() != m_matrix.rows() || res.cols() != m_matrix.cols() - || res.blocks().size() != m_matrix.blocks().size()) + Inverse(const Derived & matrix) + : m_matrix(matrix) { - need_rebuild = true; } - else + + /// @brief \copydoc Base::evalTo + template + void evalTo(BlockDiagonalMatrixTpl & res) const { - for (std::size_t i = 0; i < m_matrix.blocks().size(); ++i) - { - // Upgrading rule: inverse block keeps the same type except for Zero which becomes Plain. - const auto current_type = m_matrix.blocks()[i].type(); - auto target_type = current_type; - if (current_type == MatrixBlockType::Zero) - target_type = MatrixBlockType::Plain; + typedef BlockDiagonalMatrixTpl ResType; - if (res.blocks()[i].type() != target_type) - { - need_rebuild = true; - break; - } - if (current_type == MatrixBlockType::NestedBlockDiagonal) + // Check if rebuild is needed + bool need_rebuild = false; + if ( + res.rows() != m_matrix.rows() || res.cols() != m_matrix.cols() + || res.blocks().size() != m_matrix.blocks().size()) + { + need_rebuild = true; + } + else + { + for (std::size_t i = 0; i < m_matrix.blocks().size(); ++i) { - const auto & src_subs = m_matrix.blocks()[i].nested_blocks(); - const auto & res_subs = res.blocks()[i].nested_blocks(); - if (src_subs.size() != res_subs.size()) + // Upgrading rule: inverse block keeps the same type except for Zero which becomes + // Plain. + const auto current_type = m_matrix.blocks()[i].type(); + auto target_type = current_type; + if (current_type == MatrixBlockType::Zero) + target_type = MatrixBlockType::Plain; + + if (res.blocks()[i].type() != target_type) { need_rebuild = true; break; } - for (std::size_t j = 0; j < src_subs.size(); ++j) + if (current_type == MatrixBlockType::NestedBlockDiagonal) { - auto sub_target = src_subs[j].type(); - if (sub_target == MatrixBlockType::Zero) - sub_target = MatrixBlockType::Plain; - if (res_subs[j].type() != sub_target) + const auto & src_subs = m_matrix.blocks()[i].nested_blocks(); + const auto & res_subs = res.blocks()[i].nested_blocks(); + if (src_subs.size() != res_subs.size()) { need_rebuild = true; break; } + for (std::size_t j = 0; j < src_subs.size(); ++j) + { + auto sub_target = src_subs[j].type(); + if (sub_target == MatrixBlockType::Zero) + sub_target = MatrixBlockType::Plain; + if (res_subs[j].type() != sub_target) + { + need_rebuild = true; + break; + } + } + if (need_rebuild) + break; } - if (need_rebuild) - break; } } - } - ResType * res_ptr = &res; - ResType tmp_res; + ResType * res_ptr = &res; + ResType tmp_res; - if (need_rebuild) - { - typedef Eigen::Matrix ResMatrix; - typedef Eigen::Map ResMatrixMap; - typedef MatrixBlockElementTpl ResMatrixBlockElement; + if (need_rebuild) + { + typedef Eigen::Matrix ResMatrix; + typedef Eigen::Map ResMatrixMap; + typedef MatrixBlockElementTpl ResMatrixBlockElement; - typedef typename ResType::MatrixBlockElement MatrixBlockElement; - static_assert( - pinocchio::internal::is_same_type::value, - "MatrixBlockElement is not of type pinocchio::MatrixBlockElementTpl"); + typedef typename ResType::MatrixBlockElement MatrixBlockElement; + static_assert( + pinocchio::internal::is_same_type::value, + "MatrixBlockElement is not of type " + "pinocchio::internal::MatrixBlockElementTpl"); - const std::size_t num_blocks = m_matrix.blocks().size(); + const std::size_t num_blocks = m_matrix.blocks().size(); - // Check if any block needs type upgrading (Zero→Plain, including nested sub-blocks). - // If no upgrading is needed we can rebuild directly from the original pattern, - // avoiding any extra temporary allocation. - bool needs_upgrade = false; - for (std::size_t i = 0; i < num_blocks && !needs_upgrade; ++i) - { - const auto & block = m_matrix.blocks()[i]; - if (block.type() == MatrixBlockType::Zero) - { - needs_upgrade = true; - } - else if (block.type() == MatrixBlockType::NestedBlockDiagonal) + // Check if any block needs type upgrading (Zero→Plain, including nested sub-blocks). + // If no upgrading is needed we can rebuild directly from the original pattern, + // avoiding any extra temporary allocation. + bool needs_upgrade = false; + for (std::size_t i = 0; i < num_blocks && !needs_upgrade; ++i) { - for (const auto & sub : block.nested_blocks()) - if (sub.type() == MatrixBlockType::Zero) - { - needs_upgrade = true; - break; - } + const auto & block = m_matrix.blocks()[i]; + if (block.type() == MatrixBlockType::Zero) + { + needs_upgrade = true; + } + else if (block.type() == MatrixBlockType::NestedBlockDiagonal) + { + for (const auto & sub : block.nested_blocks()) + if (sub.type() == MatrixBlockType::Zero) + { + needs_upgrade = true; + break; + } + } } - } - if (!needs_upgrade) - { - // Pattern is already compatible: rebuild directly from existing blocks. - if (&res == &m_matrix) + if (!needs_upgrade) { - tmp_res.rebuild(m_matrix.blocks()); - res_ptr = &tmp_res; + // Pattern is already compatible: rebuild directly from existing blocks. + if (&res == &m_matrix) + { + tmp_res.rebuild(m_matrix.blocks()); + res_ptr = &tmp_res; + } + else + { + res.rebuild(m_matrix.blocks()); + } } else { - res.rebuild(m_matrix.blocks()); - } - } - else - { - // Need to upgrade some block types. Use alloca for the outer pattern array. - // For NestedBlockDiagonal entries, placement-new with a moved sub-vector and - // call the destructor explicitly afterwards to avoid a memory leak. - MatrixBlockElement * new_pattern = static_cast( - PINOCCHIO_ALLOCA(num_blocks * sizeof(MatrixBlockElement))); - bool has_nested_in_pattern = false; - for (std::size_t i = 0; i < num_blocks; ++i) - { - const auto & block = m_matrix.blocks()[i]; - if (block.type() == MatrixBlockType::NestedBlockDiagonal) + // Need to upgrade some block types. Use alloca for the outer pattern array. + // For NestedBlockDiagonal entries, placement-new with a moved sub-vector and + // call the destructor explicitly afterwards to avoid a memory leak. + MatrixBlockElement * new_pattern = static_cast( + PINOCCHIO_ALLOCA(num_blocks * sizeof(MatrixBlockElement))); + bool has_nested_in_pattern = false; + for (std::size_t i = 0; i < num_blocks; ++i) { - has_nested_in_pattern = true; - std::vector new_subs; - new_subs.reserve(block.nested_blocks().size()); - for (const auto & sub : block.nested_blocks()) + const auto & block = m_matrix.blocks()[i]; + if (block.type() == MatrixBlockType::NestedBlockDiagonal) + { + has_nested_in_pattern = true; + std::vector new_subs; + new_subs.reserve(block.nested_blocks().size()); + for (const auto & sub : block.nested_blocks()) + { + MatrixBlockType sub_type = sub.type(); + if (sub_type == MatrixBlockType::Zero) + sub_type = MatrixBlockType::Plain; + new_subs.emplace_back(sub_type, sub.size()); + } + new (new_pattern + i) + MatrixBlockElement(MatrixBlockType::NestedBlockDiagonal, std::move(new_subs)); + } + else { - MatrixBlockType sub_type = sub.type(); - if (sub_type == MatrixBlockType::Zero) - sub_type = MatrixBlockType::Plain; - new_subs.emplace_back(sub_type, sub.size()); + MatrixBlockType new_type = block.type(); + if (new_type == MatrixBlockType::Zero) + new_type = MatrixBlockType::Plain; + new (new_pattern + i) MatrixBlockElement(new_type, block.size()); } - new (new_pattern + i) - MatrixBlockElement(MatrixBlockType::NestedBlockDiagonal, std::move(new_subs)); + } + + if (&res == &m_matrix) + { + tmp_res.rebuild(new_pattern, num_blocks); + res_ptr = &tmp_res; } else { - MatrixBlockType new_type = block.type(); - if (new_type == MatrixBlockType::Zero) - new_type = MatrixBlockType::Plain; - new (new_pattern + i) MatrixBlockElement(new_type, block.size()); + res.rebuild(new_pattern, num_blocks); } - } - if (&res == &m_matrix) - { - tmp_res.rebuild(new_pattern, num_blocks); - res_ptr = &tmp_res; - } - else - { - res.rebuild(new_pattern, num_blocks); + // Explicitly destroy alloca'd entries to release any heap memory held by nested + // sub-block std::vectors. + if (has_nested_in_pattern) + for (std::size_t i = 0; i < num_blocks; ++i) + new_pattern[i].~MatrixBlockElement(); } - - // Explicitly destroy alloca'd entries to release any heap memory held by nested - // sub-block std::vectors. - if (has_nested_in_pattern) - for (std::size_t i = 0; i < num_blocks; ++i) - new_pattern[i].~MatrixBlockElement(); } - } - - const auto num_blocks = m_matrix.blocks().size(); - for (size_t block_id = 0; block_id < num_blocks; ++block_id) - { - const auto & input_block = m_matrix.blocks()[block_id]; - auto & res_block = res_ptr->blocks()[block_id]; - if (input_block.type() == MatrixBlockType::NestedBlockDiagonal) + const auto num_blocks = m_matrix.blocks().size(); + for (size_t block_id = 0; block_id < num_blocks; ++block_id) { - const bool inplace_aliasing = (!need_rebuild && &res == &m_matrix); - const auto & input_subs = input_block.nested_blocks(); - auto & res_subs = res_block.nested_blocks(); - for (std::size_t j = 0; j < input_subs.size(); ++j) + const auto & input_block = m_matrix.blocks()[block_id]; + auto & res_block = res_ptr->blocks()[block_id]; + + if (input_block.type() == MatrixBlockType::NestedBlockDiagonal) { - const auto & input_sub = input_subs[j]; - auto & res_sub = res_subs[j]; - if (inplace_aliasing && input_sub.type() == MatrixBlockType::Plain) + const bool inplace_aliasing = (!need_rebuild && &res == &m_matrix); + const auto & input_subs = input_block.nested_blocks(); + auto & res_subs = res_block.nested_blocks(); + for (std::size_t j = 0; j < input_subs.size(); ++j) { - // Aliasing: copy sub-block before inverting. - const auto sub_size = input_sub.size(); - typedef Eigen::Map - ResMatrixMap; - typedef MatrixBlockElementTpl ResMatrixBlockElement; - ResMatrixMap tmp_map(PINOCCHIO_EIGEN_MAP_ALLOCA_ALIGNED( - Scalar, sub_size, sub_size, static_cast(ResAlignment))); - ResMatrixBlockElement temp_input(input_sub.type(), sub_size, tmp_map); - temp_input.container() = input_sub.container(); - temp_input.inverse(res_sub); - } - else - { - input_sub.inverse(res_sub); + const auto & input_sub = input_subs[j]; + auto & res_sub = res_subs[j]; + if (inplace_aliasing && input_sub.type() == MatrixBlockType::Plain) + { + // Aliasing: copy sub-block before inverting. + const auto sub_size = input_sub.size(); + typedef Eigen::Map + ResMatrixMap; + typedef MatrixBlockElementTpl ResMatrixBlockElement; + ResMatrixMap tmp_map(PINOCCHIO_EIGEN_MAP_ALLOCA_ALIGNED( + Scalar, sub_size, sub_size, static_cast(ResAlignment))); + ResMatrixBlockElement temp_input(input_sub.type(), sub_size, tmp_map); + temp_input.container() = input_sub.container(); + temp_input.inverse(res_sub); + } + else + { + input_sub.inverse(res_sub); + } } } - } - else if ( - !need_rebuild // - && &res == &m_matrix // - && input_block.type() == MatrixBlockType::Plain) - { - // Be careful of aliasing on plain blocks, otherwise it's fine. - // If there is aliasing, we copy the block to invert, then we invert it. - const auto block_size = input_block.size(); - typedef Eigen::Map - ResMatrixMap; - typedef MatrixBlockElementTpl ResMatrixBlockElement; + else if ( + !need_rebuild // + && &res == &m_matrix // + && input_block.type() == MatrixBlockType::Plain) + { + // Be careful of aliasing on plain blocks, otherwise it's fine. + // If there is aliasing, we copy the block to invert, then we invert it. + const auto block_size = input_block.size(); + typedef Eigen::Map + ResMatrixMap; + typedef MatrixBlockElementTpl ResMatrixBlockElement; - ResMatrixMap tmp_map(PINOCCHIO_EIGEN_MAP_ALLOCA_ALIGNED( - Scalar, block_size, block_size, static_cast(ResAlignment))); - ResMatrixBlockElement temp_input(input_block.type(), block_size, tmp_map); + ResMatrixMap tmp_map(PINOCCHIO_EIGEN_MAP_ALLOCA_ALIGNED( + Scalar, block_size, block_size, static_cast(ResAlignment))); + ResMatrixBlockElement temp_input(input_block.type(), block_size, tmp_map); - temp_input.container() = input_block.container(); - temp_input.inverse(res_block); + temp_input.container() = input_block.container(); + temp_input.inverse(res_block); + } + else + { + input_block.inverse(res_block); + } } - else + + if (need_rebuild && &res == &m_matrix) { - input_block.inverse(res_block); + res = std::move(tmp_res); } } - if (need_rebuild && &res == &m_matrix) + /// @brief Returns the total number of rows of the full matrix. + Eigen::Index rows() const { - res = std::move(tmp_res); + return m_matrix.rows(); } - } - /// @brief Returns the total number of rows of the full matrix. - Eigen::Index rows() const - { - return m_matrix.rows(); - } - - /// @brief Returns the total number of cols of the full matrix. - Eigen::Index cols() const - { - return m_matrix.cols(); - } + /// @brief Returns the total number of cols of the full matrix. + Eigen::Index cols() const + { + return m_matrix.cols(); + } - /// @brief Returns the total number of elements in the full matrix (rows * cols). - Eigen::Index size() const - { - return m_matrix.size(); - } + /// @brief Returns the total number of elements in the full matrix (rows * cols). + Eigen::Index size() const + { + return m_matrix.size(); + } - protected: - const Derived & m_matrix; - }; // struct BlockDiagonalMatrixInverse + protected: + const Derived & m_matrix; + }; // struct BlockDiagonalMatrixInverse + } // namespace internal } // namespace pinocchio diff --git a/include/pinocchio/src/math/block-diagonal-matrix-sum.hxx b/include/pinocchio/src/math/block-diagonal-matrix-sum.hxx index 9caf10dcec..e1c9a1de86 100644 --- a/include/pinocchio/src/math/block-diagonal-matrix-sum.hxx +++ b/include/pinocchio/src/math/block-diagonal-matrix-sum.hxx @@ -13,312 +13,317 @@ namespace pinocchio { - template - struct Sum< - BlockDiagonalMatrixTpl, - Eigen::DiagonalWrapper> - : BlockDiagonalMatrixExpression, - Eigen::DiagonalWrapper>> + namespace internal { - typedef BlockDiagonalMatrixTpl LhsType; - typedef Eigen::DiagonalWrapper RhsType; - - Sum(const LhsType & lhs, const RhsType & rhs) - : m_lhs(lhs) - , m_rhs(rhs) - { - } - /// @brief \copydoc Base::evalTo - template - void evalTo(BlockDiagonalMatrixTpl & res) const + template + struct Sum< + BlockDiagonalMatrixTpl, + Eigen::DiagonalWrapper> + : BlockDiagonalMatrixExpression, + Eigen::DiagonalWrapper>> { - typedef BlockDiagonalMatrixTpl ResType; + typedef BlockDiagonalMatrixTpl LhsType; + typedef Eigen::DiagonalWrapper RhsType; - const auto & diag = rhs().diagonal(); - PINOCCHIO_CHECK_ARGUMENT_SIZE( - diag.size(), lhs().rows(), - "The size of the diagonal expression does not match the number of rows of the block " - "diagonal matrix."); - - // Check if rebuild is needed - bool need_rebuild = false; - if ( - res.rows() != lhs().rows() || res.cols() != lhs().cols() - || res.blocks().size() != lhs().blocks().size()) + Sum(const LhsType & lhs, const RhsType & rhs) + : m_lhs(lhs) + , m_rhs(rhs) { - need_rebuild = true; } - else + + /// @brief \copydoc Base::evalTo + template + void evalTo(BlockDiagonalMatrixTpl & res) const { - for (std::size_t i = 0; i < lhs().blocks().size(); ++i) - { - const auto current_type = lhs().blocks()[i].type(); + typedef BlockDiagonalMatrixTpl ResType; - if (current_type == MatrixBlockType::NestedBlockDiagonal) + const auto & diag = rhs().diagonal(); + PINOCCHIO_CHECK_ARGUMENT_SIZE( + diag.size(), lhs().rows(), + "The size of the diagonal expression does not match the number of rows of the block " + "diagonal matrix."); + + // Check if rebuild is needed + bool need_rebuild = false; + if ( + res.rows() != lhs().rows() || res.cols() != lhs().cols() + || res.blocks().size() != lhs().blocks().size()) + { + need_rebuild = true; + } + else + { + for (std::size_t i = 0; i < lhs().blocks().size(); ++i) { - // Outer stays NestedBlockDiagonal; check sub-blocks too - if (res.blocks()[i].type() != MatrixBlockType::NestedBlockDiagonal) - { - need_rebuild = true; - break; - } - const auto & src_subs = lhs().blocks()[i].nested_blocks(); - const auto & res_subs = res.blocks()[i].nested_blocks(); - if (src_subs.size() != res_subs.size()) - { - need_rebuild = true; - break; - } - for (std::size_t j = 0; j < src_subs.size(); ++j) + const auto current_type = lhs().blocks()[i].type(); + + if (current_type == MatrixBlockType::NestedBlockDiagonal) { - const auto sub_type = src_subs[j].type(); - const auto sub_target = - (sub_type != MatrixBlockType::Diagonal && sub_type != MatrixBlockType::Plain) - ? MatrixBlockType::Diagonal - : sub_type; - if (res_subs[j].type() != sub_target) + // Outer stays NestedBlockDiagonal; check sub-blocks too + if (res.blocks()[i].type() != MatrixBlockType::NestedBlockDiagonal) { need_rebuild = true; break; } + const auto & src_subs = lhs().blocks()[i].nested_blocks(); + const auto & res_subs = res.blocks()[i].nested_blocks(); + if (src_subs.size() != res_subs.size()) + { + need_rebuild = true; + break; + } + for (std::size_t j = 0; j < src_subs.size(); ++j) + { + const auto sub_type = src_subs[j].type(); + const auto sub_target = + (sub_type != MatrixBlockType::Diagonal && sub_type != MatrixBlockType::Plain) + ? MatrixBlockType::Diagonal + : sub_type; + if (res_subs[j].type() != sub_target) + { + need_rebuild = true; + break; + } + } + if (need_rebuild) + break; + continue; } - if (need_rebuild) - break; - continue; - } - // Upgrading rule: anything not Diagonal/Plain becomes Diagonal - const auto target_type = - (current_type != MatrixBlockType::Diagonal && current_type != MatrixBlockType::Plain) - ? MatrixBlockType::Diagonal - : current_type; + // Upgrading rule: anything not Diagonal/Plain becomes Diagonal + const auto target_type = + (current_type != MatrixBlockType::Diagonal && current_type != MatrixBlockType::Plain) + ? MatrixBlockType::Diagonal + : current_type; - if (res.blocks()[i].type() != target_type) - { - need_rebuild = true; - break; + if (res.blocks()[i].type() != target_type) + { + need_rebuild = true; + break; + } } } - } - ResType * res_ptr = &res; - ResType tmp_res; + ResType * res_ptr = &res; + ResType tmp_res; - if (need_rebuild) - { - typedef Eigen::Matrix ResMatrix; - typedef Eigen::Map ResMatrixMap; - typedef MatrixBlockElementTpl ResMatrixBlockElement; + if (need_rebuild) + { + typedef Eigen::Matrix ResMatrix; + typedef Eigen::Map ResMatrixMap; + typedef MatrixBlockElementTpl ResMatrixBlockElement; - typedef typename ResType::MatrixBlockElement MatrixBlockElement; - static_assert( - pinocchio::internal::is_same_type::value, - "MatrixBlockElement is not of type pinocchio::MatrixBlockElementTpl"); + typedef typename ResType::MatrixBlockElement MatrixBlockElement; + static_assert( + pinocchio::internal::is_same_type::value, + "MatrixBlockElement is not of type " + "pinocchio::internal::MatrixBlockElementTpl"); - const std::size_t num_blocks = lhs().blocks().size(); + const std::size_t num_blocks = lhs().blocks().size(); - // Check if any block needs type upgrading (non-Diagonal/Plain → Diagonal, including nested - // sub-blocks). If no upgrading is needed we can rebuild directly from the original pattern, - // avoiding any extra temporary allocation. - bool needs_upgrade = false; - for (std::size_t i = 0; i < num_blocks && !needs_upgrade; ++i) - { - const auto & block = lhs().blocks()[i]; - if ( - block.type() != MatrixBlockType::Diagonal && block.type() != MatrixBlockType::Plain - && block.type() != MatrixBlockType::NestedBlockDiagonal) - { - needs_upgrade = true; - } - else if (block.type() == MatrixBlockType::NestedBlockDiagonal) + // Check if any block needs type upgrading (non-Diagonal/Plain → Diagonal, including + // nested sub-blocks). If no upgrading is needed we can rebuild directly from the original + // pattern, avoiding any extra temporary allocation. + bool needs_upgrade = false; + for (std::size_t i = 0; i < num_blocks && !needs_upgrade; ++i) { - for (const auto & sub : block.nested_blocks()) - if (sub.type() != MatrixBlockType::Diagonal && sub.type() != MatrixBlockType::Plain) - { - needs_upgrade = true; - break; - } + const auto & block = lhs().blocks()[i]; + if ( + block.type() != MatrixBlockType::Diagonal && block.type() != MatrixBlockType::Plain + && block.type() != MatrixBlockType::NestedBlockDiagonal) + { + needs_upgrade = true; + } + else if (block.type() == MatrixBlockType::NestedBlockDiagonal) + { + for (const auto & sub : block.nested_blocks()) + if (sub.type() != MatrixBlockType::Diagonal && sub.type() != MatrixBlockType::Plain) + { + needs_upgrade = true; + break; + } + } } - } - if (!needs_upgrade) - { - // Pattern is already compatible: rebuild directly from existing blocks. - if (&res == &lhs()) + if (!needs_upgrade) { - tmp_res.rebuild(lhs().blocks()); - res_ptr = &tmp_res; + // Pattern is already compatible: rebuild directly from existing blocks. + if (&res == &lhs()) + { + tmp_res.rebuild(lhs().blocks()); + res_ptr = &tmp_res; + } + else + { + res.rebuild(lhs().blocks()); + } } else { - res.rebuild(lhs().blocks()); - } - } - else - { - // Need to upgrade some block types. Use alloca for the outer pattern array. - // For NestedBlockDiagonal entries, placement-new with a moved sub-vector and - // call the destructor explicitly afterwards to avoid a memory leak. - MatrixBlockElement * new_pattern = static_cast( - PINOCCHIO_ALLOCA(num_blocks * sizeof(MatrixBlockElement))); - bool has_nested_in_pattern = false; - for (std::size_t i = 0; i < num_blocks; ++i) - { - const auto & block = lhs().blocks()[i]; - if (block.type() == MatrixBlockType::NestedBlockDiagonal) + // Need to upgrade some block types. Use alloca for the outer pattern array. + // For NestedBlockDiagonal entries, placement-new with a moved sub-vector and + // call the destructor explicitly afterwards to avoid a memory leak. + MatrixBlockElement * new_pattern = static_cast( + PINOCCHIO_ALLOCA(num_blocks * sizeof(MatrixBlockElement))); + bool has_nested_in_pattern = false; + for (std::size_t i = 0; i < num_blocks; ++i) { - has_nested_in_pattern = true; - std::vector new_subs; - new_subs.reserve(block.nested_blocks().size()); - for (const auto & sub : block.nested_blocks()) + const auto & block = lhs().blocks()[i]; + if (block.type() == MatrixBlockType::NestedBlockDiagonal) { - MatrixBlockType sub_type = sub.type(); - if (sub_type != MatrixBlockType::Diagonal && sub_type != MatrixBlockType::Plain) - sub_type = MatrixBlockType::Diagonal; - new_subs.emplace_back(sub_type, sub.size()); + has_nested_in_pattern = true; + std::vector new_subs; + new_subs.reserve(block.nested_blocks().size()); + for (const auto & sub : block.nested_blocks()) + { + MatrixBlockType sub_type = sub.type(); + if (sub_type != MatrixBlockType::Diagonal && sub_type != MatrixBlockType::Plain) + sub_type = MatrixBlockType::Diagonal; + new_subs.emplace_back(sub_type, sub.size()); + } + new (new_pattern + i) + MatrixBlockElement(MatrixBlockType::NestedBlockDiagonal, std::move(new_subs)); } - new (new_pattern + i) - MatrixBlockElement(MatrixBlockType::NestedBlockDiagonal, std::move(new_subs)); + else + { + MatrixBlockType new_type = block.type(); + if (new_type != MatrixBlockType::Diagonal && new_type != MatrixBlockType::Plain) + new_type = MatrixBlockType::Diagonal; + new (new_pattern + i) MatrixBlockElement(new_type, block.size()); + } + } + + if (&res == &lhs()) + { + tmp_res.rebuild(new_pattern, num_blocks); + res_ptr = &tmp_res; } else { - MatrixBlockType new_type = block.type(); - if (new_type != MatrixBlockType::Diagonal && new_type != MatrixBlockType::Plain) - new_type = MatrixBlockType::Diagonal; - new (new_pattern + i) MatrixBlockElement(new_type, block.size()); + res.rebuild(new_pattern, num_blocks); } - } - if (&res == &lhs()) - { - tmp_res.rebuild(new_pattern, num_blocks); - res_ptr = &tmp_res; + // Explicitly destroy alloca'd entries to release any heap memory held by nested + // sub-block std::vectors. + if (has_nested_in_pattern) + for (std::size_t i = 0; i < num_blocks; ++i) + new_pattern[i].~MatrixBlockElement(); } - else - { - res.rebuild(new_pattern, num_blocks); - } - - // Explicitly destroy alloca'd entries to release any heap memory held by nested - // sub-block std::vectors. - if (has_nested_in_pattern) - for (std::size_t i = 0; i < num_blocks; ++i) - new_pattern[i].~MatrixBlockElement(); } - } - - Eigen::Index row_id = 0; - for (std::size_t i = 0; i < lhs().blocks().size(); ++i) - { - const auto & lhs_block = lhs().blocks()[i]; - auto & res_block = res_ptr->blocks()[i]; - const auto block_size = lhs_block.size(); - const auto diag_segment = diag.segment(row_id, block_size); - if (lhs_block.type() == MatrixBlockType::NestedBlockDiagonal) + Eigen::Index row_id = 0; + for (std::size_t i = 0; i < lhs().blocks().size(); ++i) { - const auto & lhs_subs = lhs_block.nested_blocks(); - auto & res_subs = res_block.nested_blocks(); - Eigen::Index sub_offset = 0; - for (std::size_t j = 0; j < lhs_subs.size(); ++j) + const auto & lhs_block = lhs().blocks()[i]; + auto & res_block = res_ptr->blocks()[i]; + const auto block_size = lhs_block.size(); + const auto diag_segment = diag.segment(row_id, block_size); + + if (lhs_block.type() == MatrixBlockType::NestedBlockDiagonal) { - const auto & lhs_sub = lhs_subs[j]; - auto & res_sub = res_subs[j]; - const auto sub_size = lhs_sub.size(); - const auto sub_diag = diag_segment.segment(sub_offset, sub_size); - if (!need_rebuild && &res == &lhs()) + const auto & lhs_subs = lhs_block.nested_blocks(); + auto & res_subs = res_block.nested_blocks(); + Eigen::Index sub_offset = 0; + for (std::size_t j = 0; j < lhs_subs.size(); ++j) { - // In-place: res += rhs - if (res_sub.type() == MatrixBlockType::Diagonal) - res_sub.container() += sub_diag; - else - res_sub.container().diagonal() += sub_diag; - } - else - { - if (res_sub.type() == MatrixBlockType::Diagonal) + const auto & lhs_sub = lhs_subs[j]; + auto & res_sub = res_subs[j]; + const auto sub_size = lhs_sub.size(); + const auto sub_diag = diag_segment.segment(sub_offset, sub_size); + if (!need_rebuild && &res == &lhs()) { - lhs_sub.diagonal(res_sub.container()); - res_sub.container() += sub_diag; + // In-place: res += rhs + if (res_sub.type() == MatrixBlockType::Diagonal) + res_sub.container() += sub_diag; + else + res_sub.container().diagonal() += sub_diag; } else { - assert(lhs_sub.type() == MatrixBlockType::Plain); - lhs_sub.evalTo(res_sub.container()); - res_sub.container().diagonal() += sub_diag; + if (res_sub.type() == MatrixBlockType::Diagonal) + { + lhs_sub.diagonal(res_sub.container()); + res_sub.container() += sub_diag; + } + else + { + assert(lhs_sub.type() == MatrixBlockType::Plain); + lhs_sub.evalTo(res_sub.container()); + res_sub.container().diagonal() += sub_diag; + } } + sub_offset += sub_size; } - sub_offset += sub_size; } - } - else if (!need_rebuild && &res == &lhs()) - { - // In-place: res += rhs - if (res_block.type() == MatrixBlockType::Diagonal) + else if (!need_rebuild && &res == &lhs()) { - res_block.container() += diag_segment; + // In-place: res += rhs + if (res_block.type() == MatrixBlockType::Diagonal) + { + res_block.container() += diag_segment; + } + else + { + res_block.container().diagonal() += diag_segment; + } } else { - res_block.container().diagonal() += diag_segment; + // res = lhs + rhs + if (res_block.type() == MatrixBlockType::Diagonal) + { + assert(lhs_block.type() != MatrixBlockType::Plain); + lhs_block.diagonal(res_block.container()); + res_block.container() += diag_segment; + } + else + { + assert(lhs_block.type() == MatrixBlockType::Plain); + lhs_block.evalTo(res_block.container()); + res_block.container().diagonal() += diag_segment; + } } + + row_id += block_size; } - else + + if (need_rebuild && &res == &lhs()) { - // res = lhs + rhs - if (res_block.type() == MatrixBlockType::Diagonal) - { - assert(lhs_block.type() != MatrixBlockType::Plain); - lhs_block.diagonal(res_block.container()); - res_block.container() += diag_segment; - } - else - { - assert(lhs_block.type() == MatrixBlockType::Plain); - lhs_block.evalTo(res_block.container()); - res_block.container().diagonal() += diag_segment; - } + res = std::move(tmp_res); } - - row_id += block_size; } - if (need_rebuild && &res == &lhs()) + const LhsType & lhs() const { - res = std::move(tmp_res); + return m_lhs; } - } - - const LhsType & lhs() const - { - return m_lhs; - } - const RhsType & rhs() const - { - return m_rhs; - } + const RhsType & rhs() const + { + return m_rhs; + } - Eigen::Index rows() const - { - return m_lhs.rows(); - } + Eigen::Index rows() const + { + return m_lhs.rows(); + } - Eigen::Index cols() const - { - return m_lhs.cols(); - } + Eigen::Index cols() const + { + return m_lhs.cols(); + } - Eigen::Index size() const - { - return m_lhs.size(); - } + Eigen::Index size() const + { + return m_lhs.size(); + } - protected: - const LhsType & m_lhs; // block diagonal matrix ref - const RhsType m_rhs; // diagonal expression - }; + protected: + const LhsType & m_lhs; // block diagonal matrix ref + const RhsType m_rhs; // diagonal expression + }; + } // namespace internal } // namespace pinocchio diff --git a/include/pinocchio/src/math/block-diagonal-matrix.hxx b/include/pinocchio/src/math/block-diagonal-matrix.hxx index 3cee1cf2ef..e751e503c2 100644 --- a/include/pinocchio/src/math/block-diagonal-matrix.hxx +++ b/include/pinocchio/src/math/block-diagonal-matrix.hxx @@ -13,1341 +13,1348 @@ namespace pinocchio { - - /// @brief A block-diagonal matrix with scalar type and options from the current context. - typedef BlockDiagonalMatrixTpl BlockDiagonalMatrix; - - /** - * @ingroup pinocchio_math - * @brief A memory-efficient representation of a block-diagonal matrix. - * - * @tparam _Scalar The scalar type of the matrix elements (e.g., double). - * @tparam _Options The Eigen options for matrix storage (e.g., Eigen::RowMajor). - * @tparam _Alignment The memory alignment for the data blocks. - * - * @details This class represents a block-diagonal matrix by storing only the non-zero blocks. - * It owns the memory for these blocks in a contiguous `MatrixStack` and uses a vector - * of `MatrixBlockElement` objects to describe the structure and provide views into the - * data. This approach is highly efficient for storage and for performing matrix operations. - */ - template - struct BlockDiagonalMatrixTpl - : BlockDiagonalMatrixBase> + namespace internal { - typedef _Scalar Scalar; - static constexpr int Options = _Options; - static constexpr std::size_t Alignment = _Alignment; - - typedef Eigen::Matrix Matrix; - typedef Eigen::Map MatrixMap; - typedef Eigen::Map ConstMatrixMap; - typedef Eigen::Matrix Vector; - typedef Eigen::Map VectorMap; - typedef Eigen::Map ConstVectorMap; - - typedef internal::MatrixStackTpl MatrixStack; - typedef MatrixBlockElementTpl MatrixBlockElement; - typedef MatrixBlockElementTpl ConstMatrixBlockElement; - /** - * @brief Defaut constructor. - */ - BlockDiagonalMatrixTpl() {}; - - /** - * @brief Constructs a block-diagonal matrix from a given block pattern. - * @param[in] block_pattern A vector of MatrixBlockElement describing each diagonal block in - * order. - */ - explicit BlockDiagonalMatrixTpl(const std::vector & block_pattern); + /// @brief A block-diagonal matrix with scalar type and options from the current context. + typedef BlockDiagonalMatrixTpl BlockDiagonalMatrix; /** - * @brief Constructs a block-diagonal matrix from an Eigen diagonal matrix expression. - * - * @tparam DiagonalVectorType The type of the underlying vector in the `Eigen::DiagonalWrapper`. - * - * @param[in] diagonal_expression An Eigen diagonal matrix expression, typically the result of - * calling `.asDiagonal()` on an Eigen vector. - * - * @details This constructor converts a standard Eigen diagonal matrix into the specialized - * `BlockDiagonalMatrixTpl` representation. The resulting matrix will be composed of - * `N` blocks of size 1x1, where `N` is the dimension of the input matrix. - * - * This is a deep-copy operation: the diagonal coefficients from the input - * expression are copied into the internal storage of this object. - * - * @code - * Eigen::Vector3d vec(1.0, 2.0, 3.0); - * // Create a BlockDiagonalMatrix from a standard Eigen diagonal matrix expression - * pinocchio::BlockDiagonalMatrix block_diag_matrix(vec.asDiagonal()); - * - * // block_diag_matrix now represents diag(1,2,3) using three 1x1 blocks. - * @endcode - */ - template - explicit BlockDiagonalMatrixTpl( - const Eigen::DiagonalWrapper & diagonal_expression); - - /** - * @brief Copy constructor. - * - * @param[in] other The BlockDiagonalMatrixTpl object to copy from. - * - * @details Performs a deep copy of the block-diagonal matrix. The new object will - * have its own independent copy of the underlying data in the `MatrixStack`. - * The `MatrixBlockElement` vector is also copied, and the internal `Eigen::Map`s - * are correctly re-pointed to view the newly allocated memory. - * - * @note This operation can be expensive as it involves allocating memory and copying - * all the data from the non-trivial matrix blocks. - */ - BlockDiagonalMatrixTpl(const BlockDiagonalMatrixTpl & other) - { - *this = other; - } - - /** - * @brief Copy constructor from a block-diagonal matrix expression. - * - * @tparam Derived The derived type of the block-diagonal matrix expression. + * @ingroup pinocchio_math + * @brief A memory-efficient representation of a block-diagonal matrix. * - * @param[in] other A block-diagonal matrix expression to assign from. + * @tparam _Scalar The scalar type of the matrix elements (e.g., double). + * @tparam _Options The Eigen options for matrix storage (e.g., Eigen::RowMajor). + * @tparam _Alignment The memory alignment for the data blocks. * - * @details This operator enables assignment from any type that inherits from - * `BlockDiagonalMatrixExpression`, such as `Inverse`. - * The assignment is performed by calling `evalTo()` on the expression, - * which materializes the result into this matrix. - * - * This enables lazy evaluation patterns where intermediate results - * (like matrix inverses) are not computed until they are assigned - * to a concrete storage type. - * - * @code - * pinocchio::BlockDiagonalMatrix A = ...; - * pinocchio::BlockDiagonalMatrix A_inv = A.inverse(); - * @endcode + * @details This class represents a block-diagonal matrix by storing only the non-zero blocks. + * It owns the memory for these blocks in a contiguous `MatrixStack` and uses a vector + * of `MatrixBlockElement` objects to describe the structure and provide views into the + * data. This approach is highly efficient for storage and for performing matrix operations. */ - template - BlockDiagonalMatrixTpl(const BlockDiagonalMatrixExpression & other) + template + struct BlockDiagonalMatrixTpl + : BlockDiagonalMatrixBase> { - *this = other; - } - - /** - * @brief Move constructor. - * - * @param[in,out] other The BlockDiagonalMatrixTpl object to move from. After the move, - * `other` is left in an invalid state. - * - * @details Transfers ownership of the underlying matrix data (`MatrixStack`) and block - * information from `other` to this object. This is a very efficient, - * constant-time operation as it avoids any memory allocation or data copying. - */ - BlockDiagonalMatrixTpl(BlockDiagonalMatrixTpl && other) = default; - - /** - * @brief Copy-assignment operator. - * - * @param[in] other The BlockDiagonalMatrixTpl object to copy from. - * @return A reference to `*this` after the assignment. - * - * @details Replaces the contents of this instance with a deep copy of `other`. - * The existing data in `*this` is discarded. The new instance will have its - * own independent copy of the underlying data (`MatrixStack`) and block - * information. The internal `Eigen::Map`s are correctly re-pointed to - * view the newly allocated memory. - * - * @note This operation can be expensive if a memory reallocation is required. - */ - BlockDiagonalMatrixTpl & operator=(const BlockDiagonalMatrixTpl & other); - - /** - * @brief Move-assignment operator. - * - * @param[in,out] other The BlockDiagonalMatrixTpl object to move from. After the move, - * `other` is left in a valid but unspecified state. - * @return A reference to `*this` after the assignment. - * - * @details Transfers ownership of the underlying matrix data and block information - * from `other` to `*this`. The existing data in `*this` is properly released. - * This is a very efficient, constant-time operation that avoids any - * memory allocation or data copying. - */ - BlockDiagonalMatrixTpl & operator=(BlockDiagonalMatrixTpl && other) = default; + typedef _Scalar Scalar; + static constexpr int Options = _Options; + static constexpr std::size_t Alignment = _Alignment; + + typedef Eigen::Matrix Matrix; + typedef Eigen::Map MatrixMap; + typedef Eigen::Map ConstMatrixMap; + + typedef Eigen::Matrix Vector; + typedef Eigen::Map VectorMap; + typedef Eigen::Map ConstVectorMap; + + typedef MatrixStackTpl MatrixStack; + typedef MatrixBlockElementTpl MatrixBlockElement; + typedef MatrixBlockElementTpl ConstMatrixBlockElement; + /** + * @brief Defaut constructor. + */ + BlockDiagonalMatrixTpl() {}; + + /** + * @brief Constructs a block-diagonal matrix from a given block pattern. + * @param[in] block_pattern A vector of MatrixBlockElement describing each diagonal block in + * order. + */ + explicit BlockDiagonalMatrixTpl(const std::vector & block_pattern); + + /** + * @brief Constructs a block-diagonal matrix from an Eigen diagonal matrix expression. + * + * @tparam DiagonalVectorType The type of the underlying vector in the + * `Eigen::DiagonalWrapper`. + * + * @param[in] diagonal_expression An Eigen diagonal matrix expression, typically the result of + * calling `.asDiagonal()` on an Eigen vector. + * + * @details This constructor converts a standard Eigen diagonal matrix into the specialized + * `BlockDiagonalMatrixTpl` representation. The resulting matrix will be composed of + * `N` blocks of size 1x1, where `N` is the dimension of the input matrix. + * + * This is a deep-copy operation: the diagonal coefficients from the input + * expression are copied into the internal storage of this object. + * + * @code + * Eigen::Vector3d vec(1.0, 2.0, 3.0); + * // Create a BlockDiagonalMatrix from a standard Eigen diagonal matrix expression + * pinocchio::internal::BlockDiagonalMatrix block_diag_matrix(vec.asDiagonal()); + * + * // block_diag_matrix now represents diag(1,2,3) using three 1x1 blocks. + * @endcode + */ + template + explicit BlockDiagonalMatrixTpl( + const Eigen::DiagonalWrapper & diagonal_expression); + + /** + * @brief Copy constructor. + * + * @param[in] other The BlockDiagonalMatrixTpl object to copy from. + * + * @details Performs a deep copy of the block-diagonal matrix. The new object will + * have its own independent copy of the underlying data in the `MatrixStack`. + * The `MatrixBlockElement` vector is also copied, and the internal `Eigen::Map`s + * are correctly re-pointed to view the newly allocated memory. + * + * @note This operation can be expensive as it involves allocating memory and copying + * all the data from the non-trivial matrix blocks. + */ + BlockDiagonalMatrixTpl(const BlockDiagonalMatrixTpl & other) + { + *this = other; + } - /** - * @brief Assigns an Eigen diagonal matrix expression to this block-diagonal matrix. - * - * @tparam DiagonalVectorType The type of the underlying vector in the `Eigen::DiagonalWrapper`. - * - * @param[in] diagonal_expression An Eigen diagonal matrix expression, typically from - * `.asDiagonal()`. - * - * @return A reference to `*this` after the assignment. - * - * @details This operator performs an **in-place value assignment**. It updates the numerical - * coefficients of the existing matrix blocks with the values from - * `diagonal_expression`. It does **not** change the block structure (i.e., the number or sizes - * of blocks) of this matrix. - * - * **Preconditions:** - * - The dimensions of `diagonal_expression` must match the dimensions of `*this`. - * - The block structure of `*this` must be compatible with a diagonal matrix - * (i.e., it should be composed of 1x1 blocks). - * - * @warning This operator will result in undefined behavior if the dimensions do not match or if - * the block structure is not diagonal. - * - * @code - * // Assume `block_diag_matrix` is already initialized, e.g., as a 3x3 matrix - * // of three 1x1 blocks. - * Eigen::Vector3d new_values(4, 5, 6); - * block_diag_matrix = new_values.asDiagonal(); // Updates the values in place - * @endcode - */ - template - BlockDiagonalMatrixTpl & - operator=(const Eigen::DiagonalWrapper & diagonal_expression); + /** + * @brief Copy constructor from a block-diagonal matrix expression. + * + * @tparam Derived The derived type of the block-diagonal matrix expression. + * + * @param[in] other A block-diagonal matrix expression to assign from. + * + * @details This operator enables assignment from any type that inherits from + * `BlockDiagonalMatrixExpression`, such as `Inverse`. + * The assignment is performed by calling `evalTo()` on the expression, + * which materializes the result into this matrix. + * + * This enables lazy evaluation patterns where intermediate results + * (like matrix inverses) are not computed until they are assigned + * to a concrete storage type. + * + * @code + * pinocchio::internal::BlockDiagonalMatrix A = ...; + * pinocchio::internal::BlockDiagonalMatrix A_inv = A.inverse(); + * @endcode + */ + template + BlockDiagonalMatrixTpl(const BlockDiagonalMatrixExpression & other) + { + *this = other; + } - /** - * @brief Adds a diagonal matrix to this block-diagonal matrix in-place. - * @tparam DiagonalVectorType The type of the underlying vector in the `Eigen::DiagonalWrapper`. - * @param[in] diagonal_expression An Eigen diagonal matrix expression. - * @return A reference to `*this` after the addition. - */ - template - BlockDiagonalMatrixTpl & - operator+=(const Eigen::DiagonalWrapper & diagonal_expression); + /** + * @brief Move constructor. + * + * @param[in,out] other The BlockDiagonalMatrixTpl object to move from. After the move, + * `other` is left in an invalid state. + * + * @details Transfers ownership of the underlying matrix data (`MatrixStack`) and block + * information from `other` to this object. This is a very efficient, + * constant-time operation as it avoids any memory allocation or data copying. + */ + BlockDiagonalMatrixTpl(BlockDiagonalMatrixTpl && other) = default; + + /** + * @brief Copy-assignment operator. + * + * @param[in] other The BlockDiagonalMatrixTpl object to copy from. + * @return A reference to `*this` after the assignment. + * + * @details Replaces the contents of this instance with a deep copy of `other`. + * The existing data in `*this` is discarded. The new instance will have its + * own independent copy of the underlying data (`MatrixStack`) and block + * information. The internal `Eigen::Map`s are correctly re-pointed to + * view the newly allocated memory. + * + * @note This operation can be expensive if a memory reallocation is required. + */ + BlockDiagonalMatrixTpl & operator=(const BlockDiagonalMatrixTpl & other); + + /** + * @brief Move-assignment operator. + * + * @param[in,out] other The BlockDiagonalMatrixTpl object to move from. After the move, + * `other` is left in a valid but unspecified state. + * @return A reference to `*this` after the assignment. + * + * @details Transfers ownership of the underlying matrix data and block information + * from `other` to `*this`. The existing data in `*this` is properly released. + * This is a very efficient, constant-time operation that avoids any + * memory allocation or data copying. + */ + BlockDiagonalMatrixTpl & operator=(BlockDiagonalMatrixTpl && other) = default; + + /** + * @brief Assigns an Eigen diagonal matrix expression to this block-diagonal matrix. + * + * @tparam DiagonalVectorType The type of the underlying vector in the + * `Eigen::DiagonalWrapper`. + * + * @param[in] diagonal_expression An Eigen diagonal matrix expression, typically from + * `.asDiagonal()`. + * + * @return A reference to `*this` after the assignment. + * + * @details This operator performs an **in-place value assignment**. It updates the numerical + * coefficients of the existing matrix blocks with the values from + * `diagonal_expression`. It does **not** change the block structure (i.e., the number or + * sizes of blocks) of this matrix. + * + * **Preconditions:** + * - The dimensions of `diagonal_expression` must match the dimensions of `*this`. + * - The block structure of `*this` must be compatible with a diagonal matrix + * (i.e., it should be composed of 1x1 blocks). + * + * @warning This operator will result in undefined behavior if the dimensions do not match or + * if the block structure is not diagonal. + * + * @code + * // Assume `block_diag_matrix` is already initialized, e.g., as a 3x3 matrix + * // of three 1x1 blocks. + * Eigen::Vector3d new_values(4, 5, 6); + * block_diag_matrix = new_values.asDiagonal(); // Updates the values in place + * @endcode + */ + template + BlockDiagonalMatrixTpl & + operator=(const Eigen::DiagonalWrapper & diagonal_expression); + + /** + * @brief Adds a diagonal matrix to this block-diagonal matrix in-place. + * @tparam DiagonalVectorType The type of the underlying vector in the + * `Eigen::DiagonalWrapper`. + * @param[in] diagonal_expression An Eigen diagonal matrix expression. + * @return A reference to `*this` after the addition. + */ + template + BlockDiagonalMatrixTpl & + operator+=(const Eigen::DiagonalWrapper & diagonal_expression); + + /** + * @brief Adds a diagonal matrix to this block-diagonal matrix. + * @tparam DiagonalVectorType The type of the underlying vector in the + * `Eigen::DiagonalWrapper`. + * @param[in] diagonal_expression An Eigen diagonal matrix expression. + * @return A new BlockDiagonalMatrixTpl containing the sum. + */ + template + Sum> + operator+(const Eigen::DiagonalWrapper & diagonal_expression) const; + + /** + * @brief Assignment operator from a block-diagonal matrix expression. + * + * @tparam Derived The derived type of the block-diagonal matrix expression. + * + * @param[in] other A block-diagonal matrix expression to assign from. + * + * @return A reference to `*this` after the assignment. + * + * @details This operator enables assignment from any type that inherits from + * `BlockDiagonalMatrixExpression`, such as `Inverse`. + * The assignment is performed by calling `evalTo()` on the expression, + * which materializes the result into this matrix. + * + * This enables lazy evaluation patterns where intermediate results + * (like matrix inverses) are not computed until they are assigned + * to a concrete storage type. + * + * @code + * pinocchio::internal::BlockDiagonalMatrix A = ...; + * pinocchio::internal::BlockDiagonalMatrix A_inv; + * A_inv = A.inverse(); // Computes and stores the inverse + * @endcode + */ + template + BlockDiagonalMatrixTpl & operator=(const BlockDiagonalMatrixExpression & other) + { + other.evalTo(*this); + return *this; + } - /** - * @brief Adds a diagonal matrix to this block-diagonal matrix. - * @tparam DiagonalVectorType The type of the underlying vector in the `Eigen::DiagonalWrapper`. - * @param[in] diagonal_expression An Eigen diagonal matrix expression. - * @return A new BlockDiagonalMatrixTpl containing the sum. - */ - template - Sum> - operator+(const Eigen::DiagonalWrapper & diagonal_expression) const; + /** + * @brief Checks for strict equality between two block-diagonal matrices. + * + * @param[in] other The other matrix to compare against. + * + * @return `true` if the matrices are equal, `false` otherwise. + * + * @details Two block-diagonal matrices are considered equal if and only if: + * 1. Their overall dimensions (`rows` and `cols`) are identical. + * 2. Their block patterns are identical (i.e., the `MatrixBlockElement` vectors are + * the same). + * 3. The numerical data in their underlying storage (`MatrixStack`) is + * coefficient-wise equal. + * + * @note This comparison can be expensive as it may involve a full-data comparison of all + * non-trivial blocks. + */ + bool operator==(const BlockDiagonalMatrixTpl & other) const + { + // This implementation assumes MatrixBlockElementTpl has a valid operator==. + // A correct implementation would need to handle the non-comparable Eigen::Map member. + return m_rows == other.m_rows && m_cols == other.m_cols + && m_matrix_block_elements == other.m_matrix_block_elements; + } - /** - * @brief Assignment operator from a block-diagonal matrix expression. - * - * @tparam Derived The derived type of the block-diagonal matrix expression. - * - * @param[in] other A block-diagonal matrix expression to assign from. - * - * @return A reference to `*this` after the assignment. - * - * @details This operator enables assignment from any type that inherits from - * `BlockDiagonalMatrixExpression`, such as `Inverse`. - * The assignment is performed by calling `evalTo()` on the expression, - * which materializes the result into this matrix. - * - * This enables lazy evaluation patterns where intermediate results - * (like matrix inverses) are not computed until they are assigned - * to a concrete storage type. - * - * @code - * pinocchio::BlockDiagonalMatrix A = ...; - * pinocchio::BlockDiagonalMatrix A_inv; - * A_inv = A.inverse(); // Computes and stores the inverse - * @endcode - */ - template - BlockDiagonalMatrixTpl & operator=(const BlockDiagonalMatrixExpression & other) - { - other.evalTo(*this); - return *this; - } + /** + * @brief Checks for inequality between two block-diagonal matrices. + * + * @param[in] other The other matrix to compare against. + * + * @return `true` if the matrices are not equal, `false` otherwise. + * + * @details This operator is implemented as the negation of `operator==`. + * @see operator==() + */ + bool operator!=(const BlockDiagonalMatrixTpl & other) const + { + return !(*this == other); + } - /** - * @brief Checks for strict equality between two block-diagonal matrices. - * - * @param[in] other The other matrix to compare against. - * - * @return `true` if the matrices are equal, `false` otherwise. - * - * @details Two block-diagonal matrices are considered equal if and only if: - * 1. Their overall dimensions (`rows` and `cols`) are identical. - * 2. Their block patterns are identical (i.e., the `MatrixBlockElement` vectors are - * the same). - * 3. The numerical data in their underlying storage (`MatrixStack`) is - * coefficient-wise equal. - * - * @note This comparison can be expensive as it may involve a full-data comparison of all - * non-trivial blocks. - */ - bool operator==(const BlockDiagonalMatrixTpl & other) const - { - // This implementation assumes MatrixBlockElementTpl has a valid operator==. - // A correct implementation would need to handle the non-comparable Eigen::Map member. - return m_rows == other.m_rows && m_cols == other.m_cols - && m_matrix_block_elements == other.m_matrix_block_elements; - } + /// @brief Checks if the matrix structure and its blocks are valid. + bool isValid() const; - /** - * @brief Checks for inequality between two block-diagonal matrices. - * - * @param[in] other The other matrix to compare against. - * - * @return `true` if the matrices are not equal, `false` otherwise. - * - * @details This operator is implemented as the negation of `operator==`. - * @see operator==() - */ - bool operator!=(const BlockDiagonalMatrixTpl & other) const - { - return !(*this == other); - } + /// @brief Returns the total number of rows of the full matrix. + Eigen::Index rows() const + { + return m_rows; + } - /// @brief Checks if the matrix structure and its blocks are valid. - bool isValid() const; + /// @brief Returns the total number of columns of the full matrix. + Eigen::Index cols() const + { + return m_cols; + } - /// @brief Returns the total number of rows of the full matrix. - Eigen::Index rows() const - { - return m_rows; - } + /// @brief Returns the total number of elements in the full matrix (rows * cols). + Eigen::Index size() const + { + return m_rows * m_cols; + } - /// @brief Returns the total number of columns of the full matrix. - Eigen::Index cols() const - { - return m_cols; - } + /// @brief Returns the minimum coefficient of the matrix. + Scalar minCoeff() const + { + PINOCCHIO_THROW_PRETTY_IF( + blocks().empty(), std::runtime_error, + "Unvalid use of minCoeff. You are using an empty block diagonal matrix"); - /// @brief Returns the total number of elements in the full matrix (rows * cols). - Eigen::Index size() const - { - return m_rows * m_cols; - } + Scalar min_coeff = blocks()[0].map.minCoeff(); + for (const auto & block : blocks()) + { + const Scalar block_min_coeff = block.map.minCoeff(); + if (block_min_coeff < min_coeff) + { + min_coeff = block_min_coeff; + } + } - /// @brief Returns the minimum coefficient of the matrix. - Scalar minCoeff() const - { - PINOCCHIO_THROW_PRETTY_IF( - blocks().empty(), std::runtime_error, - "Unvalid use of minCoeff. You are using an empty block diagonal matrix"); + return min_coeff; + } - Scalar min_coeff = blocks()[0].map.minCoeff(); - for (const auto & block : blocks()) + /// @brief Returns the maximum coefficient of the matrix. + Scalar maxCoeff() const { - const Scalar block_min_coeff = block.map.minCoeff(); - if (block_min_coeff < min_coeff) + PINOCCHIO_THROW_PRETTY_IF( + blocks().empty(), std::runtime_error, + "Unvalid use of maxCoeff. You are using an empty block diagonal matrix"); + + Scalar max_coeff = blocks()[0].map.maxCoeff(); + for (const auto & block : blocks()) { - min_coeff = block_min_coeff; + const Scalar block_max_coeff = block.map.maxCoeff(); + if (block_max_coeff > max_coeff) + { + max_coeff = block_max_coeff; + } } - } - return min_coeff; - } + return max_coeff; + } - /// @brief Returns the maximum coefficient of the matrix. - Scalar maxCoeff() const - { - PINOCCHIO_THROW_PRETTY_IF( - blocks().empty(), std::runtime_error, - "Unvalid use of maxCoeff. You are using an empty block diagonal matrix"); + /** + * @brief Performs the matrix-matrix product `res = (*this) * rhs`. + * + * @tparam AssignOp Assignment operation used to store the result + * (e.g. ::pinocchio::internal::assign_op for direct assignment, + * or ::pinocchio::internal::add_assign_op for accumulation). + * @tparam MatrixDerivedRhs The Eigen type of the right-hand-side matrix. + * @tparam MatrixDerivedRes The Eigen type of the result matrix. + * + * @param[in] rhs The matrix to multiply with on the right. + * @param[out] res The matrix where the result is stored. It must be pre-allocated + * with dimensions `this->rows()` by `rhs.cols()`. + * + * @details This method computes the product of this block-diagonal matrix with a dense + * matrix `rhs`. It leverages the sparse structure of `*this` to perform the + * computation efficiently, avoiding unnecessary multiplications by zero. + * + * As this version writes to a pre-allocated matrix, it avoids any dynamic + * memory allocation, making it suitable for real-time and performance-critical code. + */ + template< + typename AssignOp = pinocchio::internal::assign_op, + typename MatrixDerivedRhs, + typename MatrixDerivedRes> + void applyOnTheRight( + const Eigen::MatrixBase & rhs, + const Eigen::MatrixBase & res) const; + + /** + * @brief Performs the matrix-matrix product `res = lhs * (*this)`. + * + * @tparam AssignOp Assignment operation used to store the result + * (e.g. ::pinocchio::internal::assign_op for direct assignment, + * or ::pinocchio::internal::add_assign_op for accumulation). + * @tparam MatrixDerivedLhs The Eigen type of the left-hand-side matrix. + * @tparam MatrixDerivedRes The Eigen type of the result matrix. + * + * @param[in] lhs The matrix to multiply with on the left. + * @param[out] res The matrix where the result is stored. It must be pre-allocated + * with dimensions `this->cols()` by `lhs.rows()`. + * + * @details This method computes the product of this block-diagonal matrix with a dense + * matrix `lhs`. It leverages the sparse structure of `*this` to perform the + * computation efficiently, avoiding unnecessary multiplications by zero. + * + * As this version writes to a pre-allocated matrix, it avoids any dynamic + * memory allocation, making it suitable for real-time and performance-critical code. + */ + template< + typename AssignOp = pinocchio::internal::assign_op, + typename MatrixDerivedLhs, + typename MatrixDerivedRes> + void applyOnTheLeft( + const Eigen::MatrixBase & lhs, + const Eigen::MatrixBase & res) const; + + /** + * @brief Performs the matrix-matrix product `(*this) * rhs` and returns the result. + * + * @tparam MatrixDerivedRhs The Eigen type of the right-hand-side matrix. + * + * @param[in] rhs The matrix to multiply with on the right. + * + * @return A new matrix containing the result of the multiplication. The returned matrix + * type is deduced to be a plain, non-expression matrix. + * + * @details This is a convenience overload that allocates a new matrix to store the result + * of the product. + * + * @note For performance-critical applications, prefer the overload that accepts a + * pre-allocated result matrix to avoid repeated memory allocations. + * @see void applyOnTheRight(const Eigen::MatrixBase &, const + * Eigen::MatrixBase &) const + */ + template + typename PINOCCHIO_EIGEN_PLAIN_TYPE(MatrixDerivedRhs) + applyOnTheRight(const Eigen::MatrixBase & rhs) const + { + typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(MatrixDerivedRhs) ReturnType; + ReturnType res(rows(), rhs.cols()); // Assuming this should be rows(), rhs.cols() + applyOnTheRight(rhs.derived(), res); + return res; + } - Scalar max_coeff = blocks()[0].map.maxCoeff(); - for (const auto & block : blocks()) + /** + * @brief Performs the matrix-matrix product `lhs * (*this)` and returns the result. + * + * @tparam MatrixDerivedLhs The Eigen type of the left-hand-side matrix. + * + * @param[in] lhs The matrix to multiply with on the left. + * + * @return A new matrix containing the result of the multiplication. The returned matrix + * type is deduced to be a plain, non-expression matrix. + * + * @details This is a convenience overload that allocates a new matrix to store the result + * of the product. + * + * @note For performance-critical applications, prefer the overload that accepts a + * pre-allocated result matrix to avoid repeated memory allocations. + * @see void applyOnTheLeft(const Eigen::MatrixBase &, const + * Eigen::MatrixBase &) const + */ + template + typename PINOCCHIO_EIGEN_PLAIN_TYPE(MatrixDervideLhs) + applyOnTheLeft(const Eigen::MatrixBase & lhs) const { - const Scalar block_max_coeff = block.map.maxCoeff(); - if (block_max_coeff > max_coeff) - { - max_coeff = block_max_coeff; - } + typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(MatrixDervideLhs) ReturnType; + ReturnType res(lhs.rows(), cols()); // Assuming this should be rows(), rhs.cols() + applyOnTheLeft(lhs.derived(), res); + return res; } - return max_coeff; - } + /** + * @brief Performs the matrix-matrix product `(*this) * rhs` and returns the result. + * + * @tparam MatrixDerivedRhs The Eigen type of the right-hand-side matrix. + * + * @param[in] rhs The matrix to multiply with on the right. + * + * @return A new matrix containing the result of the multiplication. The returned matrix + * type is deduced to be a plain, non-expression matrix. + */ + template + typename PINOCCHIO_EIGEN_PLAIN_TYPE(MatrixDerivedRhs) + operator*(const Eigen::MatrixBase & rhs) const + { + return applyOnTheRight(rhs); + } - /** - * @brief Performs the matrix-matrix product `res = (*this) * rhs`. - * - * @tparam AssignOp Assignment operation used to store the result - * (e.g. ::pinocchio::internal::assign_op for direct assignment, - * or ::pinocchio::internal::add_assign_op for accumulation). - * @tparam MatrixDerivedRhs The Eigen type of the right-hand-side matrix. - * @tparam MatrixDerivedRes The Eigen type of the result matrix. - * - * @param[in] rhs The matrix to multiply with on the right. - * @param[out] res The matrix where the result is stored. It must be pre-allocated - * with dimensions `this->rows()` by `rhs.cols()`. - * - * @details This method computes the product of this block-diagonal matrix with a dense - * matrix `rhs`. It leverages the sparse structure of `*this` to perform the - * computation efficiently, avoiding unnecessary multiplications by zero. - * - * As this version writes to a pre-allocated matrix, it avoids any dynamic - * memory allocation, making it suitable for real-time and performance-critical code. - */ - template< - typename AssignOp = pinocchio::internal::assign_op, - typename MatrixDerivedRhs, - typename MatrixDerivedRes> - void applyOnTheRight( - const Eigen::MatrixBase & rhs, - const Eigen::MatrixBase & res) const; + /** + * @brief Sets a dense matrix to be equal to this block-diagonal matrix. + * @tparam MatrixDerived An Eigen dense matrix type. + * @param[out] matrix The dense matrix to be set. Its non-zero blocks will be filled, and + * its off-diagonal blocks will be set to zero. + */ + template + void evalTo(const Eigen::MatrixBase & matrix) const; + + /** + * @brief Adds this block-diagonal matrix to a dense matrix. + * @tparam MatrixDerived An Eigen dense matrix type. + * @param[in,out] matrix The dense matrix to which this object will be added. + */ + template + void addTo(const Eigen::MatrixBase & matrix) const; + + /** + * @brief Subtracts this block-diagonal matrix from a dense matrix. + * @tparam MatrixDerived An Eigen dense matrix type. + * @param[in,out] matrix The dense matrix from which this object will be subtracted. + */ + template + void subTo(const Eigen::MatrixBase & matrix) const; + + /// @brief Returns a dense `Matrix` representation of this block-diagonal matrix. + /// @note This involves a memory allocation and data copy. For performance, prefer + /// operations like `evalTo` that work on existing memory. + Matrix matrix() const; + + /** + * @brief Fills a pre-allocated dense matrix with the values of this block-diagonal matrix. + * @see evalTo + * @tparam MatrixDerived An Eigen dense matrix type. + * @param[out] matrix The dense matrix to fill. Must have the correct dimensions. + */ + template + void matrix(const Eigen::MatrixBase & matrix) const; + + /// @brief Gets a const reference to the underlying memory stack containing block data. + const MatrixStack & getMatrixStack() const + { + return m_matrix_stack; + } - /** - * @brief Performs the matrix-matrix product `res = lhs * (*this)`. - * - * @tparam AssignOp Assignment operation used to store the result - * (e.g. ::pinocchio::internal::assign_op for direct assignment, - * or ::pinocchio::internal::add_assign_op for accumulation). - * @tparam MatrixDerivedLhs The Eigen type of the left-hand-side matrix. - * @tparam MatrixDerivedRes The Eigen type of the result matrix. - * - * @param[in] lhs The matrix to multiply with on the left. - * @param[out] res The matrix where the result is stored. It must be pre-allocated - * with dimensions `this->cols()` by `lhs.rows()`. - * - * @details This method computes the product of this block-diagonal matrix with a dense - * matrix `lhs`. It leverages the sparse structure of `*this` to perform the - * computation efficiently, avoiding unnecessary multiplications by zero. - * - * As this version writes to a pre-allocated matrix, it avoids any dynamic - * memory allocation, making it suitable for real-time and performance-critical code. - */ - template< - typename AssignOp = pinocchio::internal::assign_op, - typename MatrixDerivedLhs, - typename MatrixDerivedRes> - void applyOnTheLeft( - const Eigen::MatrixBase & lhs, - const Eigen::MatrixBase & res) const; + /// @brief Gets a mutable reference to the underlying memory stack containing block data. + MatrixStack & getMatrixStack() + { + return m_matrix_stack; + } - /** - * @brief Performs the matrix-matrix product `(*this) * rhs` and returns the result. - * - * @tparam MatrixDerivedRhs The Eigen type of the right-hand-side matrix. - * - * @param[in] rhs The matrix to multiply with on the right. - * - * @return A new matrix containing the result of the multiplication. The returned matrix - * type is deduced to be a plain, non-expression matrix. - * - * @details This is a convenience overload that allocates a new matrix to store the result - * of the product. - * - * @note For performance-critical applications, prefer the overload that accepts a - * pre-allocated result matrix to avoid repeated memory allocations. - * @see void applyOnTheRight(const Eigen::MatrixBase &, const - * Eigen::MatrixBase &) const - */ - template - typename PINOCCHIO_EIGEN_PLAIN_TYPE(MatrixDerivedRhs) - applyOnTheRight(const Eigen::MatrixBase & rhs) const - { - typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(MatrixDerivedRhs) ReturnType; - ReturnType res(rows(), rhs.cols()); // Assuming this should be rows(), rhs.cols() - applyOnTheRight(rhs.derived(), res); - return res; - } + /// @brief Gets a const reference to the vector of block descriptors. + const std::vector & getMatrixBlockElements() const + { + return m_matrix_block_elements; + } - /** - * @brief Performs the matrix-matrix product `lhs * (*this)` and returns the result. - * - * @tparam MatrixDerivedLhs The Eigen type of the left-hand-side matrix. - * - * @param[in] lhs The matrix to multiply with on the left. - * - * @return A new matrix containing the result of the multiplication. The returned matrix - * type is deduced to be a plain, non-expression matrix. - * - * @details This is a convenience overload that allocates a new matrix to store the result - * of the product. - * - * @note For performance-critical applications, prefer the overload that accepts a - * pre-allocated result matrix to avoid repeated memory allocations. - * @see void applyOnTheLeft(const Eigen::MatrixBase &, const - * Eigen::MatrixBase &) const - */ - template - typename PINOCCHIO_EIGEN_PLAIN_TYPE(MatrixDervideLhs) - applyOnTheLeft(const Eigen::MatrixBase & lhs) const - { - typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(MatrixDervideLhs) ReturnType; - ReturnType res(lhs.rows(), cols()); // Assuming this should be rows(), rhs.cols() - applyOnTheLeft(lhs.derived(), res); - return res; - } + /// @brief Gets a mutable reference to the vector of block descriptors. + std::vector & getMatrixBlockElements() + { + return m_matrix_block_elements; + } - /** - * @brief Performs the matrix-matrix product `(*this) * rhs` and returns the result. - * - * @tparam MatrixDerivedRhs The Eigen type of the right-hand-side matrix. - * - * @param[in] rhs The matrix to multiply with on the right. - * - * @return A new matrix containing the result of the multiplication. The returned matrix - * type is deduced to be a plain, non-expression matrix. - */ - template - typename PINOCCHIO_EIGEN_PLAIN_TYPE(MatrixDerivedRhs) - operator*(const Eigen::MatrixBase & rhs) const - { - return applyOnTheRight(rhs); - } + /// @copydoc getMatrixBlockElements + std::vector & blocks() + { + return m_matrix_block_elements; + } - /** - * @brief Sets a dense matrix to be equal to this block-diagonal matrix. - * @tparam MatrixDerived An Eigen dense matrix type. - * @param[out] matrix The dense matrix to be set. Its non-zero blocks will be filled, and - * its off-diagonal blocks will be set to zero. - */ - template - void evalTo(const Eigen::MatrixBase & matrix) const; + /// @copydoc getMatrixBlockElements + const std::vector & blocks() const + { + return m_matrix_block_elements; + } - /** - * @brief Adds this block-diagonal matrix to a dense matrix. - * @tparam MatrixDerived An Eigen dense matrix type. - * @param[in,out] matrix The dense matrix to which this object will be added. - */ - template - void addTo(const Eigen::MatrixBase & matrix) const; + /** + * @brief Fills a pre-allocated vector with the main diagonal of this block-diagonal matrix. + * + * @tparam DiagonalVector The Eigen type of the destination vector, slice, or expression. + * + * @param[out] diagonal_elements A pre-allocated, vector-like Eigen object that will be + * filled with the diagonal elements. It **must** have a size + * equal to the number of rows of this matrix. + * + * @details This is the core, high-performance method for extracting the full matrix diagonal. + * It avoids any memory allocation by writing directly into the provided destination. + * + * The method iterates through the sequence of diagonal blocks. For each block, + * it computes its diagonal and copies it into the appropriate segment of the + * `diagonal_elements` vector. + * + * @see Vector diagonal() const + */ + template + void diagonal(const Eigen::MatrixBase & diagonal_elements) const; + + /** + * @brief Extracts the main diagonal of this block-diagonal matrix into a new dense vector. + * + * @return A new dense column vector of size `rows()` containing the diagonal elements. + * + * @details This is a convenience method that allocates a new vector and calls the in-place + * `diagonal()` overload to fill it. + * + * @note For performance-critical code where repeated memory allocations should be avoided, + * prefer the overload that fills a pre-allocated vector. + * @see void diagonal(const Eigen::MatrixBase&) const + */ + Vector diagonal() const + { + Vector diagonal_elements(rows()); + diagonal(diagonal_elements); + return diagonal_elements; + } - /** - * @brief Subtracts this block-diagonal matrix from a dense matrix. - * @tparam MatrixDerived An Eigen dense matrix type. - * @param[in,out] matrix The dense matrix from which this object will be subtracted. - */ - template - void subTo(const Eigen::MatrixBase & matrix) const; + /** + * @brief Creates a square zero matrix represented as a `BlockDiagonalMatrixTpl`. + * + * @details This is a static factory method that constructs a `BlockDiagonalMatrixTpl` of + * dimensions `size` x `size` that represents a zero matrix. + * + * The resulting object is represented with optimal efficiency, using a single + * block of type `BlockType::Zero` spanning the entire matrix dimension. This is + * a very lightweight operation as it does not require allocating memory for the + * matrix coefficients themselves. + * + * @param[in] size The dimension (number of rows and columns) for the resulting square + * zero matrix. Must be non-negative. + * + * @return A `BlockDiagonalMatrixTpl` instance representing a `size` x `size` zero matrix. + * + * @code + * const Eigen::Index matrix_size = 10; + * auto zero_mat = pinocchio::internal::BlockDiagonalMatrix::Zero(matrix_size); + * + * // Check the properties of the created matrix + * // assert(zero_mat.rows() == matrix_size); + * // assert(zero_mat.cols() == matrix_size); + * // assert(zero_mat.matrix() == Eigen::MatrixXd::Zero(matrix_size, matrix_size)); + * @endcode + */ + static BlockDiagonalMatrixTpl Zero(const Eigen::Index size); + + /** + * @brief Creates a square matrix that is a scalar multiple of the identity matrix (s*I). + * + * @details This is a static factory method that constructs a `BlockDiagonalMatrixTpl` of + * dimensions `size` x `size` where all diagonal elements are equal to `value` + * and all off-diagonal elements are zero. + * + * The resulting object is represented with optimal efficiency, using a single + * block of type `BlockType::Scalar`. This is highly memory-efficient as it only + * requires storing the single scalar `value`, regardless of the matrix size. + * + * @param[in] size The dimension (number of rows and columns) for the resulting square + * matrix. Must be non-negative. + * @param[in] value The scalar value to place on the main diagonal. + * + * @return A `BlockDiagonalMatrixTpl` instance representing a `size` x `size` scalar + * identity matrix. + * + * @code + * const Eigen::Index matrix_size = 10; + * const double scalar_value = 5.0; + * auto scalar_id_mat = + * pinocchio::internal::BlockDiagonalMatrix::ScalarIdentity(matrix_size, scalar_value); + * + * // Check the properties of the created matrix + * // assert(scalar_id_mat.rows() == matrix_size); + * // assert(scalar_id_mat.cols() == matrix_size); + * // Eigen::MatrixXd expected = Eigen::MatrixXd::Identity(matrix_size, matrix_size) * + * scalar_value; + * // assert(scalar_id_mat.matrix().isApprox(expected)); + * @endcode + */ + static BlockDiagonalMatrixTpl ScalarIdentity(const Eigen::Index size, const Scalar & value); + + /** + * @brief Rebuilds the block-diagonal matrix from a given block pattern. + * @param[in] new_block_pattern A vector of MatrixBlockElement describing each diagonal block + * in order. + */ + template + void rebuild(const std::vector<_MatrixBlockElement> & new_block_pattern); + + /** + * @brief Rebuilds the block-diagonal matrix from a given block pattern (pointer and size). + * @param[in] new_block_pattern Pointer to an array of MatrixBlockElement. + * @param[in] size Size of the array. + */ + template + void rebuild(const _MatrixBlockElement * new_block_pattern, const size_t size); + + /** + * @brief Rebuilds the block-diagonal matrix from a diagonal expression. + * @param[in] diagonal_expression An expression of the diagonal of the matrix. + */ + template + void rebuild(const Eigen::DiagonalWrapper & diagonal_expression); + + /// \brief Returns a pointer to the underlying array serving as element storage. + void * data() + { + return m_matrix_stack.data(); + } - /// @brief Returns a dense `Matrix` representation of this block-diagonal matrix. - /// @note This involves a memory allocation and data copy. For performance, prefer - /// operations like `evalTo` that work on existing memory. - Matrix matrix() const; + /// \brief Returns a pointer to the underlying array serving as element storage. + const void * data() const + { + return m_matrix_stack.data(); + } - /** - * @brief Fills a pre-allocated dense matrix with the values of this block-diagonal matrix. - * @see evalTo - * @tparam MatrixDerived An Eigen dense matrix type. - * @param[out] matrix The dense matrix to fill. Must have the correct dimensions. - */ - template - void matrix(const Eigen::MatrixBase & matrix) const; + /// \brief Returns the current memory footprint of this object in bytes. + /// \details Sums up the sizes of all internal data members. + std::size_t sizeInBytes() const + { + return 2 * ::pinocchio::sizeInBytes() + + m_matrix_stack.sizeInBytes(); // TODO(jcarpent) complete + + // sizeInBytes(m_matrix_block_elements); + } - /// @brief Gets a const reference to the underlying memory stack containing block data. - const MatrixStack & getMatrixStack() const + /// \brief Returns true if any coefficient (element) of this blocl element is NaN + /// (Not‑a‑Number). + bool hasNaN() const; + + /// \brief Returns an expression representing the inverse of this block diagonal matrix. + Inverse inverse() const; + + protected: + /** + * @brief Constructs a block-diagonal matrix from a given block pattern. + * @param[in] block_pattern A vector of MatrixBlockElement describing each diagonal block in + * order. + */ + template + void init_or_rebuild(const std::vector<_MatrixBlockElement> & block_pattern); + + template + void init_or_rebuild(const _MatrixBlockElement * block_pattern, const size_t size); + + template + void init_or_rebuild(const Eigen::DiagonalWrapper & diagonal_expression); + + /** + * @brief Clear the internal data structure for init or rebuild of the block diagonal matrix. + */ + void clear(); + + /** + * @brief Generic implementation for assignment-like operations (e.g., =, +=, -=). + * @tparam AssignOp Functor type that performs the assignment (e.g., + * `pinocchio::internal::SetTo`). + * @tparam Matrix Dense Eigen matrix type. + * @param[in,out] matrix The target matrix for the operation. + */ + template + void assign_op(const Eigen::MatrixBase & matrix) const; + + protected: + /// @brief Total number of rows of the composite matrix. + Eigen::Index m_rows = -1; + + /// @brief Total number of columns of the composite matrix. + Eigen::Index m_cols = -1; + + /// @brief Contiguous memory storage for all non-trivial matrix blocks. + MatrixStack m_matrix_stack; + + /// @brief A vector describing the sequence and properties of each diagonal block. + std::vector m_matrix_block_elements; + }; // struct BlockDiagonalMatrixTpl + + template + template + void BlockDiagonalMatrixTpl::init_or_rebuild( + const Eigen::DiagonalWrapper & diagonal_expression) { - return m_matrix_stack; - } + // typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(DiagonalVectorType) PlainDiagonalVectorType; + // const auto diagonal_terms = make_map(diagonal_expression.diagonal()); + // ConstMatrixBlockElement block_info = { + // pinocchio::MatrixBlockType::Diagonal, diagonal_terms.size(), diagonal_terms}; + const auto & diagonal_terms = diagonal_expression.diagonal(); + ConstMatrixBlockElement block_info = {MatrixBlockType::Diagonal, diagonal_terms.size()}; - /// @brief Gets a mutable reference to the underlying memory stack containing block data. - MatrixStack & getMatrixStack() - { - return m_matrix_stack; - } + const std::vector input_block_pattern = {{block_info}}; - /// @brief Gets a const reference to the vector of block descriptors. - const std::vector & getMatrixBlockElements() const - { - return m_matrix_block_elements; + init_or_rebuild(input_block_pattern); + m_matrix_block_elements.back().container() = diagonal_terms; } - /// @brief Gets a mutable reference to the vector of block descriptors. - std::vector & getMatrixBlockElements() + template + template + BlockDiagonalMatrixTpl::BlockDiagonalMatrixTpl( + const Eigen::DiagonalWrapper & diagonal_expression) { - return m_matrix_block_elements; + init_or_rebuild(diagonal_expression); } - /// @copydoc getMatrixBlockElements - std::vector & blocks() + template + template + void BlockDiagonalMatrixTpl::rebuild( + const Eigen::DiagonalWrapper & diagonal_expression) { - return m_matrix_block_elements; + init_or_rebuild(diagonal_expression); } - /// @copydoc getMatrixBlockElements - const std::vector & blocks() const + template + BlockDiagonalMatrixTpl::BlockDiagonalMatrixTpl( + const std::vector & input_block_pattern) { - return m_matrix_block_elements; + init_or_rebuild(input_block_pattern); } - /** - * @brief Fills a pre-allocated vector with the main diagonal of this block-diagonal matrix. - * - * @tparam DiagonalVector The Eigen type of the destination vector, slice, or expression. - * - * @param[out] diagonal_elements A pre-allocated, vector-like Eigen object that will be - * filled with the diagonal elements. It **must** have a size - * equal to the number of rows of this matrix. - * - * @details This is the core, high-performance method for extracting the full matrix diagonal. - * It avoids any memory allocation by writing directly into the provided destination. - * - * The method iterates through the sequence of diagonal blocks. For each block, - * it computes its diagonal and copies it into the appropriate segment of the - * `diagonal_elements` vector. - * - * @see Vector diagonal() const - */ - template - void diagonal(const Eigen::MatrixBase & diagonal_elements) const; - - /** - * @brief Extracts the main diagonal of this block-diagonal matrix into a new dense vector. - * - * @return A new dense column vector of size `rows()` containing the diagonal elements. - * - * @details This is a convenience method that allocates a new vector and calls the in-place - * `diagonal()` overload to fill it. - * - * @note For performance-critical code where repeated memory allocations should be avoided, - * prefer the overload that fills a pre-allocated vector. - * @see void diagonal(const Eigen::MatrixBase&) const - */ - Vector diagonal() const + template + template + void BlockDiagonalMatrixTpl::rebuild( + const std::vector<_MatrixBlockElement> & new_block_pattern) { - Vector diagonal_elements(rows()); - diagonal(diagonal_elements); - return diagonal_elements; + init_or_rebuild(new_block_pattern); } - /** - * @brief Creates a square zero matrix represented as a `BlockDiagonalMatrixTpl`. - * - * @details This is a static factory method that constructs a `BlockDiagonalMatrixTpl` of - * dimensions `size` x `size` that represents a zero matrix. - * - * The resulting object is represented with optimal efficiency, using a single - * block of type `BlockType::Zero` spanning the entire matrix dimension. This is - * a very lightweight operation as it does not require allocating memory for the - * matrix coefficients themselves. - * - * @param[in] size The dimension (number of rows and columns) for the resulting square - * zero matrix. Must be non-negative. - * - * @return A `BlockDiagonalMatrixTpl` instance representing a `size` x `size` zero matrix. - * - * @code - * const Eigen::Index matrix_size = 10; - * auto zero_mat = pinocchio::BlockDiagonalMatrix::Zero(matrix_size); - * - * // Check the properties of the created matrix - * // assert(zero_mat.rows() == matrix_size); - * // assert(zero_mat.cols() == matrix_size); - * // assert(zero_mat.matrix() == Eigen::MatrixXd::Zero(matrix_size, matrix_size)); - * @endcode - */ - static BlockDiagonalMatrixTpl Zero(const Eigen::Index size); - - /** - * @brief Creates a square matrix that is a scalar multiple of the identity matrix (s*I). - * - * @details This is a static factory method that constructs a `BlockDiagonalMatrixTpl` of - * dimensions `size` x `size` where all diagonal elements are equal to `value` - * and all off-diagonal elements are zero. - * - * The resulting object is represented with optimal efficiency, using a single - * block of type `BlockType::Scalar`. This is highly memory-efficient as it only - * requires storing the single scalar `value`, regardless of the matrix size. - * - * @param[in] size The dimension (number of rows and columns) for the resulting square - * matrix. Must be non-negative. - * @param[in] value The scalar value to place on the main diagonal. - * - * @return A `BlockDiagonalMatrixTpl` instance representing a `size` x `size` scalar - * identity matrix. - * - * @code - * const Eigen::Index matrix_size = 10; - * const double scalar_value = 5.0; - * auto scalar_id_mat = pinocchio::BlockDiagonalMatrix::ScalarIdentity(matrix_size, - * scalar_value); - * - * // Check the properties of the created matrix - * // assert(scalar_id_mat.rows() == matrix_size); - * // assert(scalar_id_mat.cols() == matrix_size); - * // Eigen::MatrixXd expected = Eigen::MatrixXd::Identity(matrix_size, matrix_size) * - * scalar_value; - * // assert(scalar_id_mat.matrix().isApprox(expected)); - * @endcode - */ - static BlockDiagonalMatrixTpl ScalarIdentity(const Eigen::Index size, const Scalar & value); - - /** - * @brief Rebuilds the block-diagonal matrix from a given block pattern. - * @param[in] new_block_pattern A vector of MatrixBlockElement describing each diagonal block in - * order. - */ - template - void rebuild(const std::vector<_MatrixBlockElement> & new_block_pattern); - - /** - * @brief Rebuilds the block-diagonal matrix from a given block pattern (pointer and size). - * @param[in] new_block_pattern Pointer to an array of MatrixBlockElement. - * @param[in] size Size of the array. - */ + template template - void rebuild(const _MatrixBlockElement * new_block_pattern, const size_t size); - - /** - * @brief Rebuilds the block-diagonal matrix from a diagonal expression. - * @param[in] diagonal_expression An expression of the diagonal of the matrix. - */ - template - void rebuild(const Eigen::DiagonalWrapper & diagonal_expression); - - /// \brief Returns a pointer to the underlying array serving as element storage. - void * data() + void BlockDiagonalMatrixTpl::rebuild( + const _MatrixBlockElement * new_block_pattern, const size_t size) { - return m_matrix_stack.data(); + init_or_rebuild(new_block_pattern, size); } - /// \brief Returns a pointer to the underlying array serving as element storage. - const void * data() const + template + void BlockDiagonalMatrixTpl::clear() { - return m_matrix_stack.data(); + m_rows = m_cols = -1; + m_matrix_stack.clear(); + m_matrix_block_elements.clear(); } - /// \brief Returns the current memory footprint of this object in bytes. - /// \details Sums up the sizes of all internal data members. - std::size_t sizeInBytes() const + template + template + void BlockDiagonalMatrixTpl::init_or_rebuild( + const std::vector<_MatrixBlockElement> & input_block_pattern) { - return 2 * ::pinocchio::sizeInBytes() - + m_matrix_stack - .sizeInBytes(); // TODO(jcarpent) complete + sizeInBytes(m_matrix_block_elements); + init_or_rebuild(input_block_pattern.data(), input_block_pattern.size()); } - /// \brief Returns true if any coefficient (element) of this blocl element is NaN - /// (Not‑a‑Number). - bool hasNaN() const; - - /// \brief Returns an expression representing the inverse of this block diagonal matrix. - Inverse inverse() const; - - protected: - /** - * @brief Constructs a block-diagonal matrix from a given block pattern. - * @param[in] block_pattern A vector of MatrixBlockElement describing each diagonal block in - * order. - */ - template - void init_or_rebuild(const std::vector<_MatrixBlockElement> & block_pattern); - + template template - void init_or_rebuild(const _MatrixBlockElement * block_pattern, const size_t size); - - template - void init_or_rebuild(const Eigen::DiagonalWrapper & diagonal_expression); - - /** - * @brief Clear the internal data structure for init or rebuild of the block diagonal matrix. - */ - void clear(); - - /** - * @brief Generic implementation for assignment-like operations (e.g., =, +=, -=). - * @tparam AssignOp Functor type that performs the assignment (e.g., - * `pinocchio::internal::SetTo`). - * @tparam Matrix Dense Eigen matrix type. - * @param[in,out] matrix The target matrix for the operation. - */ - template - void assign_op(const Eigen::MatrixBase & matrix) const; - - protected: - /// @brief Total number of rows of the composite matrix. - Eigen::Index m_rows = -1; - - /// @brief Total number of columns of the composite matrix. - Eigen::Index m_cols = -1; - - /// @brief Contiguous memory storage for all non-trivial matrix blocks. - MatrixStack m_matrix_stack; - - /// @brief A vector describing the sequence and properties of each diagonal block. - std::vector m_matrix_block_elements; - }; // struct BlockDiagonalMatrixTpl - - template - template - void BlockDiagonalMatrixTpl::init_or_rebuild( - const Eigen::DiagonalWrapper & diagonal_expression) - { - // typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(DiagonalVectorType) PlainDiagonalVectorType; - // const auto diagonal_terms = make_map(diagonal_expression.diagonal()); - // ConstMatrixBlockElement block_info = { - // pinocchio::MatrixBlockType::Diagonal, diagonal_terms.size(), diagonal_terms}; - const auto & diagonal_terms = diagonal_expression.diagonal(); - ConstMatrixBlockElement block_info = { - pinocchio::MatrixBlockType::Diagonal, diagonal_terms.size()}; - - const std::vector input_block_pattern = {{block_info}}; - - init_or_rebuild(input_block_pattern); - m_matrix_block_elements.back().container() = diagonal_terms; - } - - template - template - BlockDiagonalMatrixTpl::BlockDiagonalMatrixTpl( - const Eigen::DiagonalWrapper & diagonal_expression) - { - init_or_rebuild(diagonal_expression); - } - - template - template - void BlockDiagonalMatrixTpl::rebuild( - const Eigen::DiagonalWrapper & diagonal_expression) - { - init_or_rebuild(diagonal_expression); - } - - template - BlockDiagonalMatrixTpl::BlockDiagonalMatrixTpl( - const std::vector & input_block_pattern) - { - init_or_rebuild(input_block_pattern); - } - - template - template - void BlockDiagonalMatrixTpl::rebuild( - const std::vector<_MatrixBlockElement> & new_block_pattern) - { - init_or_rebuild(new_block_pattern); - } - - template - template - void BlockDiagonalMatrixTpl::rebuild( - const _MatrixBlockElement * new_block_pattern, const size_t size) - { - init_or_rebuild(new_block_pattern, size); - } - - template - void BlockDiagonalMatrixTpl::clear() - { - m_rows = m_cols = -1; - m_matrix_stack.clear(); - m_matrix_block_elements.clear(); - } - - template - template - void BlockDiagonalMatrixTpl::init_or_rebuild( - const std::vector<_MatrixBlockElement> & input_block_pattern) - { - init_or_rebuild(input_block_pattern.data(), input_block_pattern.size()); - } - - template - template - void BlockDiagonalMatrixTpl::init_or_rebuild( - const _MatrixBlockElement * input_block_pattern, const size_t size) - { - clear(); + void BlockDiagonalMatrixTpl::init_or_rebuild( + const _MatrixBlockElement * input_block_pattern, const size_t size) + { + clear(); - static_assert( - pinocchio::internal::is_specialization_of_v< - _MatrixBlockElement, pinocchio::MatrixBlockElementTpl>, - "_MatrixBlockElement is not of type pinocchio::MatrixBlockElementTpl<...>"); + static_assert( + pinocchio::internal::is_specialization_of_v<_MatrixBlockElement, MatrixBlockElementTpl>, + "_MatrixBlockElement is not of type pinocchio::MatrixBlockElementTpl<...>"); - // First pass: count total number of MatrixStack entries needed - // (handles nested sub-blocks as well as flat data blocks) - std::size_t total_memory_entries = 0; - for (std::size_t i = 0; i < size; ++i) - { - const auto & block_info = input_block_pattern[i]; - if (block_info.type() == MatrixBlockType::NestedBlockDiagonal) + // First pass: count total number of MatrixStack entries needed + // (handles nested sub-blocks as well as flat data blocks) + std::size_t total_memory_entries = 0; + for (std::size_t i = 0; i < size; ++i) { - for (const auto & sub : block_info.nested_blocks()) - if (isDataBlock(sub.type())) - total_memory_entries++; - } - else if (isDataBlock(block_info.type())) - { - total_memory_entries++; + const auto & block_info = input_block_pattern[i]; + if (block_info.type() == MatrixBlockType::NestedBlockDiagonal) + { + for (const auto & sub : block_info.nested_blocks()) + if (isDataBlock(sub.type())) + total_memory_entries++; + } + else if (isDataBlock(block_info.type())) + { + total_memory_entries++; + } } - } - // analysis block pattern and extract memory/size info - const std::size_t num_blocks = size; - MatrixInfo * memory_block_sizes = - static_cast(PINOCCHIO_ALLOCA(total_memory_entries * sizeof(MatrixInfo))); - std::size_t memory_block_id = 0; + // analysis block pattern and extract memory/size info + const std::size_t num_blocks = size; + MatrixInfo * memory_block_sizes = + static_cast(PINOCCHIO_ALLOCA(total_memory_entries * sizeof(MatrixInfo))); + std::size_t memory_block_id = 0; - m_matrix_block_elements.reserve(size); - m_rows = 0; - for (std::size_t i = 0; i < num_blocks; ++i) - { - const auto & block_info = input_block_pattern[i]; - assert(block_info.type() != MatrixBlockType::Undefined); + m_matrix_block_elements.reserve(size); + m_rows = 0; + for (std::size_t i = 0; i < num_blocks; ++i) + { + const auto & block_info = input_block_pattern[i]; + assert(block_info.type() != MatrixBlockType::Undefined); - m_rows += block_info.size(); + m_rows += block_info.size(); - if (block_info.type() == MatrixBlockType::NestedBlockDiagonal) - { - // Build nested block: copy sub-block structure (null maps to be filled later) - std::vector empty_subs; - empty_subs.reserve(block_info.nested_blocks().size()); - for (const auto & sub : block_info.nested_blocks()) + if (block_info.type() == MatrixBlockType::NestedBlockDiagonal) { - empty_subs.emplace_back(sub.type(), sub.size()); - if (sub.type() == MatrixBlockType::ScalarIdentity) + // Build nested block: copy sub-block structure (null maps to be filled later) + std::vector empty_subs; + empty_subs.reserve(block_info.nested_blocks().size()); + for (const auto & sub : block_info.nested_blocks()) + { + empty_subs.emplace_back(sub.type(), sub.size()); + if (sub.type() == MatrixBlockType::ScalarIdentity) + memory_block_sizes[memory_block_id++] = {1, 1}; + else if (sub.type() == MatrixBlockType::Diagonal) + memory_block_sizes[memory_block_id++] = {sub.size(), 1}; + else if (sub.type() == MatrixBlockType::Plain) + memory_block_sizes[memory_block_id++] = {sub.size(), sub.size()}; + } + m_matrix_block_elements.emplace_back( + MatrixBlockType::NestedBlockDiagonal, std::move(empty_subs)); + } + else + { + m_matrix_block_elements.push_back({block_info.type(), block_info.size()}); + + if (block_info.type() == MatrixBlockType::ScalarIdentity) memory_block_sizes[memory_block_id++] = {1, 1}; - else if (sub.type() == MatrixBlockType::Diagonal) - memory_block_sizes[memory_block_id++] = {sub.size(), 1}; - else if (sub.type() == MatrixBlockType::Plain) - memory_block_sizes[memory_block_id++] = {sub.size(), sub.size()}; + else if (block_info.type() == MatrixBlockType::Diagonal) + memory_block_sizes[memory_block_id++] = {block_info.size(), 1}; + else if (block_info.type() == MatrixBlockType::Plain) + memory_block_sizes[memory_block_id++] = {block_info.size(), block_info.size()}; } - m_matrix_block_elements.emplace_back( - MatrixBlockType::NestedBlockDiagonal, std::move(empty_subs)); - } - else - { - m_matrix_block_elements.push_back({block_info.type(), block_info.size()}); - - if (block_info.type() == MatrixBlockType::ScalarIdentity) - memory_block_sizes[memory_block_id++] = {1, 1}; - else if (block_info.type() == MatrixBlockType::Diagonal) - memory_block_sizes[memory_block_id++] = {block_info.size(), 1}; - else if (block_info.type() == MatrixBlockType::Plain) - memory_block_sizes[memory_block_id++] = {block_info.size(), block_info.size()}; } - } - m_matrix_stack.rebuild(memory_block_sizes, memory_block_id); + m_matrix_stack.rebuild(memory_block_sizes, memory_block_id); - // Fill with data: remap MatrixStack slots to block elements - std::size_t matrix_stack_id = 0; - for (std::size_t i = 0; i < m_matrix_block_elements.size(); ++i) - { - auto & block_info = m_matrix_block_elements[i]; - const auto & input_block_info = input_block_pattern[i]; + // Fill with data: remap MatrixStack slots to block elements + std::size_t matrix_stack_id = 0; + for (std::size_t i = 0; i < m_matrix_block_elements.size(); ++i) + { + auto & block_info = m_matrix_block_elements[i]; + const auto & input_block_info = input_block_pattern[i]; - assert(block_info.type() != MatrixBlockType::Undefined); + assert(block_info.type() != MatrixBlockType::Undefined); - if (block_info.type() == MatrixBlockType::NestedBlockDiagonal) - { - const auto & input_subs = input_block_info.nested_blocks(); - auto & subs = block_info.nested_blocks(); - for (std::size_t j = 0; j < subs.size(); ++j) + if (block_info.type() == MatrixBlockType::NestedBlockDiagonal) { - auto & sub = subs[j]; - if (isDataBlock(sub.type())) + const auto & input_subs = input_block_info.nested_blocks(); + auto & subs = block_info.nested_blocks(); + for (std::size_t j = 0; j < subs.size(); ++j) { - auto matrix_map = m_matrix_stack[matrix_stack_id]; - sub.remap(matrix_map); - if (input_subs[j].data() != nullptr) - sub.container() = input_subs[j].container(); - matrix_stack_id++; + auto & sub = subs[j]; + if (isDataBlock(sub.type())) + { + auto matrix_map = m_matrix_stack[matrix_stack_id]; + sub.remap(matrix_map); + if (input_subs[j].data() != nullptr) + sub.container() = input_subs[j].container(); + matrix_stack_id++; + } } } + else if (isDataBlock(block_info.type())) + { + auto matrix_map = m_matrix_stack[matrix_stack_id]; + block_info.remap(matrix_map); // remap data to the matrix stack + if (input_block_info.data() != nullptr) + block_info.container() = input_block_info.container(); // copy data + // otherwise, uninitialized + matrix_stack_id++; + } } - else if (isDataBlock(block_info.type())) - { - auto matrix_map = m_matrix_stack[matrix_stack_id]; - block_info.remap(matrix_map); // remap data to the matrix stack - if (input_block_info.data() != nullptr) - block_info.container() = input_block_info.container(); // copy data - // otherwise, uninitialized - matrix_stack_id++; - } + + m_cols = m_rows; } - m_cols = m_rows; - } + template + template + void BlockDiagonalMatrixTpl::evalTo( + const Eigen::MatrixBase & _matrix) const + { + PINOCCHIO_CHECK_ARGUMENT_SIZE( + _matrix.rows(), rows(), "The input matrix has not the right number of rows."); + PINOCCHIO_CHECK_ARGUMENT_SIZE( + _matrix.cols(), cols(), "The input matrix has not the right number of columns."); - template - template - void BlockDiagonalMatrixTpl::evalTo( - const Eigen::MatrixBase & _matrix) const - { - PINOCCHIO_CHECK_ARGUMENT_SIZE( - _matrix.rows(), rows(), "The input matrix has not the right number of rows."); - PINOCCHIO_CHECK_ARGUMENT_SIZE( - _matrix.cols(), cols(), "The input matrix has not the right number of columns."); - - _matrix.const_cast_derived().setZero(); - assign_op(_matrix.const_cast_derived()); - } - - template - template - void BlockDiagonalMatrixTpl::addTo( - const Eigen::MatrixBase & _matrix) const - { - PINOCCHIO_CHECK_ARGUMENT_SIZE( - _matrix.rows(), rows(), "The input matrix has not the right number of rows."); - PINOCCHIO_CHECK_ARGUMENT_SIZE( - _matrix.cols(), cols(), "The input matrix has not the right number of columns."); - - assign_op(_matrix.const_cast_derived()); - } - - template - template - void BlockDiagonalMatrixTpl::subTo( - const Eigen::MatrixBase & _matrix) const - { - PINOCCHIO_CHECK_ARGUMENT_SIZE( - _matrix.rows(), rows(), "The input matrix has not the right number of rows."); - PINOCCHIO_CHECK_ARGUMENT_SIZE( - _matrix.cols(), cols(), "The input matrix has not the right number of columns."); - - assign_op(_matrix.const_cast_derived()); - } - - template - template - void BlockDiagonalMatrixTpl::assign_op( - const Eigen::MatrixBase & _matrix) const - { - auto & matrix = _matrix.const_cast_derived(); + _matrix.const_cast_derived().setZero(); + assign_op(_matrix.const_cast_derived()); + } - Eigen::Index row_id = 0; - for (const auto & matrix_block_elt : m_matrix_block_elements) + template + template + void BlockDiagonalMatrixTpl::addTo( + const Eigen::MatrixBase & _matrix) const { - const auto size = matrix_block_elt.size(); - auto matrix_block = matrix.block(row_id, row_id, size, size); + PINOCCHIO_CHECK_ARGUMENT_SIZE( + _matrix.rows(), rows(), "The input matrix has not the right number of rows."); + PINOCCHIO_CHECK_ARGUMENT_SIZE( + _matrix.cols(), cols(), "The input matrix has not the right number of columns."); - matrix_block_elt.template assign_op(matrix_block); - row_id += size; + assign_op(_matrix.const_cast_derived()); } - assert(row_id == cols()); - } + template + template + void BlockDiagonalMatrixTpl::subTo( + const Eigen::MatrixBase & _matrix) const + { + PINOCCHIO_CHECK_ARGUMENT_SIZE( + _matrix.rows(), rows(), "The input matrix has not the right number of rows."); + PINOCCHIO_CHECK_ARGUMENT_SIZE( + _matrix.cols(), cols(), "The input matrix has not the right number of columns."); + + assign_op(_matrix.const_cast_derived()); + } - template - template - void BlockDiagonalMatrixTpl::matrix( - const Eigen::MatrixBase & _matrix) const - { - evalTo(_matrix.const_cast_derived()); - } + template + template + void BlockDiagonalMatrixTpl::assign_op( + const Eigen::MatrixBase & _matrix) const + { + auto & matrix = _matrix.const_cast_derived(); - template - typename BlockDiagonalMatrixTpl::Matrix - BlockDiagonalMatrixTpl::matrix() const - { - Matrix res(rows(), cols()); - matrix(res); - return res; - } + Eigen::Index row_id = 0; + for (const auto & matrix_block_elt : m_matrix_block_elements) + { + const auto size = matrix_block_elt.size(); + auto matrix_block = matrix.block(row_id, row_id, size, size); - template - bool BlockDiagonalMatrixTpl::hasNaN() const - { - for (const auto & block_info : m_matrix_block_elements) + matrix_block_elt.template assign_op(matrix_block); + row_id += size; + } + + assert(row_id == cols()); + } + + template + template + void BlockDiagonalMatrixTpl::matrix( + const Eigen::MatrixBase & _matrix) const { - if (block_info.hasNaN()) - return true; + evalTo(_matrix.const_cast_derived()); } - return false; - } - - template - template - void BlockDiagonalMatrixTpl::applyOnTheRight( - const Eigen::MatrixBase & rhs, - const Eigen::MatrixBase & _res) const - { - PINOCCHIO_CHECK_ARGUMENT_SIZE( - rhs.rows(), cols(), "The input rhs matrix has not the right number of rows."); - PINOCCHIO_CHECK_ARGUMENT_SIZE( - rhs.cols(), _res.cols(), "The input rhs and res matrices has not the right number of cols."); - PINOCCHIO_CHECK_ARGUMENT_SIZE( - _res.rows(), rows(), "The input res matrix has not the right number of rows."); - auto & res = _res.const_cast_derived(); + template + typename BlockDiagonalMatrixTpl::Matrix + BlockDiagonalMatrixTpl::matrix() const + { + Matrix res(rows(), cols()); + matrix(res); + return res; + } + + template + bool BlockDiagonalMatrixTpl::hasNaN() const + { + for (const auto & block_info : m_matrix_block_elements) + { + if (block_info.hasNaN()) + return true; + } + return false; + } - Eigen::Index row_id = 0; - for (const auto & block_info : m_matrix_block_elements) + template + template + void BlockDiagonalMatrixTpl::applyOnTheRight( + const Eigen::MatrixBase & rhs, + const Eigen::MatrixBase & _res) const { - const auto block_size = block_info.size(); + PINOCCHIO_CHECK_ARGUMENT_SIZE( + rhs.rows(), cols(), "The input rhs matrix has not the right number of rows."); + PINOCCHIO_CHECK_ARGUMENT_SIZE( + rhs.cols(), _res.cols(), + "The input rhs and res matrices has not the right number of cols."); + PINOCCHIO_CHECK_ARGUMENT_SIZE( + _res.rows(), rows(), "The input res matrix has not the right number of rows."); + + auto & res = _res.const_cast_derived(); + + Eigen::Index row_id = 0; + for (const auto & block_info : m_matrix_block_elements) + { + const auto block_size = block_info.size(); - const auto rhs_block = rhs.middleRows(row_id, block_size); - auto res_block = res.middleRows(row_id, block_size); + const auto rhs_block = rhs.middleRows(row_id, block_size); + auto res_block = res.middleRows(row_id, block_size); - assert( - rhs_block.data() != res_block.data() - && "There is aliasing between rhs_block and res_block."); + assert( + rhs_block.data() != res_block.data() + && "There is aliasing between rhs_block and res_block."); - switch (block_info.type()) - { - case MatrixBlockType::Zero: { - AssignOp::run(Matrix::Zero(rhs_block.rows(), rhs_block.cols()), res_block); - break; - } - case MatrixBlockType::Identity: { - AssignOp::run(rhs_block, res_block); - break; - } - case MatrixBlockType::ScalarIdentity: { - const auto & map = block_info.map; - const auto & scalar = map(0, 0); - AssignOp::run(scalar * rhs_block, res_block); - break; - } - case MatrixBlockType::Diagonal: { - const auto & map = block_info.map; - AssignOp::run(map.asDiagonal() * rhs_block, res_block.noalias()); - break; - } - case MatrixBlockType::Plain: { - const auto & map = block_info.map; - AssignOp::run(map * rhs_block, res_block.noalias()); - break; - } - case MatrixBlockType::NestedBlockDiagonal: { - Eigen::Index sub_offset = 0; - for (const auto & sub : block_info.nested_blocks()) + switch (block_info.type()) { - const auto sub_size = sub.size(); - const auto sub_rhs = rhs.middleRows(row_id + sub_offset, sub_size); - auto sub_res = res.middleRows(row_id + sub_offset, sub_size); - switch (sub.type()) + case MatrixBlockType::Zero: { + AssignOp::run(Matrix::Zero(rhs_block.rows(), rhs_block.cols()), res_block); + break; + } + case MatrixBlockType::Identity: { + AssignOp::run(rhs_block, res_block); + break; + } + case MatrixBlockType::ScalarIdentity: { + const auto & map = block_info.map; + const auto & scalar = map(0, 0); + AssignOp::run(scalar * rhs_block, res_block); + break; + } + case MatrixBlockType::Diagonal: { + const auto & map = block_info.map; + AssignOp::run(map.asDiagonal() * rhs_block, res_block.noalias()); + break; + } + case MatrixBlockType::Plain: { + const auto & map = block_info.map; + AssignOp::run(map * rhs_block, res_block.noalias()); + break; + } + case MatrixBlockType::NestedBlockDiagonal: { + Eigen::Index sub_offset = 0; + for (const auto & sub : block_info.nested_blocks()) { - case MatrixBlockType::Zero: - AssignOp::run(Matrix::Zero(sub_rhs.rows(), sub_rhs.cols()), sub_res); - break; - case MatrixBlockType::Identity: - AssignOp::run(sub_rhs, sub_res); - break; - case MatrixBlockType::ScalarIdentity: { - const auto & scalar = sub.map(0, 0); - AssignOp::run(scalar * sub_rhs, sub_res); - break; + const auto sub_size = sub.size(); + const auto sub_rhs = rhs.middleRows(row_id + sub_offset, sub_size); + auto sub_res = res.middleRows(row_id + sub_offset, sub_size); + switch (sub.type()) + { + case MatrixBlockType::Zero: + AssignOp::run(Matrix::Zero(sub_rhs.rows(), sub_rhs.cols()), sub_res); + break; + case MatrixBlockType::Identity: + AssignOp::run(sub_rhs, sub_res); + break; + case MatrixBlockType::ScalarIdentity: { + const auto & scalar = sub.map(0, 0); + AssignOp::run(scalar * sub_rhs, sub_res); + break; + } + case MatrixBlockType::Diagonal: + AssignOp::run(sub.map.asDiagonal() * sub_rhs, sub_res.noalias()); + break; + case MatrixBlockType::Plain: + AssignOp::run(sub.map * sub_rhs, sub_res.noalias()); + break; + default: + PINOCCHIO_UNREACHABLE(); + } + sub_offset += sub_size; } - case MatrixBlockType::Diagonal: - AssignOp::run(sub.map.asDiagonal() * sub_rhs, sub_res.noalias()); - break; - case MatrixBlockType::Plain: - AssignOp::run(sub.map * sub_rhs, sub_res.noalias()); - break; - default: - PINOCCHIO_UNREACHABLE(); - } - sub_offset += sub_size; + break; + } + default: + PINOCCHIO_UNREACHABLE(); } - break; - } - default: - PINOCCHIO_UNREACHABLE(); - } - row_id += block_size; + row_id += block_size; + } } - } - - template - template - void BlockDiagonalMatrixTpl::applyOnTheLeft( - const Eigen::MatrixBase & lhs, - const Eigen::MatrixBase & _res) const - { - PINOCCHIO_CHECK_ARGUMENT_SIZE( - lhs.cols(), rows(), "The input lhs matrix has not the right number of cols."); - PINOCCHIO_CHECK_ARGUMENT_SIZE( - lhs.rows(), _res.rows(), "The input lhs and res matrices has not the right number of rows."); - PINOCCHIO_CHECK_ARGUMENT_SIZE( - _res.cols(), cols(), "The input res matrix has not the right number of cols."); - auto & res = _res.const_cast_derived(); - - Eigen::Index col_id = 0; - for (const auto & block_info : m_matrix_block_elements) + template + template + void BlockDiagonalMatrixTpl::applyOnTheLeft( + const Eigen::MatrixBase & lhs, + const Eigen::MatrixBase & _res) const { - const auto block_size = block_info.size(); + PINOCCHIO_CHECK_ARGUMENT_SIZE( + lhs.cols(), rows(), "The input lhs matrix has not the right number of cols."); + PINOCCHIO_CHECK_ARGUMENT_SIZE( + lhs.rows(), _res.rows(), + "The input lhs and res matrices has not the right number of rows."); + PINOCCHIO_CHECK_ARGUMENT_SIZE( + _res.cols(), cols(), "The input res matrix has not the right number of cols."); + + auto & res = _res.const_cast_derived(); + + Eigen::Index col_id = 0; + for (const auto & block_info : m_matrix_block_elements) + { + const auto block_size = block_info.size(); - const auto lhs_block = lhs.middleCols(col_id, block_size); - auto res_block = res.middleCols(col_id, block_size); + const auto lhs_block = lhs.middleCols(col_id, block_size); + auto res_block = res.middleCols(col_id, block_size); - assert( - lhs_block.data() != res_block.data() - && "There is aliasing between lhs_block and res_block."); + assert( + lhs_block.data() != res_block.data() + && "There is aliasing between lhs_block and res_block."); - switch (block_info.type()) - { - case MatrixBlockType::Zero: { - AssignOp::run(Matrix::Zero(lhs_block.rows(), lhs_block.cols()), res_block); - break; - } - case MatrixBlockType::Identity: { - AssignOp::run(lhs_block, res_block); - break; - } - case MatrixBlockType::ScalarIdentity: { - const auto & map = block_info.map; - const auto & scalar = map(0, 0); - AssignOp::run(lhs_block * scalar, res_block); - break; - } - case MatrixBlockType::Diagonal: { - const auto & map = block_info.map; - AssignOp::run(lhs_block * map.asDiagonal(), res_block.noalias()); - break; - } - case MatrixBlockType::Plain: { - const auto & map = block_info.map; - AssignOp::run(lhs_block * map, res_block.noalias()); - break; - } - case MatrixBlockType::NestedBlockDiagonal: { - Eigen::Index sub_offset = 0; - for (const auto & sub : block_info.nested_blocks()) + switch (block_info.type()) { - const auto sub_size = sub.size(); - const auto sub_lhs = lhs.middleCols(col_id + sub_offset, sub_size); - auto sub_res = res.middleCols(col_id + sub_offset, sub_size); - switch (sub.type()) + case MatrixBlockType::Zero: { + AssignOp::run(Matrix::Zero(lhs_block.rows(), lhs_block.cols()), res_block); + break; + } + case MatrixBlockType::Identity: { + AssignOp::run(lhs_block, res_block); + break; + } + case MatrixBlockType::ScalarIdentity: { + const auto & map = block_info.map; + const auto & scalar = map(0, 0); + AssignOp::run(lhs_block * scalar, res_block); + break; + } + case MatrixBlockType::Diagonal: { + const auto & map = block_info.map; + AssignOp::run(lhs_block * map.asDiagonal(), res_block.noalias()); + break; + } + case MatrixBlockType::Plain: { + const auto & map = block_info.map; + AssignOp::run(lhs_block * map, res_block.noalias()); + break; + } + case MatrixBlockType::NestedBlockDiagonal: { + Eigen::Index sub_offset = 0; + for (const auto & sub : block_info.nested_blocks()) { - case MatrixBlockType::Zero: - AssignOp::run(Matrix::Zero(sub_lhs.rows(), sub_lhs.cols()), sub_res); - break; - case MatrixBlockType::Identity: - AssignOp::run(sub_lhs, sub_res); - break; - case MatrixBlockType::ScalarIdentity: { - const auto & scalar = sub.map(0, 0); - AssignOp::run(sub_lhs * scalar, sub_res); - break; - } - case MatrixBlockType::Diagonal: - AssignOp::run(sub_lhs * sub.map.asDiagonal(), sub_res.noalias()); - break; - case MatrixBlockType::Plain: - AssignOp::run(sub_lhs * sub.map, sub_res.noalias()); - break; - default: - PINOCCHIO_UNREACHABLE(); + const auto sub_size = sub.size(); + const auto sub_lhs = lhs.middleCols(col_id + sub_offset, sub_size); + auto sub_res = res.middleCols(col_id + sub_offset, sub_size); + switch (sub.type()) + { + case MatrixBlockType::Zero: + AssignOp::run(Matrix::Zero(sub_lhs.rows(), sub_lhs.cols()), sub_res); + break; + case MatrixBlockType::Identity: + AssignOp::run(sub_lhs, sub_res); + break; + case MatrixBlockType::ScalarIdentity: { + const auto & scalar = sub.map(0, 0); + AssignOp::run(sub_lhs * scalar, sub_res); + break; + } + case MatrixBlockType::Diagonal: + AssignOp::run(sub_lhs * sub.map.asDiagonal(), sub_res.noalias()); + break; + case MatrixBlockType::Plain: + AssignOp::run(sub_lhs * sub.map, sub_res.noalias()); + break; + default: + PINOCCHIO_UNREACHABLE(); + } + sub_offset += sub_size; } - sub_offset += sub_size; + break; + } + default: + PINOCCHIO_UNREACHABLE(); } - break; - } - default: - PINOCCHIO_UNREACHABLE(); - } - col_id += block_size; + col_id += block_size; + } } - } - template - Inverse> - BlockDiagonalMatrixTpl::inverse() const - { - return {*this}; - } + template + Inverse> + BlockDiagonalMatrixTpl::inverse() const + { + return {*this}; + } - template - BlockDiagonalMatrixTpl & - BlockDiagonalMatrixTpl::operator=( - const BlockDiagonalMatrixTpl & other) - { - if (this == &other) - return *this; + template + BlockDiagonalMatrixTpl & + BlockDiagonalMatrixTpl::operator=( + const BlockDiagonalMatrixTpl & other) + { + if (this == &other) + return *this; - m_rows = other.m_rows; - m_cols = other.m_cols; - m_matrix_stack = other.m_matrix_stack; - m_matrix_block_elements = other.m_matrix_block_elements; + m_rows = other.m_rows; + m_cols = other.m_cols; + m_matrix_stack = other.m_matrix_stack; + m_matrix_block_elements = other.m_matrix_block_elements; - size_t matrix_stack_id = 0; - for (auto & block_info : m_matrix_block_elements) - { - if (block_info.type() == MatrixBlockType::NestedBlockDiagonal) + size_t matrix_stack_id = 0; + for (auto & block_info : m_matrix_block_elements) { - for (auto & sub : block_info.nested_blocks()) + if (block_info.type() == MatrixBlockType::NestedBlockDiagonal) { - if (isDataBlock(sub.type())) + for (auto & sub : block_info.nested_blocks()) { - auto matrix_map = m_matrix_stack[matrix_stack_id]; - sub.remap(matrix_map); - matrix_stack_id++; + if (isDataBlock(sub.type())) + { + auto matrix_map = m_matrix_stack[matrix_stack_id]; + sub.remap(matrix_map); + matrix_stack_id++; + } } } + else if (isDataBlock(block_info.type())) + { + auto matrix_map = m_matrix_stack[matrix_stack_id]; + block_info.remap(matrix_map); + matrix_stack_id++; + } } - else if (isDataBlock(block_info.type())) - { - auto matrix_map = m_matrix_stack[matrix_stack_id]; - block_info.remap(matrix_map); - matrix_stack_id++; - } + + return *this; } - return *this; - } + template + template + BlockDiagonalMatrixTpl & + BlockDiagonalMatrixTpl::operator=( + const Eigen::DiagonalWrapper & diagonal_expression) + { + rebuild(diagonal_expression); + return *this; + } - template - template - BlockDiagonalMatrixTpl & - BlockDiagonalMatrixTpl::operator=( - const Eigen::DiagonalWrapper & diagonal_expression) - { - rebuild(diagonal_expression); - return *this; - } - - template - template - BlockDiagonalMatrixTpl & - BlockDiagonalMatrixTpl::operator+=( - const Eigen::DiagonalWrapper & diagonal_expression) - { + template + template + BlockDiagonalMatrixTpl & + BlockDiagonalMatrixTpl::operator+=( + const Eigen::DiagonalWrapper & diagonal_expression) + { + Sum< + BlockDiagonalMatrixTpl, + Eigen::DiagonalWrapper>(*this, diagonal_expression) + .evalTo(*this); + return *this; + } + + template + template Sum< BlockDiagonalMatrixTpl, - Eigen::DiagonalWrapper>(*this, diagonal_expression) - .evalTo(*this); - return *this; - } - - template - template - Sum< - BlockDiagonalMatrixTpl, - Eigen::DiagonalWrapper> - BlockDiagonalMatrixTpl::operator+( - const Eigen::DiagonalWrapper & diagonal_expression) const - { - return {*this, diagonal_expression}; - } + Eigen::DiagonalWrapper> + BlockDiagonalMatrixTpl::operator+( + const Eigen::DiagonalWrapper & diagonal_expression) const + { + return {*this, diagonal_expression}; + } - template - BlockDiagonalMatrixTpl - BlockDiagonalMatrixTpl::Zero(const Eigen::Index size) - { - MatrixBlockElement block_info = {pinocchio::MatrixBlockType::Zero, size}; - const std::vector input_block_pattern = {{block_info}}; + template + BlockDiagonalMatrixTpl + BlockDiagonalMatrixTpl::Zero(const Eigen::Index size) + { + MatrixBlockElement block_info = {MatrixBlockType::Zero, size}; + const std::vector input_block_pattern = {{block_info}}; - return BlockDiagonalMatrixTpl(input_block_pattern); - } + return BlockDiagonalMatrixTpl(input_block_pattern); + } - template - BlockDiagonalMatrixTpl - BlockDiagonalMatrixTpl::ScalarIdentity( - const Eigen::Index size, const Scalar & value) - { - typedef Eigen::Matrix M11; - M11 value_mat = M11(value); - const auto matrix_map = make_map(value_mat); - MatrixBlockElement block_info = {pinocchio::MatrixBlockType::ScalarIdentity, size, matrix_map}; - const std::vector input_block_pattern = {{block_info}}; - - return BlockDiagonalMatrixTpl(input_block_pattern); - } - - template - template - void BlockDiagonalMatrixTpl::diagonal( - const Eigen::MatrixBase & _diagonal_elements) const - { - auto & diagonal_elements = _diagonal_elements.const_cast_derived(); - Eigen::Index row_id = 0; - for (const auto & block_info : m_matrix_block_elements) + template + BlockDiagonalMatrixTpl + BlockDiagonalMatrixTpl::ScalarIdentity( + const Eigen::Index size, const Scalar & value) { - const auto block_size = block_info.size(); - auto diagonal_elements_segment = diagonal_elements.segment(row_id, block_size); - block_info.diagonal(diagonal_elements_segment); - row_id += block_size; + typedef Eigen::Matrix M11; + M11 value_mat = M11(value); + const auto matrix_map = make_map(value_mat); + MatrixBlockElement block_info = {MatrixBlockType::ScalarIdentity, size, matrix_map}; + const std::vector input_block_pattern = {{block_info}}; + + return BlockDiagonalMatrixTpl(input_block_pattern); + } + + template + template + void BlockDiagonalMatrixTpl::diagonal( + const Eigen::MatrixBase & _diagonal_elements) const + { + auto & diagonal_elements = _diagonal_elements.const_cast_derived(); + Eigen::Index row_id = 0; + for (const auto & block_info : m_matrix_block_elements) + { + const auto block_size = block_info.size(); + auto diagonal_elements_segment = diagonal_elements.segment(row_id, block_size); + block_info.diagonal(diagonal_elements_segment); + row_id += block_size; + } } - } + } // namespace internal } // namespace pinocchio diff --git a/include/pinocchio/src/math/fwd.hxx b/include/pinocchio/src/math/fwd.hxx index f264ef4188..7b0f75c743 100644 --- a/include/pinocchio/src/math/fwd.hxx +++ b/include/pinocchio/src/math/fwd.hxx @@ -12,31 +12,9 @@ namespace pinocchio { - template struct EigenMatrixExpression; - template - struct UnaryOperator; - - template - struct BinaryOperator; - - template - struct Inverse; - - template - struct Sum; - - template - struct BlockDiagonalMatrixTpl; - - template - struct MatrixBlockElementTpl; - - template - struct MatrixBlockElementPlain; - template struct is_floating_point : ::std::is_floating_point { @@ -46,11 +24,32 @@ namespace pinocchio template struct TaylorSeriesExpansion; - template - struct MatrixBlockElementBase; - template - struct MatrixBlockElementPlain; - template - struct MatrixBlockElementOperation; + namespace internal + { + template + struct UnaryOperator; + + template + struct BinaryOperator; + + template + struct Inverse; + template + struct Sum; + + template + struct MatrixBlockElementBase; + template + struct MatrixBlockElementPlain; + template + struct MatrixBlockElementOperation; + template + struct MatrixBlockElementTpl; + template + struct MatrixBlockElementPlain; + + template + struct BlockDiagonalMatrixTpl; + } // namespace internal } // namespace pinocchio diff --git a/include/pinocchio/src/math/matrix-block-element-base.hxx b/include/pinocchio/src/math/matrix-block-element-base.hxx index ea4bbd6970..dc9bc97a2f 100644 --- a/include/pinocchio/src/math/matrix-block-element-base.hxx +++ b/include/pinocchio/src/math/matrix-block-element-base.hxx @@ -13,94 +13,97 @@ namespace pinocchio { - /** - * @ingroup pinocchio_math - * @brief A base class providing a common interface for matrix block descriptors using CRTP. - * - * @tparam Derived The concrete, derived class that implements the block element's storage and - * logic. - * - * @details This class serves as a compile-time interface for different specializations of - * `MatrixBlockElementTpl`. It uses the Curiously Recurring Template Pattern (CRTP) - * to achieve static polymorphism, which avoids the overhead of virtual functions - * (v-tables). - * - * By inheriting from `MatrixBlockElementBase`, a derived class - * gains a common interface (`type()`, `size()`) while being required to provide the - * actual implementation for these methods. The base class can then call these derived - * class methods through the `derived()` helper function. - * - * This pattern is central to providing a consistent API for both owning (`Eigen::Matrix` - * based) and non-owning (`Eigen::Map` based) matrix block elements. - */ - template - struct MatrixBlockElementBase + namespace internal { - - typedef typename traits::Matrix Matrix; /** - * @brief Provides access to the concrete derived class instance. + * @ingroup pinocchio_math + * @brief A base class providing a common interface for matrix block descriptors using CRTP. * - * @details This is the core mechanism of the CRTP. It uses a `static_cast` to safely - * downcast the `this` pointer to the `Derived` type, allowing the base class - * to call methods that are implemented in the derived class. + * @tparam Derived The concrete, derived class that implements the block element's storage and + * logic. * - * @return A mutable reference to the derived object. - */ - Derived & derived() - { - return *static_cast(this); - } - - /** - * @brief Provides const access to the concrete derived class instance. - * @copydetails derived() - * @return A const reference to the derived object. - */ - const Derived & derived() const - { - return *static_cast(this); - } - - /** - * @brief Returns the structural type of the matrix block. + * @details This class serves as a compile-time interface for different specializations of + * `MatrixBlockElementTpl`. It uses the Curiously Recurring Template Pattern (CRTP) + * to achieve static polymorphism, which avoids the overhead of virtual functions + * (v-tables). * - * @details This method forwards the call to the `type()` method of the concrete - * `Derived` class via the `derived()` helper. The actual `type` data member - * is expected to be stored in the derived class. + * By inheriting from `MatrixBlockElementBase`, a derived class + * gains a common interface (`type()`, `size()`) while being required to provide the + * actual implementation for these methods. The base class can then call these derived + * class methods through the `derived()` helper function. * - * @return The `MatrixBlockType` as provided by the derived class. + * This pattern is central to providing a consistent API for both owning + * (`Eigen::Matrix` based) and non-owning (`Eigen::Map` based) matrix block elements. */ - MatrixBlockType type() const + template + struct MatrixBlockElementBase { - return derived().type(); - } - /** - * @brief Returns the size of the (square) matrix block. - * - * @details This method forwards the call to the `size()` method of the concrete - * `Derived` class via the `derived()` helper. The actual `size` data member - * is expected to be stored in the derived class. - * - * @return The size (`Eigen::Index`) as provided by the derived class. - */ - Eigen::Index size() const - { - return derived().size(); - } + typedef typename traits::Matrix Matrix; + /** + * @brief Provides access to the concrete derived class instance. + * + * @details This is the core mechanism of the CRTP. It uses a `static_cast` to safely + * downcast the `this` pointer to the `Derived` type, allowing the base class + * to call methods that are implemented in the derived class. + * + * @return A mutable reference to the derived object. + */ + Derived & derived() + { + return *static_cast(this); + } - template - void matrix(const Eigen::MatrixBase & _matrix) const - { - derived().matrix(_matrix.const_cast_derived()); - } + /** + * @brief Provides const access to the concrete derived class instance. + * @copydetails derived() + * @return A const reference to the derived object. + */ + const Derived & derived() const + { + return *static_cast(this); + } - Matrix matrix() const - { - return derived().matrix(); - } + /** + * @brief Returns the structural type of the matrix block. + * + * @details This method forwards the call to the `type()` method of the concrete + * `Derived` class via the `derived()` helper. The actual `type` data member + * is expected to be stored in the derived class. + * + * @return The `MatrixBlockType` as provided by the derived class. + */ + MatrixBlockType type() const + { + return derived().type(); + } + + /** + * @brief Returns the size of the (square) matrix block. + * + * @details This method forwards the call to the `size()` method of the concrete + * `Derived` class via the `derived()` helper. The actual `size` data member + * is expected to be stored in the derived class. + * + * @return The size (`Eigen::Index`) as provided by the derived class. + */ + Eigen::Index size() const + { + return derived().size(); + } + + template + void matrix(const Eigen::MatrixBase & _matrix) const + { + derived().matrix(_matrix.const_cast_derived()); + } + + Matrix matrix() const + { + return derived().matrix(); + } - }; // struct MatrixBlockElementBase + }; // struct MatrixBlockElementBase + } // namespace internal } // namespace pinocchio diff --git a/include/pinocchio/src/math/matrix-block-element-operation.hxx b/include/pinocchio/src/math/matrix-block-element-operation.hxx index b4524c5bbf..dcbb775982 100644 --- a/include/pinocchio/src/math/matrix-block-element-operation.hxx +++ b/include/pinocchio/src/math/matrix-block-element-operation.hxx @@ -13,22 +13,25 @@ namespace pinocchio { - - template - struct MatrixBlockElementPlain; - - template - struct MatrixBlockElementOperation : MatrixBlockElementBase + namespace internal { - typedef MatrixBlockElementBase Base; - using Base::derived; + template + struct MatrixBlockElementPlain; - template - void evalTo(MatrixBlockElementPlain & res) const + template + struct MatrixBlockElementOperation : MatrixBlockElementBase { - derived().evalTo(res.derived()); - } - }; + + typedef MatrixBlockElementBase Base; + using Base::derived; + + template + void evalTo(MatrixBlockElementPlain & res) const + { + derived().evalTo(res.derived()); + } + }; + } // namespace internal } // namespace pinocchio diff --git a/include/pinocchio/src/math/matrix-block-element-operations.hxx b/include/pinocchio/src/math/matrix-block-element-operations.hxx index 9b29aa088d..7692f03d30 100644 --- a/include/pinocchio/src/math/matrix-block-element-operations.hxx +++ b/include/pinocchio/src/math/matrix-block-element-operations.hxx @@ -129,112 +129,112 @@ namespace pinocchio } }; // struct sub_op_tpl - } // namespace internal - - template - struct traits, - Eigen::DiagonalWrapper>> - { - typedef MatrixBlockElementTpl LhsType; - typedef typename traits::Matrix Matrix; - }; - - template - struct BinaryOperator< - BinaryOp, - MatrixBlockElementTpl, - Eigen::DiagonalWrapper> - : MatrixBlockElementOperation + struct traits, Eigen::DiagonalWrapper>> - { - static_assert( - std::is_base_of_v, - "BinaryOp should be a binary operator."); - - typedef MatrixBlockElementTpl LhsType; - typedef Eigen::DiagonalWrapper RhsType; - - BinaryOperator(const LhsType & lhs, const RhsType & rhs) - : m_lhs(lhs) - , m_rhs(rhs) { - } + typedef MatrixBlockElementTpl LhsType; + typedef typename traits::Matrix Matrix; + }; - const LhsType & lhs() const - { - return m_lhs; - } - const RhsType & rhs() const - { - return m_rhs; - } - - template - void evalTo(ResType & res) const - { - run(lhs(), rhs(), res); - } - - template - static void - run(const LhsType & lhs_block_elt, const RhsType & diagonal_matrix, ResType & res_block_elt) + template + struct BinaryOperator< + BinaryOp, + MatrixBlockElementTpl, + Eigen::DiagonalWrapper> + : MatrixBlockElementOperation, + Eigen::DiagonalWrapper>> { + static_assert( + std::is_base_of_v, + "BinaryOp should be a binary operator."); - typedef typename BinaryOp::template op_tpl Op; + typedef MatrixBlockElementTpl LhsType; + typedef Eigen::DiagonalWrapper RhsType; - constexpr MatrixBlockType res_valid_block_types = static_cast( - static_cast(MatrixBlockType::Diagonal) - | static_cast(MatrixBlockType::Plain)); - assert(hasFlag(res_block_elt.type(), res_valid_block_types) && "res block type is invalid"); - PINOCCHIO_ONLY_USED_FOR_DEBUG(res_valid_block_types); - - // typedef typename LhsType::Matrix LhsMatrix; - typedef typename LhsType::Vector LhsVector; - - const auto lhs_size = lhs_block_elt.size(); - switch (lhs_block_elt.type()) + BinaryOperator(const LhsType & lhs, const RhsType & rhs) + : m_lhs(lhs) + , m_rhs(rhs) { - case MatrixBlockType::Identity: { - const auto lhs_matrix = LhsVector::Ones(lhs_size).asDiagonal(); - // const auto lhs_matrix = LhsMatrix::Identity(lhs_size,lhs_size); - Op::run(lhs_matrix, diagonal_matrix, res_block_elt); - break; - } - case MatrixBlockType::Zero: { - const auto lhs_matrix = LhsVector::Zero(lhs_size).asDiagonal(); - Op::run(lhs_matrix, diagonal_matrix, res_block_elt); - break; } - case MatrixBlockType::ScalarIdentity: { - const auto & mat = lhs_block_elt.container(); - const auto & scalar = mat(0, 0); - const auto lhs_matrix = LhsVector::Constant(lhs_size, scalar).asDiagonal(); - Op::run(lhs_matrix, diagonal_matrix, res_block_elt); - break; + + const LhsType & lhs() const + { + return m_lhs; } - case MatrixBlockType::Diagonal: { - const auto & diagonal_elt = lhs_block_elt.container(); - const auto lhs_matrix = diagonal_elt.asDiagonal(); - Op::run(lhs_matrix, diagonal_matrix, res_block_elt); - break; + const RhsType & rhs() const + { + return m_rhs; } - case MatrixBlockType::Plain: { - const auto & plain_mat = lhs_block_elt.container(); - Op::run(plain_mat, diagonal_matrix, res_block_elt); - break; + + template + void evalTo(ResType & res) const + { + run(lhs(), rhs(), res); } - default: - PINOCCHIO_UNREACHABLE(); + + template + static void + run(const LhsType & lhs_block_elt, const RhsType & diagonal_matrix, ResType & res_block_elt) + { + + typedef typename BinaryOp::template op_tpl Op; + + constexpr MatrixBlockType res_valid_block_types = static_cast( + static_cast(MatrixBlockType::Diagonal) + | static_cast(MatrixBlockType::Plain)); + assert(hasFlag(res_block_elt.type(), res_valid_block_types) && "res block type is invalid"); + PINOCCHIO_ONLY_USED_FOR_DEBUG(res_valid_block_types); + + // typedef typename LhsType::Matrix LhsMatrix; + typedef typename LhsType::Vector LhsVector; + + const auto lhs_size = lhs_block_elt.size(); + switch (lhs_block_elt.type()) + { + case MatrixBlockType::Identity: { + const auto lhs_matrix = LhsVector::Ones(lhs_size).asDiagonal(); + // const auto lhs_matrix = LhsMatrix::Identity(lhs_size,lhs_size); + Op::run(lhs_matrix, diagonal_matrix, res_block_elt); + break; + } + case MatrixBlockType::Zero: { + const auto lhs_matrix = LhsVector::Zero(lhs_size).asDiagonal(); + Op::run(lhs_matrix, diagonal_matrix, res_block_elt); + break; + } + case MatrixBlockType::ScalarIdentity: { + const auto & mat = lhs_block_elt.container(); + const auto & scalar = mat(0, 0); + const auto lhs_matrix = LhsVector::Constant(lhs_size, scalar).asDiagonal(); + Op::run(lhs_matrix, diagonal_matrix, res_block_elt); + break; + } + case MatrixBlockType::Diagonal: { + const auto & diagonal_elt = lhs_block_elt.container(); + const auto lhs_matrix = diagonal_elt.asDiagonal(); + Op::run(lhs_matrix, diagonal_matrix, res_block_elt); + break; + } + case MatrixBlockType::Plain: { + const auto & plain_mat = lhs_block_elt.container(); + Op::run(plain_mat, diagonal_matrix, res_block_elt); + break; + } + default: + PINOCCHIO_UNREACHABLE(); + } } - } - protected: - const LhsType & m_lhs; - const RhsType m_rhs; // rhs is a DiagonalWrapper, copying it is fine. - }; + protected: + const LhsType & m_lhs; + const RhsType m_rhs; // rhs is a DiagonalWrapper, copying it is fine. + }; + + } // namespace internal } // namespace pinocchio diff --git a/include/pinocchio/src/math/matrix-block-element-plain.hxx b/include/pinocchio/src/math/matrix-block-element-plain.hxx index e7c613fe40..0c48e5a711 100644 --- a/include/pinocchio/src/math/matrix-block-element-plain.hxx +++ b/include/pinocchio/src/math/matrix-block-element-plain.hxx @@ -13,771 +13,775 @@ namespace pinocchio { - /** - * @ingroup pinocchio_math - * @brief Describes a single block within a larger structured matrix. - * - * @tparam Matrix The underlying Eigen matrix type this block will map to. - * - * @details This struct acts as a descriptor for a matrix sub-block. It does not own any memory - * itself; instead, it uses an `Eigen::Map` to provide a non-owning view into a region of memory - * managed elsewhere (e.g., by a `MatrixStack` in `BlockDiagonalMatrixTpl`). - * - * It stores the block's structural type (e.g., `Identity`, `Plain`), its size, and the - * `Eigen::Map` to its data if the type requires it. - */ - template - struct MatrixBlockElementPlain : MatrixBlockElementBase + namespace internal { - typedef MatrixBlockElementBase Base; - typedef typename traits::MatrixContainer MatrixContainer; - typedef typename traits::Matrix Matrix; - typedef typename traits::Vector Vector; - typedef typename traits::Scalar Scalar; - typedef typename traits::PlainBlockElement PlainBlockElement; - - using Base::derived; - - /// @brief Default constructor. Initializes to an invalid state. - MatrixBlockElementPlain() - : m_type(MatrixBlockType::Undefined) - , m_size(-1) - { - } - - /** - * @brief Constructs a block info for types that do not require external data (e.g., Identity, - * Zero). - * @param[in] type The structural type of the block. - * @param[in] size The dimension of the (square) block. - */ - MatrixBlockElementPlain(const MatrixBlockType type, const Eigen::Index size) - : m_type(type) - , m_size(size) - { - } - - /// @brief Default copy constructor. - MatrixBlockElementPlain(const MatrixBlockElementPlain & other) = default; - - /// @brief Default copy-assignment operator. - MatrixBlockElementPlain & operator=(const MatrixBlockElementPlain & other) = default; - - template - Derived & operator=(const MatrixBlockElementOperation & other) - { - other.evalTo(derived()); - return derived(); - } - - /** - * @brief Checks for strict equality between two block info objects. - * - * @param[in] other The other block info to compare against. - * @return `true` if the blocks are strictly equal, `false` otherwise. - * - * @details Two `MatrixBlockElementPlain` objects are considered equal if and only if: - * 1. They have the same `type` and `size`. - * 2. The data viewed by their internal `Eigen::Map`s is **coefficient-wise equal**. - * - * @note This operator performs a deep, numerical comparison of the underlying data, which - * can be computationally expensive for large blocks. It does not simply compare pointers. - */ - bool operator==(const MatrixBlockElementPlain & other) const - { - if (this == &other) - return true; - return m_type == other.m_type && m_size == other.m_size; - } - - /** - * @brief Checks for inequality between two block info objects. - * - * @param[in] other The other block info to compare against. - * @return `true` if the blocks are not strictly equal, `false` otherwise. - * - * @details This is the logical negation of `operator==`. - * @see operator==() - */ - bool operator!=(const MatrixBlockElementPlain & other) const - { - return !(*this == other); - } - - /** - * @brief Checks if the block information is valid and self-consistent. - * - * @details A block is considered valid if: - * - Its size is positive. - * - Its type is not `Undefined`. - * - * @return `true` if the block info is valid, `false` otherwise. - */ - bool isValid() const - { - bool is_invalid = m_size <= 0 || m_type == MatrixBlockType::Undefined; - return !is_invalid; - } - - /** - * @brief Getter for this block's data container. - */ - MatrixContainer & container() - { - return derived().container(); - } - - /** - * @brief Const getter for this block's data container. - */ - const MatrixContainer & container() const - { - return derived().container(); - } - - /** - * @brief Returns a const pointer to the data this blocks points to. - */ - const Scalar * data() const - { - return derived().data(); - } - - /** - * @brief Returns a pointer to the data this blocks points to. - */ - Scalar * data() - { - return derived().data(); - } - /** - * @brief Returns the type of this block (Zero, Identity, ScalarIdentity, Diagonal or Plain). - */ - MatrixBlockType type() const - { - return m_type; - } - - /** - * @brief Returns the size of this square block (size = rows and size = cols). - */ - Eigen::Index size() const - { - return m_size; - } - - /** - * @brief Applies an assignment operation to a matrix based on this block's type. - * - * @tparam AssignOp The assignment operation functor (e.g., `internal::assign_op`, - * `internal::add_assign_op`, `internal::sub_assign_op`). - * @tparam Matrix The Eigen matrix type to assign to. + * @ingroup pinocchio_math + * @brief Describes a single block within a larger structured matrix. * - * @param[in,out] _matrix The destination matrix expression to modify. + * @tparam Matrix The underlying Eigen matrix type this block will map to. * - * @details This method dispatches on the block's `type` to efficiently apply the - * appropriate assignment operation: - * - `Zero`: Applies AssignOp with a zero matrix. - * - `Identity`: Applies AssignOp with an identity matrix. - * - `ScalarIdentity`: Applies AssignOp with a scaled identity matrix. - * - `Diagonal`: Applies AssignOp with the diagonal coefficients. - * - `Plain`: Applies AssignOp with the full dense block data. + * @details This struct acts as a descriptor for a matrix sub-block. It does not own any memory + * itself; instead, it uses an `Eigen::Map` to provide a non-owning view into a region of memory + * managed elsewhere (e.g., by a `MatrixStack` in `BlockDiagonalMatrixTpl`). * - * @note This is a low-level method used internally by `evalTo`, `addTo`, and `subTo`. + * It stores the block's structural type (e.g., `Identity`, `Plain`), its size, and the + * `Eigen::Map` to its data if the type requires it. */ - template - void assign_op(const Eigen::MatrixBase & _matrix) const + template + struct MatrixBlockElementPlain : MatrixBlockElementBase { - auto & matrix = _matrix.const_cast_derived(); - - switch (type()) + typedef MatrixBlockElementBase Base; + typedef typename traits::MatrixContainer MatrixContainer; + typedef typename traits::Matrix Matrix; + typedef typename traits::Vector Vector; + typedef typename traits::Scalar Scalar; + typedef typename traits::PlainBlockElement PlainBlockElement; + + using Base::derived; + + /// @brief Default constructor. Initializes to an invalid state. + MatrixBlockElementPlain() + : m_type(MatrixBlockType::Undefined) + , m_size(-1) { - case MatrixBlockType::Zero: { - AssignOp::run(Matrix::Zero(size(), size()), matrix); - break; - } - case MatrixBlockType::Identity: { - AssignOp::run(Matrix::Identity(size(), size()), matrix); - break; - } - case MatrixBlockType::ScalarIdentity: { - const auto & scalar = container()(0, 0); - AssignOp::run(scalar * Matrix::Identity(size(), size()), matrix); - break; - } - case MatrixBlockType::Diagonal: { - AssignOp::run(container().asDiagonal(), matrix); - break; - } - case MatrixBlockType::Plain: { - AssignOp::run(container(), matrix); - break; - } - case MatrixBlockType::NestedBlockDiagonal: { - // Use Map with OuterStride to break the recursive template instantiation chain: - // matrix.block() yields Block which causes Block> nesting - // in recursive assign_op calls, exceeding the template depth limit. - // Map> is a fixed concrete type, - // so assign_op is always called with the same instantiation (no type explosion). - const Eigen::Index outer_stride = matrix.outerStride(); - Scalar * const base_ptr = matrix.derived().data(); - Eigen::Index sub_row_id = 0; - for (const auto & sub_block : derived().nested_blocks()) - { - const auto sub_size = sub_block.size(); - Eigen::Map< - Eigen::Matrix, Eigen::Unaligned, - Eigen::OuterStride> - sub_map( - // base ptr + col ptr + row ptr - base_ptr + sub_row_id + sub_row_id * outer_stride, sub_size, sub_size, - Eigen::OuterStride(outer_stride)); - sub_block.template assign_op(sub_map); - sub_row_id += sub_size; - } - break; } - default: - PINOCCHIO_UNREACHABLE(); - } - } - /** - * @brief Evaluates this block into a dense matrix expression. - * - * @tparam Matrix The Eigen matrix type to evaluate into. - * - * @param[out] _matrix The destination matrix expression to be filled. Must be a square - * matrix with dimensions equal to this block's `size`. - * - * @details This method first zeroes out the destination matrix, then applies the - * appropriate assignment based on the block's type using `assign_op`. - * This is the primary method for converting a block descriptor into its - * dense matrix representation. - * - * @throws std::invalid_argument If the matrix dimensions do not match the block size. - * - * @see assign_op() - */ - template - void evalTo(const Eigen::MatrixBase & _matrix) const - { - PINOCCHIO_CHECK_ARGUMENT_SIZE( - _matrix.rows(), size(), "The input matrix has not the right number of rows."); - PINOCCHIO_CHECK_ARGUMENT_SIZE( - _matrix.cols(), size(), "The input matrix has not the right number of columns."); - - _matrix.const_cast_derived().setZero(); - assign_op(_matrix.const_cast_derived()); - } - - /** - * @brief Adds this block's content to an existing matrix expression. - * - * @tparam Matrix The Eigen matrix type to add to. - * - * @param[in,out] _matrix The destination matrix expression to modify. Must be a square - * matrix with dimensions equal to this block's `size`. - * - * @details Unlike `evalTo`, this method does not zero out the destination first. - * It performs an additive assignment, equivalent to `_matrix += this_block`. - * - * @throws std::invalid_argument If the matrix dimensions do not match the block size. - * - * @see assign_op(), evalTo(), subTo() - */ - template - void addTo(const Eigen::MatrixBase & _matrix) const - { - PINOCCHIO_CHECK_ARGUMENT_SIZE( - _matrix.rows(), size(), "The input matrix has not the right number of rows."); - PINOCCHIO_CHECK_ARGUMENT_SIZE( - _matrix.cols(), size(), "The input matrix has not the right number of columns."); - - assign_op(_matrix.const_cast_derived()); - } - - /** - * @brief Subtracts this block from a matrix based on the block's structural type. - * - * @tparam Matrix The Eigen matrix type to subtract from. - * - * @param[in,out] _matrix The destination matrix expression to modify. Its contents will - * be updated as: `_matrix -= this_block`. - * - * @details This method dispatches on the block's `type` to efficiently subtract the - * appropriate matrix representation: - * - `Zero`: No operation (subtracting zero). - * - `Identity`: Subtracts an identity matrix. - * - `ScalarIdentity`: Subtracts a scaled identity matrix. - * - `Diagonal`: Subtracts the diagonal coefficients. - * - `Plain`: Subtracts the full dense block data. - * - * @pre The input matrix must have dimensions equal to `size() x size()`. - * - * @throws std::invalid_argument (via PINOCCHIO_CHECK_ARGUMENT_SIZE) if the matrix - * dimensions do not match the block size. - * - * @see evalTo(), addTo() - */ - template - void subTo(const Eigen::MatrixBase & _matrix) const - { - PINOCCHIO_CHECK_ARGUMENT_SIZE( - _matrix.rows(), size(), "The input matrix has not the right number of rows."); - PINOCCHIO_CHECK_ARGUMENT_SIZE( - _matrix.cols(), size(), "The input matrix has not the right number of columns."); + /** + * @brief Constructs a block info for types that do not require external data (e.g., Identity, + * Zero). + * @param[in] type The structural type of the block. + * @param[in] size The dimension of the (square) block. + */ + MatrixBlockElementPlain(const MatrixBlockType type, const Eigen::Index size) + : m_type(type) + , m_size(size) + { + } - assign_op(_matrix.const_cast_derived()); - } + /// @brief Default copy constructor. + MatrixBlockElementPlain(const MatrixBlockElementPlain & other) = default; - /** - * @brief Evaluates this block element into a dense matrix expression. - * - * @tparam Matrix The Eigen matrix type of the destination expression. - * - * @param[out] _matrix A pre-allocated, matrix-like Eigen expression that will be - * filled with the block's contents. It **must** have dimensions - * equal to `size() x size()`. - * - * @details This is a convenience wrapper around `evalTo()` that provides a more - * intuitive interface for extracting the full matrix representation of this block. - * - * @see evalTo() for the underlying implementation. - */ - template - void matrix(const Eigen::MatrixBase & _matrix) const - { - evalTo(_matrix.const_cast_derived()); - } + /// @brief Default copy-assignment operator. + MatrixBlockElementPlain & operator=(const MatrixBlockElementPlain & other) = default; - /** - * @brief Converts this block descriptor into a dense matrix representation. - * - * @return A newly allocated dense matrix of dimensions `size x size` containing - * the full matrix representation of this block. - * - * @details This is a convenience method that allocates a new matrix and calls - * `evalTo()` to fill it based on the block's type. The resulting matrix - * will be: - * - All zeros for `MatrixBlockType::Zero` - * - Identity matrix for `MatrixBlockType::Identity` - * - Scaled identity for `MatrixBlockType::ScalarIdentity` - * - Diagonal matrix for `MatrixBlockType::Diagonal` - * - Full dense copy for `MatrixBlockType::Plain` - * - * @note For performance-critical code where repeated memory allocations should - * be avoided, prefer the overload `matrix(const Eigen::MatrixBase&)` - * that fills a pre-allocated matrix. - * - * @see evalTo(), matrix(const Eigen::MatrixBase&) const - */ - Matrix matrix() const - { - Matrix res(size(), size()); - matrix(res); - return res; - } - - /** - * @brief Fills a pre-allocated vector expression with the diagonal elements of this block. - * - * @tparam DiagonalSlice The Eigen type of the destination vector, slice, or expression. - * - * @param[out] _diagonal_slice A pre-allocated, vector-like Eigen expression that will be - * filled with the diagonal elements. It **must** have a size - * equal to this block's `size`. - * - * @details This is the core, high-performance method for extracting a diagonal. It performs - * no memory allocation and writes the result directly into the provided destination. - * - * The behavior depends on the block's `type`: - * - `Zero`: Fills the destination with zeros. - * - `Identity`: Fills the destination with ones. - * - `ScalarIdentity`: Fills the destination with the block's scalar value. - * - `Diagonal`: Copies the vector of diagonal coefficients. - * - `Plain`: Extracts the diagonal from the dense block's data. - */ - template - void diagonal(const Eigen::MatrixBase & _diagonal_slice) const - { - auto & diagonal_slice = _diagonal_slice.const_cast_derived(); - assert(diagonal_slice.size() == size()); - - switch (type()) + template + Derived & operator=(const MatrixBlockElementOperation & other) { - case MatrixBlockType::Zero: { - diagonal_slice.setZero(); - break; - } - case MatrixBlockType::Identity: { - diagonal_slice.setOnes(); - break; + other.evalTo(derived()); + return derived(); + } + + /** + * @brief Checks for strict equality between two block info objects. + * + * @param[in] other The other block info to compare against. + * @return `true` if the blocks are strictly equal, `false` otherwise. + * + * @details Two `MatrixBlockElementPlain` objects are considered equal if and only if: + * 1. They have the same `type` and `size`. + * 2. The data viewed by their internal `Eigen::Map`s is **coefficient-wise equal**. + * + * @note This operator performs a deep, numerical comparison of the underlying data, which + * can be computationally expensive for large blocks. It does not simply compare + * pointers. + */ + bool operator==(const MatrixBlockElementPlain & other) const + { + if (this == &other) + return true; + return m_type == other.m_type && m_size == other.m_size; + } + + /** + * @brief Checks for inequality between two block info objects. + * + * @param[in] other The other block info to compare against. + * @return `true` if the blocks are not strictly equal, `false` otherwise. + * + * @details This is the logical negation of `operator==`. + * @see operator==() + */ + bool operator!=(const MatrixBlockElementPlain & other) const + { + return !(*this == other); + } + + /** + * @brief Checks if the block information is valid and self-consistent. + * + * @details A block is considered valid if: + * - Its size is positive. + * - Its type is not `Undefined`. + * + * @return `true` if the block info is valid, `false` otherwise. + */ + bool isValid() const + { + bool is_invalid = m_size <= 0 || m_type == MatrixBlockType::Undefined; + return !is_invalid; } - case MatrixBlockType::ScalarIdentity: { - const auto & scalar_value = container()(0, 0); - diagonal_slice.fill(scalar_value); - break; - } - case MatrixBlockType::Diagonal: { - diagonal_slice = container(); - break; - } - case MatrixBlockType::Plain: { - diagonal_slice = container().diagonal(); - break; - } - case MatrixBlockType::NestedBlockDiagonal: { - // Use Map from raw pointer to break the recursive template instantiation - // chain. Calling .segment() would yield VectorBlock, causing - // VectorBlock> nesting on recursive calls, exceeding template depth. - // Map is a fixed concrete type: always the same instantiation (no explosion). - Scalar * const base_ptr = diagonal_slice.derived().data(); - Eigen::Index sub_offset = 0; - for (const auto & sub_block : derived().nested_blocks()) - { - const auto sub_size = sub_block.size(); - Eigen::Map> sub_map( - base_ptr + sub_offset, sub_size); - sub_block.diagonal(sub_map); - sub_offset += sub_size; - } - break; - } - default: - PINOCCHIO_UNREACHABLE(); + /** + * @brief Getter for this block's data container. + */ + MatrixContainer & container() + { + return derived().container(); } - } - /** - * @brief Extracts the main diagonal of this block into a new dense vector. - * - * @return A new dense column vector of size `size` containing the diagonal elements. - * - * @details This is a convenience method that allocates a new vector and calls the in-place - * `diagonal()` overload to fill it. - * - * @note For performance-critical code where repeated memory allocations should be avoided, - * prefer the overload that fills a pre-allocated slice. - * @see void diagonal(const Eigen::MatrixBase&) const - */ - Vector diagonal() const - { - Vector diagonal_elements(size()); - diagonal(diagonal_elements); - return diagonal_elements; - } - - /** - * @brief Fill this block with random values between -1 and 1. - */ - void setRandom() - { - switch (type()) + /** + * @brief Const getter for this block's data container. + */ + const MatrixContainer & container() const { - case MatrixBlockType::Zero: - case MatrixBlockType::Identity: - return; - case MatrixBlockType::ScalarIdentity: - case MatrixBlockType::Diagonal: - case MatrixBlockType::Plain: - container().setRandom(); - break; - case MatrixBlockType::NestedBlockDiagonal: - for (auto & sub_block : derived().nested_blocks()) - sub_block.setRandom(); - break; - default: - PINOCCHIO_UNREACHABLE(); + return derived().container(); } - } - /** - * @brief Fill this block with random values forming a Positive Definite matrix. - * - * @details For a matrix to be positive definite, all eigenvalues must be strictly positive. - * The implementation strategy depends on the block type: - * - `Zero`: Invalid operation - a zero matrix is not positive definite. - * - `Identity`: Already positive definite (all eigenvalues are 1). - * - `ScalarIdentity`: Sets a random positive scalar value. - * - `Diagonal`: Sets random positive values on the diagonal. - * - `Plain`: Generates A = R^T * R + eps*I where R is random, ensuring PD. - * - * @throws std::invalid_argument If the block type is `Zero`, which cannot be made PD. - */ - void setRandomPD() - { - switch (type()) + /** + * @brief Returns a const pointer to the data this blocks points to. + */ + const Scalar * data() const { - case MatrixBlockType::Zero: { - throw std::invalid_argument("Cannot create a positive definite matrix from a Zero block"); - } - case MatrixBlockType::Identity: - return; - case MatrixBlockType::ScalarIdentity: { - // Set a random positive scalar (between 0.1 and 1.1 to avoid near-zero values) - container().fill(Scalar(0.1) + std::abs(container().Random(1, 1)(0, 0))); - break; - } - case MatrixBlockType::Diagonal: { - // Set random positive values on the diagonal (between 0.1 and 1.1) - container().setRandom(); - container() = container().cwiseAbs().array() + Scalar(0.1); - break; - } - case MatrixBlockType::Plain: { - // Generate PD matrix as A = R^T * R + eps*I - // This guarantees positive definiteness - container().setRandom(); - Matrix R = container(); - container().noalias() = R.transpose() * R; - container().diagonal().array() += Scalar(0.1); // Ensure strict positive definiteness - break; - } - case MatrixBlockType::NestedBlockDiagonal: { - for (auto & sub_block : derived().nested_blocks()) - sub_block.setRandomPD(); - break; - } - default: - PINOCCHIO_UNREACHABLE(); + return derived().data(); } - } - /// \brief Returns true if any coefficient (element) of this blocl element is NaN - /// (Not‑a‑Number). - bool hasNaN() const - { - switch (type()) + /** + * @brief Returns a pointer to the data this blocks points to. + */ + Scalar * data() { - case MatrixBlockType::Zero: - case MatrixBlockType::Identity: - return false; - case MatrixBlockType::ScalarIdentity: - case MatrixBlockType::Diagonal: - case MatrixBlockType::Plain: - return container().hasNaN(); - case MatrixBlockType::NestedBlockDiagonal: - for (const auto & sub_block : derived().nested_blocks()) - if (sub_block.hasNaN()) - return true; - return false; - default: - PINOCCHIO_UNREACHABLE(); + return derived().data(); } - return true; - } - /** - * @brief Computes the inverse of this block and stores it in a pre-allocated result block. - * - * @tparam Other The derived type of the destination block element. - * - * @param[out] res A pre-allocated block element to store the inverse. Its `size` must - * match this block's `size`, and its `type` must be compatible with the - * inverse operation for this block's type. - * - * @details This method computes the matrix inverse based on the block's structural type, - * exploiting the structure for efficiency: - * - `Zero`: The inverse is undefined; fills `res` with infinity values. - * Requires `res.type() == Plain`. - * - `Identity`: The inverse is the identity matrix itself. - * Requires `res.type() == Identity`. - * - `ScalarIdentity`: The inverse is `1/scalar * I`. - * Requires `res.type()` to be `ScalarIdentity`, `Diagonal`, or - * `Plain`. - * - `Diagonal`: The inverse is the element-wise reciprocal of the diagonal. - * Requires `res.type()` to be `Diagonal` or `Plain`. - * - `Plain`: Computes the full dense matrix inverse. - * Requires `res.type() == Plain`. - * - * @pre `res.size() == size()` - * @pre `res.type()` must be compatible with this block's type (see details above). - * - * @note For performance-critical code, prefer this in-place version over the allocating - * `inverse()` overload to avoid repeated memory allocations. - * - * @see inverse() const for the allocating version. - */ - template - void inverse(MatrixBlockElementPlain & res) const - { - assert(res.size() == size()); - - switch (type()) + /** + * @brief Returns the type of this block (Zero, Identity, ScalarIdentity, Diagonal or Plain). + */ + MatrixBlockType type() const { - case MatrixBlockType::Zero: { - assert((res.type() == MatrixBlockType::Plain) && "res block type is invalid"); - typedef typename Other::Scalar OtherScalar; - res.container().fill(std::numeric_limits::infinity()); - break; - } - case MatrixBlockType::Identity: { - assert((res.type() == MatrixBlockType::Identity) && "res block type is invalid"); - break; + return m_type; } - case MatrixBlockType::ScalarIdentity: { - constexpr MatrixBlockType res_valid_block_types = static_cast( - static_cast(MatrixBlockType::ScalarIdentity) - | static_cast(MatrixBlockType::Diagonal) - | static_cast(MatrixBlockType::Plain)); - assert(hasFlag(res.type(), res_valid_block_types) && "res block type is invalid"); - PINOCCHIO_ONLY_USED_FOR_DEBUG(res_valid_block_types); - - const auto & value = container().coeffRef(0, 0); - const auto inverse_value = Scalar(1) / value; - switch (res.type()) + + /** + * @brief Returns the size of this square block (size = rows and size = cols). + */ + Eigen::Index size() const + { + return m_size; + } + + /** + * @brief Applies an assignment operation to a matrix based on this block's type. + * + * @tparam AssignOp The assignment operation functor (e.g., `internal::assign_op`, + * `internal::add_assign_op`, `internal::sub_assign_op`). + * @tparam Matrix The Eigen matrix type to assign to. + * + * @param[in,out] _matrix The destination matrix expression to modify. + * + * @details This method dispatches on the block's `type` to efficiently apply the + * appropriate assignment operation: + * - `Zero`: Applies AssignOp with a zero matrix. + * - `Identity`: Applies AssignOp with an identity matrix. + * - `ScalarIdentity`: Applies AssignOp with a scaled identity matrix. + * - `Diagonal`: Applies AssignOp with the diagonal coefficients. + * - `Plain`: Applies AssignOp with the full dense block data. + * + * @note This is a low-level method used internally by `evalTo`, `addTo`, and `subTo`. + */ + template + void assign_op(const Eigen::MatrixBase & _matrix) const + { + auto & matrix = _matrix.const_cast_derived(); + + switch (type()) { + case MatrixBlockType::Zero: { + AssignOp::run(Matrix::Zero(size(), size()), matrix); + break; + } + case MatrixBlockType::Identity: { + AssignOp::run(Matrix::Identity(size(), size()), matrix); + break; + } case MatrixBlockType::ScalarIdentity: { - res.container().fill(inverse_value); + const auto & scalar = container()(0, 0); + AssignOp::run(scalar * Matrix::Identity(size(), size()), matrix); break; } case MatrixBlockType::Diagonal: { - res.container().fill(inverse_value); + AssignOp::run(container().asDiagonal(), matrix); break; } case MatrixBlockType::Plain: { - res.container().setZero(); - res.container().diagonal().fill(inverse_value); + AssignOp::run(container(), matrix); + break; + } + case MatrixBlockType::NestedBlockDiagonal: { + // Use Map with OuterStride to break the recursive template instantiation chain: + // matrix.block() yields Block which causes Block> nesting + // in recursive assign_op calls, exceeding the template depth limit. + // Map> is a fixed concrete type, + // so assign_op is always called with the same instantiation (no type explosion). + const Eigen::Index outer_stride = matrix.outerStride(); + Scalar * const base_ptr = matrix.derived().data(); + Eigen::Index sub_row_id = 0; + for (const auto & sub_block : derived().nested_blocks()) + { + const auto sub_size = sub_block.size(); + Eigen::Map< + Eigen::Matrix, Eigen::Unaligned, + Eigen::OuterStride> + sub_map( + // base ptr + col ptr + row ptr + base_ptr + sub_row_id + sub_row_id * outer_stride, sub_size, sub_size, + Eigen::OuterStride(outer_stride)); + sub_block.template assign_op(sub_map); + sub_row_id += sub_size; + } break; } default: PINOCCHIO_UNREACHABLE(); } - break; } - case MatrixBlockType::Diagonal: { - constexpr MatrixBlockType res_valid_block_types = static_cast( - static_cast(MatrixBlockType::Diagonal) - | static_cast(MatrixBlockType::Plain)); - PINOCCHIO_ONLY_USED_FOR_DEBUG(res_valid_block_types); - assert(hasFlag(res.type(), res_valid_block_types) && "res block type is invalid"); + /** + * @brief Evaluates this block into a dense matrix expression. + * + * @tparam Matrix The Eigen matrix type to evaluate into. + * + * @param[out] _matrix The destination matrix expression to be filled. Must be a square + * matrix with dimensions equal to this block's `size`. + * + * @details This method first zeroes out the destination matrix, then applies the + * appropriate assignment based on the block's type using `assign_op`. + * This is the primary method for converting a block descriptor into its + * dense matrix representation. + * + * @throws std::invalid_argument If the matrix dimensions do not match the block size. + * + * @see assign_op() + */ + template + void evalTo(const Eigen::MatrixBase & _matrix) const + { + PINOCCHIO_CHECK_ARGUMENT_SIZE( + _matrix.rows(), size(), "The input matrix has not the right number of rows."); + PINOCCHIO_CHECK_ARGUMENT_SIZE( + _matrix.cols(), size(), "The input matrix has not the right number of columns."); + + _matrix.const_cast_derived().setZero(); + assign_op(_matrix.const_cast_derived()); + } + + /** + * @brief Adds this block's content to an existing matrix expression. + * + * @tparam Matrix The Eigen matrix type to add to. + * + * @param[in,out] _matrix The destination matrix expression to modify. Must be a square + * matrix with dimensions equal to this block's `size`. + * + * @details Unlike `evalTo`, this method does not zero out the destination first. + * It performs an additive assignment, equivalent to `_matrix += this_block`. + * + * @throws std::invalid_argument If the matrix dimensions do not match the block size. + * + * @see assign_op(), evalTo(), subTo() + */ + template + void addTo(const Eigen::MatrixBase & _matrix) const + { + PINOCCHIO_CHECK_ARGUMENT_SIZE( + _matrix.rows(), size(), "The input matrix has not the right number of rows."); + PINOCCHIO_CHECK_ARGUMENT_SIZE( + _matrix.cols(), size(), "The input matrix has not the right number of columns."); + + assign_op(_matrix.const_cast_derived()); + } + + /** + * @brief Subtracts this block from a matrix based on the block's structural type. + * + * @tparam Matrix The Eigen matrix type to subtract from. + * + * @param[in,out] _matrix The destination matrix expression to modify. Its contents will + * be updated as: `_matrix -= this_block`. + * + * @details This method dispatches on the block's `type` to efficiently subtract the + * appropriate matrix representation: + * - `Zero`: No operation (subtracting zero). + * - `Identity`: Subtracts an identity matrix. + * - `ScalarIdentity`: Subtracts a scaled identity matrix. + * - `Diagonal`: Subtracts the diagonal coefficients. + * - `Plain`: Subtracts the full dense block data. + * + * @pre The input matrix must have dimensions equal to `size() x size()`. + * + * @throws std::invalid_argument (via PINOCCHIO_CHECK_ARGUMENT_SIZE) if the matrix + * dimensions do not match the block size. + * + * @see evalTo(), addTo() + */ + template + void subTo(const Eigen::MatrixBase & _matrix) const + { + PINOCCHIO_CHECK_ARGUMENT_SIZE( + _matrix.rows(), size(), "The input matrix has not the right number of rows."); + PINOCCHIO_CHECK_ARGUMENT_SIZE( + _matrix.cols(), size(), "The input matrix has not the right number of columns."); + + assign_op(_matrix.const_cast_derived()); + } + + /** + * @brief Evaluates this block element into a dense matrix expression. + * + * @tparam Matrix The Eigen matrix type of the destination expression. + * + * @param[out] _matrix A pre-allocated, matrix-like Eigen expression that will be + * filled with the block's contents. It **must** have dimensions + * equal to `size() x size()`. + * + * @details This is a convenience wrapper around `evalTo()` that provides a more + * intuitive interface for extracting the full matrix representation of this block. + * + * @see evalTo() for the underlying implementation. + */ + template + void matrix(const Eigen::MatrixBase & _matrix) const + { + evalTo(_matrix.const_cast_derived()); + } + + /** + * @brief Converts this block descriptor into a dense matrix representation. + * + * @return A newly allocated dense matrix of dimensions `size x size` containing + * the full matrix representation of this block. + * + * @details This is a convenience method that allocates a new matrix and calls + * `evalTo()` to fill it based on the block's type. The resulting matrix + * will be: + * - All zeros for `MatrixBlockType::Zero` + * - Identity matrix for `MatrixBlockType::Identity` + * - Scaled identity for `MatrixBlockType::ScalarIdentity` + * - Diagonal matrix for `MatrixBlockType::Diagonal` + * - Full dense copy for `MatrixBlockType::Plain` + * + * @note For performance-critical code where repeated memory allocations should + * be avoided, prefer the overload `matrix(const Eigen::MatrixBase&)` + * that fills a pre-allocated matrix. + * + * @see evalTo(), matrix(const Eigen::MatrixBase&) const + */ + Matrix matrix() const + { + Matrix res(size(), size()); + matrix(res); + return res; + } + + /** + * @brief Fills a pre-allocated vector expression with the diagonal elements of this block. + * + * @tparam DiagonalSlice The Eigen type of the destination vector, slice, or expression. + * + * @param[out] _diagonal_slice A pre-allocated, vector-like Eigen expression that will be + * filled with the diagonal elements. It **must** have a size + * equal to this block's `size`. + * + * @details This is the core, high-performance method for extracting a diagonal. It performs + * no memory allocation and writes the result directly into the provided destination. + * + * The behavior depends on the block's `type`: + * - `Zero`: Fills the destination with zeros. + * - `Identity`: Fills the destination with ones. + * - `ScalarIdentity`: Fills the destination with the block's scalar value. + * - `Diagonal`: Copies the vector of diagonal coefficients. + * - `Plain`: Extracts the diagonal from the dense block's data. + */ + template + void diagonal(const Eigen::MatrixBase & _diagonal_slice) const + { + auto & diagonal_slice = _diagonal_slice.const_cast_derived(); + assert(diagonal_slice.size() == size()); - switch (res.type()) + switch (type()) { + case MatrixBlockType::Zero: { + diagonal_slice.setZero(); + break; + } + case MatrixBlockType::Identity: { + diagonal_slice.setOnes(); + break; + } + case MatrixBlockType::ScalarIdentity: { + + const auto & scalar_value = container()(0, 0); + diagonal_slice.fill(scalar_value); + break; + } case MatrixBlockType::Diagonal: { - res.container() = container().cwiseInverse(); + diagonal_slice = container(); break; } case MatrixBlockType::Plain: { - res.container().setZero(); - res.container().diagonal() = container().cwiseInverse(); + diagonal_slice = container().diagonal(); + break; + } + case MatrixBlockType::NestedBlockDiagonal: { + // Use Map from raw pointer to break the recursive template instantiation + // chain. Calling .segment() would yield VectorBlock, causing + // VectorBlock> nesting on recursive calls, exceeding template depth. + // Map is a fixed concrete type: always the same instantiation (no explosion). + Scalar * const base_ptr = diagonal_slice.derived().data(); + Eigen::Index sub_offset = 0; + for (const auto & sub_block : derived().nested_blocks()) + { + const auto sub_size = sub_block.size(); + Eigen::Map> sub_map( + base_ptr + sub_offset, sub_size); + sub_block.diagonal(sub_map); + sub_offset += sub_size; + } break; } default: PINOCCHIO_UNREACHABLE(); } + } - break; + /** + * @brief Extracts the main diagonal of this block into a new dense vector. + * + * @return A new dense column vector of size `size` containing the diagonal elements. + * + * @details This is a convenience method that allocates a new vector and calls the in-place + * `diagonal()` overload to fill it. + * + * @note For performance-critical code where repeated memory allocations should be avoided, + * prefer the overload that fills a pre-allocated slice. + * @see void diagonal(const Eigen::MatrixBase&) const + */ + Vector diagonal() const + { + Vector diagonal_elements(size()); + diagonal(diagonal_elements); + return diagonal_elements; } - case MatrixBlockType::Plain: { - assert((res.type() == MatrixBlockType::Plain) && "res block type is invalid"); - if (isSymmetric(container())) + + /** + * @brief Fill this block with random values between -1 and 1. + */ + void setRandom() + { + switch (type()) { - typedef Eigen::Map MapMatrix; - MapMatrix tmp = - MapMatrix(PINOCCHIO_EIGEN_MAP_ALLOCA(Scalar, container().rows(), container().cols())); - tmp = container(); - ::pinocchio::matrix_inversion(tmp, res.container()); + case MatrixBlockType::Zero: + case MatrixBlockType::Identity: + return; + case MatrixBlockType::ScalarIdentity: + case MatrixBlockType::Diagonal: + case MatrixBlockType::Plain: + container().setRandom(); + break; + case MatrixBlockType::NestedBlockDiagonal: + for (auto & sub_block : derived().nested_blocks()) + sub_block.setRandom(); + break; + default: + PINOCCHIO_UNREACHABLE(); } - else + } + + /** + * @brief Fill this block with random values forming a Positive Definite matrix. + * + * @details For a matrix to be positive definite, all eigenvalues must be strictly positive. + * The implementation strategy depends on the block type: + * - `Zero`: Invalid operation - a zero matrix is not positive definite. + * - `Identity`: Already positive definite (all eigenvalues are 1). + * - `ScalarIdentity`: Sets a random positive scalar value. + * - `Diagonal`: Sets random positive values on the diagonal. + * - `Plain`: Generates A = R^T * R + eps*I where R is random, ensuring PD. + * + * @throws std::invalid_argument If the block type is `Zero`, which cannot be made PD. + */ + void setRandomPD() + { + switch (type()) { - res.container().noalias() = container().inverse(); + case MatrixBlockType::Zero: { + throw std::invalid_argument("Cannot create a positive definite matrix from a Zero block"); + } + case MatrixBlockType::Identity: + return; + case MatrixBlockType::ScalarIdentity: { + // Set a random positive scalar (between 0.1 and 1.1 to avoid near-zero values) + container().fill(Scalar(0.1) + std::abs(container().Random(1, 1)(0, 0))); + break; + } + case MatrixBlockType::Diagonal: { + // Set random positive values on the diagonal (between 0.1 and 1.1) + container().setRandom(); + container() = container().cwiseAbs().array() + Scalar(0.1); + break; + } + case MatrixBlockType::Plain: { + // Generate PD matrix as A = R^T * R + eps*I + // This guarantees positive definiteness + container().setRandom(); + Matrix R = container(); + container().noalias() = R.transpose() * R; + container().diagonal().array() += Scalar(0.1); // Ensure strict positive definiteness + break; + } + case MatrixBlockType::NestedBlockDiagonal: { + for (auto & sub_block : derived().nested_blocks()) + sub_block.setRandomPD(); + break; + } + default: + PINOCCHIO_UNREACHABLE(); } - break; - } - case MatrixBlockType::NestedBlockDiagonal: { - assert(res.type() == MatrixBlockType::NestedBlockDiagonal && "res block type is invalid"); - const auto & src_subs = derived().nested_blocks(); - auto & res_subs = res.derived().nested_blocks(); - assert(src_subs.size() == res_subs.size()); - for (std::size_t i = 0; i < src_subs.size(); ++i) - src_subs[i].inverse(res_subs[i]); - break; - } - default: - PINOCCHIO_UNREACHABLE(); } - } - - /** - * @brief Computes the inverse of this block and returns it as a new PlainBlockElement. - * - * @return A newly allocated `PlainBlockElement` containing the matrix inverse of this block. - * - * @details This is a convenience method that allocates a new block element and calls the - * in-place `inverse()` overload to compute the result. The returned block's type - * is determined by the input block's type to preserve structural properties where - * possible: - * - `Zero` → `Plain` (filled with infinity, as inverse of zero is undefined) - * - `Identity` → `Identity` (inverse of identity is identity) - * - `ScalarIdentity` → `ScalarIdentity` (inverse of scalar `s` is `1/s`) - * - `Diagonal` → `Diagonal` (element-wise inverse of diagonal) - * - `Plain` → `Plain` (full matrix inverse) - * - * @note For performance-critical code where repeated memory allocations should be avoided, - * prefer the overload `inverse(MatrixBlockElementPlain&)` that fills a - * pre-allocated block. - * - * @see void inverse(MatrixBlockElementPlain&) const - */ - PlainBlockElement inverse() const - { - MatrixBlockType res_type = MatrixBlockType::Undefined; - switch (type()) + /// \brief Returns true if any coefficient (element) of this blocl element is NaN + /// (Not‑a‑Number). + bool hasNaN() const { - case MatrixBlockType::Zero: { - res_type = MatrixBlockType::Plain; - break; - } - case MatrixBlockType::Identity: { - res_type = MatrixBlockType::Identity; - break; - } - case MatrixBlockType::ScalarIdentity: { - res_type = MatrixBlockType::ScalarIdentity; - break; - } - case MatrixBlockType::Diagonal: { - res_type = MatrixBlockType::Diagonal; - break; - } - case MatrixBlockType::Plain: { - res_type = MatrixBlockType::Plain; - break; - } - case MatrixBlockType::NestedBlockDiagonal: { - // For NestedBlockDiagonal, we can't return a PlainBlockElement that contains nested - // blocks (PlainBlockElement = owning Matrix variant doesn't support nested blocks). - // This path should not be called — use the in-place inverse() with a matching res block. - assert( - false && "NestedBlockDiagonal inverse() factory not supported; use in-place version"); - PINOCCHIO_THROW_PRETTY( - std::runtime_error, - "Calling unsupported inverse() on NestedBlockDiagonal; use in-place version instead.") - res_type = MatrixBlockType::NestedBlockDiagonal; - break; + switch (type()) + { + case MatrixBlockType::Zero: + case MatrixBlockType::Identity: + return false; + case MatrixBlockType::ScalarIdentity: + case MatrixBlockType::Diagonal: + case MatrixBlockType::Plain: + return container().hasNaN(); + case MatrixBlockType::NestedBlockDiagonal: + for (const auto & sub_block : derived().nested_blocks()) + if (sub_block.hasNaN()) + return true; + return false; + default: + PINOCCHIO_UNREACHABLE(); + } + return true; } - default: - PINOCCHIO_UNREACHABLE(); + + /** + * @brief Computes the inverse of this block and stores it in a pre-allocated result block. + * + * @tparam Other The derived type of the destination block element. + * + * @param[out] res A pre-allocated block element to store the inverse. Its `size` must + * match this block's `size`, and its `type` must be compatible with the + * inverse operation for this block's type. + * + * @details This method computes the matrix inverse based on the block's structural type, + * exploiting the structure for efficiency: + * - `Zero`: The inverse is undefined; fills `res` with infinity values. + * Requires `res.type() == Plain`. + * - `Identity`: The inverse is the identity matrix itself. + * Requires `res.type() == Identity`. + * - `ScalarIdentity`: The inverse is `1/scalar * I`. + * Requires `res.type()` to be `ScalarIdentity`, `Diagonal`, or + * `Plain`. + * - `Diagonal`: The inverse is the element-wise reciprocal of the diagonal. + * Requires `res.type()` to be `Diagonal` or `Plain`. + * - `Plain`: Computes the full dense matrix inverse. + * Requires `res.type() == Plain`. + * + * @pre `res.size() == size()` + * @pre `res.type()` must be compatible with this block's type (see details above). + * + * @note For performance-critical code, prefer this in-place version over the allocating + * `inverse()` overload to avoid repeated memory allocations. + * + * @see inverse() const for the allocating version. + */ + template + void inverse(MatrixBlockElementPlain & res) const + { + assert(res.size() == size()); + + switch (type()) + { + case MatrixBlockType::Zero: { + assert((res.type() == MatrixBlockType::Plain) && "res block type is invalid"); + typedef typename Other::Scalar OtherScalar; + res.container().fill(std::numeric_limits::infinity()); + break; + } + case MatrixBlockType::Identity: { + assert((res.type() == MatrixBlockType::Identity) && "res block type is invalid"); + break; + } + case MatrixBlockType::ScalarIdentity: { + constexpr MatrixBlockType res_valid_block_types = static_cast( + static_cast(MatrixBlockType::ScalarIdentity) + | static_cast(MatrixBlockType::Diagonal) + | static_cast(MatrixBlockType::Plain)); + assert(hasFlag(res.type(), res_valid_block_types) && "res block type is invalid"); + PINOCCHIO_ONLY_USED_FOR_DEBUG(res_valid_block_types); + + const auto & value = container().coeffRef(0, 0); + const auto inverse_value = Scalar(1) / value; + switch (res.type()) + { + case MatrixBlockType::ScalarIdentity: { + res.container().fill(inverse_value); + break; + } + case MatrixBlockType::Diagonal: { + res.container().fill(inverse_value); + break; + } + case MatrixBlockType::Plain: { + res.container().setZero(); + res.container().diagonal().fill(inverse_value); + break; + } + default: + PINOCCHIO_UNREACHABLE(); + } + break; + } + case MatrixBlockType::Diagonal: { + constexpr MatrixBlockType res_valid_block_types = static_cast( + static_cast(MatrixBlockType::Diagonal) + | static_cast(MatrixBlockType::Plain)); + PINOCCHIO_ONLY_USED_FOR_DEBUG(res_valid_block_types); + + assert(hasFlag(res.type(), res_valid_block_types) && "res block type is invalid"); + + switch (res.type()) + { + case MatrixBlockType::Diagonal: { + res.container() = container().cwiseInverse(); + break; + } + case MatrixBlockType::Plain: { + res.container().setZero(); + res.container().diagonal() = container().cwiseInverse(); + break; + } + default: + PINOCCHIO_UNREACHABLE(); + } + + break; + } + case MatrixBlockType::Plain: { + assert((res.type() == MatrixBlockType::Plain) && "res block type is invalid"); + if (isSymmetric(container())) + { + typedef Eigen::Map MapMatrix; + MapMatrix tmp = + MapMatrix(PINOCCHIO_EIGEN_MAP_ALLOCA(Scalar, container().rows(), container().cols())); + tmp = container(); + matrix_inversion(tmp, res.container()); + } + else + { + res.container().noalias() = container().inverse(); + } + break; + } + case MatrixBlockType::NestedBlockDiagonal: { + assert(res.type() == MatrixBlockType::NestedBlockDiagonal && "res block type is invalid"); + const auto & src_subs = derived().nested_blocks(); + auto & res_subs = res.derived().nested_blocks(); + assert(src_subs.size() == res_subs.size()); + for (std::size_t i = 0; i < src_subs.size(); ++i) + src_subs[i].inverse(res_subs[i]); + break; + } + default: + PINOCCHIO_UNREACHABLE(); + } } - PlainBlockElement res(res_type, size()); - inverse(res); - return res; - } + /** + * @brief Computes the inverse of this block and returns it as a new PlainBlockElement. + * + * @return A newly allocated `PlainBlockElement` containing the matrix inverse of this block. + * + * @details This is a convenience method that allocates a new block element and calls the + * in-place `inverse()` overload to compute the result. The returned block's type + * is determined by the input block's type to preserve structural properties where + * possible: + * - `Zero` → `Plain` (filled with infinity, as inverse of zero is undefined) + * - `Identity` → `Identity` (inverse of identity is identity) + * - `ScalarIdentity` → `ScalarIdentity` (inverse of scalar `s` is `1/s`) + * - `Diagonal` → `Diagonal` (element-wise inverse of diagonal) + * - `Plain` → `Plain` (full matrix inverse) + * + * @note For performance-critical code where repeated memory allocations should be avoided, + * prefer the overload `inverse(MatrixBlockElementPlain&)` that fills a + * pre-allocated block. + * + * @see void inverse(MatrixBlockElementPlain&) const + */ + PlainBlockElement inverse() const + { + MatrixBlockType res_type = MatrixBlockType::Undefined; + + switch (type()) + { + case MatrixBlockType::Zero: { + res_type = MatrixBlockType::Plain; + break; + } + case MatrixBlockType::Identity: { + res_type = MatrixBlockType::Identity; + break; + } + case MatrixBlockType::ScalarIdentity: { + res_type = MatrixBlockType::ScalarIdentity; + break; + } + case MatrixBlockType::Diagonal: { + res_type = MatrixBlockType::Diagonal; + break; + } + case MatrixBlockType::Plain: { + res_type = MatrixBlockType::Plain; + break; + } + case MatrixBlockType::NestedBlockDiagonal: { + // For NestedBlockDiagonal, we can't return a PlainBlockElement that contains nested + // blocks (PlainBlockElement = owning Matrix variant doesn't support nested blocks). + // This path should not be called — use the in-place inverse() with a matching res block. + assert( + false && "NestedBlockDiagonal inverse() factory not supported; use in-place version"); + PINOCCHIO_THROW_PRETTY( + std::runtime_error, + "Calling unsupported inverse() on NestedBlockDiagonal; use in-place version instead.") + res_type = MatrixBlockType::NestedBlockDiagonal; + break; + } + default: + PINOCCHIO_UNREACHABLE(); + } + + PlainBlockElement res(res_type, size()); + inverse(res); + return res; + } - protected: - /// @brief The structural type of the matrix block (e.g., Zero, Diagonal, Plain). - MatrixBlockType m_type; + protected: + /// @brief The structural type of the matrix block (e.g., Zero, Diagonal, Plain). + MatrixBlockType m_type; - /// @brief The size of the block (assuming a square block of size x size). - Eigen::Index m_size; + /// @brief The size of the block (assuming a square block of size x size). + Eigen::Index m_size; - }; // struct MatrixBlockElementPlain + }; // struct MatrixBlockElementPlain + } // namespace internal } // namespace pinocchio diff --git a/include/pinocchio/src/math/matrix-block-element.hxx b/include/pinocchio/src/math/matrix-block-element.hxx index 0d90f2f0cb..513815ff4c 100644 --- a/include/pinocchio/src/math/matrix-block-element.hxx +++ b/include/pinocchio/src/math/matrix-block-element.hxx @@ -13,50 +13,50 @@ namespace pinocchio { + namespace internal + { - // Forward declaration - template - struct MatrixBlockElementTpl; + // Forward declaration + template + struct MatrixBlockElementTpl; - template - struct traits, std::enable_if_t>> - { - typedef typename MapType::PlainObject Matrix; - typedef typename Matrix::Scalar Scalar; - static constexpr auto Options = Matrix::Options; - typedef MapType MatrixContainer; - typedef Eigen::Matrix Vector; - typedef MatrixBlockElementTpl PlainBlockElement; - }; - - template - struct traits< - MatrixBlockElementTpl, - std::enable_if_t>> - { - typedef MatrixType Matrix; - typedef typename Matrix::Scalar Scalar; - static constexpr auto Options = Matrix::Options; - typedef MatrixType MatrixContainer; - typedef Eigen::Matrix Vector; - typedef MatrixBlockElementTpl PlainBlockElement; - }; - - /// @brief Returns the sum of the size of each individual block. - template - int getSumOfBlockSizes( - const std::vector> blocks_vector) - { - int size = 0; - for (const auto & block : blocks_vector) + template + struct traits, std::enable_if_t>> + { + typedef typename MapType::PlainObject Matrix; + typedef typename Matrix::Scalar Scalar; + static constexpr auto Options = Matrix::Options; + typedef MapType MatrixContainer; + typedef Eigen::Matrix Vector; + typedef MatrixBlockElementTpl PlainBlockElement; + }; + + template + struct traits< + MatrixBlockElementTpl, + std::enable_if_t>> { - size += static_cast(block.size()); + typedef MatrixType Matrix; + typedef typename Matrix::Scalar Scalar; + static constexpr auto Options = Matrix::Options; + typedef MatrixType MatrixContainer; + typedef Eigen::Matrix Vector; + typedef MatrixBlockElementTpl PlainBlockElement; + }; + + /// @brief Returns the sum of the size of each individual block. + template + int + getSumOfBlockSizes(const std::vector> blocks_vector) + { + int size = 0; + for (const auto & block : blocks_vector) + { + size += static_cast(block.size()); + } + return size; } - return size; - } - namespace internal - { /// @brief Computes the sum of sizes of a vector of MatrixBlockElementTpl objects. template static Eigen::Index getSumOfNestedSizes(const std::vector & nested_blocks) @@ -66,511 +66,517 @@ namespace pinocchio total += sub.size(); return total; } - } // namespace internal - - /** - * @ingroup pinocchio_math - * @brief A descriptor for a non-owning matrix block, implemented as a view using `Eigen::Map`. - * - * @tparam MapType The container type for the block. This specialization is enabled only when - * `MapType` is an `Eigen::Map` type. - * - * @details This struct is a template specialization that describes a single block within a - * larger structured matrix. Its defining feature is that it does **not own** any - * memory for the matrix coefficients. Instead, it uses an `Eigen::Map` (`map` member) - * to provide a non-owning, mutable view into a region of memory that is managed - * externally (e.g., by a `MatrixStack` in a `BlockDiagonalMatrixTpl`). - * - * It inherits metadata like `type` and `size` from the `MatrixBlockElementPlain` class. - * - * @note The copy constructor and copy-assignment operator perform a **shallow copy**. This means - * that both the original and the copied object will have their `map` members pointing - * to the **exact same underlying memory buffer**. - */ - template - struct MatrixBlockElementTpl>> - : MatrixBlockElementPlain> - { - /// @brief The type of this specialized class. - typedef MatrixBlockElementTpl Self; - /// @brief The scalar type of the matrix elements (e.g., double). - typedef typename traits::Scalar Scalar; - /// @brief The non-owning container type, which is an `Eigen::Map`. - typedef typename traits::MatrixContainer MatrixMap; - /// @brief The equivalent dense Eigen matrix type. - typedef typename traits::Matrix Matrix; - /// @brief The equivalent dense Eigen vector type. - typedef typename traits::Vector Vector; - - /// @brief The base class from which this class inherits. - typedef MatrixBlockElementPlain Base; - - using Base::size; - using Base::type; - using Base::operator=; - - /// @brief An Eigen::Map that provides a non-owning view of the memory corresponding to this - /// block. - /// @note For block types that don't store data (e.g., `Identity`, `Zero`, - /// `NestedBlockDiagonal`), this map will be null. - MatrixMap map; - - protected: - /// @brief Sub-blocks for NestedBlockDiagonal type. Empty for all other types. - std::vector m_nested_blocks; - - public: - /// @brief Default constructor. Initializes to an invalid state (Undefined type, size -1, null - /// map). - MatrixBlockElementTpl() - : Base() - , map(nullptr, 0, 0) - { - } /** - * @brief Constructs a block descriptor for types that do not require external data. - * @details This is used for structural blocks like `Identity` or `Zero`. The internal map - * remains null. - * @param[in] type The structural type of the block. - * @param[in] size The dimension of the (square) block. + * @ingroup pinocchio_math + * @brief A descriptor for a non-owning matrix block, implemented as a view using `Eigen::Map`. + * + * @tparam MapType The container type for the block. This specialization is enabled only when + * `MapType` is an `Eigen::Map` type. + * + * @details This struct is a template specialization that describes a single block within a + * larger structured matrix. Its defining feature is that it does **not own** any + * memory for the matrix coefficients. Instead, it uses an `Eigen::Map` (`map` member) + * to provide a non-owning, mutable view into a region of memory that is managed + * externally (e.g., by a `MatrixStack` in a `BlockDiagonalMatrixTpl`). + * + * It inherits metadata like `type` and `size` from the `MatrixBlockElementPlain` + * class. + * + * @note The copy constructor and copy-assignment operator perform a **shallow copy**. This + * means that both the original and the copied object will have their `map` members pointing to + * the **exact same underlying memory buffer**. */ - MatrixBlockElementTpl(const MatrixBlockType type, const Eigen::Index size) - : Base(type, size) - , map(nullptr, 0, 0) + template + struct MatrixBlockElementTpl>> + : MatrixBlockElementPlain> { - } + /// @brief The type of this specialized class. + typedef MatrixBlockElementTpl Self; + /// @brief The scalar type of the matrix elements (e.g., double). + typedef typename traits::Scalar Scalar; + /// @brief The non-owning container type, which is an `Eigen::Map`. + typedef typename traits::MatrixContainer MatrixMap; + /// @brief The equivalent dense Eigen matrix type. + typedef typename traits::Matrix Matrix; + /// @brief The equivalent dense Eigen vector type. + typedef typename traits::Vector Vector; + + /// @brief The base class from which this class inherits. + typedef MatrixBlockElementPlain Base; + + using Base::size; + using Base::type; + using Base::operator=; + + /// @brief An Eigen::Map that provides a non-owning view of the memory corresponding to this + /// block. + /// @note For block types that don't store data (e.g., `Identity`, `Zero`, + /// `NestedBlockDiagonal`), this map will be null. + MatrixMap map; + + protected: + /// @brief Sub-blocks for NestedBlockDiagonal type. Empty for all other types. + std::vector m_nested_blocks; + + public: + /// @brief Default constructor. Initializes to an invalid state (Undefined type, size -1, null + /// map). + MatrixBlockElementTpl() + : Base() + , map(nullptr, 0, 0) + { + } - /** - * @brief Constructs a block descriptor for types that map to existing data. - * @details This is used for data-backed blocks like `Plain`, `Diagonal`, or `ScalarIdentity`. - * @param[in] type The structural type of the block. - * @param[in] size The dimension of the (square) block. - * @param[in] matrix_map An Eigen::Map providing a non-owning view of the block's data. - */ - MatrixBlockElementTpl( - const MatrixBlockType type, const Eigen::Index size, const MatrixMap matrix_map) - : Base(type, size) - , map(matrix_map) - { - } + /** + * @brief Constructs a block descriptor for types that do not require external data. + * @details This is used for structural blocks like `Identity` or `Zero`. The internal map + * remains null. + * @param[in] type The structural type of the block. + * @param[in] size The dimension of the (square) block. + */ + MatrixBlockElementTpl(const MatrixBlockType type, const Eigen::Index size) + : Base(type, size) + , map(nullptr, 0, 0) + { + } - /** - * @brief Constructs a NestedBlockDiagonal block from a list of sub-blocks. - * @param[in] nested_blocks The sub-block descriptors (type/size populated; maps may be null - * at construction time and get remapped later by BlockDiagonalMatrixTpl::rebuild). - */ - MatrixBlockElementTpl( - const MatrixBlockType type, std::vector nested_blocks) - : Base(type, internal::getSumOfNestedSizes(nested_blocks)) - , map(nullptr, 0, 0) - , m_nested_blocks(std::move(nested_blocks)) - { - assert(type == MatrixBlockType::NestedBlockDiagonal); - } + /** + * @brief Constructs a block descriptor for types that map to existing data. + * @details This is used for data-backed blocks like `Plain`, `Diagonal`, or `ScalarIdentity`. + * @param[in] type The structural type of the block. + * @param[in] size The dimension of the (square) block. + * @param[in] matrix_map An Eigen::Map providing a non-owning view of the block's data. + */ + MatrixBlockElementTpl( + const MatrixBlockType type, const Eigen::Index size, const MatrixMap matrix_map) + : Base(type, size) + , map(matrix_map) + { + } - /** - * @brief Default copy constructor (shallow copy of map, deep copy of nested blocks). - * @details Creates a copy of the block descriptor. The new object's `map` will view the - * **same memory** as the original. `m_nested_blocks` is deep-copied (each nested - * block's map still points to the same underlying memory as the original). - */ - MatrixBlockElementTpl(const MatrixBlockElementTpl & other) - : MatrixBlockElementTpl() - { - *this = other; - } + /** + * @brief Constructs a NestedBlockDiagonal block from a list of sub-blocks. + * @param[in] nested_blocks The sub-block descriptors (type/size populated; maps may be null + * at construction time and get remapped later by BlockDiagonalMatrixTpl::rebuild). + */ + MatrixBlockElementTpl( + const MatrixBlockType type, std::vector nested_blocks) + : Base(type, getSumOfNestedSizes(nested_blocks)) + , map(nullptr, 0, 0) + , m_nested_blocks(std::move(nested_blocks)) + { + assert(type == MatrixBlockType::NestedBlockDiagonal); + } - /** - * @brief Default copy-assignment operator (shallow copy of map, deep copy of nested blocks). - * @details Assigns from another block descriptor. After assignment, this object's `map` - * will view the **same memory** as the other object. - */ - MatrixBlockElementTpl & operator=(const MatrixBlockElementTpl & other) - { - if (this != &other) + /** + * @brief Default copy constructor (shallow copy of map, deep copy of nested blocks). + * @details Creates a copy of the block descriptor. The new object's `map` will view the + * **same memory** as the original. `m_nested_blocks` is deep-copied (each nested + * block's map still points to the same underlying memory as the original). + */ + MatrixBlockElementTpl(const MatrixBlockElementTpl & other) + : MatrixBlockElementTpl() { - Base::operator=(other); - // For Eigen::Map, operator= only works if the copied map - // has the same size than the current map. - // If not, Eigen triggers an assert and fails at runtime. - // Since the map we want to copy is not necessarily the same size - // as the one we have, we recreate the map in place so that it - // points to the same data as the other map. - new (&map) MapType(other.map); - m_nested_blocks = other.m_nested_blocks; + *this = other; } - return *this; - } - /// @brief Returns a mutable reference to the nested sub-blocks (only valid for - /// NestedBlockDiagonal). - std::vector & nested_blocks() - { - assert(type() == MatrixBlockType::NestedBlockDiagonal); - return m_nested_blocks; - } + /** + * @brief Default copy-assignment operator (shallow copy of map, deep copy of nested blocks). + * @details Assigns from another block descriptor. After assignment, this object's `map` + * will view the **same memory** as the other object. + */ + MatrixBlockElementTpl & operator=(const MatrixBlockElementTpl & other) + { + if (this != &other) + { + Base::operator=(other); + // For Eigen::Map, operator= only works if the copied map + // has the same size than the current map. + // If not, Eigen triggers an assert and fails at runtime. + // Since the map we want to copy is not necessarily the same size + // as the one we have, we recreate the map in place so that it + // points to the same data as the other map. + new (&map) MapType(other.map); + m_nested_blocks = other.m_nested_blocks; + } + return *this; + } - /// @brief Returns a const reference to the nested sub-blocks (only valid for - /// NestedBlockDiagonal). - const std::vector & nested_blocks() const - { - assert(type() == MatrixBlockType::NestedBlockDiagonal); - return m_nested_blocks; - } + /// @brief Returns a mutable reference to the nested sub-blocks (only valid for + /// NestedBlockDiagonal). + std::vector & nested_blocks() + { + assert(type() == MatrixBlockType::NestedBlockDiagonal); + return m_nested_blocks; + } - /** - * @brief Checks for strict equality between two block descriptors. - * - * @param[in] other The other block descriptor to compare against. - * @return `true` if the blocks are strictly equal, `false` otherwise. - * - * @details Two block descriptors are considered equal if they have the same `type` and `size`, - * and the data viewed by their internal `map`s is **coefficient-wise equal**. - * - * @note This performs a deep, numerical comparison of the underlying data via - * `Eigen::Map::operator==`, which can be computationally expensive. It does not simply compare - * pointers. - */ - bool operator==(const MatrixBlockElementTpl & other) const - { - if (this == &other) - return true; - if (!Base::operator==(other)) - return false; - if (type() == MatrixBlockType::NestedBlockDiagonal) - return m_nested_blocks == other.m_nested_blocks; - return map == other.map; - } + /// @brief Returns a const reference to the nested sub-blocks (only valid for + /// NestedBlockDiagonal). + const std::vector & nested_blocks() const + { + assert(type() == MatrixBlockType::NestedBlockDiagonal); + return m_nested_blocks; + } - /** - * @brief Checks for inequality between two block descriptors. - * @param[in] other The other block descriptor to compare against. - * @return `true` if the blocks are not strictly equal, `false` otherwise. - * @details This is the logical negation of `operator==`. - * @see operator==() - */ - bool operator!=(const MatrixBlockElementTpl & other) const - { - return !(*this == other); - } + /** + * @brief Checks for strict equality between two block descriptors. + * + * @param[in] other The other block descriptor to compare against. + * @return `true` if the blocks are strictly equal, `false` otherwise. + * + * @details Two block descriptors are considered equal if they have the same `type` and + * `size`, and the data viewed by their internal `map`s is **coefficient-wise equal**. + * + * @note This performs a deep, numerical comparison of the underlying data via + * `Eigen::Map::operator==`, which can be computationally expensive. It does not simply + * compare pointers. + */ + bool operator==(const MatrixBlockElementTpl & other) const + { + if (this == &other) + return true; + if (!Base::operator==(other)) + return false; + if (type() == MatrixBlockType::NestedBlockDiagonal) + return m_nested_blocks == other.m_nested_blocks; + return map == other.map; + } - /** - * @brief Re-points the internal Eigen::Map to a new memory location. - * - * @tparam OtherMatrix The matrix type of the new map. - * @tparam OtherAlignment The alignment option of the new map. - * @tparam OtherStrideType The stride type of the new map. - * - * @param[in] other_map The new `Eigen::Map` to view. - * - * @details This is an advanced operation that uses placement-new to reconstruct the internal - * `map` member in-place, making it view the memory provided by `other_map`. - */ - template - void remap(Eigen::Map & other_map) - { - new (&map) MatrixMap(other_map.data(), other_map.rows(), other_map.cols()); - } + /** + * @brief Checks for inequality between two block descriptors. + * @param[in] other The other block descriptor to compare against. + * @return `true` if the blocks are not strictly equal, `false` otherwise. + * @details This is the logical negation of `operator==`. + * @see operator==() + */ + bool operator!=(const MatrixBlockElementTpl & other) const + { + return !(*this == other); + } - /** - * @brief Checks if the block descriptor is valid and self-consistent. - * - * @details A block is considered valid if its base is valid (`size > 0` and `type != - * Undefined`) and its `map` points to valid data if the `type` requires it (e.g., `Plain`, - * `Diagonal`). - * - * @return `true` if the block info is valid, `false` otherwise. - */ - bool isValid() const - { - if (!Base::isValid()) - return false; - if (type() == MatrixBlockType::NestedBlockDiagonal) - return !m_nested_blocks.empty(); - return !(isDataBlock(type()) && map.data() == nullptr); - } + /** + * @brief Re-points the internal Eigen::Map to a new memory location. + * + * @tparam OtherMatrix The matrix type of the new map. + * @tparam OtherAlignment The alignment option of the new map. + * @tparam OtherStrideType The stride type of the new map. + * + * @param[in] other_map The new `Eigen::Map` to view. + * + * @details This is an advanced operation that uses placement-new to reconstruct the internal + * `map` member in-place, making it view the memory provided by `other_map`. + */ + template + void remap(Eigen::Map & other_map) + { + new (&map) MatrixMap(other_map.data(), other_map.rows(), other_map.cols()); + } - /// @brief Returns a mutable reference to the underlying Eigen::Map. - MatrixMap & container() - { - return map; - } + /** + * @brief Checks if the block descriptor is valid and self-consistent. + * + * @details A block is considered valid if its base is valid (`size > 0` and `type != + * Undefined`) and its `map` points to valid data if the `type` requires it (e.g., `Plain`, + * `Diagonal`). + * + * @return `true` if the block info is valid, `false` otherwise. + */ + bool isValid() const + { + if (!Base::isValid()) + return false; + if (type() == MatrixBlockType::NestedBlockDiagonal) + return !m_nested_blocks.empty(); + return !(isDataBlock(type()) && map.data() == nullptr); + } - /// @brief Returns a const reference to the underlying Eigen::Map. - const MatrixMap & container() const - { - return map; - } + /// @brief Returns a mutable reference to the underlying Eigen::Map. + MatrixMap & container() + { + return map; + } + + /// @brief Returns a const reference to the underlying Eigen::Map. + const MatrixMap & container() const + { + return map; + } + + /** + * @brief Gets a const pointer to the beginning of the map's data buffer. + * + * @return A const pointer to the first element (`Scalar*`) of the map's raw data. + * + * @details This provides direct, low-level read-only access to the matrix's coefficients. + * The data is stored in a contiguous block, typically in column-major order for + * Eigen matrices. This is useful for interoperability with C-style APIs or other + * libraries that operate on raw memory buffers. + * + * @warning The returned pointer is only valid as long as this object exists and its + * `m_map` member is not reallocated. Accessing it after the object is + * destroyed leads to undefined behavior. + */ + const Scalar * data() const + { + return map.data(); + } + + /** + * @brief Gets a mutable pointer to the beginning of the map's data buffer. + * @copydoc data() const + * @return A mutable pointer to the first element (`Scalar*`) of the map's raw data. + */ + Scalar * data() + { + return map.data(); + } + + }; // struct MatrixBlockElementTpl /** - * @brief Gets a const pointer to the beginning of the map's data buffer. + * @ingroup pinocchio_math + * @brief A descriptor for an owning matrix block, which stores its data in an Eigen::Matrix. * - * @return A const pointer to the first element (`Scalar*`) of the map's raw data. + * @tparam MatrixType The container type for the block. This specialization is enabled only when + * `MatrixType` is an owning `Eigen::Matrix` type (e.g., `Eigen::MatrixXd`). * - * @details This provides direct, low-level read-only access to the matrix's coefficients. - * The data is stored in a contiguous block, typically in column-major order for - * Eigen matrices. This is useful for interoperability with C-style APIs or other - * libraries that operate on raw memory buffers. + * @details This struct is a template specialization that describes and **owns** a single block + * within a larger structured matrix. Unlike the `Eigen::Map` specialization which only + * provides a view, this version contains an `Eigen::Matrix` member (`m_matrix`) that + * holds the numerical coefficients of the block. * - * @warning The returned pointer is only valid as long as this object exists and its - * `m_map` member is not reallocated. Accessing it after the object is - * destroyed leads to undefined behavior. - */ - const Scalar * data() const - { - return map.data(); - } - - /** - * @brief Gets a mutable pointer to the beginning of the map's data buffer. - * @copydoc data() const - * @return A mutable pointer to the first element (`Scalar*`) of the map's raw data. + * It inherits metadata like `type` and `size` from the `MatrixBlockElementPlain` + * class. + * + * @note The copy constructor and copy-assignment operator perform a **deep copy**. This means + * that the `m_matrix` member is fully duplicated, ensuring that the new object has its + * own independent copy of the data. */ - Scalar * data() + template + struct MatrixBlockElementTpl< + MatrixType, + std::enable_if_t>> + : MatrixBlockElementPlain> { - return map.data(); - } + /// @brief The type of this specialized class. + typedef MatrixBlockElementTpl Self; + /// @brief The scalar type of the matrix elements (e.g., double). + typedef typename traits::Scalar Scalar; + /// @brief The owning container type, which is an `Eigen::Matrix`. + typedef typename traits::MatrixContainer Matrix; + // Note: The MatrixMap typedef from the other specialization is not applicable here. + // We will assume `MatrixContainer` is `MatrixType`. + // typedef typename traits::MatrixContainer MatrixMap; + /// @brief The equivalent dense Eigen vector type. + typedef typename traits::Vector Vector; + + /// @brief The base class from which this class inherits. + typedef MatrixBlockElementPlain Base; + + using Base::size; + using Base::type; + using Base::operator=; + + protected: + /// @brief The owning container for the matrix block's data. + MatrixType m_matrix; + + public: + /// @brief Default constructor. Initializes to an invalid state (Undefined type, size -1). + MatrixBlockElementTpl() + : Base() + { + } - }; // struct MatrixBlockElementTpl - - /** - * @ingroup pinocchio_math - * @brief A descriptor for an owning matrix block, which stores its data in an Eigen::Matrix. - * - * @tparam MatrixType The container type for the block. This specialization is enabled only when - * `MatrixType` is an owning `Eigen::Matrix` type (e.g., `Eigen::MatrixXd`). - * - * @details This struct is a template specialization that describes and **owns** a single block - * within a larger structured matrix. Unlike the `Eigen::Map` specialization which only - * provides a view, this version contains an `Eigen::Matrix` member (`m_matrix`) that - * holds the numerical coefficients of the block. - * - * It inherits metadata like `type` and `size` from the `MatrixBlockElementPlain` class. - * - * @note The copy constructor and copy-assignment operator perform a **deep copy**. This means - * that the `m_matrix` member is fully duplicated, ensuring that the new object has its - * own independent copy of the data. - */ - template - struct MatrixBlockElementTpl>> - : MatrixBlockElementPlain> - { - /// @brief The type of this specialized class. - typedef MatrixBlockElementTpl Self; - /// @brief The scalar type of the matrix elements (e.g., double). - typedef typename traits::Scalar Scalar; - /// @brief The owning container type, which is an `Eigen::Matrix`. - typedef typename traits::MatrixContainer Matrix; - // Note: The MatrixMap typedef from the other specialization is not applicable here. - // We will assume `MatrixContainer` is `MatrixType`. - // typedef typename traits::MatrixContainer MatrixMap; - /// @brief The equivalent dense Eigen vector type. - typedef typename traits::Vector Vector; - - /// @brief The base class from which this class inherits. - typedef MatrixBlockElementPlain Base; - - using Base::size; - using Base::type; - using Base::operator=; - - protected: - /// @brief The owning container for the matrix block's data. - MatrixType m_matrix; - - public: - /// @brief Default constructor. Initializes to an invalid state (Undefined type, size -1). - MatrixBlockElementTpl() - : Base() - { - } + /** + * @brief Constructs a block descriptor for types that may not require explicit data storage. + * @details This can be used for structural blocks like `Identity` or `Zero`. The internal + * `m_matrix` member will be default-initialized. + * @param[in] type The structural type of the block. + * @param[in] size The dimension of the (square) block. + */ + MatrixBlockElementTpl(const MatrixBlockType type, const Eigen::Index size) + : Base(type, size) + { + switch (type) + { + case MatrixBlockType::Zero: + case MatrixBlockType::Identity: + break; + case MatrixBlockType::ScalarIdentity: + m_matrix.resize(1, 1); + break; + case MatrixBlockType::Diagonal: + m_matrix.resize(size, 1); + break; + case MatrixBlockType::Plain: + m_matrix.resize(size, size); + break; + default: + PINOCCHIO_UNREACHABLE(); + } + } - /** - * @brief Constructs a block descriptor for types that may not require explicit data storage. - * @details This can be used for structural blocks like `Identity` or `Zero`. The internal - * `m_matrix` member will be default-initialized. - * @param[in] type The structural type of the block. - * @param[in] size The dimension of the (square) block. - */ - MatrixBlockElementTpl(const MatrixBlockType type, const Eigen::Index size) - : Base(type, size) - { - switch (type) + /** + * @brief Constructs a block descriptor and initializes its data by copying from another + * matrix. + * @details This is the primary constructor for data-backed blocks like `Plain` or `Diagonal`. + * It performs a **deep copy** of the input matrix data into its internal storage. + * @param[in] type The structural type of the block. + * @param[in] size The dimension of the (square) block. + * @param[in] matrix_data An Eigen matrix or expression whose data will be copied. + */ + template + MatrixBlockElementTpl( + const MatrixBlockType type, + const Eigen::Index size, + const Eigen::MatrixBase & matrix_data) + : Base(type, size) + , m_matrix(matrix_data) { - case MatrixBlockType::Zero: - case MatrixBlockType::Identity: - break; - case MatrixBlockType::ScalarIdentity: - m_matrix.resize(1, 1); - break; - case MatrixBlockType::Diagonal: - m_matrix.resize(size, 1); - break; - case MatrixBlockType::Plain: - m_matrix.resize(size, size); - break; - default: - PINOCCHIO_UNREACHABLE(); } - } - /** - * @brief Constructs a block descriptor and initializes its data by copying from another matrix. - * @details This is the primary constructor for data-backed blocks like `Plain` or `Diagonal`. - * It performs a **deep copy** of the input matrix data into its internal storage. - * @param[in] type The structural type of the block. - * @param[in] size The dimension of the (square) block. - * @param[in] matrix_data An Eigen matrix or expression whose data will be copied. - */ - template - MatrixBlockElementTpl( - const MatrixBlockType type, - const Eigen::Index size, - const Eigen::MatrixBase & matrix_data) - : Base(type, size) - , m_matrix(matrix_data) - { - } + /** + * @brief Copy constructor (deep copy). + * @details Creates a complete, independent copy of the block descriptor, including a full + * duplication of the underlying matrix data in `m_matrix`. + */ + MatrixBlockElementTpl(const MatrixBlockElementTpl & other) = default; + + /** + * @brief Copy-assignment operator (deep copy). + * @details Replaces the content of this block with a full copy of the other block, including + * its matrix data. + */ + MatrixBlockElementTpl & operator=(const MatrixBlockElementTpl & other) = default; + + /** + * @brief Checks for strict equality between two block descriptors. + * + * @param[in] other The other block descriptor to compare against. + * @return `true` if the blocks are strictly equal, `false` otherwise. + * + * @details Two block descriptors are considered equal if they have the same `type` and + * `size`, and their internal `m_matrix` members are **coefficient-wise equal**. + */ + bool operator==(const MatrixBlockElementTpl & other) const + { + if (this == &other) + return true; + return Base::operator==(other) && m_matrix == other.m_matrix; + } - /** - * @brief Copy constructor (deep copy). - * @details Creates a complete, independent copy of the block descriptor, including a full - * duplication of the underlying matrix data in `m_matrix`. - */ - MatrixBlockElementTpl(const MatrixBlockElementTpl & other) = default; + /** + * @brief Checks for inequality between two block descriptors. + * @param[in] other The other block descriptor to compare against. + * @return `true` if the blocks are not strictly equal, `false` otherwise. + * @details This is the logical negation of `operator==`. + * @see operator==() + */ + bool operator!=(const MatrixBlockElementTpl & other) const + { + return !(*this == other); + } - /** - * @brief Copy-assignment operator (deep copy). - * @details Replaces the content of this block with a full copy of the other block, including - * its matrix data. - */ - MatrixBlockElementTpl & operator=(const MatrixBlockElementTpl & other) = default; + /** + * @brief Checks if the block descriptor is valid. + * @details A block is considered valid if its base is valid (`size > 0` and `type != + * Undefined`). Unlike the non-owning version, it doesn't need to check for null data + * pointers. + * @return `true` if the block info is valid, `false` otherwise. + */ + bool isValid() const + { + return Base::isValid(); + } - /** - * @brief Checks for strict equality between two block descriptors. - * - * @param[in] other The other block descriptor to compare against. - * @return `true` if the blocks are strictly equal, `false` otherwise. - * - * @details Two block descriptors are considered equal if they have the same `type` and `size`, - * and their internal `m_matrix` members are **coefficient-wise equal**. - */ - bool operator==(const MatrixBlockElementTpl & other) const - { - if (this == &other) - return true; - return Base::operator==(other) && m_matrix == other.m_matrix; - } + /// @brief Returns a mutable reference to the owning Eigen::Matrix. + MatrixType & container() + { + return m_matrix; + } - /** - * @brief Checks for inequality between two block descriptors. - * @param[in] other The other block descriptor to compare against. - * @return `true` if the blocks are not strictly equal, `false` otherwise. - * @details This is the logical negation of `operator==`. - * @see operator==() - */ - bool operator!=(const MatrixBlockElementTpl & other) const - { - return !(*this == other); - } + /// @brief Returns a const reference to the owning Eigen::Matrix. + const MatrixType & container() const + { + return m_matrix; + } - /** - * @brief Checks if the block descriptor is valid. - * @details A block is considered valid if its base is valid (`size > 0` and `type != - * Undefined`). Unlike the non-owning version, it doesn't need to check for null data pointers. - * @return `true` if the block info is valid, `false` otherwise. - */ - bool isValid() const - { - return Base::isValid(); - } + /** + * @brief Gets a const pointer to the beginning of the matrix's data buffer. + * + * @return A const pointer to the first element (`Scalar*`) of the matrix's raw data. + * + * @details This provides direct, low-level read-only access to the matrix's coefficients. + * The data is stored in a contiguous block, typically in column-major order for + * Eigen matrices. This is useful for interoperability with C-style APIs or other + * libraries that operate on raw memory buffers. + * + * @warning The returned pointer is only valid as long as this object exists and its + * `m_matrix` member is not reallocated. Accessing it after the object is + * destroyed leads to undefined behavior. + */ + const Scalar * data() const + { + return m_matrix.data(); + } - /// @brief Returns a mutable reference to the owning Eigen::Matrix. - MatrixType & container() - { - return m_matrix; - } + /** + * @brief Gets a mutable pointer to the beginning of the map's data buffer. + * @copydoc data() const + * @return A mutable pointer to the first element (`Scalar*`) of the map's raw data. + */ + Scalar * data() + { + return m_matrix.data(); + } - /// @brief Returns a const reference to the owning Eigen::Matrix. - const MatrixType & container() const - { - return m_matrix; - } + /// @brief Stub for NestedBlockDiagonal — the owning variant does not support nested blocks. + /// This method exists only to allow MatrixBlockElementPlain to compile the + /// NestedBlockDiagonal switch case; it must never be called at runtime. + std::vector & nested_blocks() + { + assert( + false && "NestedBlockDiagonal not supported by the owning MatrixBlockElementTpl variant"); + static std::vector empty; + return empty; + } - /** - * @brief Gets a const pointer to the beginning of the matrix's data buffer. - * - * @return A const pointer to the first element (`Scalar*`) of the matrix's raw data. - * - * @details This provides direct, low-level read-only access to the matrix's coefficients. - * The data is stored in a contiguous block, typically in column-major order for - * Eigen matrices. This is useful for interoperability with C-style APIs or other - * libraries that operate on raw memory buffers. - * - * @warning The returned pointer is only valid as long as this object exists and its - * `m_matrix` member is not reallocated. Accessing it after the object is - * destroyed leads to undefined behavior. - */ - const Scalar * data() const - { - return m_matrix.data(); - } + /// @brief Const overload of nested_blocks() stub. + const std::vector & nested_blocks() const + { + assert( + false && "NestedBlockDiagonal not supported by the owning MatrixBlockElementTpl variant"); + static std::vector empty; + return empty; + } - /** - * @brief Gets a mutable pointer to the beginning of the map's data buffer. - * @copydoc data() const - * @return A mutable pointer to the first element (`Scalar*`) of the map's raw data. - */ - Scalar * data() - { - return m_matrix.data(); - } + }; // struct MatrixBlockElementTpl - /// @brief Stub for NestedBlockDiagonal — the owning variant does not support nested blocks. - /// This method exists only to allow MatrixBlockElementPlain to compile the NestedBlockDiagonal - /// switch case; it must never be called at runtime. - std::vector & nested_blocks() + template + BinaryOperator< + internal::add_op, + MatrixBlockElementTpl, + Eigen::DiagonalWrapper> + operator+( + const MatrixBlockElementTpl & matrix_block_elt, + const Eigen::DiagonalWrapper & diagonal_matrix) { - assert( - false && "NestedBlockDiagonal not supported by the owning MatrixBlockElementTpl variant"); - static std::vector empty; - return empty; + return {matrix_block_elt, diagonal_matrix}; } - /// @brief Const overload of nested_blocks() stub. - const std::vector & nested_blocks() const + template + BinaryOperator< + internal::sub_op, + MatrixBlockElementTpl, + Eigen::DiagonalWrapper> + operator-( + const MatrixBlockElementTpl & matrix_block_elt, + const Eigen::DiagonalWrapper & diagonal_matrix) { - assert( - false && "NestedBlockDiagonal not supported by the owning MatrixBlockElementTpl variant"); - static std::vector empty; - return empty; + return {matrix_block_elt, diagonal_matrix}; } - }; // struct MatrixBlockElementTpl - - template - BinaryOperator< - internal::add_op, - MatrixBlockElementTpl, - Eigen::DiagonalWrapper> - operator+( - const MatrixBlockElementTpl & matrix_block_elt, - const Eigen::DiagonalWrapper & diagonal_matrix) - { - return {matrix_block_elt, diagonal_matrix}; - } - - template - BinaryOperator< - internal::sub_op, - MatrixBlockElementTpl, - Eigen::DiagonalWrapper> - operator-( - const MatrixBlockElementTpl & matrix_block_elt, - const Eigen::DiagonalWrapper & diagonal_matrix) - { - return {matrix_block_elt, diagonal_matrix}; - } - + } // namespace internal } // namespace pinocchio diff --git a/include/pinocchio/src/math/matrix-block-type.hxx b/include/pinocchio/src/math/matrix-block-type.hxx index c0f105de22..7619dc8196 100644 --- a/include/pinocchio/src/math/matrix-block-type.hxx +++ b/include/pinocchio/src/math/matrix-block-type.hxx @@ -13,71 +13,75 @@ namespace pinocchio { - /// @brief Enumeration of block types for structured or generic matrix blocks. - enum class MatrixBlockType : unsigned char + namespace internal { - /// Block equal to the identity matrix. - Identity = 1, - - /// Block filled with zeros. - Zero = 2, - - /// The block is a scalar multiple of the identity matrix (s*I). - ScalarIdentity = 4, - - /// Diagonal block with arbitrary diagonal coefficients. - Diagonal = 8, - - /// Generic, dense block with arbitrary values. - Plain = 16, - - /// A block that is itself block-diagonal, composed of a list of sub-blocks. - /// Used to represent a pool of constraints as a single outer block. - NestedBlockDiagonal = 32, - - /// Undefined type. - Undefined = 64 - }; // enum class MatrixBlockType - - ///  @brief Block type tags - template - struct MatrixBlockTypeTag - { - }; // struct MatrixBlockTypeTag - - constexpr bool hasFlag(MatrixBlockType value, MatrixBlockType flag) - { - using T = std::underlying_type_t; - return (static_cast(value) & static_cast(flag)) != 0; - } - - /// @brief Helper constexpr to test whether a block type implies structural sparsity. - constexpr bool isStructuredBlock(MatrixBlockType type) - { - constexpr MatrixBlockType structured_block_types = static_cast( - static_cast(MatrixBlockType::Identity) - | static_cast(MatrixBlockType::Zero) - | static_cast(MatrixBlockType::Diagonal)); - return hasFlag(type, structured_block_types); - } - - /// @brief Helper constexpr to test whether a block type implies raw non-trivial data. - /// Example: Identity and Zero don't need to be associated to any data. - /// On the contrary, ScalarIdentity, Diagonal and Plain are typically associated to some data. - /// NestedBlockDiagonal stores data only in its sub-blocks, not in the outer block itself. - constexpr bool isDataBlock(MatrixBlockType type) - { - constexpr MatrixBlockType data_block_types = static_cast( - static_cast(MatrixBlockType::ScalarIdentity) - | static_cast(MatrixBlockType::Diagonal) - | static_cast(MatrixBlockType::Plain)); - return hasFlag(type, data_block_types); - } - - /// @brief Helper constexpr to test whether a block is a nested block-diagonal. - constexpr bool isNestedBlock(MatrixBlockType type) - { - return type == MatrixBlockType::NestedBlockDiagonal; - } + /// @brief Enumeration of block types for structured or generic matrix blocks. + enum class MatrixBlockType : unsigned char + { + /// Block equal to the identity matrix. + Identity = 1, + + /// Block filled with zeros. + Zero = 2, + + /// The block is a scalar multiple of the identity matrix (s*I). + ScalarIdentity = 4, + + /// Diagonal block with arbitrary diagonal coefficients. + Diagonal = 8, + + /// Generic, dense block with arbitrary values. + Plain = 16, + + /// A block that is itself block-diagonal, composed of a list of sub-blocks. + /// Used to represent a pool of constraints as a single outer block. + NestedBlockDiagonal = 32, + + /// Undefined type. + Undefined = 64 + }; // enum class MatrixBlockType + + ///  @brief Block type tags + template + struct MatrixBlockTypeTag + { + }; // struct MatrixBlockTypeTag + + constexpr bool hasFlag(MatrixBlockType value, MatrixBlockType flag) + { + using T = std::underlying_type_t; + return (static_cast(value) & static_cast(flag)) != 0; + } + + /// @brief Helper constexpr to test whether a block type implies structural sparsity. + constexpr bool isStructuredBlock(MatrixBlockType type) + { + constexpr MatrixBlockType structured_block_types = static_cast( + static_cast(MatrixBlockType::Identity) + | static_cast(MatrixBlockType::Zero) + | static_cast(MatrixBlockType::Diagonal)); + return hasFlag(type, structured_block_types); + } + + /// @brief Helper constexpr to test whether a block type implies raw non-trivial data. + /// Example: Identity and Zero don't need to be associated to any data. + /// On the contrary, ScalarIdentity, Diagonal and Plain are typically associated to some data. + /// NestedBlockDiagonal stores data only in its sub-blocks, not in the outer block itself. + constexpr bool isDataBlock(MatrixBlockType type) + { + constexpr MatrixBlockType data_block_types = static_cast( + static_cast(MatrixBlockType::ScalarIdentity) + | static_cast(MatrixBlockType::Diagonal) + | static_cast(MatrixBlockType::Plain)); + return hasFlag(type, data_block_types); + } + + /// @brief Helper constexpr to test whether a block is a nested block-diagonal. + constexpr bool isNestedBlock(MatrixBlockType type) + { + return type == MatrixBlockType::NestedBlockDiagonal; + } + + } // namespace internal } // namespace pinocchio diff --git a/include/pinocchio/src/math/matrix-inverse-code-generated.hxx b/include/pinocchio/src/math/matrix-inverse-code-generated.hxx index bde0adb2e7..648c61183a 100644 --- a/include/pinocchio/src/math/matrix-inverse-code-generated.hxx +++ b/include/pinocchio/src/math/matrix-inverse-code-generated.hxx @@ -26,14 +26,16 @@ namespace pinocchio PINOCCHIO_UNREACHABLE(); } }; - } // namespace internal - template - EIGEN_STRONG_INLINE void matrix_inversion_code_generated( - const Eigen::MatrixBase & matrix, const Eigen::MatrixBase & matrix_inverse) - { - typedef internal::MatrixInversionCodeGeneratedImpl - Runner; - Runner::run(matrix, matrix_inverse.const_cast_derived()); - } + template + EIGEN_STRONG_INLINE void matrix_inversion_code_generated( + const Eigen::MatrixBase & matrix, const Eigen::MatrixBase & matrix_inverse) + { + typedef internal::MatrixInversionCodeGeneratedImpl< + M1::RowsAtCompileTime, M1::ColsAtCompileTime> + Runner; + Runner::run(matrix, matrix_inverse.const_cast_derived()); + } + + } // namespace internal } // namespace pinocchio diff --git a/include/pinocchio/src/math/matrix-inverse.hxx b/include/pinocchio/src/math/matrix-inverse.hxx index fba62d9b7a..865abfa173 100644 --- a/include/pinocchio/src/math/matrix-inverse.hxx +++ b/include/pinocchio/src/math/matrix-inverse.hxx @@ -140,12 +140,12 @@ namespace pinocchio } }; - } // namespace internal + template + EIGEN_STRONG_INLINE void matrix_inversion( + const Eigen::MatrixBase & matrix, const Eigen::MatrixBase & matrix_inverse) + { + internal::MatrixInversion::run(matrix, matrix_inverse.const_cast_derived()); + } - template - EIGEN_STRONG_INLINE void matrix_inversion( - const Eigen::MatrixBase & matrix, const Eigen::MatrixBase & matrix_inverse) - { - internal::MatrixInversion::run(matrix, matrix_inverse.const_cast_derived()); - } + } // namespace internal } // namespace pinocchio diff --git a/include/pinocchio/src/math/matrix-product.hxx b/include/pinocchio/src/math/matrix-product.hxx index 8577959781..3466560a93 100644 --- a/include/pinocchio/src/math/matrix-product.hxx +++ b/include/pinocchio/src/math/matrix-product.hxx @@ -33,25 +33,20 @@ namespace pinocchio const Eigen::MatrixBase & rhs, const Eigen::MatrixBase & res); - } // namespace internal - - template class EigenOp, typename Lhs, typename Rhs, typename Res> - void matrix_product( - const Eigen::MatrixBase & lhs, - const Eigen::MatrixBase & rhs, - const Eigen::MatrixBase & res) - { - const auto max_size = std::max(lhs.rows(), std::max(lhs.cols(), rhs.cols())); - if (max_size <= 0) - internal::matrix_product_small_size( - lhs.derived(), rhs.derived(), res.const_cast_derived()); - else - internal::matrix_product_generic( - lhs.derived(), rhs.derived(), res.const_cast_derived()); - }; - - namespace internal - { + template class EigenOp, typename Lhs, typename Rhs, typename Res> + void matrix_product( + const Eigen::MatrixBase & lhs, + const Eigen::MatrixBase & rhs, + const Eigen::MatrixBase & res) + { + const auto max_size = std::max(lhs.rows(), std::max(lhs.cols(), rhs.cols())); + if (max_size <= 0) + internal::matrix_product_small_size( + lhs.derived(), rhs.derived(), res.const_cast_derived()); + else + internal::matrix_product_generic( + lhs.derived(), rhs.derived(), res.const_cast_derived()); + }; template< template class EigenOp, diff --git a/include/pinocchio/src/multibody/joint/joint-composite.hxx b/include/pinocchio/src/multibody/joint/joint-composite.hxx index 1ddf81a611..865f9fff53 100644 --- a/include/pinocchio/src/multibody/joint/joint-composite.hxx +++ b/include/pinocchio/src/multibody/joint/joint-composite.hxx @@ -372,7 +372,7 @@ namespace pinocchio data.StU.noalias() = data.S.matrix().transpose() * data.U; data.StU.diagonal() += armature; - matrix_inversion(data.StU, data.Dinv); + internal::matrix_inversion(data.StU, data.Dinv); data.UDinv.noalias() = data.U * data.Dinv; if (update_I) diff --git a/include/pinocchio/src/multibody/joint/joint-ellipsoid.hxx b/include/pinocchio/src/multibody/joint/joint-ellipsoid.hxx index aee3b17a3d..1980668cb7 100644 --- a/include/pinocchio/src/multibody/joint/joint-ellipsoid.hxx +++ b/include/pinocchio/src/multibody/joint/joint-ellipsoid.hxx @@ -547,7 +547,7 @@ namespace pinocchio data.StU.noalias() = data.S.transpose() * data.U; data.StU.diagonal() += armature; - matrix_inversion(data.StU, data.Dinv); + internal::matrix_inversion(data.StU, data.Dinv); data.UDinv.noalias() = data.U * data.Dinv; if (update_I) diff --git a/include/pinocchio/src/multibody/joint/joint-free-flyer.hxx b/include/pinocchio/src/multibody/joint/joint-free-flyer.hxx index e2a9309c58..a4a09ec14a 100644 --- a/include/pinocchio/src/multibody/joint/joint-free-flyer.hxx +++ b/include/pinocchio/src/multibody/joint/joint-free-flyer.hxx @@ -364,7 +364,7 @@ namespace pinocchio data.StU = I; data.StU.diagonal() += armature; - matrix_inversion(data.StU, data.Dinv); + internal::matrix_inversion(data.StU, data.Dinv); data.UDinv.noalias() = I * data.Dinv; if (update_I) diff --git a/include/pinocchio/src/multibody/joint/joint-planar.hxx b/include/pinocchio/src/multibody/joint/joint-planar.hxx index 543bd7189b..adf7909d28 100644 --- a/include/pinocchio/src/multibody/joint/joint-planar.hxx +++ b/include/pinocchio/src/multibody/joint/joint-planar.hxx @@ -627,7 +627,7 @@ namespace pinocchio data.StU.template rightCols<1>() = data.U.template bottomRows<1>(); data.StU.diagonal() += armature; - matrix_inversion(data.StU, data.Dinv); + internal::matrix_inversion(data.StU, data.Dinv); data.UDinv.noalias() = data.U * data.Dinv; diff --git a/include/pinocchio/src/multibody/joint/joint-spherical-ZYX.hxx b/include/pinocchio/src/multibody/joint/joint-spherical-ZYX.hxx index 50fc969cd6..4b09fb7b63 100644 --- a/include/pinocchio/src/multibody/joint/joint-spherical-ZYX.hxx +++ b/include/pinocchio/src/multibody/joint/joint-spherical-ZYX.hxx @@ -459,7 +459,7 @@ namespace pinocchio data.S.angularSubspace().transpose() * data.U.template middleRows<3>(Motion::ANGULAR); data.StU.diagonal() += armature; - matrix_inversion(data.StU, data.Dinv); + internal::matrix_inversion(data.StU, data.Dinv); data.UDinv.noalias() = data.U * data.Dinv; diff --git a/include/pinocchio/src/multibody/joint/joint-spherical.hxx b/include/pinocchio/src/multibody/joint/joint-spherical.hxx index 46cf269433..31e0930a7d 100644 --- a/include/pinocchio/src/multibody/joint/joint-spherical.hxx +++ b/include/pinocchio/src/multibody/joint/joint-spherical.hxx @@ -556,7 +556,7 @@ namespace pinocchio data.StU = data.U.template middleRows<3>(Inertia::ANGULAR); data.StU.diagonal() += armature; - matrix_inversion(data.StU, data.Dinv); + internal::matrix_inversion(data.StU, data.Dinv); data.UDinv.noalias() = data.U * data.Dinv; if (update_I) diff --git a/include/pinocchio/src/multibody/joint/joint-translation.hxx b/include/pinocchio/src/multibody/joint/joint-translation.hxx index 340b611a52..c286fd09f3 100644 --- a/include/pinocchio/src/multibody/joint/joint-translation.hxx +++ b/include/pinocchio/src/multibody/joint/joint-translation.hxx @@ -610,7 +610,7 @@ namespace pinocchio data.StU = data.U.template middleRows<3>(Inertia::LINEAR); data.StU.diagonal() += armature; - matrix_inversion(data.StU, data.Dinv); + internal::matrix_inversion(data.StU, data.Dinv); data.UDinv.noalias() = data.U * data.Dinv; diff --git a/include/pinocchio/src/multibody/joint/joint-universal.hxx b/include/pinocchio/src/multibody/joint/joint-universal.hxx index 68df275c1e..e04111fe3d 100644 --- a/include/pinocchio/src/multibody/joint/joint-universal.hxx +++ b/include/pinocchio/src/multibody/joint/joint-universal.hxx @@ -559,7 +559,7 @@ namespace pinocchio data.StU.noalias() = data.S.angularSubspace().transpose() * data.U.template middleRows<3>(Motion::ANGULAR); data.StU.diagonal() += armature; - matrix_inversion(data.StU, data.Dinv); + internal::matrix_inversion(data.StU, data.Dinv); data.UDinv.noalias() = data.U * data.Dinv; diff --git a/include/pinocchio/src/serialization/block-diagonal-matrix.hxx b/include/pinocchio/src/serialization/block-diagonal-matrix.hxx index 61d30d2ecb..977e354d96 100644 --- a/include/pinocchio/src/serialization/block-diagonal-matrix.hxx +++ b/include/pinocchio/src/serialization/block-diagonal-matrix.hxx @@ -20,9 +20,9 @@ namespace boost { template struct BlockDiagonalMatrixAccessor - : public ::pinocchio::BlockDiagonalMatrixTpl + : public ::pinocchio::internal::BlockDiagonalMatrixTpl { - typedef ::pinocchio::BlockDiagonalMatrixTpl Base; + typedef ::pinocchio::internal::BlockDiagonalMatrixTpl Base; using Base::m_cols; using Base::m_matrix_block_elements; using Base::m_matrix_stack; @@ -33,7 +33,7 @@ namespace boost template void serialize( Archive & ar, - ::pinocchio::BlockDiagonalMatrixTpl & matrix, + ::pinocchio::internal::BlockDiagonalMatrixTpl & matrix, const unsigned int /*version*/) { typedef internal::BlockDiagonalMatrixAccessor Accessor; @@ -52,7 +52,7 @@ namespace boost std::size_t idx = 0; for (auto & block : m_matrix_block_elements) { - if (block.type() == ::pinocchio::MatrixBlockType::NestedBlockDiagonal) + if (block.type() == ::pinocchio::internal::MatrixBlockType::NestedBlockDiagonal) { // Remap each sub-block; the outer NestedBlockDiagonal block itself has no data. for (auto & sub : block.nested_blocks()) @@ -89,7 +89,7 @@ namespace boost for (const auto & block : m_matrix_block_elements) { - if (block.type() == ::pinocchio::MatrixBlockType::NestedBlockDiagonal) + if (block.type() == ::pinocchio::internal::MatrixBlockType::NestedBlockDiagonal) { // The outer block has no data; push one entry per sub-block. for (const auto & sub : block.nested_blocks()) diff --git a/include/pinocchio/src/serialization/matrix-block-element.hxx b/include/pinocchio/src/serialization/matrix-block-element.hxx index 1637ff1619..1ebd32a413 100644 --- a/include/pinocchio/src/serialization/matrix-block-element.hxx +++ b/include/pinocchio/src/serialization/matrix-block-element.hxx @@ -20,9 +20,10 @@ namespace boost { /// Accessor for the owning variant (Eigen::Matrix). template - struct MatrixBlockElementTplAccessor : public ::pinocchio::MatrixBlockElementTpl + struct MatrixBlockElementTplAccessor + : public ::pinocchio::internal::MatrixBlockElementTpl { - typedef ::pinocchio::MatrixBlockElementTpl Base; + typedef ::pinocchio::internal::MatrixBlockElementTpl Base; using Base::m_size; using Base::m_type; }; @@ -32,9 +33,9 @@ namespace boost struct MatrixBlockElementTplAccessor< MapType, std::enable_if_t>> - : public ::pinocchio::MatrixBlockElementTpl + : public ::pinocchio::internal::MatrixBlockElementTpl { - typedef ::pinocchio::MatrixBlockElementTpl Base; + typedef ::pinocchio::internal::MatrixBlockElementTpl Base; using Base::m_nested_blocks; using Base::m_size; using Base::m_type; @@ -44,7 +45,7 @@ namespace boost template void serialize( Archive & ar, - ::pinocchio::MatrixBlockElementTpl & _matrix_block_element, + ::pinocchio::internal::MatrixBlockElementTpl & _matrix_block_element, const unsigned int /*version*/) { typedef internal::MatrixBlockElementTplAccessor Accessor; @@ -61,7 +62,8 @@ namespace boost else { // Map variant: serialize nested block structure for NestedBlockDiagonal. - if (matrix_block_element.m_type == pinocchio::MatrixBlockType::NestedBlockDiagonal) + if ( + matrix_block_element.m_type == pinocchio::internal::MatrixBlockType::NestedBlockDiagonal) { ar & make_nvp("nested_blocks", matrix_block_element.m_nested_blocks); } diff --git a/unittest/block-diagonal-matrix.cpp b/unittest/block-diagonal-matrix.cpp index 9f7fb464ef..5b7a0462c4 100644 --- a/unittest/block-diagonal-matrix.cpp +++ b/unittest/block-diagonal-matrix.cpp @@ -11,14 +11,14 @@ BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE) using namespace pinocchio; typedef Eigen::Matrix M11; -typedef BlockDiagonalMatrix::Matrix Matrix; -typedef BlockDiagonalMatrix::MatrixMap MatrixMap; -typedef BlockDiagonalMatrix::ConstMatrixMap ConstMatrixMap; -typedef BlockDiagonalMatrix::Vector Vector; -typedef BlockDiagonalMatrix::MatrixBlockElement MatrixBlockElement; -typedef BlockDiagonalMatrix::ConstMatrixBlockElement ConstMatrixBlockElement; - -void test_assignment(const BlockDiagonalMatrix & block_diagonal_matrix) +typedef internal::BlockDiagonalMatrix::Matrix Matrix; +typedef internal::BlockDiagonalMatrix::MatrixMap MatrixMap; +typedef internal::BlockDiagonalMatrix::ConstMatrixMap ConstMatrixMap; +typedef internal::BlockDiagonalMatrix::Vector Vector; +typedef internal::BlockDiagonalMatrix::MatrixBlockElement MatrixBlockElement; +typedef internal::BlockDiagonalMatrix::ConstMatrixBlockElement ConstMatrixBlockElement; + +void test_assignment(const internal::BlockDiagonalMatrix & block_diagonal_matrix) { const auto size = block_diagonal_matrix.rows(); const Matrix square_matrix = Matrix::Random(size, size); @@ -52,7 +52,7 @@ void test_assignment(const BlockDiagonalMatrix & block_diagonal_matrix) } } -void test_applyOnTheRight(const BlockDiagonalMatrix & block_diagonal_matrix) +void test_applyOnTheRight(const internal::BlockDiagonalMatrix & block_diagonal_matrix) { const auto rows = block_diagonal_matrix.rows(); const auto cols = 20; @@ -99,7 +99,7 @@ void test_applyOnTheRight(const BlockDiagonalMatrix & block_diagonal_matrix) } } -void test_applyOnTheLeft(const BlockDiagonalMatrix & block_diagonal_matrix) +void test_applyOnTheLeft(const internal::BlockDiagonalMatrix & block_diagonal_matrix) { const auto cols = block_diagonal_matrix.cols(); const auto rows = 20; @@ -150,7 +150,7 @@ void test_applyOnTheLeft(const BlockDiagonalMatrix & block_diagonal_matrix) BOOST_AUTO_TEST_CASE(test_default_constructor) { - BlockDiagonalMatrix matrix; + internal::BlockDiagonalMatrix matrix; BOOST_CHECK(matrix.data() == nullptr); BOOST_CHECK(matrix.rows() == -1); BOOST_CHECK(matrix.cols() == -1); @@ -162,10 +162,10 @@ BOOST_AUTO_TEST_CASE(test_single_block) // Zero block { - MatrixBlockElement single_block_info = {pinocchio::MatrixBlockType::Zero, size}; + MatrixBlockElement single_block_info = {pinocchio::internal::MatrixBlockType::Zero, size}; BOOST_CHECK(single_block_info.isValid()); - BlockDiagonalMatrix block_diagonal_matrix({single_block_info}); + internal::BlockDiagonalMatrix block_diagonal_matrix({single_block_info}); BOOST_CHECK(block_diagonal_matrix.rows() == size); BOOST_CHECK(block_diagonal_matrix.cols() == size); @@ -182,10 +182,10 @@ BOOST_AUTO_TEST_CASE(test_single_block) // Identity block { - MatrixBlockElement single_block_info = {pinocchio::MatrixBlockType::Identity, size}; + MatrixBlockElement single_block_info = {pinocchio::internal::MatrixBlockType::Identity, size}; BOOST_CHECK(single_block_info.isValid()); - BlockDiagonalMatrix block_diagonal_matrix({single_block_info}); + internal::BlockDiagonalMatrix block_diagonal_matrix({single_block_info}); BOOST_CHECK(block_diagonal_matrix.rows() == size); BOOST_CHECK(block_diagonal_matrix.cols() == size); @@ -206,10 +206,10 @@ BOOST_AUTO_TEST_CASE(test_single_block) M11 scale_mat = M11(scale); const auto matrix_map = make_map(scale_mat); MatrixBlockElement single_block_info = { - pinocchio::MatrixBlockType::ScalarIdentity, size, matrix_map}; + pinocchio::internal::MatrixBlockType::ScalarIdentity, size, matrix_map}; BOOST_CHECK(single_block_info.isValid()); - BlockDiagonalMatrix block_diagonal_matrix({single_block_info}); + internal::BlockDiagonalMatrix block_diagonal_matrix({single_block_info}); BOOST_CHECK(block_diagonal_matrix.rows() == size); BOOST_CHECK(block_diagonal_matrix.cols() == size); @@ -237,10 +237,11 @@ BOOST_AUTO_TEST_CASE(test_single_block) Matrix diagonal_vector = Matrix::Ones(size, 1); const auto matrix_map = make_map(diagonal_vector); - MatrixBlockElement single_block_info = {pinocchio::MatrixBlockType::Diagonal, size, matrix_map}; + MatrixBlockElement single_block_info = { + pinocchio::internal::MatrixBlockType::Diagonal, size, matrix_map}; BOOST_CHECK(single_block_info.isValid()); - BlockDiagonalMatrix block_diagonal_matrix({single_block_info}); + internal::BlockDiagonalMatrix block_diagonal_matrix({single_block_info}); BOOST_CHECK(block_diagonal_matrix.rows() == size); BOOST_CHECK(block_diagonal_matrix.cols() == size); @@ -260,10 +261,11 @@ BOOST_AUTO_TEST_CASE(test_single_block) Matrix diagonal_plain = Matrix::Identity(size, size); const auto matrix_map = make_map(diagonal_plain); - MatrixBlockElement single_block_info = {pinocchio::MatrixBlockType::Plain, size, matrix_map}; + MatrixBlockElement single_block_info = { + pinocchio::internal::MatrixBlockType::Plain, size, matrix_map}; BOOST_CHECK(single_block_info.isValid()); - BlockDiagonalMatrix block_diagonal_matrix({single_block_info}); + internal::BlockDiagonalMatrix block_diagonal_matrix({single_block_info}); BOOST_CHECK(block_diagonal_matrix.rows() == size); BOOST_CHECK(block_diagonal_matrix.cols() == size); @@ -282,7 +284,7 @@ BOOST_AUTO_TEST_CASE(test_single_block) BOOST_AUTO_TEST_CASE(test_Zero_constructor) { const Eigen::Index size = 20; - const auto block_diagonal_matrix = BlockDiagonalMatrix::Zero(size); + const auto block_diagonal_matrix = internal::BlockDiagonalMatrix::Zero(size); const auto bdm_plain = block_diagonal_matrix.matrix(); BOOST_CHECK(bdm_plain == Matrix::Zero(size, size)); @@ -292,7 +294,7 @@ BOOST_AUTO_TEST_CASE(test_ScalarIdentity_constructor) { const Eigen::Index size = 20; const double scale = 2; - const auto block_diagonal_matrix = BlockDiagonalMatrix::ScalarIdentity(size, scale); + const auto block_diagonal_matrix = internal::BlockDiagonalMatrix::ScalarIdentity(size, scale); const auto bdm_plain = block_diagonal_matrix.matrix(); BOOST_CHECK(bdm_plain == Matrix(Vector::Constant(size, scale).asDiagonal())); @@ -305,7 +307,7 @@ BOOST_AUTO_TEST_CASE(test_construct_from_diagonal_matrix) { const Vector diagonal_terms = Vector::Random(size); - const BlockDiagonalMatrix block_diagonal_matrix(diagonal_terms.asDiagonal()); + const internal::BlockDiagonalMatrix block_diagonal_matrix(diagonal_terms.asDiagonal()); BOOST_CHECK(block_diagonal_matrix.getMatrixBlockElements().size() == 1); BOOST_CHECK( block_diagonal_matrix.getMatrixBlockElements().back().container() == diagonal_terms); @@ -318,7 +320,7 @@ BOOST_AUTO_TEST_CASE(test_construct_from_diagonal_matrix) { const auto diagonal_terms = Vector::Constant(size, 2.); - const BlockDiagonalMatrix block_diagonal_matrix(diagonal_terms.asDiagonal()); + const internal::BlockDiagonalMatrix block_diagonal_matrix(diagonal_terms.asDiagonal()); BOOST_CHECK(block_diagonal_matrix.getMatrixBlockElements().size() == 1); BOOST_CHECK( block_diagonal_matrix.getMatrixBlockElements().back().container() == diagonal_terms); @@ -332,7 +334,7 @@ BOOST_AUTO_TEST_CASE(test_construct_from_diagonal_matrix) BOOST_AUTO_TEST_CASE(test_size_in_bytes) { const Eigen::Index size = 20; - const auto block_diagonal_matrix = BlockDiagonalMatrix::Zero(size); + const auto block_diagonal_matrix = internal::BlockDiagonalMatrix::Zero(size); BOOST_CHECK(block_diagonal_matrix.sizeInBytes() >= 2 * sizeof(Eigen::Index)); } @@ -342,25 +344,26 @@ BOOST_AUTO_TEST_CASE(test_copy_diagonal_matrix) const Eigen::Index size = 20; const Vector diagonal_terms = Vector::Random(size); - BlockDiagonalMatrix block_diagonal_matrix; + internal::BlockDiagonalMatrix block_diagonal_matrix; block_diagonal_matrix = diagonal_terms.asDiagonal(); const auto bdm_plain = block_diagonal_matrix.matrix(); BOOST_CHECK(bdm_plain == Matrix(diagonal_terms.asDiagonal())); } -BlockDiagonalMatrix create_multiple_block_info(const Eigen::Index block_size) +internal::BlockDiagonalMatrix create_multiple_block_info(const Eigen::Index block_size) { std::vector matrix_block_elements_vector; { - MatrixBlockElement single_block_info = {pinocchio::MatrixBlockType::Zero, block_size}; + MatrixBlockElement single_block_info = {pinocchio::internal::MatrixBlockType::Zero, block_size}; matrix_block_elements_vector.push_back(single_block_info); BOOST_CHECK(matrix_block_elements_vector.back().isValid()); } { - MatrixBlockElement single_block_info = {pinocchio::MatrixBlockType::Identity, block_size}; + MatrixBlockElement single_block_info = { + pinocchio::internal::MatrixBlockType::Identity, block_size}; matrix_block_elements_vector.push_back(single_block_info); BOOST_CHECK(matrix_block_elements_vector.back().isValid()); } @@ -370,7 +373,7 @@ BlockDiagonalMatrix create_multiple_block_info(const Eigen::Index block_size) { const auto matrix_map = make_map(scale_mat); matrix_block_elements_vector.push_back( - {pinocchio::MatrixBlockType::ScalarIdentity, block_size, matrix_map}); + {pinocchio::internal::MatrixBlockType::ScalarIdentity, block_size, matrix_map}); BOOST_CHECK(matrix_block_elements_vector.back().isValid()); } @@ -378,7 +381,7 @@ BlockDiagonalMatrix create_multiple_block_info(const Eigen::Index block_size) { const auto matrix_map = make_map(diagonal_vector); matrix_block_elements_vector.push_back( - {pinocchio::MatrixBlockType::Diagonal, block_size, matrix_map}); + {pinocchio::internal::MatrixBlockType::Diagonal, block_size, matrix_map}); BOOST_CHECK(matrix_block_elements_vector.back().isValid()); } @@ -386,11 +389,11 @@ BlockDiagonalMatrix create_multiple_block_info(const Eigen::Index block_size) { const auto matrix_map = make_map(identity_plain); matrix_block_elements_vector.push_back( - {pinocchio::MatrixBlockType::Plain, block_size, matrix_map}); + {pinocchio::internal::MatrixBlockType::Plain, block_size, matrix_map}); BOOST_CHECK(matrix_block_elements_vector.back().isValid()); } - BlockDiagonalMatrix res(matrix_block_elements_vector); + internal::BlockDiagonalMatrix res(matrix_block_elements_vector); BOOST_CHECK(res.getMatrixStack()[0].data() != matrix_block_elements_vector[2].map.data()); BOOST_CHECK(res.getMatrixStack()[1].data() != matrix_block_elements_vector[3].map.data()); BOOST_CHECK(res.getMatrixStack()[2].data() != matrix_block_elements_vector[4].map.data()); @@ -462,7 +465,7 @@ BOOST_AUTO_TEST_CASE(test_add_bdm_diag_operator_plus) const Matrix bdm_plain = bdm.matrix(); const Matrix res_ref = bdm_plain + Matrix(diag_mat); - const BlockDiagonalMatrix bdm_res = bdm + diag_mat; + const internal::BlockDiagonalMatrix bdm_res = bdm + diag_mat; BOOST_CHECK(bdm_res.matrix().isApprox(res_ref)); } @@ -474,9 +477,10 @@ BOOST_AUTO_TEST_CASE(test_inverse) const auto block_diagonal_matrix_inverse_expression = block_diagonal_matrix.inverse(); - pinocchio::BlockDiagonalMatrix block_diagonal_matrix_inverse_value; + pinocchio::internal::BlockDiagonalMatrix block_diagonal_matrix_inverse_value; { - std::vector inverse_pattern; + std::vector + inverse_pattern; for (const auto & block : block_diagonal_matrix.getMatrixBlockElements()) { inverse_pattern.push_back(block.inverse()); @@ -524,12 +528,13 @@ BOOST_AUTO_TEST_CASE(test_inverse_rebuild) const Eigen::Index block_size = 10; const auto bdm = create_multiple_block_info(block_size); - BlockDiagonalMatrix res; // Empty + internal::BlockDiagonalMatrix res; // Empty res = bdm.inverse(); // Check structure BOOST_CHECK(res.blocks().size() == bdm.blocks().size()); - BOOST_CHECK(res.blocks()[0].type() == pinocchio::MatrixBlockType::Plain); // Zero -> Plain + BOOST_CHECK( + res.blocks()[0].type() == pinocchio::internal::MatrixBlockType::Plain); // Zero -> Plain // Check values (excluding Zero block) const auto bdm_plain = bdm.matrix(); @@ -574,32 +579,33 @@ BOOST_AUTO_TEST_CASE(test_rebuild) BOOST_AUTO_TEST_CASE(test_operator_equal) { - BlockDiagonalMatrix bdm; + internal::BlockDiagonalMatrix bdm; bdm = Eigen::VectorXd::Constant(0, 3.14).asDiagonal(); - BlockDiagonalMatrix bdm2 = bdm; + internal::BlockDiagonalMatrix bdm2 = bdm; BOOST_CHECK(bdm2 == bdm); } /// Helper: build a 2-outer-block BDM where one block is NestedBlockDiagonal. /// Layout: [Diagonal(3×3)] [NestedBlockDiagonal: (Diagonal(2×2), Plain(3×3))] -static BlockDiagonalMatrix create_nested_block_diagonal_matrix() +static internal::BlockDiagonalMatrix create_nested_block_diagonal_matrix() { const Eigen::Index flat_diag_size = 3; const Eigen::Index sub_diag_size = 2; const Eigen::Index sub_plain_size = 3; // Outer block 0: plain Diagonal - MatrixBlockElement flat_block(pinocchio::MatrixBlockType::Diagonal, flat_diag_size); + MatrixBlockElement flat_block(pinocchio::internal::MatrixBlockType::Diagonal, flat_diag_size); // Outer block 1: NestedBlockDiagonal containing Diagonal + Plain sub-blocks std::vector subs; - subs.emplace_back(pinocchio::MatrixBlockType::Diagonal, sub_diag_size); - subs.emplace_back(pinocchio::MatrixBlockType::Plain, sub_plain_size); - MatrixBlockElement nested_block(pinocchio::MatrixBlockType::NestedBlockDiagonal, std::move(subs)); + subs.emplace_back(pinocchio::internal::MatrixBlockType::Diagonal, sub_diag_size); + subs.emplace_back(pinocchio::internal::MatrixBlockType::Plain, sub_plain_size); + MatrixBlockElement nested_block( + pinocchio::internal::MatrixBlockType::NestedBlockDiagonal, std::move(subs)); - BlockDiagonalMatrix bdm({flat_block, nested_block}); + internal::BlockDiagonalMatrix bdm({flat_block, nested_block}); for (auto & block : bdm.blocks()) block.setRandomPD(); // make it invertible for inverse tests return bdm; @@ -613,11 +619,13 @@ BOOST_AUTO_TEST_CASE(test_nested_block_diagonal_construction) BOOST_CHECK(bdm.rows() == expected_rows); BOOST_CHECK(bdm.cols() == expected_rows); BOOST_CHECK(bdm.blocks().size() == 2); // ONE outer block per constraint - BOOST_CHECK(bdm.blocks()[0].type() == pinocchio::MatrixBlockType::Diagonal); - BOOST_CHECK(bdm.blocks()[1].type() == pinocchio::MatrixBlockType::NestedBlockDiagonal); + BOOST_CHECK(bdm.blocks()[0].type() == pinocchio::internal::MatrixBlockType::Diagonal); + BOOST_CHECK(bdm.blocks()[1].type() == pinocchio::internal::MatrixBlockType::NestedBlockDiagonal); BOOST_CHECK(bdm.blocks()[1].nested_blocks().size() == 2); - BOOST_CHECK(bdm.blocks()[1].nested_blocks()[0].type() == pinocchio::MatrixBlockType::Diagonal); - BOOST_CHECK(bdm.blocks()[1].nested_blocks()[1].type() == pinocchio::MatrixBlockType::Plain); + BOOST_CHECK( + bdm.blocks()[1].nested_blocks()[0].type() == pinocchio::internal::MatrixBlockType::Diagonal); + BOOST_CHECK( + bdm.blocks()[1].nested_blocks()[1].type() == pinocchio::internal::MatrixBlockType::Plain); // Plain matrix should have zeros off the diagonal sub-blocks const Matrix plain = bdm.matrix(); @@ -655,10 +663,10 @@ BOOST_AUTO_TEST_CASE(test_nested_block_diagonal_diagonal) BOOST_AUTO_TEST_CASE(test_nested_block_diagonal_inverse) { const auto bdm = create_nested_block_diagonal_matrix(); - const BlockDiagonalMatrix inv = bdm.inverse(); + const internal::BlockDiagonalMatrix inv = bdm.inverse(); BOOST_CHECK(inv.blocks().size() == bdm.blocks().size()); - BOOST_CHECK(inv.blocks()[1].type() == pinocchio::MatrixBlockType::NestedBlockDiagonal); + BOOST_CHECK(inv.blocks()[1].type() == pinocchio::internal::MatrixBlockType::NestedBlockDiagonal); const Matrix plain = bdm.matrix(); const Matrix inv_plain = inv.matrix(); @@ -674,20 +682,20 @@ BOOST_AUTO_TEST_CASE(test_nested_block_diagonal_sum_with_diagonal) const Vector delta = Vector::Random(n).cwiseAbs(); // positive entries const auto diag_mat = delta.asDiagonal(); - const BlockDiagonalMatrix res = bdm + diag_mat; + const internal::BlockDiagonalMatrix res = bdm + diag_mat; const Matrix plain_bdm = bdm.matrix(); const Matrix expected = plain_bdm + Matrix(diag_mat); BOOST_CHECK(res.matrix().isApprox(expected)); // The result's blocks()[1] should still be NestedBlockDiagonal (types upgraded) - BOOST_CHECK(res.blocks()[1].type() == pinocchio::MatrixBlockType::NestedBlockDiagonal); + BOOST_CHECK(res.blocks()[1].type() == pinocchio::internal::MatrixBlockType::NestedBlockDiagonal); } BOOST_AUTO_TEST_CASE(test_nested_block_diagonal_copy) { const auto bdm = create_nested_block_diagonal_matrix(); - const BlockDiagonalMatrix bdm_copy = bdm; + const internal::BlockDiagonalMatrix bdm_copy = bdm; BOOST_CHECK(bdm_copy == bdm); // Copies must be independent (no aliasing in NestedBlockDiagonal sub-blocks) diff --git a/unittest/delassus-operations.cpp b/unittest/delassus-operations.cpp index 16bae543c2..433b1d0fe9 100644 --- a/unittest/delassus-operations.cpp +++ b/unittest/delassus-operations.cpp @@ -198,7 +198,7 @@ BOOST_AUTO_TEST_CASE(delassus_dense_block_operations) const Eigen::Index size = scene.delassus_matrix_gt.rows(); Eigen::VectorXd compliance = scene.compliance; BOOST_CHECK(compliance.minCoeff() >= 0); - BlockDiagonalMatrix block_damping; + internal::BlockDiagonalMatrix block_damping; constructPositiveDefiniteBlockDiagonalMatrix(scene.constraint_models, block_damping); // Use the dense matrix from scene @@ -458,7 +458,7 @@ BOOST_AUTO_TEST_CASE(delassus_rigid_body_block_operations) const Eigen::Index size = scene.delassus_matrix_gt.rows(); Eigen::VectorXd compliance = scene.compliance; BOOST_CHECK(compliance.minCoeff() >= 0); - BlockDiagonalMatrix block_damping; + internal::BlockDiagonalMatrix block_damping; constructPositiveDefiniteBlockDiagonalMatrix(scene.constraint_models, block_damping); typedef ConstrainedHumanoidScene::ConstraintModel ConstraintModel; @@ -674,7 +674,7 @@ BOOST_AUTO_TEST_CASE(delassus_cholesky_expression_block_operations) const Eigen::Index size = scene.delassus_matrix_gt.rows(); Eigen::VectorXd compliance = scene.compliance; BOOST_CHECK(compliance.minCoeff() >= 0); - BlockDiagonalMatrix block_damping; + internal::BlockDiagonalMatrix block_damping; constructPositiveDefiniteBlockDiagonalMatrix(scene.constraint_models, block_damping); // Build a ConstraintCholeskyDecomposition and compute @@ -810,7 +810,7 @@ BOOST_AUTO_TEST_CASE(delassus_cholesky_expression_unsafe) delassus.updateDamping(damping_val); // Test unsafe().damping() gives direct write access to the block diagonal damping - BlockDiagonalMatrix block_damping; + internal::BlockDiagonalMatrix block_damping; constructPositiveDefiniteBlockDiagonalMatrix(scene.constraint_models, block_damping); delassus.unsafe().damping() = block_damping; diff --git a/unittest/matrix-block-element.cpp b/unittest/matrix-block-element.cpp index 6143ac3578..7902157a9b 100644 --- a/unittest/matrix-block-element.cpp +++ b/unittest/matrix-block-element.cpp @@ -12,17 +12,18 @@ BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE) using namespace pinocchio; typedef Eigen::Matrix M11; -typedef BlockDiagonalMatrix::Matrix Matrix; -typedef BlockDiagonalMatrix::MatrixMap MatrixMap; -typedef BlockDiagonalMatrix::ConstMatrixMap ConstMatrixMap; -typedef BlockDiagonalMatrix::Vector Vector; -typedef MatrixBlockElementTpl MatrixBlockElement; -typedef MatrixBlockElementTpl MatrixMapBlockElement; -typedef MatrixBlockElementTpl ConstMatrixBlockElement; -typedef MatrixBlockElementTpl ConstMatrixMapBlockElement; +typedef internal::BlockDiagonalMatrix::Matrix Matrix; +typedef internal::BlockDiagonalMatrix::MatrixMap MatrixMap; +typedef internal::BlockDiagonalMatrix::ConstMatrixMap ConstMatrixMap; +typedef internal::BlockDiagonalMatrix::Vector Vector; +typedef internal::MatrixBlockElementTpl MatrixBlockElement; +typedef internal::MatrixBlockElementTpl MatrixMapBlockElement; +typedef internal::MatrixBlockElementTpl ConstMatrixBlockElement; +typedef internal::MatrixBlockElementTpl ConstMatrixMapBlockElement; template -void test_assignment(const MatrixBlockElementPlain<_MatrixBlockElement> & matrix_block_element) +void test_assignment( + const internal::MatrixBlockElementPlain<_MatrixBlockElement> & matrix_block_element) { const auto size = matrix_block_element.size(); const Matrix square_matrix = Matrix::Random(size, size); @@ -68,14 +69,15 @@ void test_assignment(const MatrixBlockElementPlain<_MatrixBlockElement> & matrix } template -void test_inverse(const MatrixBlockElementPlain<_MatrixBlockElement> & matrix_block_element) +void test_inverse( + const internal::MatrixBlockElementPlain<_MatrixBlockElement> & matrix_block_element) { const auto size = matrix_block_element.size(); const auto matrix_block_element_plain = matrix_block_element.matrix(); const auto matrix_block_element_inverse = matrix_block_element.inverse(); const auto matrix_block_element_inverse_plain = matrix_block_element_inverse.matrix(); - if (matrix_block_element.type() == pinocchio::MatrixBlockType::Zero) + if (matrix_block_element.type() == pinocchio::internal::MatrixBlockType::Zero) { BOOST_CHECK( matrix_block_element_inverse_plain @@ -101,7 +103,7 @@ void test_inverse(const MatrixBlockElementPlain<_MatrixBlockElement> & matrix_bl matrix_block_element_copy.setRandom(); const auto matrix_block_element_copy_inverse = matrix_block_element_copy.inverse(); - if (matrix_block_element_copy.type() == pinocchio::MatrixBlockType::Zero) + if (matrix_block_element_copy.type() == pinocchio::internal::MatrixBlockType::Zero) { BOOST_CHECK( matrix_block_element_copy_inverse.matrix() @@ -116,7 +118,8 @@ void test_inverse(const MatrixBlockElementPlain<_MatrixBlockElement> & matrix_bl } template -void test_operations(const MatrixBlockElementPlain<_MatrixBlockElement> & _matrix_block_element) +void test_operations( + const internal::MatrixBlockElementPlain<_MatrixBlockElement> & _matrix_block_element) { const auto & matrix_block_element = _matrix_block_element.derived(); @@ -138,7 +141,8 @@ void test_operations(const MatrixBlockElementPlain<_MatrixBlockElement> & _matri { auto block_sum = matrix_block_element + diagonal_vector.asDiagonal(); - MatrixBlockElement res_block = {pinocchio::MatrixBlockType::Diagonal, size, diagonal_vector}; + MatrixBlockElement res_block = { + pinocchio::internal::MatrixBlockType::Diagonal, size, diagonal_vector}; res_block = block_sum; const auto res_eigen = @@ -152,7 +156,8 @@ void test_operations(const MatrixBlockElementPlain<_MatrixBlockElement> & _matri { auto block_sum = matrix_block_element - diagonal_vector.asDiagonal(); - MatrixBlockElement res_block = {pinocchio::MatrixBlockType::Diagonal, size, diagonal_vector}; + MatrixBlockElement res_block = { + pinocchio::internal::MatrixBlockType::Diagonal, size, diagonal_vector}; res_block = block_sum; const auto res_eigen = @@ -211,8 +216,8 @@ void test_operations(const MatrixBlockElementPlain<_MatrixBlockElement> & _matri BOOST_AUTO_TEST_CASE(test_matrix_block_elements_eigen_map) { - typedef MatrixBlockElementTpl MatrixBlockElement; - typedef MatrixBlockElementTpl ConstMatrixBlockElement; + typedef internal::MatrixBlockElementTpl MatrixBlockElement; + typedef internal::MatrixBlockElementTpl ConstMatrixBlockElement; const Eigen::Index size = 10; // Test non const version @@ -220,7 +225,8 @@ BOOST_AUTO_TEST_CASE(test_matrix_block_elements_eigen_map) Matrix diagonal_vector = Matrix::Ones(size, 1); const auto matrix_map = make_map(diagonal_vector); - MatrixBlockElement lhs_block = {pinocchio::MatrixBlockType::Diagonal, size, matrix_map}; + MatrixBlockElement lhs_block = { + pinocchio::internal::MatrixBlockType::Diagonal, size, matrix_map}; BOOST_CHECK(lhs_block.map == diagonal_vector); BOOST_CHECK(lhs_block.map.data() == diagonal_vector.data()); @@ -234,7 +240,8 @@ BOOST_AUTO_TEST_CASE(test_matrix_block_elements_eigen_map) Matrix diagonal_vector = Matrix::Ones(size, 1); const auto matrix_map = make_map(diagonal_vector); - ConstMatrixBlockElement lhs_block = {pinocchio::MatrixBlockType::Diagonal, size, matrix_map}; + ConstMatrixBlockElement lhs_block = { + pinocchio::internal::MatrixBlockType::Diagonal, size, matrix_map}; BOOST_CHECK(lhs_block.map == diagonal_vector); BOOST_CHECK(lhs_block.map.data() == diagonal_vector.data()); @@ -246,14 +253,15 @@ BOOST_AUTO_TEST_CASE(test_matrix_block_elements_eigen_map) BOOST_AUTO_TEST_CASE(test_matrix_block_elements_eigen_matrix) { const Eigen::Index size = 10; - typedef MatrixBlockElementTpl MatrixBlockElement; - typedef MatrixBlockElementTpl ConstMatrixBlockElement; + typedef internal::MatrixBlockElementTpl MatrixBlockElement; + typedef internal::MatrixBlockElementTpl ConstMatrixBlockElement; // Test non const version { Matrix diagonal_vector = Matrix::Ones(size, 1); - MatrixBlockElement lhs_block = {pinocchio::MatrixBlockType::Diagonal, size, diagonal_vector}; + MatrixBlockElement lhs_block = { + pinocchio::internal::MatrixBlockType::Diagonal, size, diagonal_vector}; BOOST_CHECK(lhs_block.container() == diagonal_vector); @@ -266,7 +274,7 @@ BOOST_AUTO_TEST_CASE(test_matrix_block_elements_eigen_matrix) const Matrix diagonal_vector = Matrix::Ones(size, 1); ConstMatrixBlockElement lhs_block = { - pinocchio::MatrixBlockType::Diagonal, size, diagonal_vector}; + pinocchio::internal::MatrixBlockType::Diagonal, size, diagonal_vector}; BOOST_CHECK(lhs_block.container() == diagonal_vector); @@ -277,18 +285,19 @@ BOOST_AUTO_TEST_CASE(test_matrix_block_elements_eigen_matrix) BOOST_AUTO_TEST_CASE(test_inverse_method) { const Eigen::Index size = 10; - typedef MatrixBlockElementTpl MatrixBlockElement; + typedef internal::MatrixBlockElementTpl MatrixBlockElement; // Zero block { - MatrixBlockElement matrix_block_element = {pinocchio::MatrixBlockType::Zero, size}; + MatrixBlockElement matrix_block_element = {pinocchio::internal::MatrixBlockType::Zero, size}; test_inverse(matrix_block_element); } // Identity block { - MatrixBlockElement matrix_block_element = {pinocchio::MatrixBlockType::Identity, size}; + MatrixBlockElement matrix_block_element = { + pinocchio::internal::MatrixBlockType::Identity, size}; test_inverse(matrix_block_element); } @@ -298,7 +307,7 @@ BOOST_AUTO_TEST_CASE(test_inverse_method) const double scale_value = 1.; M11 scale_mat = M11(scale_value); MatrixBlockElement matrix_block_element = { - pinocchio::MatrixBlockType::ScalarIdentity, size, scale_mat}; + pinocchio::internal::MatrixBlockType::ScalarIdentity, size, scale_mat}; test_inverse(matrix_block_element); } @@ -307,7 +316,7 @@ BOOST_AUTO_TEST_CASE(test_inverse_method) { Matrix diagonal_vector = Matrix::Ones(size, 1); MatrixBlockElement matrix_block_element = { - pinocchio::MatrixBlockType::Diagonal, size, diagonal_vector}; + pinocchio::internal::MatrixBlockType::Diagonal, size, diagonal_vector}; test_inverse(matrix_block_element); } @@ -316,7 +325,7 @@ BOOST_AUTO_TEST_CASE(test_inverse_method) { Matrix plain_matrix = Matrix::Identity(size, size); MatrixBlockElement matrix_block_element = { - pinocchio::MatrixBlockType::Plain, size, plain_matrix}; + pinocchio::internal::MatrixBlockType::Plain, size, plain_matrix}; test_inverse(matrix_block_element); } @@ -340,19 +349,20 @@ BOOST_AUTO_TEST_CASE(test_nested_block_diagonal_block_element) const Eigen::Index plain_size = 4; const Eigen::Index total_size = diag_size + plain_size; - typedef BlockDiagonalMatrix::MatrixBlockElement BDMBlockElement; + typedef internal::BlockDiagonalMatrix::MatrixBlockElement BDMBlockElement; std::vector nested_subs; - nested_subs.emplace_back(pinocchio::MatrixBlockType::Diagonal, diag_size); - nested_subs.emplace_back(pinocchio::MatrixBlockType::Plain, plain_size); + nested_subs.emplace_back(pinocchio::internal::MatrixBlockType::Diagonal, diag_size); + nested_subs.emplace_back(pinocchio::internal::MatrixBlockType::Plain, plain_size); BDMBlockElement nested_block( - pinocchio::MatrixBlockType::NestedBlockDiagonal, std::move(nested_subs)); + pinocchio::internal::MatrixBlockType::NestedBlockDiagonal, std::move(nested_subs)); - BlockDiagonalMatrix bdm({nested_block}); + internal::BlockDiagonalMatrix bdm({nested_block}); BOOST_CHECK(bdm.rows() == total_size); BOOST_CHECK(bdm.cols() == total_size); BOOST_CHECK(bdm.blocks().size() == 1); - BOOST_CHECK(bdm.blocks()[0].type() == pinocchio::MatrixBlockType::NestedBlockDiagonal); + BOOST_CHECK( + bdm.blocks()[0].type() == pinocchio::internal::MatrixBlockType::NestedBlockDiagonal); BOOST_CHECK(bdm.blocks()[0].nested_blocks().size() == 2); // Populate sub-blocks with random data (make it invertible for inverse test) @@ -397,11 +407,11 @@ BOOST_AUTO_TEST_CASE(test_nested_block_diagonal_block_element) { std::vector res_nested_subs; - res_nested_subs.emplace_back(pinocchio::MatrixBlockType::Diagonal, diag_size); - res_nested_subs.emplace_back(pinocchio::MatrixBlockType::Plain, plain_size); + res_nested_subs.emplace_back(pinocchio::internal::MatrixBlockType::Diagonal, diag_size); + res_nested_subs.emplace_back(pinocchio::internal::MatrixBlockType::Plain, plain_size); BDMBlockElement res_nested_block( - pinocchio::MatrixBlockType::NestedBlockDiagonal, std::move(res_nested_subs)); - BlockDiagonalMatrix res_bdm({res_nested_block}); + pinocchio::internal::MatrixBlockType::NestedBlockDiagonal, std::move(res_nested_subs)); + pinocchio::internal::BlockDiagonalMatrix res_bdm({res_nested_block}); block.inverse(res_bdm.blocks()[0]); diff --git a/unittest/matrix-inverse.cpp b/unittest/matrix-inverse.cpp index b9deea6984..c0cf6165aa 100644 --- a/unittest/matrix-inverse.cpp +++ b/unittest/matrix-inverse.cpp @@ -44,7 +44,7 @@ void test_generated_inverse_impl() } Matrix res = Matrix::Zero(); - matrix_inversion_code_generated(mat, res); + internal::matrix_inversion_code_generated(mat, res); BOOST_CHECK((res * mat).isIdentity(1e-14 * res.norm())); BOOST_CHECK(mat.inverse().isApprox(res, 1e-14 * res.norm())); @@ -85,7 +85,7 @@ void test_matrix_inverse_on_dynamic_matrix_impl() } Matrix res = Matrix::Zero(size, size); - matrix_inversion(mat, res); + internal::matrix_inversion(mat, res); const double max_mat_value = mat.array().abs().maxCoeff(); const double max_res_value = res.array().abs().maxCoeff(); diff --git a/unittest/matrix-product.cpp b/unittest/matrix-product.cpp index 5c51eb0047..0d65398f85 100644 --- a/unittest/matrix-product.cpp +++ b/unittest/matrix-product.cpp @@ -38,7 +38,7 @@ void test( else if constexpr (internal::is_specialization_of_v) res_gt = res - (lhs * rhs).eval(); - matrix_product(lhs, rhs, res); + internal::matrix_product(lhs, rhs, res); BOOST_CHECK(res.isApprox(res_gt, 1e-14)); } diff --git a/unittest/serialization-math.cpp b/unittest/serialization-math.cpp index e90e2e4d9e..361a37542d 100644 --- a/unittest/serialization-math.cpp +++ b/unittest/serialization-math.cpp @@ -87,14 +87,14 @@ BOOST_AUTO_TEST_CASE(double_entry_container) BOOST_AUTO_TEST_CASE(matrix_block_element) { typedef Eigen::MatrixXd Matrix; - typedef pinocchio::MatrixBlockElementTpl MatrixBlockElement; + typedef pinocchio::internal::MatrixBlockElementTpl MatrixBlockElement; const Eigen::Index size = 10; Matrix diagonal_vector = Matrix::Ones(size, 1); MatrixBlockElement matrix_block_elt = { - pinocchio::MatrixBlockType::Diagonal, size, diagonal_vector}; + pinocchio::internal::MatrixBlockType::Diagonal, size, diagonal_vector}; BOOST_CHECK(matrix_block_elt.container() == diagonal_vector); @@ -105,7 +105,7 @@ BOOST_AUTO_TEST_CASE(block_diagonal_matrix) { typedef Eigen::MatrixXd Matrix; - typedef pinocchio::BlockDiagonalMatrix::MatrixBlockElement MatrixBlockElement; + typedef pinocchio::internal::BlockDiagonalMatrix::MatrixBlockElement MatrixBlockElement; typedef MatrixBlockElement::MatrixMap MatrixMap; const Eigen::Index size = 10; @@ -114,13 +114,13 @@ BOOST_AUTO_TEST_CASE(block_diagonal_matrix) auto diagonal_vector_map = pinocchio::make_map(diagonal_vector); MatrixBlockElement matrix_block_elt1 = { - pinocchio::MatrixBlockType::Diagonal, size, diagonal_vector_map}; + pinocchio::internal::MatrixBlockType::Diagonal, size, diagonal_vector_map}; - MatrixBlockElement matrix_block_elt2 = {pinocchio::MatrixBlockType::Zero, size}; + MatrixBlockElement matrix_block_elt2 = {pinocchio::internal::MatrixBlockType::Zero, size}; - MatrixBlockElement matrix_block_elt3 = {pinocchio::MatrixBlockType::Identity, size}; + MatrixBlockElement matrix_block_elt3 = {pinocchio::internal::MatrixBlockType::Identity, size}; - pinocchio::BlockDiagonalMatrix block_diagonal_matrix( + pinocchio::internal::BlockDiagonalMatrix block_diagonal_matrix( {matrix_block_elt1, matrix_block_elt2, matrix_block_elt3, matrix_block_elt1}); generic_test( @@ -129,7 +129,7 @@ BOOST_AUTO_TEST_CASE(block_diagonal_matrix) BOOST_AUTO_TEST_CASE(block_diagonal_matrix_nested) { - typedef pinocchio::BlockDiagonalMatrix::MatrixBlockElement MatrixBlockElement; + typedef pinocchio::internal::BlockDiagonalMatrix::MatrixBlockElement MatrixBlockElement; const Eigen::Index flat_diag_size = 3; const Eigen::Index sub_diag_size = 2; @@ -137,14 +137,15 @@ BOOST_AUTO_TEST_CASE(block_diagonal_matrix_nested) // Build a BDM with one flat Diagonal block and one NestedBlockDiagonal block // (containing a Diagonal sub-block and a Plain sub-block). - MatrixBlockElement flat_block(pinocchio::MatrixBlockType::Diagonal, flat_diag_size); + MatrixBlockElement flat_block(pinocchio::internal::MatrixBlockType::Diagonal, flat_diag_size); std::vector subs; - subs.emplace_back(pinocchio::MatrixBlockType::Diagonal, sub_diag_size); - subs.emplace_back(pinocchio::MatrixBlockType::Plain, sub_plain_size); - MatrixBlockElement nested_block(pinocchio::MatrixBlockType::NestedBlockDiagonal, std::move(subs)); + subs.emplace_back(pinocchio::internal::MatrixBlockType::Diagonal, sub_diag_size); + subs.emplace_back(pinocchio::internal::MatrixBlockType::Plain, sub_plain_size); + MatrixBlockElement nested_block( + pinocchio::internal::MatrixBlockType::NestedBlockDiagonal, std::move(subs)); - pinocchio::BlockDiagonalMatrix block_diagonal_matrix({flat_block, nested_block}); + pinocchio::internal::BlockDiagonalMatrix block_diagonal_matrix({flat_block, nested_block}); for (auto & block : block_diagonal_matrix.blocks()) block.setRandom(); From 52b74e3cc1bd3dcabf42efcf846c3ac6352d5e83 Mon Sep 17 00:00:00 2001 From: Joris Vaillant Date: Thu, 2 Apr 2026 15:29:01 +0200 Subject: [PATCH 05/14] math: Turn ALLOCA macro internal --- .../src/algorithm/constraint-cholesky-def.hxx | 7 ++++--- .../delassus-operator-cholesky-expression.hxx | 2 +- .../delassus-operator-rigid-body-visitors.hxx | 8 ++++---- .../src/algorithm/delassus-operator-rigid-body.hxx | 6 +++--- .../src/algorithm/delassus-operator-sparse.hxx | 3 ++- .../src/algorithm/loop-constrained-aba.hxx | 4 ++-- .../src/math/block-diagonal-matrix-inverse.hxx | 6 +++--- .../src/math/block-diagonal-matrix-sum.hxx | 2 +- .../pinocchio/src/math/block-diagonal-matrix.hxx | 2 +- .../src/math/matrix-block-element-plain.hxx | 4 ++-- include/pinocchio/src/math/matrix.hxx | 2 +- include/pinocchio/src/utils/alloca.hxx | 14 +++++++------- unittest/alloca.cpp | 2 +- 13 files changed, 32 insertions(+), 30 deletions(-) diff --git a/include/pinocchio/src/algorithm/constraint-cholesky-def.hxx b/include/pinocchio/src/algorithm/constraint-cholesky-def.hxx index 86d786d7a2..f38f43d586 100644 --- a/include/pinocchio/src/algorithm/constraint-cholesky-def.hxx +++ b/include/pinocchio/src/algorithm/constraint-cholesky-def.hxx @@ -338,7 +338,7 @@ namespace pinocchio // { // const Eigen::Index slice_dim = nv; // typedef Eigen::Map MapVector; - // MapVector DUt_partial = MapVector(PINOCCHIO_EIGEN_MAP_ALLOCA(Scalar,slice_dim,1)); + // MapVector DUt_partial = MapVector(_PINOCCHIO_EIGEN_MAP_ALLOCA(Scalar,slice_dim,1)); // DUt_partial.noalias() = // UtopRight.row(j).transpose().cwiseProduct(Dtail); @@ -349,8 +349,9 @@ namespace pinocchio // } // typedef Eigen::Map MapRowMatrix; - // MapRowMatrix OSIMinv = MapRowMatrix(PINOCCHIO_EIGEN_MAP_ALLOCA(Scalar, total_constraint_size, - // nv)); OSIMinv.noalias() = UtopRight * Dtail.asDiagonal(); delassus_block.noalias() = OSIMinv + // MapRowMatrix OSIMinv = MapRowMatrix(_PINOCCHIO_EIGEN_MAP_ALLOCA(Scalar, + // total_constraint_size, nv)); OSIMinv.noalias() = UtopRight * Dtail.asDiagonal(); + // delassus_block.noalias() = OSIMinv // * UtopRight.transpose(); delassus_block.noalias() = (UtopRight * Dtail.asDiagonal()) * UtopRight.transpose(); diff --git a/include/pinocchio/src/algorithm/delassus-operator-cholesky-expression.hxx b/include/pinocchio/src/algorithm/delassus-operator-cholesky-expression.hxx index 7937a79c3a..4f8767e055 100644 --- a/include/pinocchio/src/algorithm/delassus-operator-cholesky-expression.hxx +++ b/include/pinocchio/src/algorithm/delassus-operator-cholesky-expression.hxx @@ -172,7 +172,7 @@ namespace pinocchio // const auto U1 = self.U.topLeftCorner(self.constraintDim(), self.constraintDim()); // { // typedef Eigen::Map MapType; - // MapType tmp_mat = MapType(PINOCCHIO_EIGEN_MAP_ALLOCA(Scalar, x.rows(), x.cols())); + // MapType tmp_mat = MapType(_PINOCCHIO_EIGEN_MAP_ALLOCA(Scalar, x.rows(), x.cols())); // // tmp_mat.noalias() = U1.adjoint() * x; // triangularMatrixMatrixProduct(U1.adjoint(), x.derived(), tmp_mat); diff --git a/include/pinocchio/src/algorithm/delassus-operator-rigid-body-visitors.hxx b/include/pinocchio/src/algorithm/delassus-operator-rigid-body-visitors.hxx index e8d780a10e..1319dc6fd0 100644 --- a/include/pinocchio/src/algorithm/delassus-operator-rigid-body-visitors.hxx +++ b/include/pinocchio/src/algorithm/delassus-operator-rigid-body-visitors.hxx @@ -99,8 +99,8 @@ namespace pinocchio using Matrix6xNV = std::remove_reference_t; typedef Eigen::Map MapMatrix6xNV; - MapMatrix6xNV mat1_tmp = MapMatrix6xNV(PINOCCHIO_EIGEN_MAP_ALLOCA(Scalar, 6, jmodel.nv())); - MapMatrix6xNV mat2_tmp = MapMatrix6xNV(PINOCCHIO_EIGEN_MAP_ALLOCA(Scalar, 6, jmodel.nv())); + MapMatrix6xNV mat1_tmp = MapMatrix6xNV(_PINOCCHIO_EIGEN_MAP_ALLOCA(Scalar, 6, jmodel.nv())); + MapMatrix6xNV mat2_tmp = MapMatrix6xNV(_PINOCCHIO_EIGEN_MAP_ALLOCA(Scalar, 6, jmodel.nv())); auto & JDinv = mat1_tmp; DO_NOT_PROMOTE_STATIC_EVAL(JDinv.noalias()) = Jcols * jdata_augmented.Dinv(); @@ -308,7 +308,7 @@ namespace pinocchio { using VectorNV = std::remove_reference_t; using MapVectorNV = Eigen::Map; - MapVectorNV res = MapVectorNV(PINOCCHIO_EIGEN_MAP_ALLOCA(Scalar, jmodel.nv(), 1)); + MapVectorNV res = MapVectorNV(_PINOCCHIO_EIGEN_MAP_ALLOCA(Scalar, jmodel.nv(), 1)); DO_NOT_PROMOTE_STATIC_EVAL(res.noalias()) = (jdata.Dinv() * jmodel.jointVelocitySelector(internal_data.u)); @@ -373,7 +373,7 @@ namespace pinocchio using VectorNV = std::remove_reference_t; using MapVectorNV = Eigen::Map; MapVectorNV projected_coupling_forces = - MapVectorNV(PINOCCHIO_EIGEN_MAP_ALLOCA(Scalar, jmodel.nv(), 1)); + MapVectorNV(_PINOCCHIO_EIGEN_MAP_ALLOCA(Scalar, jmodel.nv(), 1)); projected_coupling_forces.setZero(); for (const JointIndex joint_j : joint_neighbours) diff --git a/include/pinocchio/src/algorithm/delassus-operator-rigid-body.hxx b/include/pinocchio/src/algorithm/delassus-operator-rigid-body.hxx index 5ed4612fe1..9f9e379ea7 100644 --- a/include/pinocchio/src/algorithm/delassus-operator-rigid-body.hxx +++ b/include/pinocchio/src/algorithm/delassus-operator-rigid-body.hxx @@ -787,7 +787,7 @@ namespace pinocchio // TODO(jcarpent): extend the code to operator on matrices // typedef Eigen::Map MapVectorXs; - // MapVectorXs u = MapVectorXs(PINOCCHIO_EIGEN_MAP_ALLOCA(Scalar, model_ref.nv, 1)); + // MapVectorXs u = MapVectorXs(_PINOCCHIO_EIGEN_MAP_ALLOCA(Scalar, model_ref.nv, 1)); // { // auto & u = internal_data.u; // u.setZero(); @@ -909,7 +909,7 @@ namespace pinocchio auto & internal_data = this->m_internal_data; typedef Eigen::Map MapVectorXs; - MapVectorXs mat_tmp = MapVectorXs(PINOCCHIO_EIGEN_MAP_ALLOCA(Scalar, size(), 1)); + MapVectorXs mat_tmp = MapVectorXs(_PINOCCHIO_EIGEN_MAP_ALLOCA(Scalar, size(), 1)); // mat.array() *= m_sum_compliance_damping_inverse.array(); m_sum_compliance_damping_inverse.template applyOnTheRight<::pinocchio::internal::assign_op>( @@ -919,7 +919,7 @@ namespace pinocchio // Make a pass over the whole set of constraints to add the contributions of constraint typedef Eigen::Map MapVectorXs; - MapVectorXs u = MapVectorXs(PINOCCHIO_EIGEN_MAP_ALLOCA(Scalar, model_ref.nv, 1)); + MapVectorXs u = MapVectorXs(_PINOCCHIO_EIGEN_MAP_ALLOCA(Scalar, model_ref.nv, 1)); // u and internal_data.of_augmented are reset by mapConstraintForcesToJointSpace mapConstraintForcesToJointSpace( model_ref, data_ref, constraint_models_ref, constraint_datas_ref, mat, diff --git a/include/pinocchio/src/algorithm/delassus-operator-sparse.hxx b/include/pinocchio/src/algorithm/delassus-operator-sparse.hxx index 8d59d42537..efe9a20de2 100644 --- a/include/pinocchio/src/algorithm/delassus-operator-sparse.hxx +++ b/include/pinocchio/src/algorithm/delassus-operator-sparse.hxx @@ -43,7 +43,8 @@ namespace pinocchio typedef typename PlainMatrix::Scalar Scalar; typedef Eigen::Map MapPlainMatrix; - MapPlainMatrix tmp = MapPlainMatrix(PINOCCHIO_EIGEN_MAP_ALLOCA(Scalar, b.rows(), b.cols())); + MapPlainMatrix tmp = + MapPlainMatrix(_PINOCCHIO_EIGEN_MAP_ALLOCA(Scalar, b.rows(), b.cols())); if (m_P.size() > 0) tmp.noalias() = m_P * b; else diff --git a/include/pinocchio/src/algorithm/loop-constrained-aba.hxx b/include/pinocchio/src/algorithm/loop-constrained-aba.hxx index 63862e5770..f74a1ad68b 100644 --- a/include/pinocchio/src/algorithm/loop-constrained-aba.hxx +++ b/include/pinocchio/src/algorithm/loop-constrained-aba.hxx @@ -137,8 +137,8 @@ namespace pinocchio using Matrix6xNV = std::remove_reference_t; using MapMatrix6xNV = Eigen::Map; - MapMatrix6xNV mat1_tmp = MapMatrix6xNV(PINOCCHIO_EIGEN_MAP_ALLOCA(Scalar, 6, jmodel.nv())); - MapMatrix6xNV mat2_tmp = MapMatrix6xNV(PINOCCHIO_EIGEN_MAP_ALLOCA(Scalar, 6, jmodel.nv())); + MapMatrix6xNV mat1_tmp = MapMatrix6xNV(_PINOCCHIO_EIGEN_MAP_ALLOCA(Scalar, 6, jmodel.nv())); + MapMatrix6xNV mat2_tmp = MapMatrix6xNV(_PINOCCHIO_EIGEN_MAP_ALLOCA(Scalar, 6, jmodel.nv())); auto & JDinv = mat1_tmp; JDinv.noalias() = Jcols * jdata.Dinv(); diff --git a/include/pinocchio/src/math/block-diagonal-matrix-inverse.hxx b/include/pinocchio/src/math/block-diagonal-matrix-inverse.hxx index 33ece61847..c66e6c5d2e 100644 --- a/include/pinocchio/src/math/block-diagonal-matrix-inverse.hxx +++ b/include/pinocchio/src/math/block-diagonal-matrix-inverse.hxx @@ -142,7 +142,7 @@ namespace pinocchio // For NestedBlockDiagonal entries, placement-new with a moved sub-vector and // call the destructor explicitly afterwards to avoid a memory leak. MatrixBlockElement * new_pattern = static_cast( - PINOCCHIO_ALLOCA(num_blocks * sizeof(MatrixBlockElement))); + _PINOCCHIO_ALLOCA(num_blocks * sizeof(MatrixBlockElement))); bool has_nested_in_pattern = false; for (std::size_t i = 0; i < num_blocks; ++i) { @@ -211,7 +211,7 @@ namespace pinocchio typedef Eigen::Map ResMatrixMap; typedef MatrixBlockElementTpl ResMatrixBlockElement; - ResMatrixMap tmp_map(PINOCCHIO_EIGEN_MAP_ALLOCA_ALIGNED( + ResMatrixMap tmp_map(_PINOCCHIO_EIGEN_MAP_ALLOCA_ALIGNED( Scalar, sub_size, sub_size, static_cast(ResAlignment))); ResMatrixBlockElement temp_input(input_sub.type(), sub_size, tmp_map); temp_input.container() = input_sub.container(); @@ -235,7 +235,7 @@ namespace pinocchio ResMatrixMap; typedef MatrixBlockElementTpl ResMatrixBlockElement; - ResMatrixMap tmp_map(PINOCCHIO_EIGEN_MAP_ALLOCA_ALIGNED( + ResMatrixMap tmp_map(_PINOCCHIO_EIGEN_MAP_ALLOCA_ALIGNED( Scalar, block_size, block_size, static_cast(ResAlignment))); ResMatrixBlockElement temp_input(input_block.type(), block_size, tmp_map); diff --git a/include/pinocchio/src/math/block-diagonal-matrix-sum.hxx b/include/pinocchio/src/math/block-diagonal-matrix-sum.hxx index e1c9a1de86..24600aa66a 100644 --- a/include/pinocchio/src/math/block-diagonal-matrix-sum.hxx +++ b/include/pinocchio/src/math/block-diagonal-matrix-sum.hxx @@ -166,7 +166,7 @@ namespace pinocchio // For NestedBlockDiagonal entries, placement-new with a moved sub-vector and // call the destructor explicitly afterwards to avoid a memory leak. MatrixBlockElement * new_pattern = static_cast( - PINOCCHIO_ALLOCA(num_blocks * sizeof(MatrixBlockElement))); + _PINOCCHIO_ALLOCA(num_blocks * sizeof(MatrixBlockElement))); bool has_nested_in_pattern = false; for (std::size_t i = 0; i < num_blocks; ++i) { diff --git a/include/pinocchio/src/math/block-diagonal-matrix.hxx b/include/pinocchio/src/math/block-diagonal-matrix.hxx index e751e503c2..9a92abffc4 100644 --- a/include/pinocchio/src/math/block-diagonal-matrix.hxx +++ b/include/pinocchio/src/math/block-diagonal-matrix.hxx @@ -873,7 +873,7 @@ namespace pinocchio // analysis block pattern and extract memory/size info const std::size_t num_blocks = size; MatrixInfo * memory_block_sizes = - static_cast(PINOCCHIO_ALLOCA(total_memory_entries * sizeof(MatrixInfo))); + static_cast(_PINOCCHIO_ALLOCA(total_memory_entries * sizeof(MatrixInfo))); std::size_t memory_block_id = 0; m_matrix_block_elements.reserve(size); diff --git a/include/pinocchio/src/math/matrix-block-element-plain.hxx b/include/pinocchio/src/math/matrix-block-element-plain.hxx index 0c48e5a711..03cc82a055 100644 --- a/include/pinocchio/src/math/matrix-block-element-plain.hxx +++ b/include/pinocchio/src/math/matrix-block-element-plain.hxx @@ -681,8 +681,8 @@ namespace pinocchio if (isSymmetric(container())) { typedef Eigen::Map MapMatrix; - MapMatrix tmp = - MapMatrix(PINOCCHIO_EIGEN_MAP_ALLOCA(Scalar, container().rows(), container().cols())); + MapMatrix tmp = MapMatrix( + _PINOCCHIO_EIGEN_MAP_ALLOCA(Scalar, container().rows(), container().cols())); tmp = container(); matrix_inversion(tmp, res.container()); } diff --git a/include/pinocchio/src/math/matrix.hxx b/include/pinocchio/src/math/matrix.hxx index fb113e10bb..18b62413b0 100644 --- a/include/pinocchio/src/math/matrix.hxx +++ b/include/pinocchio/src/math/matrix.hxx @@ -670,7 +670,7 @@ namespace pinocchio typedef Eigen::Map MapMatrix; auto & mat = mat_.const_cast_derived(); - MapMatrix tmp = MapMatrix(PINOCCHIO_EIGEN_MAP_ALLOCA(Scalar, mat.rows(), mat.rows())); + MapMatrix tmp = MapMatrix(_PINOCCHIO_EIGEN_MAP_ALLOCA(Scalar, mat.rows(), mat.rows())); tmp = 0.5 * (mat + mat.transpose()); mat = tmp; diff --git a/include/pinocchio/src/utils/alloca.hxx b/include/pinocchio/src/utils/alloca.hxx index f10cb1d2e2..151b0b491e 100644 --- a/include/pinocchio/src/utils/alloca.hxx +++ b/include/pinocchio/src/utils/alloca.hxx @@ -11,12 +11,12 @@ #include "pinocchio/utils/alloca.hpp" #endif // PINOCCHIO_LSP -#define PINOCCHIO_ALLOCA EIGEN_ALLOCA -#define PINOCCHIO_ALIGNED_PTR(ptr, align) \ +#define _PINOCCHIO_ALLOCA EIGEN_ALLOCA +#define _PINOCCHIO_ALIGNED_PTR(ptr, align) \ reinterpret_cast(((intptr_t)ptr + (align - 1)) & ~(align - 1)) -#define PINOCCHIO_EIGEN_MAP_ALLOCA(S, rows, cols) \ - PINOCCHIO_EIGEN_MAP_ALLOCA_ALIGNED(S, rows, cols, EIGEN_DEFAULT_ALIGN_BYTES) -#define PINOCCHIO_EIGEN_MAP_ALLOCA_ALIGNED(S, rows, cols, align) \ - static_cast(PINOCCHIO_ALIGNED_PTR( \ - PINOCCHIO_ALLOCA(size_t(rows * cols) * sizeof(S) + (align > 0 ? (align - 1) : 0)), align)), \ +#define _PINOCCHIO_EIGEN_MAP_ALLOCA(S, rows, cols) \ + _PINOCCHIO_EIGEN_MAP_ALLOCA_ALIGNED(S, rows, cols, EIGEN_DEFAULT_ALIGN_BYTES) +#define _PINOCCHIO_EIGEN_MAP_ALLOCA_ALIGNED(S, rows, cols, align) \ + static_cast(_PINOCCHIO_ALIGNED_PTR( \ + _PINOCCHIO_ALLOCA(size_t(rows * cols) * sizeof(S) + (align > 0 ? (align - 1) : 0)), align)), \ rows, cols diff --git a/unittest/alloca.cpp b/unittest/alloca.cpp index 4b1197256d..070b3d8682 100644 --- a/unittest/alloca.cpp +++ b/unittest/alloca.cpp @@ -45,7 +45,7 @@ BOOST_AUTO_TEST_CASE(macro) { const Eigen::Index rows = 10, cols = 20; typedef Eigen::Map MapType; - MapType map = MapType(PINOCCHIO_EIGEN_MAP_ALLOCA(Eigen::MatrixXd::Scalar, rows, cols)); + MapType map = MapType(_PINOCCHIO_EIGEN_MAP_ALLOCA(Eigen::MatrixXd::Scalar, rows, cols)); map.setZero(); BOOST_CHECK(map.rows() == rows); BOOST_CHECK(map.cols() == cols); From 46f15435f62f36cc84cc90486fff40600f0f04b4 Mon Sep 17 00:00:00 2001 From: Joris Vaillant Date: Thu, 2 Apr 2026 15:53:09 +0200 Subject: [PATCH 06/14] math: Turn MatrixInfo internal --- include/pinocchio/src/math/matrix-info.hxx | 112 +++++++++++---------- unittest/matrix-stack.cpp | 14 +-- 2 files changed, 65 insertions(+), 61 deletions(-) diff --git a/include/pinocchio/src/math/matrix-info.hxx b/include/pinocchio/src/math/matrix-info.hxx index 305e5481ad..85da60ce45 100644 --- a/include/pinocchio/src/math/matrix-info.hxx +++ b/include/pinocchio/src/math/matrix-info.hxx @@ -13,76 +13,80 @@ /** * @file matrix-info.hpp - * @brief Defines the pinocchio::MatrixInfo struct, a utility for storing matrix dimensions. + * @brief Defines the pinocchio::internal::MatrixInfo struct, a utility for storing matrix + * dimensions. */ namespace pinocchio { - - /** - * @ingroup pinocchio_math_linalg - * @brief A structure to hold and query the dimensions (rows and columns) of a matrix. - * - * @details This utility struct is used to store and pass around the size information of a matrix, - * particularly when the dimensions need to be queried from an algorithm or function. - * - * A default-constructed instance is initialized to an invalid state (`rows = -1`, `cols - * = -1`), which can be verified using the isValid() method. - */ - struct MatrixInfo + namespace internal { - /// @brief Default constructor. Initializes to an invalid state (-1, -1). - MatrixInfo() = default; - - /** - * @brief Constructs a MatrixInfo object with specified dimensions. - * @param[in] rows The number of rows for the matrix. - * @param[in] cols The number of columns for the matrix. - */ - MatrixInfo(const Eigen::Index rows, const Eigen::Index cols) - : m_rows(rows) - , m_cols(cols) - { - } /** - * @brief Checks if the stored matrix dimensions are valid. + * @ingroup pinocchio_math_linalg + * @brief A structure to hold and query the dimensions (rows and columns) of a matrix. * - * @details A MatrixInfo object is considered valid if and only if both its `rows` and `cols` - * attributes are non-negative. + * @details This utility struct is used to store and pass around the size information of a + * matrix, particularly when the dimensions need to be queried from an algorithm or function. * - * @return `true` if both rows and cols are non-negative, `false` otherwise. + * A default-constructed instance is initialized to an invalid state (`rows = -1`, + * `cols = -1`), which can be verified using the isValid() method. */ - bool isValid() const + struct MatrixInfo { - return m_rows >= 0 && m_cols >= 0; - } + /// @brief Default constructor. Initializes to an invalid state (-1, -1). + MatrixInfo() = default; - /// @brief Returns the number of rows. - Eigen::Index rows() const - { - return m_rows; - } + /** + * @brief Constructs a MatrixInfo object with specified dimensions. + * @param[in] rows The number of rows for the matrix. + * @param[in] cols The number of columns for the matrix. + */ + MatrixInfo(const Eigen::Index rows, const Eigen::Index cols) + : m_rows(rows) + , m_cols(cols) + { + } - /// @brief Returns the number of columns. - Eigen::Index cols() const - { - return m_cols; - } + /** + * @brief Checks if the stored matrix dimensions are valid. + * + * @details A MatrixInfo object is considered valid if and only if both its `rows` and `cols` + * attributes are non-negative. + * + * @return `true` if both rows and cols are non-negative, `false` otherwise. + */ + bool isValid() const + { + return m_rows >= 0 && m_cols >= 0; + } - /// @brief Returns the total number of elements in the matrix (rows * cols). - Eigen::Index size() const - { - return m_rows * m_cols; - } + /// @brief Returns the number of rows. + Eigen::Index rows() const + { + return m_rows; + } + + /// @brief Returns the number of columns. + Eigen::Index cols() const + { + return m_cols; + } + + /// @brief Returns the total number of elements in the matrix (rows * cols). + Eigen::Index size() const + { + return m_rows * m_cols; + } - protected: - /// @brief The number of rows of the matrix. Initialized to -1. - Eigen::Index m_rows = -1; + protected: + /// @brief The number of rows of the matrix. Initialized to -1. + Eigen::Index m_rows = -1; - /// @brief The number of columns of the matrix. Initialized to -1. - Eigen::Index m_cols = -1; + /// @brief The number of columns of the matrix. Initialized to -1. + Eigen::Index m_cols = -1; - }; // struct MatrixInfo + }; // struct MatrixInfo + } // namespace internal } // namespace pinocchio diff --git a/unittest/matrix-stack.cpp b/unittest/matrix-stack.cpp index 7a7756d882..ad54b5d5c9 100644 --- a/unittest/matrix-stack.cpp +++ b/unittest/matrix-stack.cpp @@ -160,7 +160,7 @@ BOOST_AUTO_TEST_CASE(matrix_stack_from_matrix_info) { // Zero block allocation { - const std::vector matrix_infos; + const std::vector matrix_infos; const MatrixXsStack matrix_stack(matrix_infos); BOOST_CHECK(matrix_stack.empty()); @@ -169,7 +169,7 @@ BOOST_AUTO_TEST_CASE(matrix_stack_from_matrix_info) // Single block allocation { const Eigen::Index rows = 10, cols = 20; - const std::vector matrix_infos = {{rows, cols}}; + const std::vector matrix_infos = {{rows, cols}}; const MatrixXsStack matrix_stack(matrix_infos); @@ -183,7 +183,7 @@ BOOST_AUTO_TEST_CASE(matrix_stack_from_matrix_info) { const size_t num_blocks = 10; const Eigen::Index rows = 10, cols = 20; - std::vector matrix_infos(num_blocks); + std::vector matrix_infos(num_blocks); for (const auto & block_info : matrix_infos) { @@ -209,7 +209,7 @@ BOOST_AUTO_TEST_CASE(matrix_stack_from_matrix_info) BOOST_AUTO_TEST_CASE(matrix_stack_move_constructor) { const Eigen::Index rows = 10, cols = 20; - const std::vector matrix_infos = {{rows, cols}}; + const std::vector matrix_infos = {{rows, cols}}; MatrixXsStack matrix_stack(matrix_infos); matrix_stack.back().setZero(); @@ -222,7 +222,7 @@ BOOST_AUTO_TEST_CASE(matrix_stack_move_constructor) BOOST_AUTO_TEST_CASE(matrix_stack_move_assignment_operator) { const Eigen::Index rows = 10, cols = 20; - const std::vector matrix_infos = {{rows, cols}}; + const std::vector matrix_infos = {{rows, cols}}; MatrixXsStack matrix_stack(matrix_infos); matrix_stack.back().setIdentity(); @@ -241,14 +241,14 @@ BOOST_AUTO_TEST_CASE(matrix_stack_move_assignment_operator) BOOST_AUTO_TEST_CASE(matrix_stack_rebuild) { const Eigen::Index rows1 = 10, cols1 = 20; - const std::vector matrix_infos1 = {{rows1, cols1}}; + const std::vector matrix_infos1 = {{rows1, cols1}}; MatrixXsStack matrix_stack1(matrix_infos1); const void * matrix_stack1_data_ptr = matrix_stack1.data(); matrix_stack1.back().setIdentity(); const Eigen::Index rows2 = 20, cols2 = 40; - const std::vector matrix_infos2 = {{rows2, cols2}}; + const std::vector matrix_infos2 = {{rows2, cols2}}; MatrixXsStack matrix_stack2(matrix_infos2); const void * matrix_stack2_data_ptr = matrix_stack2.data(); From c0fa2bc09ae208d08cc4bee641224ac14e6712ef Mon Sep 17 00:00:00 2001 From: Joris Vaillant Date: Thu, 2 Apr 2026 16:28:13 +0200 Subject: [PATCH 07/14] utils: Turn size-in-byte, eigen-helper, promote-static-eval, std-vector and reference internal --- benchmark/timings-eigen.cpp | 2 +- .../src/algorithm/constraint-cholesky-def.hxx | 15 +- .../algorithm/contact-inverse-dynamics.hxx | 14 +- .../delassus-operator-rigid-body-visitors.hxx | 2 +- .../delassus-operator-rigid-body.hxx | 66 +-- .../solvers/constraint-solver-utils.hxx | 20 +- .../src/algorithm/solvers/pgs-solver.hxx | 4 +- .../src/constraints/constraint-ordering.hxx | 10 +- .../src/constraints/contact-info.hxx | 2 +- include/pinocchio/src/constraints/utils.hxx | 48 +- .../src/math/block-diagonal-matrix.hxx | 2 +- .../src/math/matrix-block-element.hxx | 12 +- .../serialization/matrix-block-element.hxx | 4 +- include/pinocchio/src/utils/eigen-helpers.hxx | 229 ++++---- .../src/utils/promote-static-eval.hxx | 63 +-- include/pinocchio/src/utils/reference.hxx | 380 ++++++------- include/pinocchio/src/utils/size-in-bytes.hxx | 269 +++++----- include/pinocchio/src/utils/std-vector.hxx | 501 +++++++++--------- unittest/delassus-operations.cpp | 36 +- unittest/delassus-operator-rigid-body.cpp | 24 +- unittest/eigen-basic-op.cpp | 6 +- unittest/promote-static-eval.cpp | 48 +- unittest/promote-static-op.cpp | 34 +- unittest/reference.cpp | 2 +- unittest/size-in-bytes.cpp | 28 +- 25 files changed, 922 insertions(+), 899 deletions(-) diff --git a/benchmark/timings-eigen.cpp b/benchmark/timings-eigen.cpp index 538eeb3cb1..a0b03ca90f 100644 --- a/benchmark/timings-eigen.cpp +++ b/benchmark/timings-eigen.cpp @@ -110,7 +110,7 @@ void matrix_mult_matrix_call( const MatrixBase & m, const MatrixBase & rhs, const MatrixBase & lhs) { if constexpr (evaluation_mode == EvaluationMode::STATIC_OP) - pinocchio::promote_static_eval<10>(lhs.const_cast_derived().noalias()) = m * rhs; + pinocchio::internal::promote_static_eval<10>(lhs.const_cast_derived().noalias()) = m * rhs; else if constexpr (evaluation_mode == EvaluationMode::MANUAL) pinocchio::internal::matrix_product( m.derived(), rhs.derived(), lhs.const_cast_derived()); diff --git a/include/pinocchio/src/algorithm/constraint-cholesky-def.hxx b/include/pinocchio/src/algorithm/constraint-cholesky-def.hxx index f38f43d586..67afdd16b3 100644 --- a/include/pinocchio/src/algorithm/constraint-cholesky-def.hxx +++ b/include/pinocchio/src/algorithm/constraint-cholesky-def.hxx @@ -116,8 +116,8 @@ namespace pinocchio Eigen::Index row_id = 0; for (std::size_t i = 0; i < constraint_models.size(); i++) { - const auto & cmodel = helper::get_ref(constraint_models[i]); - const auto & cdata = helper::get_ref(constraint_datas[i]); + const auto & cmodel = internal::helper::get_ref(constraint_models[i]); + const auto & cdata = internal::helper::get_ref(constraint_datas[i]); for (Eigen::Index k = 0; k < cmodel.residualSize(); ++k, row_id++) { cmodel.getRowIndexes(model, data, cdata, k, m_scratch_row_indexes); @@ -237,8 +237,8 @@ namespace pinocchio U.topRightCorner(total_constraint_size, model.nv).setZero(); for (size_t constraint_id = 0; constraint_id < num_constraints; ++constraint_id) { - const auto & cmodel = helper::get_ref(constraint_models[constraint_id]); - const auto & cdata = helper::get_ref(constraint_datas[constraint_id]); + const auto & cmodel = internal::helper::get_ref(constraint_models[constraint_id]); + const auto & cdata = internal::helper::get_ref(constraint_datas[constraint_id]); const Eigen::Index constraint_size = cmodel.residualSize(); auto U_block = U.block(current_row, total_constraint_size, constraint_size, model.nv); @@ -277,8 +277,8 @@ namespace pinocchio for (size_t index = 0; index < num_constraints; ++index) { const size_t constraint_id = num_constraints - 1 - index; - const auto & cmodel = helper::get_ref(constraint_models[constraint_id]); - const auto & cdata = helper::get_ref(constraint_datas[constraint_id]); + const auto & cmodel = internal::helper::get_ref(constraint_models[constraint_id]); + const auto & cdata = internal::helper::get_ref(constraint_datas[constraint_id]); const Eigen::Index constraint_size = cmodel.residualSize(); for (Eigen::Index constraint_row_id = constraint_size - 1; constraint_row_id >= 0; @@ -1016,7 +1016,8 @@ namespace pinocchio return U_storage.sizeInBytes() + D_storage.sizeInBytes() + Dinv_storage.sizeInBytes() + compliance_storage.sizeInBytes() + m_damping.sizeInBytes() + m_sum_compliance_damping.sizeInBytes() + delassus_block_storage.sizeInBytes() - + pinocchio::sizeInBytes(parents_fromRow) + pinocchio::sizeInBytes(nv_subtree_fromRow) + + pinocchio::internal::sizeInBytes(parents_fromRow) + + pinocchio::internal::sizeInBytes(nv_subtree_fromRow) // + pinocchio::sizeInBytes(rowise_sparsity_pattern) ; } diff --git a/include/pinocchio/src/algorithm/contact-inverse-dynamics.hxx b/include/pinocchio/src/algorithm/contact-inverse-dynamics.hxx index 39fd9ab803..ce3fefe698 100644 --- a/include/pinocchio/src/algorithm/contact-inverse-dynamics.hxx +++ b/include/pinocchio/src/algorithm/contact-inverse-dynamics.hxx @@ -29,10 +29,10 @@ namespace pinocchio bool solve_ncp) { static_assert( - helper::is_std_vector_v, + internal::helper::is_std_vector_v, "PointContactConstraintModelVector should be a std::vector"); static_assert( - helper::is_std_vector_v, + internal::helper::is_std_vector_v, "PointContactConstraintDataVector should be a std::vector"); typedef Eigen::Matrix VectorXs; @@ -44,7 +44,7 @@ namespace pinocchio Eigen::Index constraint_index = 0; for (std::size_t i = 0; i < constraint_models.size(); i++) { - const auto & cmodel = helper::get_ref(constraint_models[i]); + const auto & cmodel = internal::helper::get_ref(constraint_models[i]); const auto csize = cmodel.residualSize(); cmodel.retrieveCompliance(R.segment(constraint_index, csize)); constraint_index += csize; @@ -75,8 +75,8 @@ namespace pinocchio Eigen::Index row_id = 0; for (std::size_t constraint_id = 0; constraint_id < n_constraints; ++constraint_id) { - const auto & cmodel = helper::get_ref(constraint_models[constraint_id]); - const auto & cdata = helper::get_ref(constraint_datas[constraint_id]); + const auto & cmodel = internal::helper::get_ref(constraint_models[constraint_id]); + const auto & cdata = internal::helper::get_ref(constraint_datas[constraint_id]); const auto constraint_size = cmodel.residualSize(); const auto cone = cmodel.set(cdata); @@ -166,10 +166,10 @@ namespace pinocchio bool solve_ncp) { static_assert( - helper::is_std_vector_v, + internal::helper::is_std_vector_v, "PointContactConstraintModelVector should be a std::vector"); static_assert( - helper::is_std_vector_v, + internal::helper::is_std_vector_v, "PointContactConstraintDataVector should be a std::vector"); typedef ModelTpl Model; diff --git a/include/pinocchio/src/algorithm/delassus-operator-rigid-body-visitors.hxx b/include/pinocchio/src/algorithm/delassus-operator-rigid-body-visitors.hxx index 1319dc6fd0..3b274e21f7 100644 --- a/include/pinocchio/src/algorithm/delassus-operator-rigid-body-visitors.hxx +++ b/include/pinocchio/src/algorithm/delassus-operator-rigid-body-visitors.hxx @@ -11,7 +11,7 @@ #include "pinocchio/algorithm/delassus-operator.hpp" #endif // PINOCCHIO_LSP -#define PROMOTE_STATIC_EVAL(expression) promote_static_eval<0>(expression) +#define PROMOTE_STATIC_EVAL(expression) internal::promote_static_eval<0>(expression) // #define PROMOTE_STATIC_EVAL(expression) expression #define DO_NOT_PROMOTE_STATIC_EVAL(expression) expression diff --git a/include/pinocchio/src/algorithm/delassus-operator-rigid-body.hxx b/include/pinocchio/src/algorithm/delassus-operator-rigid-body.hxx index 9f9e379ea7..8cd29e270a 100644 --- a/include/pinocchio/src/algorithm/delassus-operator-rigid-body.hxx +++ b/include/pinocchio/src/algorithm/delassus-operator-rigid-body.hxx @@ -44,12 +44,14 @@ namespace pinocchio typedef typename Model::Data Data; typedef _ConstraintModel ConstraintModel; - typedef typename helper::remove_holder::type InnerConstraintModel; - typedef typename helper::remove_holder::ref_type ConstraintModelReference; - static constexpr bool ConstraintModelIsConst = helper::remove_holder::is_const; + typedef typename internal::helper::remove_holder::type InnerConstraintModel; + typedef + typename internal::helper::remove_holder::ref_type ConstraintModelReference; + static constexpr bool ConstraintModelIsConst = + internal::helper::remove_holder::is_const; typedef typename InnerConstraintModel::ConstraintData InnerConstraintData; - typedef typename helper::remove_holder::template rebind< + typedef typename internal::helper::remove_holder::template rebind< typename std:: conditional::type> ConstraintData; @@ -171,13 +173,13 @@ namespace pinocchio const ConstraintDataVectorHolder & constraint_datas_ref, const Scalar min_damping_value = 0) : Base() - , m_size(residualSize(helper::get_ref(constraint_models_ref))) + , m_size(residualSize(internal::helper::get_ref(constraint_models_ref))) , m_min_damping_value(min_damping_value) , m_model_ref(model_ref) , m_data_ref(data_ref) , m_constraint_models_ref(constraint_models_ref) , m_constraint_datas_ref(constraint_datas_ref) - , m_internal_data(helper::get_ref(model_ref)) + , m_internal_data(internal::helper::get_ref(model_ref)) , m_solve_in_place_dirty(true) , m_damping(VectorXs::Constant(m_size, min_damping_value).asDiagonal()) , m_compliance_storage(m_size) @@ -262,31 +264,31 @@ namespace pinocchio /// \brief Const getter for model. const Model & model() const { - return helper::get_ref(m_model_ref); + return internal::helper::get_ref(m_model_ref); } /// \brief Getter for data. Data & data() { - return helper::get_ref(m_data_ref); + return internal::helper::get_ref(m_data_ref); } /// /// \brief Const getter for data. const Data & data() const { - return helper::get_ref(m_data_ref); + return internal::helper::get_ref(m_data_ref); } /// \brief Const getter of constraint models. const ConstraintModelVector & constraint_models() const { - return helper::get_ref(m_constraint_models_ref); + return internal::helper::get_ref(m_constraint_models_ref); } /// \brief Const getter of constraint datas. const ConstraintDataVector & constraint_datas() const { - return helper::get_ref(m_constraint_datas_ref); + return internal::helper::get_ref(m_constraint_datas_ref); } // ------------------------------- @@ -476,9 +478,10 @@ namespace pinocchio /// \details Sums up the sizes of all internal data members. std::size_t sizeInBytes() const { - return pinocchio::sizeInBytes(a) + pinocchio::sizeInBytes(oa_augmented) - + pinocchio::sizeInBytes(u) + pinocchio::sizeInBytes(ddq) + pinocchio::sizeInBytes(f) - + pinocchio::sizeInBytes(of_augmented); + return pinocchio::internal::sizeInBytes(a) + pinocchio::internal::sizeInBytes(oa_augmented) + + pinocchio::internal::sizeInBytes(u) + pinocchio::internal::sizeInBytes(ddq) + + pinocchio::internal::sizeInBytes(f) + + pinocchio::internal::sizeInBytes(of_augmented); } }; @@ -577,7 +580,7 @@ namespace pinocchio m_constraint_models_ref = constraint_models_ref; m_constraint_datas_ref = constraint_datas_ref; - m_size = residualSize(helper::get_ref(constraint_models_ref)); + m_size = residualSize(internal::helper::get_ref(constraint_models_ref)); // resize quantities m_damping = VectorXs::Constant(m_size, m_min_damping_value).asDiagonal(); @@ -592,9 +595,9 @@ namespace pinocchio assert(m_sum_compliance_damping.rows() == m_size); assert(m_sum_compliance_damping_inverse.rows() == m_size); - retrieveConstraintCompliance(helper::get_ref(constraint_models_ref), m_compliance); + retrieveConstraintCompliance(internal::helper::get_ref(constraint_models_ref), m_compliance); - computeJointMinimalOrdering(model(), data(), helper::get_ref(constraint_models_ref)); + computeJointMinimalOrdering(model(), data(), internal::helper::get_ref(constraint_models_ref)); updateSumComplianceDamping(); } @@ -655,7 +658,8 @@ namespace pinocchio PINOCCHIO_TRACY_ZONE_SCOPED_N("appendCouplingConstraintInertias"); const auto & blocks = m_sum_compliance_damping_inverse.blocks(); PINOCCHIO_THROW_PRETTY_IF( - getSumOfBlockSizes(blocks) != residualSize(helper::get_ref(constraint_models_ref)), + getSumOfBlockSizes(blocks) + != residualSize(internal::helper::get_ref(constraint_models_ref)), std::runtime_error, "The sum of sizes of the blocks should be the same as the total residual size of the " "constraints vector."); @@ -668,15 +672,15 @@ namespace pinocchio const auto & compliance_damping_inverse_vector = remap(diagonal_block.container()); - assert(residualSize(helper::get_ref(constraint_models_ref)) == m_size); + assert(residualSize(internal::helper::get_ref(constraint_models_ref)) == m_size); assert(compliance_damping_inverse_vector.size() == m_size); Eigen::Index row_id = 0; for (std::size_t constraint_id = 0; constraint_id < constraint_models_ref.size(); ++constraint_id) { - const auto & cmodel = helper::get_ref(constraint_models_ref[constraint_id]); - const auto & cdata = helper::get_ref(constraint_datas_ref[constraint_id]); + const auto & cmodel = internal::helper::get_ref(constraint_models_ref[constraint_id]); + const auto & cdata = internal::helper::get_ref(constraint_datas_ref[constraint_id]); const auto constraint_size = cmodel.residualSize(); const auto constraint_diagonal_inertia = @@ -699,8 +703,8 @@ namespace pinocchio for (std::size_t constraint_id = 0; constraint_id < constraint_models_ref.size(); ++constraint_id) { - const auto & cmodel = helper::get_ref(constraint_models_ref[constraint_id]); - const auto & cdata = helper::get_ref(constraint_datas_ref[constraint_id]); + const auto & cmodel = internal::helper::get_ref(constraint_models_ref[constraint_id]); + const auto & cdata = internal::helper::get_ref(constraint_datas_ref[constraint_id]); cmodel.appendCouplingConstraintInertias( model_ref, data_ref, cdata, blocks[constraint_id], WorldFrameTag()); } @@ -796,9 +800,9 @@ namespace pinocchio // ++constraint_id) // { // const auto & cmodel = - // helper::get_ref(constraint_models_ref[constraint_id]); + // internal::helper::get_ref(constraint_models_ref[constraint_id]); // const auto & cdata = - // helper::get_ref(constraint_datas_ref[constraint_id]); + // internal::helper::get_ref(constraint_datas_ref[constraint_id]); // const auto csize = cmodel.size(); // const auto rhs_rows = rhs.middleRows(row_id, csize); // @@ -854,9 +858,9 @@ namespace pinocchio // ++constraint_id) // { // const auto & cmodel = - // helper::get_ref(constraint_models_ref[constraint_id]); + // internal::helper::get_ref(constraint_models_ref[constraint_id]); // const auto & cdata = - // helper::get_ref(constraint_datas_ref[constraint_id]); + // internal::helper::get_ref(constraint_datas_ref[constraint_id]); // const auto csize = cmodel.size(); // // cmodel.jacobianMatrixProduct( @@ -932,9 +936,9 @@ namespace pinocchio // ++constraint_id) // { // const auto & cmodel = - // helper::get_ref(constraint_models_ref[constraint_id]); + // internal::helper::get_ref(constraint_models_ref[constraint_id]); // const auto & cdata = - // helper::get_ref(constraint_datas_ref[constraint_id]); + // internal::helper::get_ref(constraint_datas_ref[constraint_id]); // const auto csize = cmodel.size(); // const auto mat_rows = mat.middleRows(row_id, csize); // @@ -954,9 +958,9 @@ namespace pinocchio // ++constraint_id) // { // const auto & cmodel = - // helper::get_ref(constraint_models_ref[constraint_id]); + // internal::helper::get_ref(constraint_models_ref[constraint_id]); // const auto & cdata = - // helper::get_ref(constraint_datas_ref[constraint_id]); + // internal::helper::get_ref(constraint_datas_ref[constraint_id]); // const auto csize = cmodel.size(); // // cmodel.jacobianMatrixProduct( diff --git a/include/pinocchio/src/algorithm/solvers/constraint-solver-utils.hxx b/include/pinocchio/src/algorithm/solvers/constraint-solver-utils.hxx index 8e55761a36..a4a183ba9f 100644 --- a/include/pinocchio/src/algorithm/solvers/constraint-solver-utils.hxx +++ b/include/pinocchio/src/algorithm/solvers/constraint-solver-utils.hxx @@ -109,8 +109,8 @@ namespace pinocchio for (size_t constraint_id = 0; constraint_id < constraint_models.size(); ++constraint_id) { - const auto & cmodel = helper::get_ref(constraint_models[constraint_id]); - const auto & cdata = helper::get_ref(constraint_datas[constraint_id]); + const auto & cmodel = internal::helper::get_ref(constraint_models[constraint_id]); + const auto & cdata = internal::helper::get_ref(constraint_datas[constraint_id]); const auto csize = cmodel.residualSize(); SegmentType1 force_segment = x.derived().segment(index, csize); @@ -222,8 +222,8 @@ namespace pinocchio for (size_t constraint_id = 0; constraint_id < constraint_models.size(); ++constraint_id) { - const auto & cmodel = helper::get_ref(constraint_models[constraint_id]); - const auto & cdata = helper::get_ref(constraint_datas[constraint_id]); + const auto & cmodel = internal::helper::get_ref(constraint_models[constraint_id]); + const auto & cdata = internal::helper::get_ref(constraint_datas[constraint_id]); const auto csize = cmodel.residualSize(); SegmentType1 force_segment = x.derived().segment(index, csize); @@ -368,8 +368,8 @@ namespace pinocchio for (size_t constraint_id = 0; constraint_id < constraint_models.size(); ++constraint_id) { - const auto & cmodel = helper::get_ref(constraint_models[constraint_id]); - const auto & cdata = helper::get_ref(constraint_datas[constraint_id]); + const auto & cmodel = internal::helper::get_ref(constraint_models[constraint_id]); + const auto & cdata = internal::helper::get_ref(constraint_datas[constraint_id]); const auto csize = cmodel.residualSize(); SegmentType1 velocity_segment = x.segment(index, csize); @@ -536,8 +536,8 @@ namespace pinocchio for (size_t constraint_id = 0; constraint_id < constraint_models.size(); ++constraint_id) { - const auto & cmodel = helper::get_ref(constraint_models[constraint_id]); - const auto & cdata = helper::get_ref(constraint_datas[constraint_id]); + const auto & cmodel = internal::helper::get_ref(constraint_models[constraint_id]); + const auto & cdata = internal::helper::get_ref(constraint_datas[constraint_id]); const auto csize = cmodel.residualSize(); SegmentType1 velocity_segment = velocities.segment(index, csize); @@ -658,8 +658,8 @@ namespace pinocchio Eigen::Index index = 0; for (size_t constraint_id = 0; constraint_id < constraint_models.size(); ++constraint_id) { - const auto & cmodel = helper::get_ref(constraint_models[constraint_id]); - const auto & cdata = helper::get_ref(constraint_datas[constraint_id]); + const auto & cmodel = internal::helper::get_ref(constraint_models[constraint_id]); + const auto & cdata = internal::helper::get_ref(constraint_datas[constraint_id]); const auto csize = cmodel.residualSize(); SegmentType1 velocity_segment = velocities.segment(index, csize); diff --git a/include/pinocchio/src/algorithm/solvers/pgs-solver.hxx b/include/pinocchio/src/algorithm/solvers/pgs-solver.hxx index ab73bac0e8..95d71c0593 100644 --- a/include/pinocchio/src/algorithm/solvers/pgs-solver.hxx +++ b/include/pinocchio/src/algorithm/solvers/pgs-solver.hxx @@ -681,8 +681,8 @@ namespace pinocchio Eigen::Index row_id = 0; for (size_t constraint_id = 0; constraint_id < num_constraints; ++constraint_id) { - const auto & cmodel = helper::get_ref(constraint_models[constraint_id]); - const auto & cdata = helper::get_ref(constraint_datas[constraint_id]); + const auto & cmodel = internal::helper::get_ref(constraint_models[constraint_id]); + const auto & cdata = internal::helper::get_ref(constraint_datas[constraint_id]); const Eigen::Index constraint_size = cmodel.residualSize(); auto G_block = G.block(row_id, row_id, constraint_size, constraint_size); diff --git a/include/pinocchio/src/constraints/constraint-ordering.hxx b/include/pinocchio/src/constraints/constraint-ordering.hxx index 70392407fe..0c64ee5c3f 100644 --- a/include/pinocchio/src/constraints/constraint-ordering.hxx +++ b/include/pinocchio/src/constraints/constraint-ordering.hxx @@ -81,10 +81,10 @@ namespace pinocchio data.joint_coupling_info(Eigen::Index(joint2_id), Eigen::Index(joint1_id)) = true; auto & joint1_neighbours = neighbours[joint1_id]; - if (!helper::exists(joint1_neighbours, joint2_id)) + if (!internal::helper::exists(joint1_neighbours, joint2_id)) joint1_neighbours.push_back(joint2_id); auto & joint2_neighbours = neighbours[joint2_id]; - if (!helper::exists(joint2_neighbours, joint1_id)) + if (!internal::helper::exists(joint2_neighbours, joint1_id)) joint2_neighbours.push_back(joint1_id); } } @@ -158,7 +158,7 @@ namespace pinocchio CollectorStep; for (std::size_t i = 0; i < constraint_models.size(); ++i) { - const auto & cmodel = helper::get_ref(constraint_models[i]); + const auto & cmodel = internal::helper::get_ref(constraint_models[i]); CollectorStep::run(cmodel, model, data); } @@ -230,7 +230,7 @@ namespace pinocchio { REGISTER_JOINT_PAIR(jp_pair); - if (!helper::exists(parent_neighbours, neighbour_j)) + if (!internal::helper::exists(parent_neighbours, neighbour_j)) { parent_neighbours.push_back(neighbour_j); neighbour_j_neighbours.push_back(parent_id); @@ -239,7 +239,7 @@ namespace pinocchio } // Remove joint_id from the list of neighbours for neighbour_j_neighbours - helper::erase(neighbour_j_neighbours, joint_id, helper::erase_first); + internal::helper::erase(neighbour_j_neighbours, joint_id, internal::helper::erase_first); for (size_t k = j + 1; k < joint_neighbours.size(); ++k) { diff --git a/include/pinocchio/src/constraints/contact-info.hxx b/include/pinocchio/src/constraints/contact-info.hxx index a239bd4e9d..aa6ff59e5b 100644 --- a/include/pinocchio/src/constraints/contact-info.hxx +++ b/include/pinocchio/src/constraints/contact-info.hxx @@ -935,7 +935,7 @@ namespace pinocchio Eigen::Index total_size = 0; for (size_t k = 0; k < constraint_models.size(); ++k) { - const auto & constraint_model = helper::get_ref(constraint_models[k]); + const auto & constraint_model = internal::helper::get_ref(constraint_models[k]); total_size += constraint_model.residualSize(sel); } diff --git a/include/pinocchio/src/constraints/utils.hxx b/include/pinocchio/src/constraints/utils.hxx index 9a3d61bd28..a15dc02a03 100644 --- a/include/pinocchio/src/constraints/utils.hxx +++ b/include/pinocchio/src/constraints/utils.hxx @@ -86,7 +86,7 @@ namespace pinocchio Eigen::Index active_size = 0; for (std::size_t i = 0; i < constraint_models.size(); ++i) { - const auto & cmodel = helper::get_ref(constraint_models[i]); + const auto & cmodel = internal::helper::get_ref(constraint_models[i]); active_size += cmodel.residualSize(sel); } @@ -124,7 +124,7 @@ namespace pinocchio Eigen::Index active_size = 0; for (std::size_t i = 0; i < constraint_models.size(); ++i) { - const auto & cmodel = helper::get_ref(constraint_models[i]); + const auto & cmodel = internal::helper::get_ref(constraint_models[i]); active_size += cmodel.symmetricConeResidualSize(sel); } @@ -162,7 +162,7 @@ namespace pinocchio Eigen::Index active_size = 0; for (std::size_t i = 0; i < constraint_models.size(); ++i) { - const auto & cmodel = helper::get_ref(constraint_models[i]); + const auto & cmodel = internal::helper::get_ref(constraint_models[i]); active_size += cmodel.symmetricConeResidualScalingSize(sel); } @@ -187,7 +187,7 @@ namespace pinocchio for (std::size_t i = 0; i < constraint_models.size(); i++) { - auto & cmodel = helper::get_ref(constraint_models[i]); + auto & cmodel = internal::helper::get_ref(constraint_models[i]); const auto csize = cmodel.residualSize(sel); cmodel.setCompliance(compliance.segment(constraint_index, csize), sel); constraint_index += csize; @@ -213,7 +213,7 @@ namespace pinocchio for (std::size_t i = 0; i < constraint_models.size(); i++) { - const auto & cmodel = helper::get_ref(constraint_models[i]); + const auto & cmodel = internal::helper::get_ref(constraint_models[i]); const auto csize = cmodel.residualSize(sel); cmodel.retrieveCompliance(compliance.segment(constraint_index, csize), sel); constraint_index += csize; @@ -268,8 +268,8 @@ namespace pinocchio { for (size_t k = 0; k < constraint_models.size(); ++k) { - const auto & cmodel = helper::get_ref(constraint_models[k]); - auto & cdata = helper::get_ref(constraint_datas[k]); + const auto & cmodel = internal::helper::get_ref(constraint_models[k]); + auto & cdata = internal::helper::get_ref(constraint_datas[k]); cmodel.calc(model, data, cdata); } @@ -578,8 +578,8 @@ namespace pinocchio Eigen::Index row_id = 0; for (size_t constraint_id = 0; constraint_id < constraint_models.size(); ++constraint_id) { - const auto & cmodel = helper::get_ref(constraint_models[constraint_id]); - const auto & cdata = helper::get_ref(constraint_datas[constraint_id]); + const auto & cmodel = internal::helper::get_ref(constraint_models[constraint_id]); + const auto & cdata = internal::helper::get_ref(constraint_datas[constraint_id]); const auto constraint_size = cmodel.residualSize(); const auto constraint_force = constraint_forces.segment(row_id, constraint_size); @@ -631,8 +631,8 @@ namespace pinocchio Eigen::Index row_id = 0; for (size_t constraint_id = 0; constraint_id < constraint_models.size(); ++constraint_id) { - const auto & cmodel = helper::get_ref(constraint_models[constraint_id]); - const auto & cdata = helper::get_ref(constraint_datas[constraint_id]); + const auto & cmodel = internal::helper::get_ref(constraint_models[constraint_id]); + const auto & cdata = internal::helper::get_ref(constraint_datas[constraint_id]); const auto constraint_size = cmodel.residualSize(); const auto constraint_force = constraint_forces.segment(row_id, constraint_size); @@ -674,8 +674,8 @@ namespace pinocchio Eigen::Index row_id = 0; for (size_t constraint_id = 0; constraint_id < constraint_models.size(); ++constraint_id) { - const auto & cmodel = helper::get_ref(constraint_models[constraint_id]); - const auto & cdata = helper::get_ref(constraint_datas[constraint_id]); + const auto & cmodel = internal::helper::get_ref(constraint_models[constraint_id]); + const auto & cdata = internal::helper::get_ref(constraint_datas[constraint_id]); const auto constraint_size = cmodel.residualSize(); auto constraint_motion = constraint_motions.segment(row_id, constraint_size); @@ -720,8 +720,8 @@ namespace pinocchio Eigen::Index row_id = 0; for (size_t constraint_id = 0; constraint_id < constraint_models.size(); ++constraint_id) { - const auto & cmodel = helper::get_ref(constraint_models[constraint_id]); - const auto & cdata = helper::get_ref(constraint_datas[constraint_id]); + const auto & cmodel = internal::helper::get_ref(constraint_models[constraint_id]); + const auto & cdata = internal::helper::get_ref(constraint_datas[constraint_id]); const auto constraint_size = cmodel.residualSize(); auto constraint_motion = constraint_motions.segment(row_id, constraint_size); @@ -748,8 +748,8 @@ namespace pinocchio const Eigen::MatrixBase & J_) { JacobianMatrixLike & J = J_.const_cast_derived(); - const auto & constraint_model = helper::get_ref(constraint_model_.derived()); - const auto & constraint_data = helper::get_ref(constraint_data_.derived()); + const auto & constraint_model = internal::helper::get_ref(constraint_model_.derived()); + const auto & constraint_data = internal::helper::get_ref(constraint_data_.derived()); assert(model.check(data) && "data is not consistent with model."); PINOCCHIO_CHECK_ARGUMENT_SIZE(J_.rows(), constraint_model.residualSize()); @@ -786,8 +786,8 @@ namespace pinocchio Eigen::Index row_id = 0; for (size_t k = 0; k < constraint_models.size(); ++k) { - const auto & cmodel = helper::get_ref(constraint_models[k]); - const auto & cdata = helper::get_ref(constraint_datas[k]); + const auto & cmodel = internal::helper::get_ref(constraint_models[k]); + const auto & cdata = internal::helper::get_ref(constraint_datas[k]); const auto csize = cmodel.residualSize(); getConstraintJacobian(model, data, cmodel, cdata, J.middleRows(row_id, csize)); @@ -859,8 +859,8 @@ namespace pinocchio Eigen::Index row_id = 0; for (size_t constraint_id = 0; constraint_id < constraint_models.size(); ++constraint_id) { - const auto & cmodel = helper::get_ref(constraint_models[constraint_id]); - const auto & cdata = helper::get_ref(constraint_datas[constraint_id]); + const auto & cmodel = internal::helper::get_ref(constraint_models[constraint_id]); + const auto & cdata = internal::helper::get_ref(constraint_datas[constraint_id]); const auto constraint_size = cmodel.residualSize(); auto res_block = res.middleRows(row_id, constraint_size); @@ -909,8 +909,8 @@ namespace pinocchio for (size_t constraint_id = 0; constraint_id < constraint_models.size(); ++constraint_id) { - const auto & cmodel = helper::get_ref(constraint_models[constraint_id]); - const auto & cdata = helper::get_ref(constraint_datas[constraint_id]); + const auto & cmodel = internal::helper::get_ref(constraint_models[constraint_id]); + const auto & cdata = internal::helper::get_ref(constraint_datas[constraint_id]); const auto constraint_size = cmodel.residualSize(); const auto rhs_block = rhs.middleRows(row_id, constraint_size); @@ -1138,7 +1138,7 @@ namespace pinocchio for (std::size_t i = 0; i < constraint_models.size(); ++i) { - const auto & cmodel = helper::get_ref(constraint_models[i]); + const auto & cmodel = internal::helper::get_ref(constraint_models[i]); typedef internal::ComputeBlockDiagonalPatternVisitor Algo; Algo::run(cmodel, block_diagonal_infos, dispatcher); diff --git a/include/pinocchio/src/math/block-diagonal-matrix.hxx b/include/pinocchio/src/math/block-diagonal-matrix.hxx index 9a92abffc4..111a9a86b3 100644 --- a/include/pinocchio/src/math/block-diagonal-matrix.hxx +++ b/include/pinocchio/src/math/block-diagonal-matrix.hxx @@ -712,7 +712,7 @@ namespace pinocchio /// \details Sums up the sizes of all internal data members. std::size_t sizeInBytes() const { - return 2 * ::pinocchio::sizeInBytes() + return 2 * ::pinocchio::internal::sizeInBytes() + m_matrix_stack.sizeInBytes(); // TODO(jcarpent) complete + // sizeInBytes(m_matrix_block_elements); } diff --git a/include/pinocchio/src/math/matrix-block-element.hxx b/include/pinocchio/src/math/matrix-block-element.hxx index 513815ff4c..fd79f74f76 100644 --- a/include/pinocchio/src/math/matrix-block-element.hxx +++ b/include/pinocchio/src/math/matrix-block-element.hxx @@ -21,7 +21,9 @@ namespace pinocchio struct MatrixBlockElementTpl; template - struct traits, std::enable_if_t>> + struct traits< + MatrixBlockElementTpl, + std::enable_if_t>> { typedef typename MapType::PlainObject Matrix; typedef typename Matrix::Scalar Scalar; @@ -34,7 +36,7 @@ namespace pinocchio template struct traits< MatrixBlockElementTpl, - std::enable_if_t>> + std::enable_if_t>> { typedef MatrixType Matrix; typedef typename Matrix::Scalar Scalar; @@ -88,7 +90,9 @@ namespace pinocchio * the **exact same underlying memory buffer**. */ template - struct MatrixBlockElementTpl>> + struct MatrixBlockElementTpl< + MapType, + std::enable_if_t>> : MatrixBlockElementPlain> { /// @brief The type of this specialized class. @@ -356,7 +360,7 @@ namespace pinocchio template struct MatrixBlockElementTpl< MatrixType, - std::enable_if_t>> + std::enable_if_t>> : MatrixBlockElementPlain> { /// @brief The type of this specialized class. diff --git a/include/pinocchio/src/serialization/matrix-block-element.hxx b/include/pinocchio/src/serialization/matrix-block-element.hxx index 1ebd32a413..4a53cc1510 100644 --- a/include/pinocchio/src/serialization/matrix-block-element.hxx +++ b/include/pinocchio/src/serialization/matrix-block-element.hxx @@ -32,7 +32,7 @@ namespace boost template struct MatrixBlockElementTplAccessor< MapType, - std::enable_if_t>> + std::enable_if_t>> : public ::pinocchio::internal::MatrixBlockElementTpl { typedef ::pinocchio::internal::MatrixBlockElementTpl Base; @@ -54,7 +54,7 @@ namespace boost ar & make_nvp("type", matrix_block_element.m_type); ar & make_nvp("size", matrix_block_element.m_size); - if constexpr (pinocchio::helper::is_eigen_matrix_v) + if constexpr (pinocchio::internal::helper::is_eigen_matrix_v) { auto & container = matrix_block_element.container(); ar & make_nvp("container", container); diff --git a/include/pinocchio/src/utils/eigen-helpers.hxx b/include/pinocchio/src/utils/eigen-helpers.hxx index dee105e15a..d7b0192089 100644 --- a/include/pinocchio/src/utils/eigen-helpers.hxx +++ b/include/pinocchio/src/utils/eigen-helpers.hxx @@ -13,138 +13,141 @@ namespace pinocchio { - - namespace helper + namespace internal { - template - struct is_eigen_noalias : std::false_type - { - }; - - template class StorageBase> - struct is_eigen_noalias> : std::true_type - { - }; - template - inline constexpr bool is_eigen_noalias_v = is_eigen_noalias>::value; - - template - struct is_eigen_product : std::false_type + namespace helper { - }; + template + struct is_eigen_noalias : std::false_type + { + }; - template - struct is_eigen_product> : std::true_type - { - }; + template class StorageBase> + struct is_eigen_noalias> : std::true_type + { + }; - template - inline constexpr bool is_eigen_product_v = is_eigen_product>::value; + template + inline constexpr bool is_eigen_noalias_v = is_eigen_noalias>::value; - template - struct remove_eigen_noalias - { - typedef T type; - static T & get(T & t) - { - return t; - } - static const T & get(const T & t) + template + struct is_eigen_product : std::false_type { - return t; - } - }; + }; - template class StorageBase> - struct remove_eigen_noalias> - { - typedef ExpressionType type; - static ExpressionType & get(Eigen::NoAlias & t) - { - return t.expression(); - } - static const ExpressionType & get(const Eigen::NoAlias & t) + template + struct is_eigen_product> : std::true_type { - return t.expression(); - } - }; + }; - template - inline constexpr bool has_fixed_rows_v = false; + template + inline constexpr bool is_eigen_product_v = is_eigen_product>::value; - template - inline constexpr bool has_fixed_rows_v> = - (T::RowsAtCompileTime != Eigen::Dynamic); - - template - inline constexpr bool has_fixed_cols_v = false; - - template - inline constexpr bool has_fixed_cols_v> = - (T::ColsAtCompileTime != Eigen::Dynamic); + template + struct remove_eigen_noalias + { + typedef T type; + static T & get(T & t) + { + return t; + } + static const T & get(const T & t) + { + return t; + } + }; + + template class StorageBase> + struct remove_eigen_noalias> + { + typedef ExpressionType type; + static ExpressionType & get(Eigen::NoAlias & t) + { + return t.expression(); + } + static const ExpressionType & get(const Eigen::NoAlias & t) + { + return t.expression(); + } + }; + + template + inline constexpr bool has_fixed_rows_v = false; + + template + inline constexpr bool has_fixed_rows_v> = + (T::RowsAtCompileTime != Eigen::Dynamic); + + template + inline constexpr bool has_fixed_cols_v = false; + + template + inline constexpr bool has_fixed_cols_v> = + (T::ColsAtCompileTime != Eigen::Dynamic); + + template + inline constexpr bool has_fixed_size_v = false; + + template + inline constexpr bool has_fixed_size_v< + T, + std::void_t> = + has_fixed_rows_v && has_fixed_cols_v; + + template + struct is_eigen_map : std::false_type + { + }; - template - inline constexpr bool has_fixed_size_v = false; + template + struct is_eigen_map> : std::true_type + { + }; - template - inline constexpr bool has_fixed_size_v< - T, - std::void_t> = - has_fixed_rows_v && has_fixed_cols_v; + template + struct is_eigen_map> : std::true_type + { + }; - template - struct is_eigen_map : std::false_type - { - }; + template + inline constexpr bool is_eigen_map_v = is_eigen_map>::value; - template - struct is_eigen_map> : std::true_type - { - }; + template + struct is_eigen_matrix : std::false_type + { + }; - template - struct is_eigen_map> : std::true_type - { - }; + // detect Eigen::Matrix + template + struct is_eigen_matrix> + : std::true_type + { + }; - template - inline constexpr bool is_eigen_map_v = is_eigen_map>::value; + // also handle const-qualified matrices + template + struct is_eigen_matrix> + : std::true_type + { + }; - template - struct is_eigen_matrix : std::false_type - { - }; + // handy variable template + template + inline constexpr bool is_eigen_matrix_v = is_eigen_matrix>::value; - // detect Eigen::Matrix - template - struct is_eigen_matrix> - : std::true_type - { - }; + } // namespace helper - // also handle const-qualified matrices - template - struct is_eigen_matrix> - : std::true_type + template + bool + compare_maps(const Eigen::MapBase & map1, const Eigen::MapBase & map2) { - }; - - // handy variable template - template - inline constexpr bool is_eigen_matrix_v = is_eigen_matrix>::value; - - } // namespace helper - - template - bool - compare_maps(const Eigen::MapBase & map1, const Eigen::MapBase & map2) - { - if ((map1.rows() != map2.rows()) || (map1.cols() != map2.cols())) - return false; - if (map1 != map2) - return false; - return true; - } - + if ((map1.rows() != map2.rows()) || (map1.cols() != map2.cols())) + return false; + if (map1 != map2) + return false; + return true; + } + + } // namespace internal } // namespace pinocchio diff --git a/include/pinocchio/src/utils/promote-static-eval.hxx b/include/pinocchio/src/utils/promote-static-eval.hxx index 79997515cf..6476ac8ad8 100644 --- a/include/pinocchio/src/utils/promote-static-eval.hxx +++ b/include/pinocchio/src/utils/promote-static-eval.hxx @@ -13,10 +13,8 @@ namespace pinocchio { - namespace internal { - template struct make_static_matrix { @@ -434,36 +432,39 @@ namespace pinocchio }; // struct PromoteStaticEval - } // namespace internal + template + PromoteStaticEval + promote_static_eval(const Eigen::MatrixBase & matrix_expression) + { + return {matrix_expression.const_cast_derived()}; + } - template - internal::PromoteStaticEval - promote_static_eval(const Eigen::MatrixBase & matrix_expression) - { - return {matrix_expression.const_cast_derived()}; - } + template + PromoteStaticEval<0, MatrixExpression, Eigen::MatrixBase> + promote_static_eval(const Eigen::MatrixBase & matrix_expression) + { + return {matrix_expression.const_cast_derived()}; + } - template - internal::PromoteStaticEval<0, MatrixExpression, Eigen::MatrixBase> - promote_static_eval(const Eigen::MatrixBase & matrix_expression) - { - return {matrix_expression.const_cast_derived()}; - } - - template class StorageBase> - internal::PromoteStaticEval< - MaxStaticUnfolding, - Eigen::NoAlias, - Eigen::MatrixBase> - promote_static_eval(Eigen::NoAlias && matrix_expression) - { - return {std::forward>(matrix_expression)}; - } + template< + int MaxStaticUnfolding, + typename MatrixExpression, + template class StorageBase> + PromoteStaticEval< + MaxStaticUnfolding, + Eigen::NoAlias, + Eigen::MatrixBase> + promote_static_eval(Eigen::NoAlias && matrix_expression) + { + return {std::forward>(matrix_expression)}; + } - template class StorageBase> - internal::PromoteStaticEval<0, Eigen::NoAlias, Eigen::MatrixBase> - promote_static_eval(Eigen::NoAlias && matrix_expression) - { - return {std::forward>(matrix_expression)}; - } + template class StorageBase> + PromoteStaticEval<0, Eigen::NoAlias, Eigen::MatrixBase> + promote_static_eval(Eigen::NoAlias && matrix_expression) + { + return {std::forward>(matrix_expression)}; + } + + } // namespace internal } // namespace pinocchio diff --git a/include/pinocchio/src/utils/reference.hxx b/include/pinocchio/src/utils/reference.hxx index 08c0f6ca15..69b559c039 100644 --- a/include/pinocchio/src/utils/reference.hxx +++ b/include/pinocchio/src/utils/reference.hxx @@ -13,223 +13,227 @@ namespace pinocchio { - namespace helper + namespace internal { - // std::reference_wrapper - template - T * get_pointer(const std::reference_wrapper & ref) + namespace helper { - return &ref.get(); - } - template - const T * get_pointer(const std::reference_wrapper & ref) - { - return &ref.get(); - } - - // std::shared_ptr - template - T * get_pointer(const std::shared_ptr & ptr) - { - return ptr.get(); - } - template - const T * get_pointer(const std::shared_ptr & ptr) - { - return ptr.get(); - } - - template - std::reference_wrapper make_ref(T & value) - { - return std::reference_wrapper(value); - } - - template - std::reference_wrapper make_ref(const T & value) - { - return std::reference_wrapper(value); - } - - template - struct remove_holder - { - typedef T type; - typedef T & ref_type; - static constexpr bool is_const = false; - template - using rebind = U; - static ref_type get_ref(T & v) - { - return v; + // std::reference_wrapper + template + T * get_pointer(const std::reference_wrapper & ref) + { + return &ref.get(); } - }; - - template - struct remove_holder - { - typedef T type; - typedef const T & ref_type; - static constexpr bool is_const = true; - template - using rebind = U; - static ref_type get_ref(const T & v) - { - return v; + template + const T * get_pointer(const std::reference_wrapper & ref) + { + return &ref.get(); } - }; - template - struct remove_holder> - { - typedef typename remove_holder::type type; - typedef typename remove_holder::ref_type ref_type; - static constexpr bool is_const = remove_holder::is_const; - template - using rebind = std::reference_wrapper; - - static ref_type get_ref(std::reference_wrapper & ref) + // std::shared_ptr + template + T * get_pointer(const std::shared_ptr & ptr) { - return ref.get(); + return ptr.get(); } - }; - - template - struct remove_holder> : remove_holder> - { - typedef typename remove_holder::type type; - typedef typename remove_holder::ref_type ref_type; - - static ref_type get_ref(const std::reference_wrapper & ref) + template + const T * get_pointer(const std::shared_ptr & ptr) { - return ref.get(); + return ptr.get(); } - }; - template - struct remove_holder> - { - typedef typename remove_holder::type type; - typedef typename remove_holder::ref_type ref_type; - static constexpr bool is_const = true; - template - using rebind = std::reference_wrapper; - - static ref_type get_ref(const std::reference_wrapper & ref) + template + std::reference_wrapper make_ref(T & value) { - return ref.get(); + return std::reference_wrapper(value); } - }; - - template - struct remove_holder> - { - typedef typename remove_holder::type type; - typedef typename remove_holder::ref_type ref_type; - static constexpr bool is_const = remove_holder::is_const; - template - using rebind = std::shared_ptr; - static ref_type get_ref(const std::shared_ptr & ptr) + template + std::reference_wrapper make_ref(const T & value) { - return *ptr; + return std::reference_wrapper(value); } - }; - - template - struct remove_holder> : remove_holder> - { - }; - template - struct remove_holder> - { - typedef typename remove_holder::type type; - typedef typename remove_holder::ref_type ref_type; - static constexpr bool is_const = true; - template - using rebind = std::shared_ptr; - - static ref_type get_ref(const std::shared_ptr & ptr) + template + struct remove_holder { - return *ptr; - } - }; + typedef T type; + typedef T & ref_type; + static constexpr bool is_const = false; + template + using rebind = U; + static ref_type get_ref(T & v) + { + return v; + } + }; + + template + struct remove_holder + { + typedef T type; + typedef const T & ref_type; + static constexpr bool is_const = true; + template + using rebind = U; + static ref_type get_ref(const T & v) + { + return v; + } + }; + + template + struct remove_holder> + { + typedef typename remove_holder::type type; + typedef typename remove_holder::ref_type ref_type; + static constexpr bool is_const = remove_holder::is_const; + template + using rebind = std::reference_wrapper; + + static ref_type get_ref(std::reference_wrapper & ref) + { + return ref.get(); + } + }; + + template + struct remove_holder> + : remove_holder> + { + typedef typename remove_holder::type type; + typedef typename remove_holder::ref_type ref_type; - template - struct remove_holder> : remove_holder> - { - }; + static ref_type get_ref(const std::reference_wrapper & ref) + { + return ref.get(); + } + }; - template - struct remove_holder> - { - typedef typename remove_holder::type type; - typedef typename remove_holder::ref_type ref_type; - static constexpr bool is_const = remove_holder::is_const; - template - using rebind = std::unique_ptr; + template + struct remove_holder> + { + typedef typename remove_holder::type type; + typedef typename remove_holder::ref_type ref_type; + static constexpr bool is_const = true; + template + using rebind = std::reference_wrapper; + + static ref_type get_ref(const std::reference_wrapper & ref) + { + return ref.get(); + } + }; + + template + struct remove_holder> + { + typedef typename remove_holder::type type; + typedef typename remove_holder::ref_type ref_type; + static constexpr bool is_const = remove_holder::is_const; + template + using rebind = std::shared_ptr; + + static ref_type get_ref(const std::shared_ptr & ptr) + { + return *ptr; + } + }; + + template + struct remove_holder> : remove_holder> + { + }; - static ref_type get_ref(const std::unique_ptr & ptr) + template + struct remove_holder> { - return *ptr; - } - }; + typedef typename remove_holder::type type; + typedef typename remove_holder::ref_type ref_type; + static constexpr bool is_const = true; + template + using rebind = std::shared_ptr; + + static ref_type get_ref(const std::shared_ptr & ptr) + { + return *ptr; + } + }; + + template + struct remove_holder> : remove_holder> + { + }; - template - struct remove_holder> : remove_holder> - { - }; + template + struct remove_holder> + { + typedef typename remove_holder::type type; + typedef typename remove_holder::ref_type ref_type; + static constexpr bool is_const = remove_holder::is_const; + template + using rebind = std::unique_ptr; + + static ref_type get_ref(const std::unique_ptr & ptr) + { + return *ptr; + } + }; + + template + struct remove_holder> : remove_holder> + { + }; - template - struct remove_holder> - { - typedef typename remove_holder::type type; - typedef typename remove_holder::ref_type ref_type; - static constexpr bool is_const = true; - template - using rebind = std::unique_ptr; + template + struct remove_holder> + { + typedef typename remove_holder::type type; + typedef typename remove_holder::ref_type ref_type; + static constexpr bool is_const = true; + template + using rebind = std::unique_ptr; + + static ref_type get_ref(const std::unique_ptr & ptr) + { + return *ptr; + } + }; + + template + struct remove_holder> : remove_holder> + { + }; - static ref_type get_ref(const std::unique_ptr & ptr) + template + typename remove_holder::ref_type get_ref(T & v) { - return *ptr; + return remove_holder::get_ref(v); } - }; - - template - struct remove_holder> : remove_holder> - { - }; - template - typename remove_holder::ref_type get_ref(T & v) - { - return remove_holder::get_ref(v); - } - - template - const typename remove_holder::ref_type get_ref(const T & v) - { - return remove_holder::get_ref(v); - } + template + const typename remove_holder::ref_type get_ref(const T & v) + { + return remove_holder::get_ref(v); + } - template - struct is_type_holder - { - static constexpr bool value = false; - }; + template + struct is_type_holder + { + static constexpr bool value = false; + }; - template - struct is_type_holder> - { - static constexpr bool value = true; - }; + template + struct is_type_holder> + { + static constexpr bool value = true; + }; - template - struct is_type_holder> - { - static constexpr bool value = true; - }; + template + struct is_type_holder> + { + static constexpr bool value = true; + }; - } // namespace helper + } // namespace helper + } // namespace internal } // namespace pinocchio diff --git a/include/pinocchio/src/utils/size-in-bytes.hxx b/include/pinocchio/src/utils/size-in-bytes.hxx index aa44613c05..48fb83abc9 100644 --- a/include/pinocchio/src/utils/size-in-bytes.hxx +++ b/include/pinocchio/src/utils/size-in-bytes.hxx @@ -13,166 +13,169 @@ namespace pinocchio { - namespace helper + namespace internal { - template - struct has_method_sizeInBytes : std::false_type + namespace helper { - }; + template + struct has_method_sizeInBytes : std::false_type + { + }; - template - struct has_method_sizeInBytes().sizeInBytes())>> - : std::true_type + template + struct has_method_sizeInBytes().sizeInBytes())>> + : std::true_type + { + }; + + template + inline constexpr bool has_method_sizeInBytes_v = has_method_sizeInBytes::value; + } // namespace helper + + template + struct sizeInBytesImpl; + + /** + * @brief Helper struct providing a fallback implementation to compute the size (in bytes) + * of a given object or type. + * + * Specialize this struct for custom types that do not have a @c sizeInBytes() member function, + * in order to define how their size should be computed. + * + * @tparam T Type of the object whose size in bytes will be computed. + */ + template + struct sizeInBytesImpl { + /** + * @brief Compute the size in bytes of a given object value of type @p T. + * + * This static method should return the number of bytes occupied by @p value. + * The default implementation may rely on sizeof(T), or it may be specialized + * for custom data structures that store variable-length data. + * + * @param value The object whose size in bytes should be computed. + * @return The size in bytes of @p value. + */ + static std::size_t run(const T & value); }; - template - inline constexpr bool has_method_sizeInBytes_v = has_method_sizeInBytes::value; - } // namespace helper - - template - struct sizeInBytesImpl; - - /** - * @brief Helper struct providing a fallback implementation to compute the size (in bytes) - * of a given object or type. - * - * Specialize this struct for custom types that do not have a @c sizeInBytes() member function, - * in order to define how their size should be computed. - * - * @tparam T Type of the object whose size in bytes will be computed. - */ - template - struct sizeInBytesImpl - { /** - * @brief Compute the size in bytes of a given object value of type @p T. + * @brief Compute the size in bytes of an object @p value. + * + * This function first checks whether the type @p T provides a member function + * named @c sizeInBytes(). If it does, that method is called. + * Otherwise, it falls back to calling the static implementation + * provided by the @c sizeInBytesImpl struct. * - * This static method should return the number of bytes occupied by @p value. - * The default implementation may rely on sizeof(T), or it may be specialized - * for custom data structures that store variable-length data. + * Typical use case: + * @code + * MyType obj; + * std::size_t sz = sizeInBytes(obj); + * @endcode * - * @param value The object whose size in bytes should be computed. + * @tparam T Type of the input object. + * @param value The input object whose size in bytes will be computed. * @return The size in bytes of @p value. */ - static std::size_t run(const T & value); - }; - - /** - * @brief Compute the size in bytes of an object @p value. - * - * This function first checks whether the type @p T provides a member function - * named @c sizeInBytes(). If it does, that method is called. - * Otherwise, it falls back to calling the static implementation - * provided by the @c sizeInBytesImpl struct. - * - * Typical use case: - * @code - * MyType obj; - * std::size_t sz = sizeInBytes(obj); - * @endcode - * - * @tparam T Type of the input object. - * @param value The input object whose size in bytes will be computed. - * @return The size in bytes of @p value. - */ - template - std::size_t sizeInBytes(const T & value) - { - if constexpr (helper::has_method_sizeInBytes_v) + template + std::size_t sizeInBytes(const T & value) { - return value.sizeInBytes(); + if constexpr (helper::has_method_sizeInBytes_v) + { + return value.sizeInBytes(); + } + else if constexpr (std::is_fundamental_v) + return sizeof(T); + else + return sizeInBytesImpl::run(value); } - else if constexpr (std::is_fundamental_v) + + /** + * @brief Get the size in bytes of a type @p T at compile time. + * + * This overload simply returns @c sizeof(T), which is sufficient for + * trivial or fixed-size types. + * + * Typical use case: + * @code + * std::size_t sz = sizeInBytes(); // returns sizeof(int) + * @endcode + * + * @tparam T Type whose compile-time size in bytes is requested. + * @return The size in bytes of type @p T. + */ + template + std::size_t sizeInBytes() + { return sizeof(T); - else - return sizeInBytesImpl::run(value); - } - - /** - * @brief Get the size in bytes of a type @p T at compile time. - * - * This overload simply returns @c sizeof(T), which is sufficient for - * trivial or fixed-size types. - * - * Typical use case: - * @code - * std::size_t sz = sizeInBytes(); // returns sizeof(int) - * @endcode - * - * @tparam T Type whose compile-time size in bytes is requested. - * @return The size in bytes of type @p T. - */ - template - std::size_t sizeInBytes() - { - return sizeof(T); - } + } - template - struct sizeInBytesImpl> - { - static std::size_t run(const std::vector & vector) + template + struct sizeInBytesImpl> { - std::size_t size_value = 0; - for (const auto & elt : vector) + static std::size_t run(const std::vector & vector) { - size_value += sizeInBytes(elt); + std::size_t size_value = 0; + for (const auto & elt : vector) + { + size_value += sizeInBytes(elt); + } + return size_value; } - return size_value; - } - }; // sizeInBytesImpl + }; // sizeInBytesImpl - template - struct sizeInBytesImpl> - { - static std::size_t run(const std::array & array) + template + struct sizeInBytesImpl> { - std::size_t size_value = 0; - for (const auto & elt : array) + static std::size_t run(const std::array & array) { - size_value += sizeInBytes(elt); + std::size_t size_value = 0; + for (const auto & elt : array) + { + size_value += sizeInBytes(elt); + } + return size_value; } - return size_value; - } - }; + }; - template - struct sizeInBytesImpl< - Derived, - std::enable_if_t, Derived>>> - { - template - static std::enable_if_t, std::size_t> - run(const Eigen::PlainObjectBase & matrix) + template + struct sizeInBytesImpl< + Derived, + std::enable_if_t, Derived>>> { - PINOCCHIO_UNUSED_VARIABLE(matrix); - std::size_t size_value = sizeof(Derived); - return size_value; - } + template + static std::enable_if_t, std::size_t> + run(const Eigen::PlainObjectBase & matrix) + { + PINOCCHIO_UNUSED_VARIABLE(matrix); + std::size_t size_value = sizeof(Derived); + return size_value; + } - template - static std::enable_if_t, std::size_t> - run(const Eigen::PlainObjectBase & matrix) - { - typedef typename Derived::Scalar Scalar; - typedef Eigen::Matrix Matrix0x0; - std::size_t size_value = sizeof(Scalar) * std::size_t(matrix.size()) + sizeof(Matrix0x0); - return size_value; - } - }; // struct sizeInBytesImpl> + template + static std::enable_if_t, std::size_t> + run(const Eigen::PlainObjectBase & matrix) + { + typedef typename Derived::Scalar Scalar; + typedef Eigen::Matrix Matrix0x0; + std::size_t size_value = sizeof(Scalar) * std::size_t(matrix.size()) + sizeof(Matrix0x0); + return size_value; + } + }; // struct sizeInBytesImpl> - template - struct sizeInBytesImpl> - { - static std::size_t run(const Eigen::Map & map) + template + struct sizeInBytesImpl> { - typedef typename PlainObjectType::Scalar Scalar; - std::size_t size_value = sizeof(Scalar) * std::size_t(map.size()); - return size_value; - } + static std::size_t run(const Eigen::Map & map) + { + typedef typename PlainObjectType::Scalar Scalar; + std::size_t size_value = sizeof(Scalar) * std::size_t(map.size()); + return size_value; + } - }; // struct sizeInBytesImpl> }; // - // sizeInBytesImpl + }; // struct sizeInBytesImpl> }; // + // sizeInBytesImpl + } // namespace internal } // namespace pinocchio diff --git a/include/pinocchio/src/utils/std-vector.hxx b/include/pinocchio/src/utils/std-vector.hxx index f75b7d791d..d984ca1736 100644 --- a/include/pinocchio/src/utils/std-vector.hxx +++ b/include/pinocchio/src/utils/std-vector.hxx @@ -53,302 +53,301 @@ namespace pinocchio using type = std::vector>; }; - } // namespace internal - - /** - * @brief Applies a given function to each element in a std::vector. - * - * This function uses `std::for_each` to apply the provided function - * to each element in the input vector. - * - * @tparam T The type of elements stored in the vector. - * @tparam Allocator The allocator used by the vector. - * @tparam Func The type of the function to be applied. - * - * @param vector The vector whose elements the function will be applied to. - * @param func The function to apply to each element. It should accept a single argument of type - * `T&`. - */ - template - void apply_for_each(std::vector & vector, const Func & func) - { - std::for_each(vector.begin(), vector.end(), func); - } - - /** - * @brief Creates a vector of holder objects that wrap the elements of a given vector. - * - * This function takes a reference to a `std::vector` of elements of type `T` - * and constructs a new `std::vector` containing `Holder` objects, - * each created from the corresponding element in the input vector. - * - * Typical use case: producing a vector of `std::reference_wrapper` or - * other holder objects for easy element access or reference semantics. - * - * @tparam Holder A class template that accepts a single template parameter - * (for example, `std::reference_wrapper` or a custom holder template). - * @tparam T The element type stored in the input vector. - * @tparam Allocator The allocator type used by the input vector. - * @param vec Reference to the vector containing elements of type `T`. - * - * @return A new vector of type `std::vector>`, where each element - * wraps or references the corresponding element from the input vector. - * - * @note If `Holder` is a reference wrapper (e.g. `std::reference_wrapper`), - * the returned holders will refer to the original elements in `vec`. - * Make sure the lifetime of `vec` exceeds the lifetime of the returned vector - * to avoid dangling references. - * - * @see std::reference_wrapper - */ - template class Holder, typename T, typename Allocator> - std::vector> make_held_vector(std::vector & vec) - { - typedef std::vector> WrappedTVector; - return WrappedTVector(vec.cbegin(), vec.cend()); - } - - /** - * @brief Creates a vector of holder objects that wrap the elements of a given const vector. - * - * This function takes a constant reference to a `std::vector` of elements of type `T` - * and returns a new `std::vector` containing `Holder` objects constructed - * from each element of the input vector. - * - * Typical use case: producing a vector of `std::reference_wrapper` or - * another lightweight holder type from a vector of const elements. - * - * @tparam Holder A class template that accepts a single type parameter - * (e.g., `std::reference_wrapper` or a custom holder template). - * @tparam T The element type stored (const-qualified) in the input vector. - * @tparam Allocator The allocator type used by the input vector. - * @param vec The input vector containing elements of type `const T`. - * - * @return A new vector of type `std::vector>`, where each element - * wraps the corresponding element from the input vector. - * - * @note The elements are copied or wrapped using the constructor of `Holder` - * that takes a `const T&`. To avoid dangling references, ensure the lifetime - * of the original elements outlives the returned holders if `Holder` is a reference - * wrapper. - * - * @see std::reference_wrapper - */ - template class Holder, typename T, typename Allocator> - std::vector> make_held_vector(const std::vector & vec) - { - typedef std::vector> WrappedTVector; - return WrappedTVector(vec.cbegin(), vec.cend()); - } - - namespace helper - { /** - * @brief Type trait to detect whether a given type is an instantiation of `std::vector`. - * - * This trait provides a compile‑time Boolean constant that is `true` if the - * specified type @p T is (after removal of const/volatile qualifiers and - * references) an instantiation of `std::vector<...>`, and `false` otherwise. - * - * It can be used in `static_assert` expressions, `if constexpr` branches, or - * to enable/disable function or class template overloads through SFINAE. - * - * ### Example - * @code - * static_assert(is_std_vector>::value, "is a vector"); - * static_assert(!is_std_vector::value, "not a vector"); + * @brief Applies a given function to each element in a std::vector. * - * void f(const auto& x) { - * if constexpr (is_std_vector_v) - * std::cout << "x is an std::vector\n"; - * } - * @endcode + * This function uses `std::for_each` to apply the provided function + * to each element in the input vector. * - * @tparam T The type to test. Any cv‑qualified or reference form of - * an `std::vector` is normalized before the check. + * @tparam T The type of elements stored in the vector. + * @tparam Allocator The allocator used by the vector. + * @tparam Func The type of the function to be applied. * - * @see std::false_type, std::true_type, std::remove_cv_t, std::remove_reference_t + * @param vector The vector whose elements the function will be applied to. + * @param func The function to apply to each element. It should accept a single argument of type + * `T&`. */ - template - struct is_std_vector : std::false_type + template + void apply_for_each(std::vector & vector, const Func & func) { - }; + std::for_each(vector.begin(), vector.end(), func); + } /** - * @brief Partial specialization for types of the form `std::vector`. + * @brief Creates a vector of holder objects that wrap the elements of a given vector. + * + * This function takes a reference to a `std::vector` of elements of type `T` + * and constructs a new `std::vector` containing `Holder` objects, + * each created from the corresponding element in the input vector. + * + * Typical use case: producing a vector of `std::reference_wrapper` or + * other holder objects for easy element access or reference semantics. * - * This specialization derives from `std::true_type`, indicating that - * the tested type is indeed a standard vector instantiation. + * @tparam Holder A class template that accepts a single template parameter + * (for example, `std::reference_wrapper` or a custom holder template). + * @tparam T The element type stored in the input vector. + * @tparam Allocator The allocator type used by the input vector. + * @param vec Reference to the vector containing elements of type `T`. * - * @tparam T The element type of the vector. - * @tparam Alloc The allocator type used by the vector. + * @return A new vector of type `std::vector>`, where each element + * wraps or references the corresponding element from the input vector. + * + * @note If `Holder` is a reference wrapper (e.g. `std::reference_wrapper`), + * the returned holders will refer to the original elements in `vec`. + * Make sure the lifetime of `vec` exceeds the lifetime of the returned vector + * to avoid dangling references. + * + * @see std::reference_wrapper */ - template - struct is_std_vector> : std::true_type + template class Holder, typename T, typename Allocator> + std::vector> make_held_vector(std::vector & vec) { - }; + typedef std::vector> WrappedTVector; + return WrappedTVector(vec.cbegin(), vec.cend()); + } /** - * @brief Convenience variable template yielding the `is_std_vector` result. + * @brief Creates a vector of holder objects that wrap the elements of a given const vector. + * + * This function takes a constant reference to a `std::vector` of elements of type `T` + * and returns a new `std::vector` containing `Holder` objects constructed + * from each element of the input vector. + * + * Typical use case: producing a vector of `std::reference_wrapper` or + * another lightweight holder type from a vector of const elements. * - * Expands to a `bool` constant equal to `is_std_vector>::value`, - * allowing easy usage as `is_std_vector_v`. + * @tparam Holder A class template that accepts a single type parameter + * (e.g., `std::reference_wrapper` or a custom holder template). + * @tparam T The element type stored (const-qualified) in the input vector. + * @tparam Allocator The allocator type used by the input vector. + * @param vec The input vector containing elements of type `const T`. * - * @tparam T The type to test. + * @return A new vector of type `std::vector>`, where each element + * wraps the corresponding element from the input vector. * - * @return `true` if @p T denotes an `std::vector` type (ignoring cv/ref qualifiers), - * `false` otherwise. + * @note The elements are copied or wrapped using the constructor of `Holder` + * that takes a `const T&`. To avoid dangling references, ensure the lifetime + * of the original elements outlives the returned holders if `Holder` is a reference + * wrapper. * - * @since C++17 + * @see std::reference_wrapper */ - template - inline constexpr bool is_std_vector_v = is_std_vector>::value; - - /// @brief Tag type used to indicate that only the first matching element - /// should be erased from the vector. - struct erase_first_t + template class Holder, typename T, typename Allocator> + std::vector> make_held_vector(const std::vector & vec) { - }; + typedef std::vector> WrappedTVector; + return WrappedTVector(vec.cbegin(), vec.cend()); + } - /// @brief Tag type used to indicate that all matching elements - /// should be erased from the vector. - struct erase_all_t + namespace helper { - }; + /** + * @brief Type trait to detect whether a given type is an instantiation of `std::vector`. + * + * This trait provides a compile‑time Boolean constant that is `true` if the + * specified type @p T is (after removal of const/volatile qualifiers and + * references) an instantiation of `std::vector<...>`, and `false` otherwise. + * + * It can be used in `static_assert` expressions, `if constexpr` branches, or + * to enable/disable function or class template overloads through SFINAE. + * + * ### Example + * @code + * static_assert(is_std_vector>::value, "is a vector"); + * static_assert(!is_std_vector::value, "not a vector"); + * + * void f(const auto& x) { + * if constexpr (is_std_vector_v) + * std::cout << "x is an std::vector\n"; + * } + * @endcode + * + * @tparam T The type to test. Any cv‑qualified or reference form of + * an `std::vector` is normalized before the check. + * + * @see std::false_type, std::true_type, std::remove_cv_t, std::remove_reference_t + */ + template + struct is_std_vector : std::false_type + { + }; - /// @brief Tag type used to indicate that an element should be erased - /// by its index position in the vector. - struct erase_by_index_t - { - }; + /** + * @brief Partial specialization for types of the form `std::vector`. + * + * This specialization derives from `std::true_type`, indicating that + * the tested type is indeed a standard vector instantiation. + * + * @tparam T The element type of the vector. + * @tparam Alloc The allocator type used by the vector. + */ + template + struct is_std_vector> : std::true_type + { + }; - /// @brief Convenient inline tag instance corresponding to @ref erase_first_t. - inline constexpr erase_first_t erase_first{}; + /** + * @brief Convenience variable template yielding the `is_std_vector` result. + * + * Expands to a `bool` constant equal to `is_std_vector>::value`, + * allowing easy usage as `is_std_vector_v`. + * + * @tparam T The type to test. + * + * @return `true` if @p T denotes an `std::vector` type (ignoring cv/ref qualifiers), + * `false` otherwise. + * + * @since C++17 + */ + template + inline constexpr bool is_std_vector_v = is_std_vector>::value; + + /// @brief Tag type used to indicate that only the first matching element + /// should be erased from the vector. + struct erase_first_t + { + }; + + /// @brief Tag type used to indicate that all matching elements + /// should be erased from the vector. + struct erase_all_t + { + }; - /// @brief Convenient inline tag instance corresponding to @ref erase_all_t. - inline constexpr erase_all_t erase_all{}; + /// @brief Tag type used to indicate that an element should be erased + /// by its index position in the vector. + struct erase_by_index_t + { + }; - /// @brief Convenient inline tag instance corresponding to @ref erase_by_index_t. - inline constexpr erase_by_index_t erase_by_index{}; + /// @brief Convenient inline tag instance corresponding to @ref erase_first_t. + inline constexpr erase_first_t erase_first{}; - /// @brief Template interface for tag-based eraser. - /// @tparam Tag Erasure policy tag (e.g., @ref erase_first_t, @ref erase_all_t, @ref - /// erase_by_index_t) - template - struct eraser; + /// @brief Convenient inline tag instance corresponding to @ref erase_all_t. + inline constexpr erase_all_t erase_all{}; + + /// @brief Convenient inline tag instance corresponding to @ref erase_by_index_t. + inline constexpr erase_by_index_t erase_by_index{}; + + /// @brief Template interface for tag-based eraser. + /// @tparam Tag Erasure policy tag (e.g., @ref erase_first_t, @ref erase_all_t, @ref + /// erase_by_index_t) + template + struct eraser; + + /// @brief Eraser specialization for removing only the first occurrence of a value. + /// @tparam T Type of elements stored in the vector. + /// @tparam Allocator Allocator type used by the vector. + template<> + struct eraser + { + /** + * @brief Removes the first element in the vector equal to `value`. + * + * @param vec Reference to the vector to modify. + * @param value Element value to remove. + * + * If the value does not exist, the vector remains unchanged. + */ + template + static void apply(std::vector & vec, const T & value) + { + auto it = std::find(vec.begin(), vec.end(), value); + if (it != vec.end()) + vec.erase(it); + } + }; + + /// @brief Eraser specialization for removing all occurrences of a value. + /// @tparam T Type of elements stored in the vector. + /// @tparam Allocator Allocator type used by the vector. + template<> + struct eraser + { + /** + * @brief Removes all elements in the vector equal to `value`. + * + * @param vec Reference to the vector to modify. + * @param value Element value to remove. + * + * Uses the remove‑erase idiom internally. + */ + template + static void apply(std::vector & vec, const T & value) + { + vec.erase(std::remove(vec.begin(), vec.end(), value), vec.end()); + } + }; + + /// @brief Eraser specialization for removing an element by index. + /// @tparam T Type of elements stored in the vector. + /// @tparam Allocator Allocator type used by the vector. + template<> + struct eraser + { + /** + * @brief Removes the element at the specified index from the vector. + * + * @param vec Reference to the vector to modify. + * @param index Index of the element to remove. + * + * @throw std::out_of_range if @p index is greater than or equal to `vec.size()`. + */ + template + static void apply(std::vector & vec, std::size_t index) + { + PINOCCHIO_THROW_IF(index >= vec.size(), std::out_of_range, "Index out of range"); + vec.erase(vec.begin() + index); + } + }; - /// @brief Eraser specialization for removing only the first occurrence of a value. - /// @tparam T Type of elements stored in the vector. - /// @tparam Allocator Allocator type used by the vector. - template<> - struct eraser - { /** - * @brief Removes the first element in the vector equal to `value`. - * - * @param vec Reference to the vector to modify. - * @param value Element value to remove. + * @brief Erase elements from a vector based on the provided tag type. * - * If the value does not exist, the vector remains unchanged. + * @tparam Tag Tag struct choosing the erasure mode + * (e.g., @ref erase_first_t, @ref erase_all_t). + * @tparam T Type of elements stored in the vector. + * @tparam Allocator Allocator type used by the vector. + * @param vec Reference to the vector to modify. + * @param value Value of the element(s) to remove. */ - template - static void apply(std::vector & vec, const T & value) + template + void erase(std::vector & vec, const T & value, Tag) { - auto it = std::find(vec.begin(), vec.end(), value); - if (it != vec.end()) - vec.erase(it); + eraser::apply(vec, value); } - }; - /// @brief Eraser specialization for removing all occurrences of a value. - /// @tparam T Type of elements stored in the vector. - /// @tparam Allocator Allocator type used by the vector. - template<> - struct eraser - { /** - * @brief Removes all elements in the vector equal to `value`. + * @brief Erase the element at the given index in a vector. * - * @param vec Reference to the vector to modify. - * @param value Element value to remove. + * @tparam Tag Dummy template parameter, ignored here. + * @tparam T Type of elements stored in the vector. + * @tparam Allocator Allocator type used by the vector. + * @param vec Reference to the vector to modify. + * @param index Index of the element to remove. * - * Uses the remove‑erase idiom internally. + * @throw std::out_of_range if @p index is out of bounds. */ - template - static void apply(std::vector & vec, const T & value) + template + void erase(std::vector & vec, const std::size_t index) { - vec.erase(std::remove(vec.begin(), vec.end(), value), vec.end()); + eraser::apply(vec, index); } - }; - /// @brief Eraser specialization for removing an element by index. - /// @tparam T Type of elements stored in the vector. - /// @tparam Allocator Allocator type used by the vector. - template<> - struct eraser - { /** - * @brief Removes the element at the specified index from the vector. - * - * @param vec Reference to the vector to modify. - * @param index Index of the element to remove. + * @brief Checks whether a given value exists in a vector. * - * @throw std::out_of_range if @p index is greater than or equal to `vec.size()`. + * @tparam T Type of elements stored in the vector. + * @tparam Allocator Allocator type used by the vector. + * @param vec Constant reference to the vector to inspect. + * @param value Value to search for. + * @return true if the value is found, false otherwise. */ template - static void apply(std::vector & vec, std::size_t index) + bool exists(const std::vector & vec, const T & value) { - PINOCCHIO_THROW_IF(index >= vec.size(), std::out_of_range, "Index out of range"); - vec.erase(vec.begin() + index); + return std::find(vec.begin(), vec.end(), value) != vec.end(); } - }; - - /** - * @brief Erase elements from a vector based on the provided tag type. - * - * @tparam Tag Tag struct choosing the erasure mode - * (e.g., @ref erase_first_t, @ref erase_all_t). - * @tparam T Type of elements stored in the vector. - * @tparam Allocator Allocator type used by the vector. - * @param vec Reference to the vector to modify. - * @param value Value of the element(s) to remove. - */ - template - void erase(std::vector & vec, const T & value, Tag) - { - eraser::apply(vec, value); - } - - /** - * @brief Erase the element at the given index in a vector. - * - * @tparam Tag Dummy template parameter, ignored here. - * @tparam T Type of elements stored in the vector. - * @tparam Allocator Allocator type used by the vector. - * @param vec Reference to the vector to modify. - * @param index Index of the element to remove. - * - * @throw std::out_of_range if @p index is out of bounds. - */ - template - void erase(std::vector & vec, const std::size_t index) - { - eraser::apply(vec, index); - } - - /** - * @brief Checks whether a given value exists in a vector. - * - * @tparam T Type of elements stored in the vector. - * @tparam Allocator Allocator type used by the vector. - * @param vec Constant reference to the vector to inspect. - * @param value Value to search for. - * @return true if the value is found, false otherwise. - */ - template - bool exists(const std::vector & vec, const T & value) - { - return std::find(vec.begin(), vec.end(), value) != vec.end(); - } - } // namespace helper + } // namespace helper + } // namespace internal } // namespace pinocchio diff --git a/unittest/delassus-operations.cpp b/unittest/delassus-operations.cpp index 433b1d0fe9..da2be6dd26 100644 --- a/unittest/delassus-operations.cpp +++ b/unittest/delassus-operations.cpp @@ -67,9 +67,9 @@ BOOST_AUTO_TEST_CASE(delassus_dense_rebuild) DelassusOperatorRigidBody; DelassusOperatorRigidBody delassus_rigid_body( - helper::make_ref(scene.model), helper::make_ref(scene.data), - helper::make_ref(scene.constraint_models), helper::make_ref(scene.constraint_datas), - damping_val); + internal::helper::make_ref(scene.model), internal::helper::make_ref(scene.data), + internal::helper::make_ref(scene.constraint_models), + internal::helper::make_ref(scene.constraint_datas), damping_val); BOOST_CHECK(delassus_rigid_body.getCompliance().isApprox(compliance)); delassus_rigid_body.updateDamping(damping_val); delassus_rigid_body.compute(); @@ -301,21 +301,22 @@ BOOST_AUTO_TEST_CASE(delassus_rigid_body_rebuild) DelassusOperatorRigidBody; DelassusOperatorRigidBody delassus_rigid_body( - helper::make_ref(scene.model), helper::make_ref(scene.data), - helper::make_ref(scene.constraint_models), helper::make_ref(scene.constraint_datas), - damping_val); + internal::helper::make_ref(scene.model), internal::helper::make_ref(scene.data), + internal::helper::make_ref(scene.constraint_models), + internal::helper::make_ref(scene.constraint_datas), damping_val); BOOST_CHECK(delassus_rigid_body.getCompliance().isApprox(compliance)); delassus_rigid_body.updateDamping(damping_val); delassus_rigid_body.compute(); // Test rebuild from rigid body delassus to another rigid body delassus DelassusOperatorRigidBody delassus_rigid_body_rebuilt( - helper::make_ref(scene.model), helper::make_ref(scene.data), - helper::make_ref(scene.constraint_models), helper::make_ref(scene.constraint_datas), - damping_val); + internal::helper::make_ref(scene.model), internal::helper::make_ref(scene.data), + internal::helper::make_ref(scene.constraint_models), + internal::helper::make_ref(scene.constraint_datas), damping_val); delassus_rigid_body_rebuilt.rebuild( - helper::make_ref(scene.model), helper::make_ref(scene.data), - helper::make_ref(scene.constraint_models), helper::make_ref(scene.constraint_datas)); + internal::helper::make_ref(scene.model), internal::helper::make_ref(scene.data), + internal::helper::make_ref(scene.constraint_models), + internal::helper::make_ref(scene.constraint_datas)); // -- test compliance and damping BOOST_CHECK(delassus_rigid_body_rebuilt.getCompliance().isApprox(compliance)); @@ -349,10 +350,10 @@ BOOST_AUTO_TEST_CASE(delassus_rigid_body_diag_operations) DelassusOperatorRigidBody; DelassusOperatorRigidBody delassus( - helper::make_ref(scene.model), // - helper::make_ref(scene.data), // - helper::make_ref(scene.constraint_models), // - helper::make_ref(scene.constraint_datas), // + internal::helper::make_ref(scene.model), // + internal::helper::make_ref(scene.data), // + internal::helper::make_ref(scene.constraint_models), // + internal::helper::make_ref(scene.constraint_datas), // damping_val); delassus.compute(); @@ -467,8 +468,9 @@ BOOST_AUTO_TEST_CASE(delassus_rigid_body_block_operations) DelassusOperatorRigidBody; DelassusOperatorRigidBody delassus( - helper::make_ref(scene.model), helper::make_ref(scene.data), - helper::make_ref(scene.constraint_models), helper::make_ref(scene.constraint_datas), 1e-8); + internal::helper::make_ref(scene.model), internal::helper::make_ref(scene.data), + internal::helper::make_ref(scene.constraint_models), + internal::helper::make_ref(scene.constraint_datas), 1e-8); delassus.compute(); Eigen::VectorXd res(size); diff --git a/unittest/delassus-operator-rigid-body.cpp b/unittest/delassus-operator-rigid-body.cpp index 031e72374e..0cec36953a 100644 --- a/unittest/delassus-operator-rigid-body.cpp +++ b/unittest/delassus-operator-rigid-body.cpp @@ -98,12 +98,12 @@ BOOST_AUTO_TEST_CASE(default_constructor_reference_wrapper) ConstraintModelVector constraint_models; std::reference_wrapper constraint_models_ref = constraint_models; ConstraintDataVector constraint_datas; - auto constraint_datas_ref = helper::make_ref(constraint_datas); + auto constraint_datas_ref = pinocchio::internal::helper::make_ref(constraint_datas); DelassusOperatorRigidBodyReferenceWrapper delassus_operator( model_ref, data_ref, constraint_models_ref, constraint_datas_ref); - const auto csize = residualSize(helper::get_ref(constraint_models_ref)); + const auto csize = residualSize(pinocchio::internal::helper::get_ref(constraint_models_ref)); BOOST_CHECK(delassus_operator.size() == csize); BOOST_CHECK(delassus_operator.size() == 0); @@ -155,12 +155,12 @@ BOOST_AUTO_TEST_CASE(default_constructor_const_reference_wrapper) cmodel.calc(model, data, cdata); // make constraint data up to date with system state WrappedDelassusOperatorRigidBody delassus_operator( - helper::make_ref(model), // - helper::make_ref(data), // - helper::make_ref(constraint_models), // - helper::make_ref(constraint_datas)); + pinocchio::internal::helper::make_ref(model), // + pinocchio::internal::helper::make_ref(data), // + pinocchio::internal::helper::make_ref(constraint_models), // + pinocchio::internal::helper::make_ref(constraint_datas)); - const auto csize = residualSize(helper::get_ref(constraint_models)); + const auto csize = residualSize(pinocchio::internal::helper::get_ref(constraint_models)); BOOST_CHECK(delassus_operator.size() == csize); BOOST_CHECK(&delassus_operator.model() == &model); @@ -1194,10 +1194,12 @@ void test_solve_in_place( double, 0, JointCollectionDefaultTpl, ConstraintModel, std::reference_wrapper> DelassusOperatorRigidBodyReferenceWrapper; - const Model & model = helper::get_ref(model_ref); - Data & data = helper::get_ref(data_ref); - const ConstraintModelVector & constraint_models = helper::get_ref(constraint_models_ref); - ConstraintDataVector & constraint_datas = helper::get_ref(constraint_datas_ref); + const Model & model = pinocchio::internal::helper::get_ref(model_ref); + Data & data = pinocchio::internal::helper::get_ref(data_ref); + const ConstraintModelVector & constraint_models = + pinocchio::internal::helper::get_ref(constraint_models_ref); + ConstraintDataVector & constraint_datas = + pinocchio::internal::helper::get_ref(constraint_datas_ref); // Necessary to update data oMi, lMi and J for internal rigid body computations computeJointJacobians(model, data, q_neutral); diff --git a/unittest/eigen-basic-op.cpp b/unittest/eigen-basic-op.cpp index 9d47cee0dc..7818f18d70 100644 --- a/unittest/eigen-basic-op.cpp +++ b/unittest/eigen-basic-op.cpp @@ -67,19 +67,19 @@ BOOST_AUTO_TEST_CASE(test_eigen_helpers_on_std_vector) std::vector vec(10, MatrixXd::Ones(m, n)); - apply_for_each(vec, setZero); + pinocchio::internal::apply_for_each(vec, setZero); for (const auto & val : vec) { BOOST_CHECK(val.isZero(0)); } - apply_for_each(vec, setOnes); + pinocchio::internal::apply_for_each(vec, setOnes); for (const auto & val : vec) { BOOST_CHECK(val.isOnes(0)); } - apply_for_each(vec, setIdentity); + pinocchio::internal::apply_for_each(vec, setIdentity); for (const auto & val : vec) { BOOST_CHECK(val.isIdentity(0)); diff --git a/unittest/promote-static-eval.cpp b/unittest/promote-static-eval.cpp index 2dd3ea54f0..b0d26ae20d 100644 --- a/unittest/promote-static-eval.cpp +++ b/unittest/promote-static-eval.cpp @@ -15,23 +15,23 @@ BOOST_AUTO_TEST_CASE(test_helpers) { Eigen::MatrixXd A = Eigen::MatrixXd::Random(2, 2); - BOOST_CHECK(!helper::is_eigen_noalias_v); - BOOST_CHECK(helper::is_eigen_noalias_v); + BOOST_CHECK(!internal::helper::is_eigen_noalias_v); + BOOST_CHECK(internal::helper::is_eigen_noalias_v); typedef Eigen::Matrix Matrix3d; - BOOST_CHECK(helper::has_fixed_rows_v); - BOOST_CHECK(!helper::has_fixed_cols_v); - BOOST_CHECK(!helper::has_fixed_size_v); + BOOST_CHECK(internal::helper::has_fixed_rows_v); + BOOST_CHECK(!internal::helper::has_fixed_cols_v); + BOOST_CHECK(!internal::helper::has_fixed_size_v); typedef Eigen::Matrix Matrixd3; - BOOST_CHECK(!helper::has_fixed_rows_v); - BOOST_CHECK(helper::has_fixed_cols_v); - BOOST_CHECK(!helper::has_fixed_size_v); + BOOST_CHECK(!internal::helper::has_fixed_rows_v); + BOOST_CHECK(internal::helper::has_fixed_cols_v); + BOOST_CHECK(!internal::helper::has_fixed_size_v); typedef Eigen::Matrix Matrix33; - BOOST_CHECK(helper::has_fixed_rows_v); - BOOST_CHECK(helper::has_fixed_cols_v); - BOOST_CHECK(helper::has_fixed_size_v); + BOOST_CHECK(internal::helper::has_fixed_rows_v); + BOOST_CHECK(internal::helper::has_fixed_cols_v); + BOOST_CHECK(internal::helper::has_fixed_size_v); } BOOST_AUTO_TEST_CASE(test_make_map) @@ -90,7 +90,7 @@ BOOST_AUTO_TEST_CASE(test_dynamic_matrix) BOOST_CHECK(C_expression.rows() == A.rows()); BOOST_CHECK(C_expression.cols() == B.cols()); - auto C_op = promote_static_eval<10>(C); + auto C_op = internal::promote_static_eval<10>(C); BOOST_CHECK(&C_op.expression() == &C); BOOST_CHECK( @@ -106,7 +106,7 @@ BOOST_AUTO_TEST_CASE(test_dynamic_matrix) // Specific case where MaxUnfolding == 0 { - auto C_op = promote_static_eval(C); + auto C_op = internal::promote_static_eval(C); BOOST_CHECK(&C_op.expression() == &C); C_op = A * B; @@ -117,7 +117,7 @@ BOOST_AUTO_TEST_CASE(test_dynamic_matrix) A.setConstant(3); B.setConstant(4); - auto C_noalias_op = promote_static_eval<10>(C.noalias()); + auto C_noalias_op = internal::promote_static_eval<10>(C.noalias()); BOOST_CHECK(&C_noalias_op.expression().expression() == &C); BOOST_CHECK( @@ -134,7 +134,7 @@ BOOST_AUTO_TEST_CASE(test_dynamic_matrix) // Specific case where MaxUnfolding == 0 { - auto C_noalias_op = promote_static_eval(C.noalias()); + auto C_noalias_op = internal::promote_static_eval(C.noalias()); BOOST_CHECK(&C_noalias_op.expression().expression() == &C); C_noalias_op = A * B; @@ -158,7 +158,7 @@ BOOST_AUTO_TEST_CASE(test_static_matrix) BOOST_CHECK(C_expression.rows() == A.rows()); BOOST_CHECK(C_expression.cols() == B.cols()); - auto C_op = promote_static_eval<10>(C); + auto C_op = internal::promote_static_eval<10>(C); BOOST_CHECK(&C_op.expression() == &C); BOOST_CHECK( @@ -174,7 +174,7 @@ BOOST_AUTO_TEST_CASE(test_static_matrix) // Specific case where MaxUnfolding == 0 { - auto C_op = promote_static_eval(C); + auto C_op = internal::promote_static_eval(C); BOOST_CHECK(&C_op.expression() == &C); C_op = A * B; @@ -185,7 +185,7 @@ BOOST_AUTO_TEST_CASE(test_static_matrix) A.setConstant(3); B.setConstant(4); - auto C_noalias_op = promote_static_eval<10>(C.noalias()); + auto C_noalias_op = internal::promote_static_eval<10>(C.noalias()); BOOST_CHECK(&C_noalias_op.expression().expression() == &C); BOOST_CHECK( @@ -202,7 +202,7 @@ BOOST_AUTO_TEST_CASE(test_static_matrix) // Specific case where MaxUnfolding == 0 { - auto C_noalias_op = promote_static_eval(C.noalias()); + auto C_noalias_op = internal::promote_static_eval(C.noalias()); BOOST_CHECK(&C_noalias_op.expression().expression() == &C); C_noalias_op = A * B; @@ -227,7 +227,7 @@ BOOST_AUTO_TEST_CASE(test_partial_static_matrix) BOOST_CHECK(C_expression.rows() == A.rows()); BOOST_CHECK(C_expression.cols() == B.cols()); - auto C_op = promote_static_eval<10>(C); + auto C_op = internal::promote_static_eval<10>(C); BOOST_CHECK(&C_op.expression() == &C); BOOST_CHECK( @@ -243,7 +243,7 @@ BOOST_AUTO_TEST_CASE(test_partial_static_matrix) // Specific case where MaxUnfolding == 0 { - auto C_op = promote_static_eval(C); + auto C_op = internal::promote_static_eval(C); BOOST_CHECK(&C_op.expression() == &C); C_op = A * B; @@ -254,7 +254,7 @@ BOOST_AUTO_TEST_CASE(test_partial_static_matrix) A.setConstant(3); B.setConstant(4); - auto C_noalias_op = promote_static_eval<10>(C.noalias()); + auto C_noalias_op = internal::promote_static_eval<10>(C.noalias()); BOOST_CHECK(&C_noalias_op.expression().expression() == &C); BOOST_CHECK( @@ -271,7 +271,7 @@ BOOST_AUTO_TEST_CASE(test_partial_static_matrix) // Specific case where MaxUnfolding == 0 { - auto C_noalias_op = promote_static_eval(C.noalias()); + auto C_noalias_op = internal::promote_static_eval(C.noalias()); BOOST_CHECK(&C_noalias_op.expression().expression() == &C); C_noalias_op = A * B; @@ -301,7 +301,7 @@ BOOST_AUTO_TEST_CASE(test_specitic_6x6_case) const auto matrix_product = A * B; BOOST_CHECK( - promote_static_eval(res.noalias()).dispatch_type(matrix_product) + internal::promote_static_eval(res.noalias()).dispatch_type(matrix_product) == pinocchio::internal::DispatchType::STATIC); } diff --git a/unittest/promote-static-op.cpp b/unittest/promote-static-op.cpp index 8ff0bde634..c6d1313585 100644 --- a/unittest/promote-static-op.cpp +++ b/unittest/promote-static-op.cpp @@ -15,23 +15,23 @@ BOOST_AUTO_TEST_CASE(test_helpers) { Eigen::MatrixXd A = Eigen::MatrixXd::Random(2, 2); - BOOST_CHECK(!helper::is_eigen_noalias_v); - BOOST_CHECK(helper::is_eigen_noalias_v); + BOOST_CHECK(!internal::helper::is_eigen_noalias_v); + BOOST_CHECK(internal::helper::is_eigen_noalias_v); typedef Eigen::Matrix Matrix3d; - BOOST_CHECK(helper::has_fixed_rows_v); - BOOST_CHECK(!helper::has_fixed_cols_v); - BOOST_CHECK(!helper::has_fixed_size_v); + BOOST_CHECK(internal::helper::has_fixed_rows_v); + BOOST_CHECK(!internal::helper::has_fixed_cols_v); + BOOST_CHECK(!internal::helper::has_fixed_size_v); typedef Eigen::Matrix Matrixd3; - BOOST_CHECK(!helper::has_fixed_rows_v); - BOOST_CHECK(helper::has_fixed_cols_v); - BOOST_CHECK(!helper::has_fixed_size_v); + BOOST_CHECK(!internal::helper::has_fixed_rows_v); + BOOST_CHECK(internal::helper::has_fixed_cols_v); + BOOST_CHECK(!internal::helper::has_fixed_size_v); typedef Eigen::Matrix Matrix33; - BOOST_CHECK(helper::has_fixed_rows_v); - BOOST_CHECK(helper::has_fixed_cols_v); - BOOST_CHECK(helper::has_fixed_size_v); + BOOST_CHECK(internal::helper::has_fixed_rows_v); + BOOST_CHECK(internal::helper::has_fixed_cols_v); + BOOST_CHECK(internal::helper::has_fixed_size_v); } BOOST_AUTO_TEST_CASE(test_size_product) @@ -80,7 +80,7 @@ BOOST_AUTO_TEST_CASE(test_dynamic_matrix) BOOST_CHECK(C_expression.rows() == A.rows()); BOOST_CHECK(C_expression.cols() == B.cols()); - auto C_op = promote_static_eval<10>(C); + auto C_op = internal::promote_static_eval<10>(C); BOOST_CHECK(&C_op.expression() == &C); BOOST_CHECK( @@ -98,7 +98,7 @@ BOOST_AUTO_TEST_CASE(test_dynamic_matrix) A.setConstant(3); B.setConstant(4); - auto C_noalias_op = promote_static_eval<10>(C.noalias()); + auto C_noalias_op = internal::promote_static_eval<10>(C.noalias()); BOOST_CHECK(&C_noalias_op.expression().expression() == &C); BOOST_CHECK( @@ -130,7 +130,7 @@ BOOST_AUTO_TEST_CASE(test_static_matrix) BOOST_CHECK(C_expression.rows() == A.rows()); BOOST_CHECK(C_expression.cols() == B.cols()); - auto C_op = promote_static_eval<10>(C); + auto C_op = internal::promote_static_eval<10>(C); BOOST_CHECK(&C_op.expression() == &C); BOOST_CHECK( @@ -148,7 +148,7 @@ BOOST_AUTO_TEST_CASE(test_static_matrix) A.setConstant(3); B.setConstant(4); - auto C_noalias_op = promote_static_eval<10>(C.noalias()); + auto C_noalias_op = internal::promote_static_eval<10>(C.noalias()); BOOST_CHECK(&C_noalias_op.expression().expression() == &C); BOOST_CHECK( @@ -181,7 +181,7 @@ BOOST_AUTO_TEST_CASE(test_partial_static_matrix) BOOST_CHECK(C_expression.rows() == A.rows()); BOOST_CHECK(C_expression.cols() == B.cols()); - auto C_op = promote_static_eval<10>(C); + auto C_op = internal::promote_static_eval<10>(C); BOOST_CHECK(&C_op.expression() == &C); BOOST_CHECK( @@ -199,7 +199,7 @@ BOOST_AUTO_TEST_CASE(test_partial_static_matrix) A.setConstant(3); B.setConstant(4); - auto C_noalias_op = promote_static_eval<10>(C.noalias()); + auto C_noalias_op = internal::promote_static_eval<10>(C.noalias()); BOOST_CHECK(&C_noalias_op.expression().expression() == &C); BOOST_CHECK( diff --git a/unittest/reference.cpp b/unittest/reference.cpp index edb3eb6dbf..f5eeae1352 100644 --- a/unittest/reference.cpp +++ b/unittest/reference.cpp @@ -13,7 +13,7 @@ BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE) BOOST_AUTO_TEST_CASE(test_get_ref) { - using namespace ::pinocchio::helper; + using namespace ::pinocchio::internal::helper; { double v = 10; diff --git a/unittest/size-in-bytes.cpp b/unittest/size-in-bytes.cpp index 5f8c34acde..0f929478e7 100644 --- a/unittest/size-in-bytes.cpp +++ b/unittest/size-in-bytes.cpp @@ -34,51 +34,51 @@ BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE) BOOST_AUTO_TEST_CASE(test_simple_struct) { BOOST_CHECK(1 == SimpleStruct1().sizeInBytes()); - BOOST_CHECK(sizeInBytes(SimpleStruct1()) == SimpleStruct1().sizeInBytes()); + BOOST_CHECK(internal::sizeInBytes(SimpleStruct1()) == SimpleStruct1().sizeInBytes()); // BOOST_CHECK(10 == SimpleStruct10().sizeInBytes()); - BOOST_CHECK(sizeInBytes(SimpleStruct10()) == SimpleStruct10().sizeInBytes()); + BOOST_CHECK(internal::sizeInBytes(SimpleStruct10()) == SimpleStruct10().sizeInBytes()); // BOOST_CHECK(100 == SimpleStruct100().sizeInBytes()); - BOOST_CHECK(sizeInBytes(SimpleStruct100()) == SimpleStruct100().sizeInBytes()); + BOOST_CHECK(internal::sizeInBytes(SimpleStruct100()) == SimpleStruct100().sizeInBytes()); } BOOST_AUTO_TEST_CASE(test_std_vector) { std::vector vector(100); - BOOST_CHECK(sizeInBytes(vector) == vector.size() * vector[0].sizeInBytes()); + BOOST_CHECK(internal::sizeInBytes(vector) == vector.size() * vector[0].sizeInBytes()); } BOOST_AUTO_TEST_CASE(test_std_array) { std::array array; - BOOST_CHECK(sizeInBytes(array) == array.size() * array[0].sizeInBytes()); + BOOST_CHECK(internal::sizeInBytes(array) == array.size() * array[0].sizeInBytes()); } BOOST_AUTO_TEST_CASE(test_eigen_matrix) { const Eigen::Matrix3d mat33; - BOOST_CHECK(sizeInBytes(mat33) == sizeof(mat33)); + BOOST_CHECK(internal::sizeInBytes(mat33) == sizeof(mat33)); const Eigen::MatrixXd mat(mat33); - BOOST_CHECK(sizeInBytes(mat) - sizeInBytes(mat33) == 1); + BOOST_CHECK(internal::sizeInBytes(mat) - internal::sizeInBytes(mat33) == 1); } BOOST_AUTO_TEST_CASE(test_eigen_map) { const Eigen::Matrix3d mat; const auto mat_map = make_default_map(mat); - BOOST_CHECK(sizeInBytes(mat_map) == sizeInBytes(mat)); + BOOST_CHECK(internal::sizeInBytes(mat_map) == internal::sizeInBytes(mat)); } BOOST_AUTO_TEST_CASE(test_fundamental_types) { - BOOST_CHECK(sizeInBytes(bool(1)) == sizeof(bool)); - BOOST_CHECK(sizeInBytes(char(1)) == sizeof(char)); - BOOST_CHECK(sizeInBytes(int(1)) == sizeof(int)); - BOOST_CHECK(sizeInBytes(float(1)) == sizeof(float)); - BOOST_CHECK(sizeInBytes(double(1)) == sizeof(double)); - BOOST_CHECK(sizeInBytes((long double)(1)) == sizeof(long double)); + BOOST_CHECK(internal::sizeInBytes(bool(1)) == sizeof(bool)); + BOOST_CHECK(internal::sizeInBytes(char(1)) == sizeof(char)); + BOOST_CHECK(internal::sizeInBytes(int(1)) == sizeof(int)); + BOOST_CHECK(internal::sizeInBytes(float(1)) == sizeof(float)); + BOOST_CHECK(internal::sizeInBytes(double(1)) == sizeof(double)); + BOOST_CHECK(internal::sizeInBytes((long double)(1)) == sizeof(long double)); } BOOST_AUTO_TEST_SUITE_END() From e26853dabe3db0aa7622933117346febe0900347 Mon Sep 17 00:00:00 2001 From: Joris Vaillant Date: Thu, 2 Apr 2026 16:46:48 +0200 Subject: [PATCH 08/14] convention: Update convention --- development/convention.md | 29 +++++++++++++++++++++++++++++ 1 file changed, 29 insertions(+) diff --git a/development/convention.md b/development/convention.md index d4d9c27f47..a62a09eecb 100644 --- a/development/convention.md +++ b/development/convention.md @@ -295,3 +295,32 @@ CompileFlags: Add: - -DPINOCCHIO_LSP ``` + +## API + +Pinocchio have a public and private API. +Public API evolve follwing the [SemVer semantic](https://semver.org/). +Private API doesn't have any constraints and can change in any version. + +Struct/Class/Union/Alias/Enum/Function inside the following namespace are part of the private API: +- `pinocchio::internal` +- `pinocchio::detail` +- `pinocchio::details` +- `pinocchio::impl` +- `pinocchio::fix` +- `pinocchio::optimized` + +Macro beginning by a `_` are part of the private API. + +Struct/Class/Union member (public/protected/private) beginning by a `_` are part of the private API. + +### Deprecation + +Before making an API break we mark it as deprecated. + +We do that by (when possible): +- Adding an entry in the changelog +- Use `PINOCCHIO_DEPRECATED` macro for struct/class/union/alias/enum/function +- Use `PINOCCHIO_DEPRECATED_HEADER` macro for headers + +In the next major release after the deprecation we can then remove the deprecated API. From d880e33eab92a839c251140d28e9c88906a88aa6 Mon Sep 17 00:00:00 2001 From: Joris Vaillant Date: Thu, 2 Apr 2026 17:32:55 +0200 Subject: [PATCH 09/14] Update development/convention.md Co-authored-by: j-matheron --- development/convention.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/development/convention.md b/development/convention.md index a62a09eecb..a0cb43bb3b 100644 --- a/development/convention.md +++ b/development/convention.md @@ -299,7 +299,7 @@ CompileFlags: ## API Pinocchio have a public and private API. -Public API evolve follwing the [SemVer semantic](https://semver.org/). +Public API evolve following the [SemVer semantic](https://semver.org/). Private API doesn't have any constraints and can change in any version. Struct/Class/Union/Alias/Enum/Function inside the following namespace are part of the private API: From 4e596a2f4f5ae26f2d5c87b334d20357c392d2db Mon Sep 17 00:00:00 2001 From: Joris Vaillant Date: Thu, 2 Apr 2026 17:27:47 +0200 Subject: [PATCH 10/14] math: Remove some useless internal:: scope --- include/pinocchio/src/math/matrix-inverse.hxx | 2 +- include/pinocchio/src/math/matrix-product.hxx | 18 ++++++++---------- 2 files changed, 9 insertions(+), 11 deletions(-) diff --git a/include/pinocchio/src/math/matrix-inverse.hxx b/include/pinocchio/src/math/matrix-inverse.hxx index 865abfa173..b6fde9e525 100644 --- a/include/pinocchio/src/math/matrix-inverse.hxx +++ b/include/pinocchio/src/math/matrix-inverse.hxx @@ -144,7 +144,7 @@ namespace pinocchio EIGEN_STRONG_INLINE void matrix_inversion( const Eigen::MatrixBase & matrix, const Eigen::MatrixBase & matrix_inverse) { - internal::MatrixInversion::run(matrix, matrix_inverse.const_cast_derived()); + MatrixInversion::run(matrix, matrix_inverse.const_cast_derived()); } } // namespace internal diff --git a/include/pinocchio/src/math/matrix-product.hxx b/include/pinocchio/src/math/matrix-product.hxx index 3466560a93..b4b47f5491 100644 --- a/include/pinocchio/src/math/matrix-product.hxx +++ b/include/pinocchio/src/math/matrix-product.hxx @@ -41,11 +41,9 @@ namespace pinocchio { const auto max_size = std::max(lhs.rows(), std::max(lhs.cols(), rhs.cols())); if (max_size <= 0) - internal::matrix_product_small_size( - lhs.derived(), rhs.derived(), res.const_cast_derived()); + matrix_product_small_size(lhs.derived(), rhs.derived(), res.const_cast_derived()); else - internal::matrix_product_generic( - lhs.derived(), rhs.derived(), res.const_cast_derived()); + matrix_product_generic(lhs.derived(), rhs.derived(), res.const_cast_derived()); }; template< @@ -76,15 +74,15 @@ namespace pinocchio const auto matrix_product = lhs_static * rhs_static; - if constexpr (internal::is_specialization_of_v) + if constexpr (is_specialization_of_v) { res_static.noalias() = matrix_product; } - else if constexpr (internal::is_specialization_of_v) + else if constexpr (is_specialization_of_v) { res_static.noalias() += matrix_product; } - else if constexpr (internal::is_specialization_of_v) + else if constexpr (is_specialization_of_v) { res_static.noalias() -= matrix_product; } @@ -140,15 +138,15 @@ namespace pinocchio sum += lhs_data[lhs_index(i, k)] * rhs_data[rhs_index(k, j)]; } typedef EigenOp Op; - if constexpr (internal::is_specialization_of_v) + if constexpr (is_specialization_of_v) { res_data[res_index(i, j)] = sum; } - else if constexpr (internal::is_specialization_of_v) + else if constexpr (is_specialization_of_v) { res_data[res_index(i, j)] += sum; } - else if constexpr (internal::is_specialization_of_v) + else if constexpr (is_specialization_of_v) { res_data[res_index(i, j)] -= sum; } From 5bcdfae778a2cb5366f5f48d4f0224a42889a22b Mon Sep 17 00:00:00 2001 From: Joris Vaillant Date: Thu, 2 Apr 2026 17:39:17 +0200 Subject: [PATCH 11/14] changelog: Add entries --- CHANGELOG.md | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index a0cb298248..532871d383 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -23,8 +23,14 @@ The format is based on [Keep a Changelog](https://keepachangelog.com/en/1.0.0/). - Add `PINOCCHIO_DISABLE_UNSUPPORTED_WARNINGS` C++ definition to disable unsupported algorithm warnings - Add `PINOCCHIO_BUILD_MPFR_TESTING` CMake option to build MPFR tests - Add `pinocchio/utils/alloca.hpp`: Helpers for mapping stack allocation for Eigen::Map -- Add `pinochio/container/eigen-storage.hpp`: Introduce `EigenStorageTpl` +- Add `pinochio/container/eigen-storage.hpp`: Introduce `internal::EigenStorageTpl` - Add `pinochio/container/matrix-stack.hpp`: Introduce `internal::MatrixStackTpl` +- Add `pinochio/container/double-entry-container.hpp`: Introduce `internal::DoubleEntryContainer` +- Add `internal::MatrixBlockElementTpl` in `math.hpp` +- Add `internal::BlockDiagonalMatrixTpl` in `math.hpp` +- Add `internal::matrix_product` in `math.hpp` +- Add `internal::matrix_inversion` in `math.hpp` +- Add `internal::matrix_inversion_code_generated` in `math.hpp` ### Changed - Clean delassus API: DelassusOperatorBase define the main delassus API and each method calls `derived().[name-of-method]Impl` From 2705894d65a056badd733b4a24fcc2515e613403 Mon Sep 17 00:00:00 2001 From: Joris Vaillant Date: Wed, 8 Apr 2026 16:51:07 +0200 Subject: [PATCH 12/14] algo: Call internal API --- include/pinocchio/src/algorithm/delassus-operator-base.hxx | 5 +++-- .../src/algorithm/delassus-operator-rigid-body.hxx | 7 ++++--- 2 files changed, 7 insertions(+), 5 deletions(-) diff --git a/include/pinocchio/src/algorithm/delassus-operator-base.hxx b/include/pinocchio/src/algorithm/delassus-operator-base.hxx index 0691a183ed..22e7e7dd68 100644 --- a/include/pinocchio/src/algorithm/delassus-operator-base.hxx +++ b/include/pinocchio/src/algorithm/delassus-operator-base.hxx @@ -251,7 +251,7 @@ namespace pinocchio /// \brief Update numerical damping by copying an input block diagonal matrix. template void updateDamping( - const BlockDiagonalMatrixTpl & + const internal::BlockDiagonalMatrixTpl & block_diagonal_damping_matrix) { derived().updateDampingImpl(block_diagonal_damping_matrix); @@ -260,7 +260,8 @@ namespace pinocchio /// \brief Update numerical damping by moving an input block diagonal matrix. template void updateDamping( - BlockDiagonalMatrixTpl && block_diagonal_damping_matrix) + internal::BlockDiagonalMatrixTpl && + block_diagonal_damping_matrix) { derived().updateDampingImpl(std::move(block_diagonal_damping_matrix)); } diff --git a/include/pinocchio/src/algorithm/delassus-operator-rigid-body.hxx b/include/pinocchio/src/algorithm/delassus-operator-rigid-body.hxx index 8cd29e270a..216b248506 100644 --- a/include/pinocchio/src/algorithm/delassus-operator-rigid-body.hxx +++ b/include/pinocchio/src/algorithm/delassus-operator-rigid-body.hxx @@ -316,7 +316,7 @@ namespace pinocchio template void updateDampingImpl( - const BlockDiagonalMatrixTpl & + const internal::BlockDiagonalMatrixTpl & block_diagonal_damping_matrix) { if (&block_diagonal_damping_matrix == &m_damping) @@ -328,7 +328,8 @@ namespace pinocchio template void updateDampingImpl( - BlockDiagonalMatrixTpl && block_diagonal_damping_matrix) + internal::BlockDiagonalMatrixTpl && + block_diagonal_damping_matrix) { if (&block_diagonal_damping_matrix == &m_damping) return; @@ -362,7 +363,7 @@ namespace pinocchio { MatrixType & res_ = res.const_cast_derived(); typedef Eigen::Map MapVectorXs; - MapVectorXs x = MapVectorXs(PINOCCHIO_EIGEN_MAP_ALLOCA(Scalar, this->size(), 1)); + MapVectorXs x = MapVectorXs(_PINOCCHIO_EIGEN_MAP_ALLOCA(Scalar, this->size(), 1)); for (Eigen::Index i = 0; i < this->size(); ++i) { From df5ab7b044229a666fc039652c4f4ebc4939a246 Mon Sep 17 00:00:00 2001 From: Joris Vaillant Date: Wed, 8 Apr 2026 17:05:16 +0200 Subject: [PATCH 13/14] unittest: Call internal API --- unittest/delassus-operator-rigid-body.cpp | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/unittest/delassus-operator-rigid-body.cpp b/unittest/delassus-operator-rigid-body.cpp index 0cec36953a..cc5e8e8581 100644 --- a/unittest/delassus-operator-rigid-body.cpp +++ b/unittest/delassus-operator-rigid-body.cpp @@ -1603,8 +1603,9 @@ BOOST_AUTO_TEST_CASE(test_copy) const double compliance_value = 1e-2; DelassusOperator delassus( - helper::make_ref(model), helper::make_ref(data), helper::make_ref(constraint_models), - helper::make_ref(constraint_datas)); + pinocchio::internal::helper::make_ref(model), pinocchio::internal::helper::make_ref(data), + pinocchio::internal::helper::make_ref(constraint_models), + pinocchio::internal::helper::make_ref(constraint_datas)); delassus.updateCompliance(compliance_value); delassus.compute(); @@ -1620,8 +1621,9 @@ BOOST_AUTO_TEST_CASE(test_copy) // copy assignment: maps must point to the assigned object's own storage DelassusOperator delassus_assigned( - helper::make_ref(model), helper::make_ref(data), helper::make_ref(constraint_models), - helper::make_ref(constraint_datas)); + pinocchio::internal::helper::make_ref(model), pinocchio::internal::helper::make_ref(data), + pinocchio::internal::helper::make_ref(constraint_models), + pinocchio::internal::helper::make_ref(constraint_datas)); delassus_assigned = delassus; BOOST_CHECK( access(delassus_assigned).m_compliance.data() != access(delassus).m_compliance.data()); From 0903d51ee09d49511c8ba0bcf37ced4caeece6ca Mon Sep 17 00:00:00 2001 From: Joris Vaillant Date: Wed, 8 Apr 2026 17:13:39 +0200 Subject: [PATCH 14/14] core: Try to improve back compatibility --- include/pinocchio/deprecated/pinocchio/multibody/fcl.hpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/include/pinocchio/deprecated/pinocchio/multibody/fcl.hpp b/include/pinocchio/deprecated/pinocchio/multibody/fcl.hpp index 39bdc3e218..9e52279fb2 100644 --- a/include/pinocchio/deprecated/pinocchio/multibody/fcl.hpp +++ b/include/pinocchio/deprecated/pinocchio/multibody/fcl.hpp @@ -10,4 +10,7 @@ PINOCCHIO_MOVED_HEADER_PINOCCHIO4(pinocchio/multibody/fcl.hpp, pinocchio/multibody/coal.hpp) // clang-format on +// Include this header to improve back compatibility +#include "pinocchio/collision/fcl-pinocchio-conversions.hpp" + #include "pinocchio/multibody/coal.hpp"