algorithm/contact-cholesky.hpp

Go to the documentation of this file.

//
// Copyright (c) 2019-2024 INRIA
//
 
#ifndef __pinocchio_algorithm_contact_cholesky_hpp__
#define __pinocchio_algorithm_contact_cholesky_hpp__
 
#include "pinocchio/multibody/model.hpp"
#include "pinocchio/math/matrix-block.hpp"
#include "pinocchio/math/triangular-matrix.hpp"
 
#include "pinocchio/algorithm/contact-info.hpp"
#include "pinocchio/algorithm/delassus-operator-base.hpp"
 
namespace pinocchio
{
 
  // Forward declaration of algo
  namespace details
  {
    template<typename MatrixLike, int ColsAtCompileTime = MatrixLike::ColsAtCompileTime>
    struct UvAlgo;
 
    template<typename MatrixLike, int ColsAtCompileTime = MatrixLike::ColsAtCompileTime>
    struct UtvAlgo;
 
    template<typename MatrixLike, int ColsAtCompileTime = MatrixLike::ColsAtCompileTime>
    struct UivAlgo;
 
    template<typename MatrixLike, int ColsAtCompileTime = MatrixLike::ColsAtCompileTime>
    struct UtivAlgo;
 
    template<typename Scalar, int Options, typename VectorLike>
    VectorLike & inverseAlgo(
      const ContactCholeskyDecompositionTpl<Scalar, Options> & chol,
      const Eigen::DenseIndex col,
      const Eigen::MatrixBase<VectorLike> & vec);
  } // namespace details
 
  template<typename _ContactCholeskyDecomposition>
  struct DelassusCholeskyExpressionTpl;
 
  template<typename _Scalar, int _Options>
  struct PINOCCHIO_UNSUPPORTED_MESSAGE("The API will change towards more flexibility")
    ContactCholeskyDecompositionTpl
  {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
    typedef pinocchio::Index Index;
    typedef _Scalar Scalar;
    enum
    {
      LINEAR = 0,
      ANGULAR = 3,
      Options = _Options
    };
 
    typedef Eigen::Matrix<Scalar, Eigen::Dynamic, 1, Options> Vector;
    typedef Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, Options> Matrix;
    typedef typename PINOCCHIO_EIGEN_PLAIN_ROW_MAJOR_TYPE(Matrix) RowMatrix;
    // TODO Remove when API is stabilized
    PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH
    PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
    typedef RigidConstraintModelTpl<Scalar, Options> RigidConstraintModel;
    typedef RigidConstraintDataTpl<Scalar, Options> RigidConstraintData;
    PINOCCHIO_COMPILER_DIAGNOSTIC_POP
    typedef Eigen::Matrix<Eigen::DenseIndex, Eigen::Dynamic, 1, Options> IndexVector;
    typedef typename PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(IndexVector) VectorOfIndexVector;
    typedef Eigen::Matrix<bool, Eigen::Dynamic, 1, Options> BooleanVector;
 
    struct Slice
    {
      Slice(const Eigen::DenseIndex & first_index, const Eigen::DenseIndex & size)
      : first_index(first_index)
      , size(size)
      {
      }
 
      Eigen::DenseIndex first_index;
      Eigen::DenseIndex size;
    };
 
    typedef DelassusCholeskyExpressionTpl<ContactCholeskyDecompositionTpl>
      DelassusCholeskyExpression;
    friend struct DelassusCholeskyExpressionTpl<ContactCholeskyDecompositionTpl>;
 
    typedef std::vector<Slice> SliceVector;
    typedef std::vector<SliceVector> VectorOfSliceVector;
 
    ContactCholeskyDecompositionTpl()
    {
    }
 
    template<typename S1, int O1, template<typename, int> class JointCollectionTpl>
    explicit ContactCholeskyDecompositionTpl(const ModelTpl<S1, O1, JointCollectionTpl> & model)
    {
      // TODO Remove when API is stabilized
      PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH
      PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
      PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) empty_contact_models;
      PINOCCHIO_COMPILER_DIAGNOSTIC_POP
      allocate(model, empty_contact_models);
    }
 
