algo: rename variables for better clarity

bindings/python/algorithm/expose-contact-dynamics.cpp

@@ -65,9 +65,9 @@ namespace pinocchio
                                                                          Data & data,
                                                                          const Eigen::MatrixXd & J)
     {
-      Eigen::MatrixXd MJtJ_inv(model.nv+J.rows(), model.nv+J.rows());
-      getKKTContactDynamicMatrixInverse(model, data, J, MJtJ_inv);
-      return MJtJ_inv;
+      Eigen::MatrixXd KKTMatrix_inv(model.nv+J.rows(), model.nv+J.rows());
+      getKKTContactDynamicMatrixInverse(model, data, J, KKTMatrix_inv);
+      return KKTMatrix_inv;
     }
     
 

src/algorithm/contact-dynamics.hpp

 //
-// Copyright (c) 2016-2019 CNRS INRIA
+// Copyright (c) 2016-2020 CNRS INRIA
 //
 
 #ifndef __pinocchio_contact_dynamics_hpp__
@@ -144,7 +144,6 @@ namespace pinocchio
       return forwardDynamics(model,data,tau,J,gamma,inv_damping);
   }
 
-  
   ///
   /// \brief Computes the inverse of the KKT matrix for dynamics with contact constraints, [[M JT], [J 0]].
   /// The matrix is defined when we call forwardDynamics/impulseDynamics. This method makes use of
@@ -153,14 +152,15 @@ namespace pinocchio
   /// Thus you should call forward Dynamics/impulseDynamics first.
   /// \param[in] model The model structure of the rigid body system.
   /// \param[in] data The data structure of the rigid body system.
-  /// \param[out] MJtJ_inv inverse of the MJtJ matrix.
+  /// \param[in] J Jacobian of the constraints.
+  /// \param[out] KKTMatrix_inv inverse of the MJtJ matrix.
   ///
   template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl,
            typename ConstraintMatrixType, typename KKTMatrixType>
   inline void getKKTContactDynamicMatrixInverse(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
                                                 const DataTpl<Scalar,Options,JointCollectionTpl> & data,
                                                 const Eigen::MatrixBase<ConstraintMatrixType> & J,
-                                                const Eigen::MatrixBase<KKTMatrixType> & MJtJ_inv);
+                                                const Eigen::MatrixBase<KKTMatrixType> & KKTMatrix_inv);
   
   ///
   /// \brief Compute the impulse dynamics with contact constraints. Internally, pinocchio::crba is called.

src/algorithm/contact-dynamics.hxx

 //
-// Copyright (c) 2016-2019 CNRS INRIA
+// Copyright (c) 2016-2020 CNRS INRIA
 //
 
 #ifndef __pinocchio_contact_dynamics_hxx__
