Verified Commit 5bff98d8 authored by Justin Carpentier's avatar Justin Carpentier
Browse files

algo/contact: add computeKKTContactDynamicMatrixInverse

parent 5a8f3f84
......@@ -150,6 +150,29 @@ namespace pinocchio
/// 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.
///
/// \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, [[M JT], [J 0]].
/// The matrix is defined when we 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.
///
/// \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.
......
......@@ -86,6 +86,39 @@ 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,
......
......@@ -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;
......
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