Unverified Commit 3daef0a1 authored by Justin Carpentier's avatar Justin Carpentier Committed by GitHub
Browse files

Merge pull request #1327 from jcarpent/topic/dynamics

Add computeKKTContactDynamicMatrixInverse
parents 9e179085 44c0e8a2
Pipeline #11809 passed with stage
in 139 minutes and 18 seconds
//
// 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.");
......
//
// 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.
......
//
// 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);
......
//
// 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 )
......
......@@ -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()
......
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