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

algo: rename variables for better clarity

parent 1e1253a9
......@@ -65,9 +65,9 @@ namespace pinocchio
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;
}
......
//
// Copyright (c) 2016-2019 CNRS INRIA
// Copyright (c) 2016-2020 CNRS INRIA
//
#ifndef __pinocchio_contact_dynamics_hpp__
......@@ -144,7 +144,6 @@ 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
......@@ -153,14 +152,15 @@ namespace pinocchio
/// 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[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__
......@@ -91,22 +91,25 @@ namespace pinocchio
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"
......@@ -125,10 +125,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 +138,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 )
......
Markdown is supported
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