@@ -91,22 +91,25 @@ namespace pinocchio
   inline void getKKTContactDynamicMatrixInverse(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
                                                 const DataTpl<Scalar,Options,JointCollectionTpl> & data,
                                                 const Eigen::MatrixBase<ConstraintMatrixType> & J,
-                                                const Eigen::MatrixBase<KKTMatrixType> & MJtJ_inv)
+                                                const Eigen::MatrixBase<KKTMatrixType> & KKTMatrix_inv)
   {
+    assert(model.check(data));
+    PINOCCHIO_CHECK_ARGUMENT_SIZE(KKTMatrix_inv.cols(), data.JMinvJt.cols() + model.nv);
+    PINOCCHIO_CHECK_ARGUMENT_SIZE(KKTMatrix_inv.rows(), data.JMinvJt.rows() + model.nv);
+    
     typedef DataTpl<Scalar,Options,JointCollectionTpl> Data;
-    PINOCCHIO_CHECK_ARGUMENT_SIZE(MJtJ_inv.cols(), data.JMinvJt.cols() + model.nv);
-    PINOCCHIO_CHECK_ARGUMENT_SIZE(MJtJ_inv.rows(), data.JMinvJt.rows() + model.nv);
-    const typename Data::MatrixXs::Index& nc = data.JMinvJt.cols();
+    const Eigen::DenseIndex nc = data.JMinvJt.cols();
     
-    KKTMatrixType& MJtJ_inv_ = PINOCCHIO_EIGEN_CONST_CAST(KKTMatrixType,MJtJ_inv);
+    KKTMatrixType & KKTMatrix_inv_ = PINOCCHIO_EIGEN_CONST_CAST(KKTMatrixType,KKTMatrix_inv);
     
-    Eigen::Block<typename Data::MatrixXs> topLeft = MJtJ_inv_.topLeftCorner(model.nv, model.nv);
-    Eigen::Block<typename Data::MatrixXs> topRight = MJtJ_inv_.topRightCorner(model.nv, nc);
-    Eigen::Block<typename Data::MatrixXs> bottomLeft = MJtJ_inv_.bottomLeftCorner(nc, model.nv);
-    Eigen::Block<typename Data::MatrixXs> bottomRight = MJtJ_inv_.bottomRightCorner(nc, nc);
+    typedef Eigen::Block<KKTMatrixType> BlockType;
+    BlockType topLeft = KKTMatrix_inv_.topLeftCorner(model.nv, model.nv);
+    BlockType topRight = KKTMatrix_inv_.topRightCorner(model.nv, nc);
+    BlockType bottomLeft = KKTMatrix_inv_.bottomLeftCorner(nc, model.nv);
+    BlockType bottomRight = KKTMatrix_inv_.bottomRightCorner(nc, nc);
     
-    bottomRight = -Data::MatrixXs::Identity(nc,nc);    topLeft.setIdentity();
-    data.llt_JMinvJt.solveInPlace(bottomRight);    cholesky::solve(model, data, topLeft);
+    bottomRight = -Data::MatrixXs::Identity(nc,nc); data.llt_JMinvJt.solveInPlace(bottomRight);
+    topLeft.setIdentity(); cholesky::solve(model, data, topLeft);
     
     bottomLeft.noalias() = J*topLeft;
     topRight.noalias() = bottomLeft.transpose() * (-bottomRight);

unittest/contact-dynamics.cpp

 //
-// Copyright (c) 2016-2019 CNRS, INRIA
+// Copyright (c) 2016-2020 CNRS, INRIA
 //
 
 #include "pinocchio/spatial/se3.hpp"
@@ -125,10 +125,10 @@ BOOST_AUTO_TEST_CASE (test_KKTMatrix)
   MJtJ << data.M, J.transpose(),
     J, Eigen::MatrixXd::Zero(12, 12);
 
-  Eigen::MatrixXd MJtJ_inv(model.nv+12, model.nv+12);
-  getKKTContactDynamicMatrixInverse(model, data, J, MJtJ_inv);
+  Eigen::MatrixXd KKTMatrix_inv(model.nv+12, model.nv+12);
+  getKKTContactDynamicMatrixInverse(model, data, J, KKTMatrix_inv);
 
-  BOOST_CHECK(MJtJ_inv.isApprox(MJtJ.inverse()));
+  BOOST_CHECK(KKTMatrix_inv.isApprox(MJtJ.inverse()));
 
   //Check Impulse Dynamics
   const double r_coeff = 1.;
@@ -138,10 +138,9 @@ BOOST_AUTO_TEST_CASE (test_KKTMatrix)
   MJtJ << data.M, J.transpose(),
     J, Eigen::MatrixXd::Zero(12, 12);
 
-  getKKTContactDynamicMatrixInverse(model, data, J, MJtJ_inv);
+  getKKTContactDynamicMatrixInverse(model, data, J, KKTMatrix_inv);
 
-  BOOST_CHECK(MJtJ_inv.isApprox(MJtJ.inverse()));
-  
+  BOOST_CHECK(KKTMatrix_inv.isApprox(MJtJ.inverse()));
 }
 
 BOOST_AUTO_TEST_CASE ( test_FD_with_damping )