Merge pull request #1327 from jcarpent/topic/dynamics

Add computeKKTContactDynamicMatrixInverse

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

 //
-// Copyright (c) 2015-2019 CNRS, INRIA
+// Copyright (c) 2015-2020 CNRS, INRIA
 //
 
 #include "pinocchio/bindings/python/algorithm/algorithms.hpp"
@@ -61,15 +61,28 @@ namespace pinocchio
 
     BOOST_PYTHON_FUNCTION_OVERLOADS(impulseDynamics_overloads_no_q, impulseDynamics_proxy_no_q, 4, 6)
 
+    static Eigen::MatrixXd computeKKTContactDynamicMatrixInverse_proxy(const Model & model,
+                                                                       Data & data,
+                                                                       const Eigen::VectorXd & q,
+                                                                       const Eigen::MatrixXd & J,
+                                                                       const double mu = 0)
+    {
+      Eigen::MatrixXd KKTMatrix_inv(model.nv+J.rows(), model.nv+J.rows());
+      computeKKTContactDynamicMatrixInverse(model, data, q, J, KKTMatrix_inv, mu);
+      return KKTMatrix_inv;
+    }
+  
+    BOOST_PYTHON_FUNCTION_OVERLOADS(computeKKTContactDynamicMatrixInverse_overload,
+                                    computeKKTContactDynamicMatrixInverse_proxy, 4, 5)
+
     static const Eigen::MatrixXd getKKTContactDynamicMatrixInverse_proxy(const Model & model,
                                                                          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;
     }
-    
 
     void exposeDynamics()
     {
@@ -127,7 +140,13 @@ namespace pinocchio
               " Assumes pinocchio.crba has been called."
               ));
       
-      bp::def("getKKTContactDynamicMatrixInverse",getKKTContactDynamicMatrixInverse_proxy,
+      bp::def("computeKKTContactDynamicMatrixInverse",
+              computeKKTContactDynamicMatrixInverse_proxy,
+              computeKKTContactDynamicMatrixInverse_overload(bp::args("model","data","q","J","damping"),
+              "Computes the inverse of the constraint matrix [[M J^T], [J 0]]."));
+      
+      bp::def("getKKTContactDynamicMatrixInverse",
+              getKKTContactDynamicMatrixInverse_proxy,
               bp::args("Model","Data",
                        "Contact Jacobian J(size nb_constraint * Model::nv)"),
               "Computes the inverse of the constraint matrix [[M JT], [J 0]]. forward/impulseDynamics must be called first. The jacobian should be the same that was provided to forward/impulseDynamics.");

src/algorithm/contact-dynamics.hpp

 //
-// Copyright (c) 2016-2019 CNRS INRIA
+// Copyright (c) 2016-2020 CNRS INRIA
 //
 
 #ifndef __pinocchio_contact_dynamics_hpp__
@@ -109,7 +109,6 @@ namespace pinocchio
   /// \tparam TangentVectorType2 Type of the joint torque vector.
   /// \tparam ConstraintMatrixType Type of the constraint matrix.
   /// \tparam DriftVectorType Type of the drift vector.
-
   ///
   /// \param[in] model The model structure of the rigid body system.
   /// \param[in] data The data structure of the rigid body system.
@@ -144,23 +143,47 @@ 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
+  /// \brief Computes the inverse of the KKT matrix for dynamics with contact constraints.
+  /// It computes the following matrix: <BR>
+  ///       <CENTER> \f$ \left[\begin{matrix}\mathbf{M}^{-1}-\mathbf{M}^{-1}\mathbf{J}^{\top}_c\widehat{\mathbf{M}}^{-1}\mathbf{J}_c\mathbf{M}^{-1} & \mathbf{M}^{-1}\mathbf{J}^{\top}_c\widehat{\mathbf{M}}^{-1} \\ \widehat{\mathbf{M}}^{-1}\mathbf{J}_c\mathbf{M}^{-1} & -\widehat{\mathbf{M}}^{-1}\end{matrix}\right] \f$ </CENTER> <BR>
+  ///
+  /// \param[in] model The model structure of the rigid body system.
+  /// \param[in] data The data structure of the rigid body system.
+  /// \param[in] J Jacobian of the constraints.
+  /// \param[out] KKTMatrix_inv inverse of the MJtJ matrix.
+  /// \param[in] inv_damping regularization coefficient.
+  ///
+  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl,
+           typename ConfigVectorType, typename ConstraintMatrixType, typename KKTMatrixType>
+  void computeKKTContactDynamicMatrixInverse(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
+                                             DataTpl<Scalar,Options,JointCollectionTpl> & data,
+                                             const Eigen::MatrixBase<ConfigVectorType> & q,
+                                             const Eigen::MatrixBase<ConstraintMatrixType> & J,
+                                             const Eigen::MatrixBase<KKTMatrixType> & KKTMatrix_inv,
+                                             const Scalar & inv_damping = 0.);
+
+  ///
+  /// \brief Computes the inverse of the KKT matrix for dynamics with contact constraints.
+  /// It computes the following matrix: <BR>
+  ///       <CENTER> \f$ \left[\begin{matrix}\mathbf{M}^{-1}-\mathbf{M}^{-1}\mathbf{J}^{\top}_c\widehat{\mathbf{M}}^{-1}\mathbf{J}_c\mathbf{M}^{-1} & \mathbf{M}^{-1}\mathbf{J}^{\top}_c\widehat{\mathbf{M}}^{-1} \\ \widehat{\mathbf{M}}^{-1}\mathbf{J}_c\mathbf{M}^{-1} & -\widehat{\mathbf{M}}^{-1}\end{matrix}\right] \f$ </CENTER> <BR>
+  ///
+  /// \remarks The matrix is defined when one's call forwardDynamics/impulseDynamics. This method makes use of
   /// the matrix decompositions performed during the forwardDynamics/impulseDynamics and returns the inverse.
-  /// The jacobian should be the same that was provided to forwardDynamics/impulseDynamics.
-  /// Thus you should call forward Dynamics/impulseDynamics first.
+  /// The jacobian should be the same that the one provided to forwardDynamics/impulseDynamics.
+  /// Thus forward Dynamics/impulseDynamics should have been called 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__
@@ -86,27 +86,63 @@ namespace pinocchio
     return forwardDynamics(model,data,tau,J,gamma,inv_damping);
   }
 