    // TODO Remove when API is stabilized
    PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH
    PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
    template<typename S1, int O1, template<typename, int> class JointCollectionTpl, class Allocator>
    ContactCholeskyDecompositionTpl(
      const ModelTpl<S1, O1, JointCollectionTpl> & model,
      const std::vector<RigidConstraintModelTpl<S1, O1>, Allocator> & contact_models)
    {
      allocate(model, contact_models);
    }
    PINOCCHIO_COMPILER_DIAGNOSTIC_POP
 
    ContactCholeskyDecompositionTpl(const ContactCholeskyDecompositionTpl & copy) = default;
 
    // TODO Remove when API is stabilized
    PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH
    PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
    template<typename S1, int O1, template<typename, int> class JointCollectionTpl, class Allocator>
    void allocate(
      const ModelTpl<S1, O1, JointCollectionTpl> & model,
      const std::vector<RigidConstraintModelTpl<S1, O1>, Allocator> & contact_models);
    PINOCCHIO_COMPILER_DIAGNOSTIC_POP
 
    Matrix getInverseOperationalSpaceInertiaMatrix() const
    {
      Matrix res(constraintDim(), constraintDim());
      getInverseOperationalSpaceInertiaMatrix(res);
      return res;
    }
 
    template<typename MatrixType>
    void getInverseOperationalSpaceInertiaMatrix(const Eigen::MatrixBase<MatrixType> & res) const
    {
      typedef typename SizeDepType<Eigen::Dynamic>::template BlockReturn<RowMatrix>::ConstType
        ConstBlockXpr;
      //        typedef typename RowMatrix::ConstBlockXpr ConstBlockXpr;
      const ConstBlockXpr U1 = U.topLeftCorner(constraintDim(), constraintDim());
 
      PINOCCHIO_EIGEN_MALLOC_NOT_ALLOWED();
      MatrixType & res_ = PINOCCHIO_EIGEN_CONST_CAST(MatrixType, res);
      OSIMinv_tmp.noalias() = D.head(constraintDim()).asDiagonal() * U1.adjoint();
      res_.noalias() = -U1 * OSIMinv_tmp;
      PINOCCHIO_EIGEN_MALLOC_ALLOWED();
    }
 
    DelassusCholeskyExpression getDelassusCholeskyExpression() const
    {
      return DelassusCholeskyExpression(*this);
    }
 
    Matrix getOperationalSpaceInertiaMatrix() const
    {
      Matrix res(constraintDim(), constraintDim());
      getOperationalSpaceInertiaMatrix(res);
      return res;
    }
 
    template<typename MatrixType>
    void getOperationalSpaceInertiaMatrix(const Eigen::MatrixBase<MatrixType> & res_) const
    {
      MatrixType & res = PINOCCHIO_EIGEN_CONST_CAST(MatrixType, res_);
      typedef typename SizeDepType<Eigen::Dynamic>::template BlockReturn<RowMatrix>::ConstType
        ConstBlockXpr;
      //        typedef typename RowMatrix::ConstBlockXpr ConstBlockXpr;
      const Eigen::TriangularView<ConstBlockXpr, Eigen::UnitUpper> U1 =
        U.topLeftCorner(constraintDim(), constraintDim())
          .template triangularView<Eigen::UnitUpper>();
 
      PINOCCHIO_EIGEN_MALLOC_NOT_ALLOWED();
      U1inv.setIdentity();
      U1.solveInPlace(U1inv); // TODO: implement Sparse Inverse
      OSIMinv_tmp.noalias() = -U1inv.adjoint() * Dinv.head(constraintDim()).asDiagonal();
      res.noalias() = OSIMinv_tmp * U1inv;
      PINOCCHIO_EIGEN_MALLOC_ALLOWED();
    }
 
