python: bind 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,6 +61,20 @@ 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)
@@ -69,7 +83,6 @@ namespace pinocchio
       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.");

unittest/python/bindings_dynamics.py

@@ -49,6 +49,23 @@ 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)
+        self.assertApprox(KKT_inverse,KKT_inverse_ref)
+
     def test_forwardDynamics_rcoeff(self):
         data_no_q = self.model.createData()