+  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl,
+           typename ConfigVectorType, typename ConstraintMatrixType, typename KKTMatrixType>
+  void computeKKTContactDynamicMatrixInverse(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
+                                             DataTpl<Scalar,Options,JointCollectionTpl> & data,
+                                             const Eigen::MatrixBase<ConfigVectorType> & q,
+                                             const Eigen::MatrixBase<ConstraintMatrixType> & J,
+                                             const Eigen::MatrixBase<KKTMatrixType> & KKTMatrix_inv,
+                                             const Scalar & inv_damping)
+  {
+    assert(model.check(data));
+    PINOCCHIO_CHECK_INPUT_ARGUMENT(inv_damping >= 0., "mu must be positive.");
+    
+    // Compute the mass matrix.
+    crbaMinimal(model,data,q);
+    
+    // Compute the UDUt decomposition of data.M.
+    cholesky::decompose(model, data);
+    
+    using std::sqrt;
+    data.sDUiJt = J.transpose();
+    // Compute U^-1 * J.T
+    cholesky::Uiv(model, data, data.sDUiJt);
+    for(Eigen::DenseIndex k=0;k<model.nv;++k)
+      data.sDUiJt.row(k) /= sqrt(data.D[k]);
+    
+    data.JMinvJt.noalias() = data.sDUiJt.transpose() * data.sDUiJt;
+    
+    data.JMinvJt.diagonal().array() += inv_damping;
+    data.llt_JMinvJt.compute(data.JMinvJt);
+    
+    getKKTContactDynamicMatrixInverse(model,data,J,KKTMatrix_inv.const_cast_derived());
+  }
+
   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)
   {
+    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"
@@ -82,7 +82,49 @@ BOOST_AUTO_TEST_CASE ( test_FD )
 
 }
 
-BOOST_AUTO_TEST_CASE (test_KKTMatrix)
+BOOST_AUTO_TEST_CASE(test_computeKKTMatrix)
+{
+  using namespace Eigen;
+  using namespace pinocchio;
+  pinocchio::Model model;
+  pinocchio::buildModels::humanoidRandom(model,true);
+  pinocchio::Data data(model), data_ref(model);
+  
+  VectorXd q = VectorXd::Ones(model.nq);
+  q.segment<4>(3).normalize();
+  
+  pinocchio::computeJointJacobians(model, data_ref, q);
+  
+  const std::string RF = "rleg6_joint";
+  const std::string LF = "lleg6_joint";
+  
+  Data::Matrix6x J_RF(6, model.nv);
+  J_RF.setZero();
+  getJointJacobian(model, data_ref, model.getJointId(RF), LOCAL, J_RF);
+  Data::Matrix6x J_LF(6, model.nv);
+  J_LF.setZero();
+  getJointJacobian(model, data_ref, model.getJointId(LF), LOCAL, J_LF);
+  
+  Eigen::MatrixXd J(12, model.nv);
+  J.setZero();
+  J.topRows<6>() = J_RF;
+  J.bottomRows<6>() = J_LF;
+  
+  //Check Forward Dynamics
+  pinocchio::crba(model,data_ref,q);
+  data_ref.M.triangularView<Eigen::StrictlyLower>() = data_ref.M.transpose().triangularView<Eigen::StrictlyLower>();
+
+  Eigen::MatrixXd MJtJ(model.nv+12, model.nv+12);
+  MJtJ << data_ref.M, J.transpose(),
+    J, Eigen::MatrixXd::Zero(12, 12);
+
+  Eigen::MatrixXd KKTMatrix_inv(model.nv+12, model.nv+12);
+  computeKKTContactDynamicMatrixInverse(model, data, q, J, KKTMatrix_inv);
+
+  BOOST_CHECK(KKTMatrix_inv.isApprox(MJtJ.inverse()));
+}
+
+BOOST_AUTO_TEST_CASE(test_getKKTMatrix)
 {
   using namespace Eigen;
   using namespace pinocchio;
@@ -125,10 +167,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 +180,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 )

unittest/python/bindings_dynamics.py

@@ -49,6 +49,25 @@ class TestDynamicsBindings(TestCase):
 
         self.assertApprox(ddq,ddq_no_q)
 
+    def test_computeKKTMatrix(self):
+        model = self.model
+        data = model.createData()
+        data_ref = model.createData()
+
+        q = self.q
+        v = self.v
+        tau = self.tau0
+        J = self.J
+        gamma = self.gamma
+
+        pin.forwardDynamics(model,data_ref,q,v,tau,J,gamma)
+        KKT_inverse_ref = pin.getKKTContactDynamicMatrixInverse(model,data_ref,J)
+
+        KKT_inverse = pin.computeKKTContactDynamicMatrixInverse(model,data,q,J)
+        KKT_inverse2 = pin.computeKKTContactDynamicMatrixInverse(model,data,q,J,0.)
+        self.assertApprox(KKT_inverse,KKT_inverse_ref)
+        self.assertApprox(KKT_inverse2,KKT_inverse_ref)
+
     def test_forwardDynamics_rcoeff(self):
         data_no_q = self.model.createData()