    Matrix getInverseMassMatrix() const
    {
      Matrix res(nv, nv);
      getInverseMassMatrix(res);
      return res;
    }
 
    template<typename MatrixType>
    void getInverseMassMatrix(const Eigen::MatrixBase<MatrixType> & res_) const
    {
      MatrixType & res = PINOCCHIO_EIGEN_CONST_CAST(MatrixType, res_);
      typedef typename SizeDepType<Eigen::Dynamic>::template BlockReturn<RowMatrix>::ConstType
        ConstBlockXpr;
      //        typedef typename RowMatrix::ConstBlockXpr ConstBlockXpr;
      const Eigen::TriangularView<ConstBlockXpr, Eigen::UnitUpper> U4 =
        U.bottomRightCorner(nv, nv).template triangularView<Eigen::UnitUpper>();
 
      PINOCCHIO_EIGEN_MALLOC_NOT_ALLOWED();
      U4inv.setIdentity();
      U4.solveInPlace(U4inv); // TODO: implement Sparse Inverse
      Minv_tmp.noalias() = U4inv.adjoint() * Dinv.tail(nv).asDiagonal();
      res.noalias() = Minv_tmp * U4inv;
      PINOCCHIO_EIGEN_MALLOC_ALLOWED();
    }
 
    template<typename MatrixType>
    void getJMinv(const Eigen::MatrixBase<MatrixType> & res_) const
    {
      MatrixType & res = PINOCCHIO_EIGEN_CONST_CAST(MatrixType, res_);
      typedef typename SizeDepType<Eigen::Dynamic>::template BlockReturn<RowMatrix>::ConstType
        ConstBlockXpr;
      const Eigen::TriangularView<ConstBlockXpr, Eigen::UnitUpper> U4 =
        U.bottomRightCorner(nv, nv).template triangularView<Eigen::UnitUpper>();
      ConstBlockXpr U2 = U.topRightCorner(constraintDim(), nv);
      PINOCCHIO_EIGEN_MALLOC_NOT_ALLOWED();
      U4inv.setIdentity();
      U4.solveInPlace(U4inv); // TODO: implement Sparse Inverse
      res.noalias() = U2 * U4inv;
      PINOCCHIO_EIGEN_MALLOC_ALLOWED();
    }
 
    // TODO Remove when API is stabilized
    PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH
    PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
    template<
      typename S1,
      int O1,
      template<typename, int>
      class JointCollectionTpl,
      class ConstraintModelAllocator,
      class ConstraintDataAllocator>
    void compute(
      const ModelTpl<S1, O1, JointCollectionTpl> & model,
      DataTpl<S1, O1, JointCollectionTpl> & data,
      const std::vector<RigidConstraintModelTpl<S1, O1>, ConstraintModelAllocator> & contact_models,
      std::vector<RigidConstraintDataTpl<S1, O1>, ConstraintDataAllocator> & contact_datas,
      const S1 mu = S1(0.))
    {
      compute(model, data, contact_models, contact_datas, Vector::Constant(U1inv.rows(), mu));
    }
 
    template<
      typename S1,
      int O1,
      template<typename, int>
      class JointCollectionTpl,
      class ConstraintModelAllocator,
      class ConstraintDataAllocator,
      typename VectorLike>
    void compute(
      const ModelTpl<S1, O1, JointCollectionTpl> & model,
      DataTpl<S1, O1, JointCollectionTpl> & data,
      const std::vector<RigidConstraintModelTpl<S1, O1>, ConstraintModelAllocator> & contact_models,
      std::vector<RigidConstraintDataTpl<S1, O1>, ConstraintDataAllocator> & contact_datas,
      const Eigen::MatrixBase<VectorLike> & mus);
    PINOCCHIO_COMPILER_DIAGNOSTIC_POP
 
    template<typename VectorLike>
    void updateDamping(const Eigen::MatrixBase<VectorLike> & mus);
 
    void updateDamping(const Scalar & mu);
 
    Eigen::DenseIndex size() const
    {
      return D.size();
    }
 
    Eigen::DenseIndex constraintDim() const
    {
      return size() - nv;
    }
 
    Eigen::DenseIndex numContacts() const
    {
      return num_contacts;
    }
 
    template<typename MatrixLike>
    void solveInPlace(const Eigen::MatrixBase<MatrixLike> & mat) const;
 
    template<typename MatrixLike>
    Matrix solve(const Eigen::MatrixBase<MatrixLike> & mat) const;
 
    template<typename S1, int O1, template<typename, int> class JointCollectionTpl>
    ContactCholeskyDecompositionTpl
    getMassMatrixChoeslkyDecomposition(const ModelTpl<S1, O1, JointCollectionTpl> & model) const;
 
    template<typename MatrixLike>
    void Uv(const Eigen::MatrixBase<MatrixLike> & mat) const;
 
    template<typename MatrixLike>
    void Utv(const Eigen::MatrixBase<MatrixLike> & mat) const;
 
    template<typename MatrixLike>
    void Uiv(const Eigen::MatrixBase<MatrixLike> & mat) const;
 
    template<typename MatrixLike>
    void Utiv(const Eigen::MatrixBase<MatrixLike> & mat) const;
 
    Matrix matrix() const;
 
    template<typename MatrixType>
    void matrix(const Eigen::MatrixBase<MatrixType> & res) const;
 
    Matrix inverse() const;
 
    template<typename MatrixType>
    void inverse(const Eigen::MatrixBase<MatrixType> & res) const;
 
    // data
    Vector D, Dinv;
    RowMatrix U;
 
    template<typename MatrixLike, int ColsAtCompileTime>
    friend struct details::UvAlgo;
 
    template<typename MatrixLike, int ColsAtCompileTime>
    friend struct details::UtvAlgo;
 
    template<typename MatrixLike, int ColsAtCompileTime>
    friend struct details::UivAlgo;
 
    template<typename MatrixLike, int ColsAtCompileTime>
    friend struct details::UtivAlgo;
 
    // TODO Remove when API is stabilized
    PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH
    PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
    template<typename Scalar, int Options, typename VectorLike>
    friend VectorLike & details::inverseAlgo(
      const ContactCholeskyDecompositionTpl<Scalar, Options> & chol,
      const Eigen::DenseIndex col,
      const Eigen::MatrixBase<VectorLike> & vec);
 
    template<typename S1, int O1>
    bool operator==(const ContactCholeskyDecompositionTpl<S1, O1> & other) const
    {
      bool is_same = true;
 
      if (nv != other.nv || num_contacts != other.num_contacts)
        return false;
 
      if (
        D.size() != other.D.size() || Dinv.size() != other.Dinv.size() || U.rows() != other.U.rows()
        || U.cols() != other.U.cols())
        return false;
 
      is_same &= (D == other.D);
      is_same &= (Dinv == other.Dinv);
      is_same &= (U == other.U);
 
      is_same &= (parents_fromRow == other.parents_fromRow);
      is_same &= (nv_subtree_fromRow == other.nv_subtree_fromRow);
      is_same &= (last_child == other.last_child);
      //        is_same &= (rowise_sparsity_pattern == other.rowise_sparsity_pattern);
 
      return is_same;
    }
 
    template<typename S1, int O1>
    bool operator!=(const ContactCholeskyDecompositionTpl<S1, O1> & other) const
    {
      return !(*this == other);
    }
    PINOCCHIO_COMPILER_DIAGNOSTIC_POP
 
  protected:
    IndexVector parents_fromRow;
    IndexVector nv_subtree_fromRow;
 
    IndexVector last_child;
 
    Vector DUt; // temporary containing the results of D * U^t
 
    Eigen::DenseIndex nv;
 
    Eigen::DenseIndex num_contacts;
 
    VectorOfSliceVector rowise_sparsity_pattern;
 
    mutable Matrix U1inv;
    mutable Matrix U4inv;
 
    mutable RowMatrix OSIMinv_tmp, Minv_tmp;
  };
 
  template<typename ContactCholeskyDecomposition>
  struct traits<DelassusCholeskyExpressionTpl<ContactCholeskyDecomposition>>
  {
    enum
    {
      RowsAtCompileTime = Eigen::Dynamic
    };
    typedef typename ContactCholeskyDecomposition::Scalar Scalar;
    typedef typename ContactCholeskyDecomposition::Matrix Matrix;
    typedef typename ContactCholeskyDecomposition::Vector Vector;
  };
 
  template<typename _ContactCholeskyDecomposition>
  struct DelassusCholeskyExpressionTpl
  : DelassusOperatorBase<DelassusCholeskyExpressionTpl<_ContactCholeskyDecomposition>>
  {
    typedef _ContactCholeskyDecomposition ContactCholeskyDecomposition;
    typedef typename ContactCholeskyDecomposition::Scalar Scalar;
    typedef typename ContactCholeskyDecomposition::Vector Vector;
    typedef typename ContactCholeskyDecomposition::Matrix Matrix;
    typedef typename ContactCholeskyDecomposition::RowMatrix RowMatrix;
    typedef DelassusCholeskyExpressionTpl<_ContactCholeskyDecomposition> Self;
    typedef DelassusOperatorBase<Self> Base;
 
    typedef
      typename SizeDepType<Eigen::Dynamic>::template BlockReturn<RowMatrix>::Type RowMatrixBlockXpr;
    typedef typename SizeDepType<Eigen::Dynamic>::template BlockReturn<RowMatrix>::ConstType
      RowMatrixConstBlockXpr;
 
    enum
    {
      RowsAtCompileTime = traits<DelassusCholeskyExpressionTpl>::RowsAtCompileTime
    };
 
    explicit DelassusCholeskyExpressionTpl(const ContactCholeskyDecomposition & self)
    : Base(self.constraintDim())
    , self(self)
    {
    }
 
    template<typename MatrixIn, typename MatrixOut>
    void applyOnTheRight(
      const Eigen::MatrixBase<MatrixIn> & x, const Eigen::MatrixBase<MatrixOut> & res) const
    {
      PINOCCHIO_CHECK_ARGUMENT_SIZE(x.rows(), self.constraintDim());
      PINOCCHIO_CHECK_ARGUMENT_SIZE(res.rows(), self.constraintDim());
      PINOCCHIO_CHECK_ARGUMENT_SIZE(res.cols(), x.cols());
 
      PINOCCHIO_EIGEN_MALLOC_NOT_ALLOWED();
 
      const RowMatrixConstBlockXpr U1 =
        self.U.topLeftCorner(self.constraintDim(), self.constraintDim());
 
      if (x.cols() <= self.constraintDim())
      {
        RowMatrixBlockXpr tmp_mat =
          const_cast<ContactCholeskyDecomposition &>(self).OSIMinv_tmp.topLeftCorner(
            self.constraintDim(), x.cols());
        //            tmp_mat.noalias() = U1.adjoint() * x;
        triangularMatrixMatrixProduct<Eigen::UnitLower>(U1.adjoint(), x.derived(), tmp_mat);
        tmp_mat.array().colwise() *= -self.D.head(self.constraintDim()).array();
        //            res.const_cast_derived().noalias() = U1 * tmp_mat;
        triangularMatrixMatrixProduct<Eigen::UnitUpper>(U1, tmp_mat, res.const_cast_derived());
      }
      else // do memory allocation
      {
        RowMatrix tmp_mat(x.rows(), x.cols());
        //            tmp_mat.noalias() = U1.adjoint() * x;
        triangularMatrixMatrixProduct<Eigen::UnitLower>(U1.adjoint(), x.derived(), tmp_mat);
        tmp_mat.array().colwise() *= -self.D.head(self.constraintDim()).array();
        //            res.const_cast_derived().noalias() = U1 * tmp_mat;
        triangularMatrixMatrixProduct<Eigen::UnitUpper>(U1, tmp_mat, res.const_cast_derived());
      }
 
      PINOCCHIO_EIGEN_MALLOC_ALLOWED();
    }
 
    template<typename MatrixDerived>
    typename PINOCCHIO_EIGEN_PLAIN_TYPE(MatrixDerived)
    operator*(const Eigen::MatrixBase<MatrixDerived> & x) const
    {
      typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(MatrixDerived) ReturnType;
      ReturnType res(self.constraintDim(), x.cols());
      applyOnTheRight(x.derived(), res);
      return res;
    }
 
    template<typename MatrixDerived>
    void solveInPlace(const Eigen::MatrixBase<MatrixDerived> & x) const
    {
      PINOCCHIO_CHECK_ARGUMENT_SIZE(x.rows(), self.constraintDim());
 
      const Eigen::TriangularView<RowMatrixConstBlockXpr, Eigen::UnitUpper> U1 =
        self.U.topLeftCorner(self.constraintDim(), self.constraintDim())
          .template triangularView<Eigen::UnitUpper>();
 
      PINOCCHIO_EIGEN_MALLOC_NOT_ALLOWED();
      U1.solveInPlace(x.const_cast_derived());
      x.const_cast_derived().array().colwise() *= -self.Dinv.head(self.constraintDim()).array();
      U1.adjoint().solveInPlace(x);
      PINOCCHIO_EIGEN_MALLOC_ALLOWED();
    }
 
    template<typename MatrixDerivedIn, typename MatrixDerivedOut>
    void solve(
      const Eigen::MatrixBase<MatrixDerivedIn> & x,
      const Eigen::MatrixBase<MatrixDerivedOut> & res) const
    {
      res.const_cast_derived() = x;
      solveInPlace(res.const_cast_derived());
    }
 
    template<typename MatrixDerived>
    typename PINOCCHIO_EIGEN_PLAIN_TYPE(MatrixDerived)
      solve(const Eigen::MatrixBase<MatrixDerived> & x) const
    {
      typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(MatrixDerived) ReturnType;
      ReturnType res(self.constraintDim(), x.cols());
      solve(x.derived(), res);
      return res;
    }
 
    const ContactCholeskyDecomposition & cholesky() const
    {
      return self;
    }
 
    Matrix matrix() const
    {
      return self.getInverseOperationalSpaceInertiaMatrix();
    }
 
    Matrix inverse() const
    {
      return self.getOperationalSpaceInertiaMatrix();
    }
 
    template<typename VectorLike>
    void updateDamping(const Eigen::MatrixBase<VectorLike> & mus)
    {
      const_cast<ContactCholeskyDecomposition &>(self).updateDamping(mus);
    }
 
    void updateDamping(const Scalar & mu)
    {
      const_cast<ContactCholeskyDecomposition &>(self).updateDamping(mu);
    }
 
    Eigen::DenseIndex size() const
    {
      return self.constraintDim();
    }
    Eigen::DenseIndex rows() const
    {
      return size();
    }
    Eigen::DenseIndex cols() const
    {
      return size();
    }
 
  protected:
    const ContactCholeskyDecomposition & self;
  }; // DelassusCholeskyExpression
 
} // namespace pinocchio
 
#include "pinocchio/algorithm/contact-cholesky.hxx"
 
#if PINOCCHIO_ENABLE_TEMPLATE_INSTANTIATION
  #include "pinocchio/algorithm/contact-cholesky.txx"
#endif // PINOCCHIO_ENABLE_TEMPLATE_INSTANTIATION
 
#endif // ifndef __pinocchio_algorithm_contact_cholesky_hpp__