Verified Commit 3b375262 authored by Justin Carpentier's avatar Justin Carpentier
Browse files

python: bind computeKKTContactDynamicMatrixInverse

parent 5bff98d8
//
// 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.");
......
......@@ -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()
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment