Turn lot of API internal

CHANGELOG.md

@@ -23,7 +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`

benchmark/timings-eigen.cpp

@@ -110,9 +110,9 @@ void matrix_mult_matrix_call(
   const MatrixBase<M1> & m, const MatrixBase<M2> & rhs, const MatrixBase<Mout> & 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::matrix_product<Eigen::internal::assign_op>(
+    pinocchio::internal::matrix_product<Eigen::internal::assign_op>(
       m.derived(), rhs.derived(), lhs.const_cast_derived());
   else
     lhs.const_cast_derived().noalias() = m * rhs;

benchmark/timings-linalg-inverse.cpp

@@ -84,7 +84,7 @@ struct MatrixInversePinocchio
   PINOCCHIO_DONT_INLINE static void
   run(const Eigen::MatrixBase<M1> & mat, const Eigen::MatrixBase<M2> & 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<M1> & mat, const Eigen::MatrixBase<M2> & 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());
   }
 };
 

development/convention.md

@@ -295,3 +295,32 @@ CompileFlags:
    Add:
       - -DPINOCCHIO_LSP
 ```
+
+## API
+
+Pinocchio have a public and private API.
+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:
+- `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.

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"

include/pinocchio/algorithm/solvers/admm-solver.hpp

@@ -466,7 +466,7 @@ namespace pinocchio
     typedef ConstraintSolverResultBase<Self> Base;
 
     typedef Eigen::Matrix<Scalar, Eigen::Dynamic, 1, Options> VectorXs;
-    typedef EigenStorageTpl<VectorXs> VectorXsStorage;
+    typedef internal::EigenStorageTpl<VectorXs> VectorXsStorage;
     typedef Eigen::Ref<const VectorXs> RefConstVectorXs;
 
     using Base::constraintSize;
@@ -818,7 +818,7 @@ namespace pinocchio
       static constexpr int Options = _Options;
       typedef Eigen::Matrix<Scalar, Eigen::Dynamic, 1, Options> VectorXs;
       typedef Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, Options> MatrixXs;
-      typedef EigenStorageTpl<VectorXs> VectorXsStorage;
+      typedef internal::EigenStorageTpl<VectorXs> VectorXsStorage;
       typedef LanczosDecompositionTpl<MatrixXs> LanczosDecomposition;
       typedef AndersonAccelerationTpl<Scalar> AndersonAcceleration;
 

include/pinocchio/algorithm/solvers/pgs-solver.hpp

@@ -238,7 +238,7 @@ namespace pinocchio
 
     typedef Eigen::Matrix<Scalar, Eigen::Dynamic, 1, Options> VectorXs;
     typedef Eigen::Ref<const VectorXs> RefConstVectorXs;
-    typedef EigenStorageTpl<VectorXs> VectorXsStorage;
+    typedef internal::EigenStorageTpl<VectorXs> VectorXsStorage;
 
     using Base::constraintSize;
     using Base::setConstraintImpulseGuess;
@@ -477,9 +477,9 @@ namespace pinocchio
       typedef _Scalar Scalar;
       static constexpr int Options = _Options;
       typedef Eigen::Matrix<Scalar, Eigen::Dynamic, 1, Options> VectorXs;
-      typedef EigenStorageTpl<VectorXs> VectorXsStorage;
+      typedef internal::EigenStorageTpl<VectorXs> VectorXsStorage;
       typedef Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, Options> MatrixXs;
-      typedef EigenStorageTpl<MatrixXs> MatrixXsStorage;
+      typedef internal::EigenStorageTpl<MatrixXs> MatrixXsStorage;
 
       /// \brief Constructor given problem_size.
       PGSSolverWorkspaceTpl(std::size_t problem_size = 0)

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"

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);

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();

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)

include/pinocchio/src/algorithm/constraint-cholesky-decl.hxx

@@ -65,10 +65,10 @@ namespace pinocchio
     typedef Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, Options> Matrix;
     typedef typename PINOCCHIO_EIGEN_PLAIN_ROW_MAJOR_TYPE(Matrix) RowMatrix;
 
-    typedef EigenStorageTpl<Vector> EigenStorageVector;
-    typedef EigenStorageTpl<Matrix> EigenStorageMatrix;
-    typedef EigenStorageTpl<RowMatrix> EigenStorageRowMatrix;
-    typedef BlockDiagonalMatrixTpl<Scalar, Options> BlockDiagonalMatrix;
+    typedef internal::EigenStorageTpl<Vector> EigenStorageVector;
+    typedef internal::EigenStorageTpl<Matrix> EigenStorageMatrix;
+    typedef internal::EigenStorageTpl<RowMatrix> EigenStorageRowMatrix;
+    typedef internal::BlockDiagonalMatrixTpl<Scalar, Options> BlockDiagonalMatrix;
     typedef BlockDiagonalMatrix DampingType;
 
     typedef Eigen::Matrix<Eigen::Index, Eigen::Dynamic, 1, Options> EigenIndexVector;
@@ -427,14 +427,14 @@ namespace pinocchio
     ///
     template<int OtherOptions, std::size_t OtherAlignment>
     void updateDamping(
-      const BlockDiagonalMatrixTpl<Scalar, OtherOptions, OtherAlignment> & block_damping);
+      const internal::BlockDiagonalMatrixTpl<Scalar, OtherOptions, OtherAlignment> & block_damping);
 
     ///
     /// \brief Update the damping from a block diagonal matrix (move overload).
     ///
     template<int OtherOptions, std::size_t OtherAlignment>
-    void
-    updateDamping(BlockDiagonalMatrixTpl<Scalar, OtherOptions, OtherAlignment> && block_damping);
+    void updateDamping(
+      internal::BlockDiagonalMatrixTpl<Scalar, OtherOptions, OtherAlignment> && block_damping);
 
     ///
     /// \brief Returns the current damping as a block diagonal matrix.

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;
@@ -338,7 +338,7 @@ namespace pinocchio
     //   {
     //     const Eigen::Index slice_dim = nv;
     //     typedef Eigen::Map<Vector,EIGEN_DEFAULT_ALIGN_BYTES> 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<RowMatrix> 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();
@@ -425,7 +426,7 @@ namespace pinocchio
   template<typename Scalar, int Options>
   template<int OtherOptions, std::size_t OtherAlignment>
   void ConstraintCholeskyDecompositionTpl<Scalar, Options>::updateDamping(
-    const BlockDiagonalMatrixTpl<Scalar, OtherOptions, OtherAlignment> & block_damping)
+    const internal::BlockDiagonalMatrixTpl<Scalar, OtherOptions, OtherAlignment> & block_damping)
   {
     if (&block_damping == &m_damping)
       return;
@@ -436,7 +437,7 @@ namespace pinocchio
   template<typename Scalar, int Options>
   template<int OtherOptions, std::size_t OtherAlignment>
   void ConstraintCholeskyDecompositionTpl<Scalar, Options>::updateDamping(
-    BlockDiagonalMatrixTpl<Scalar, OtherOptions, OtherAlignment> && block_damping)
+    internal::BlockDiagonalMatrixTpl<Scalar, OtherOptions, OtherAlignment> && block_damping)
   {
     if (&block_damping == &m_damping)
       return;
@@ -1015,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)
       ;
   }

include/pinocchio/src/algorithm/contact-inverse-dynamics.hxx

@@ -29,10 +29,10 @@ namespace pinocchio
     bool solve_ncp)
   {
     static_assert(
-      helper::is_std_vector_v<PointContactConstraintModelVector>,
+      internal::helper::is_std_vector_v<PointContactConstraintModelVector>,
       "PointContactConstraintModelVector should be a std::vector<T,Allocator>");
     static_assert(
-      helper::is_std_vector_v<PointContactConstraintDataVector>,
+      internal::helper::is_std_vector_v<PointContactConstraintDataVector>,
       "PointContactConstraintDataVector should be a std::vector<T,Allocator>");
 
     typedef Eigen::Matrix<Scalar, Eigen::Dynamic, 1> 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<PointContactConstraintModelVector>,
+      internal::helper::is_std_vector_v<PointContactConstraintModelVector>,
       "PointContactConstraintModelVector should be a std::vector<T,Allocator>");
     static_assert(
-      helper::is_std_vector_v<PointContactConstraintDataVector>,
+      internal::helper::is_std_vector_v<PointContactConstraintDataVector>,
       "PointContactConstraintDataVector should be a std::vector<T,Allocator>");
 
     typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;

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<int OtherOptions, std::size_t OtherAlignment>
     void updateDamping(
-      const BlockDiagonalMatrixTpl<Scalar, OtherOptions, OtherAlignment> &
+      const internal::BlockDiagonalMatrixTpl<Scalar, OtherOptions, OtherAlignment> &
         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<int OtherOptions, std::size_t OtherAlignment>
     void updateDamping(
-      BlockDiagonalMatrixTpl<Scalar, OtherOptions, OtherAlignment> && block_diagonal_damping_matrix)
+      internal::BlockDiagonalMatrixTpl<Scalar, OtherOptions, OtherAlignment> &&
+        block_diagonal_damping_matrix)
     {
       derived().updateDampingImpl(std::move(block_diagonal_damping_matrix));
     }

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<RowMatrix> 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<Eigen::UnitLower>(U1.adjoint(), x.derived(), tmp_mat);
 
@@ -258,14 +258,14 @@ namespace pinocchio
 
     template<int OtherOptions, std::size_t OtherAlignment>
     void updateDampingImpl(
-      const BlockDiagonalMatrixTpl<Scalar, OtherOptions, OtherAlignment> & block_damping)
+      const internal::BlockDiagonalMatrixTpl<Scalar, OtherOptions, OtherAlignment> & block_damping)
     {
       const_cast<ConstraintCholeskyDecomposition &>(self).updateDamping(block_damping);
     }
 
     template<int OtherOptions, std::size_t OtherAlignment>
-    void
-    updateDampingImpl(BlockDiagonalMatrixTpl<Scalar, OtherOptions, OtherAlignment> && block_damping)
+    void updateDampingImpl(
+      internal::BlockDiagonalMatrixTpl<Scalar, OtherOptions, OtherAlignment> && block_damping)
     {
       const_cast<ConstraintCholeskyDecomposition &>(self).updateDamping(std::move(block_damping));
     }