delassus-operator-dense.hpp

//
// Copyright (c) 2024 INRIA
//

#ifndef __pinocchio_algorithm_delassus_operator_dense_hpp__
#define __pinocchio_algorithm_delassus_operator_dense_hpp__

#include "pinocchio/algorithm/fwd.hpp"
#include "pinocchio/algorithm/delassus-operator-base.hpp"

namespace pinocchio
{

 template<typename _Scalar, int _Options>
 struct traits<DelassusOperatorDenseTpl<_Scalar, _Options>>
 {
 typedef _Scalar Scalar;
 enum
 {
 Options = _Options,
 RowsAtCompileTime = Eigen::Dynamic
 };

 typedef Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, Options> Matrix;
 typedef Eigen::Matrix<Scalar, Eigen::Dynamic, 1, Options> Vector;
 };

 template<typename _Scalar, int _Options>
 struct DelassusOperatorDenseTpl
 : DelassusOperatorBase<DelassusOperatorDenseTpl<_Scalar, _Options>>
 {
 typedef _Scalar Scalar;
 typedef DelassusOperatorDenseTpl Self;
 enum
 {
 Options = _Options,
 RowsAtCompileTime = traits<DelassusOperatorDenseTpl>::RowsAtCompileTime
 };

 typedef typename traits<Self>::Matrix Matrix;
 typedef typename traits<Self>::Vector Vector;
 typedef Eigen::LLT<Matrix> CholeskyDecomposition;
 typedef DelassusOperatorBase<Self> Base;

 template<typename MatrixDerived>
 explicit DelassusOperatorDenseTpl(const Eigen::MatrixBase<MatrixDerived> & mat)
 : Base(mat.rows())
 , delassus_matrix(mat)
 , mat_tmp(mat.rows(), mat.cols())
 , llt(mat)
 , damping(Vector::Zero(mat.rows()))
 {
 PINOCCHIO_CHECK_ARGUMENT_SIZE(mat.rows(), mat.cols());
 }

 template<typename VectorLike>
 void updateDamping(const Eigen::MatrixBase<VectorLike> & vec)
 {
 damping = vec;
 mat_tmp = delassus_matrix;
 mat_tmp += vec.asDiagonal();
 llt.compute(mat_tmp);
 }

 void updateDamping(const Scalar & mu)
 {
 updateDamping(Vector::Constant(size(), mu));
 }

 template<typename MatrixLike>
 void solveInPlace(const Eigen::MatrixBase<MatrixLike> & mat) const
 {
 llt.solveInPlace(mat.const_cast_derived());
 }

 template<typename MatrixLike>
 typename PINOCCHIO_EIGEN_PLAIN_TYPE(MatrixLike)
 solve(const Eigen::MatrixBase<MatrixLike> & mat) const
 {
 typename PINOCCHIO_EIGEN_PLAIN_TYPE(MatrixLike) res(mat);
 solveInPlace(res);
 return res;
 }

 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 MatrixIn, typename MatrixOut>
 void applyOnTheRight(
 const Eigen::MatrixBase<MatrixIn> & x, const Eigen::MatrixBase<MatrixOut> & res_) const
 {
 MatrixOut & res = res_.const_cast_derived();
 res.noalias() = delassus_matrix * x;
 res.array() += damping.array() * x.array();
 }

 template<typename MatrixDerived>
 typename PINOCCHIO_EIGEN_PLAIN_TYPE(MatrixDerived)
 operator*(const Eigen::MatrixBase<MatrixDerived> & x) const
 {
 typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(MatrixDerived) ReturnType;

 PINOCCHIO_CHECK_ARGUMENT_SIZE(x.rows(), size());
 ReturnType res(x.rows(), x.cols());
 applyOnTheRight(x, res);
 return res;
 }

 Eigen::DenseIndex size() const
 {
 return delassus_matrix.rows();
 }
 Eigen::DenseIndex rows() const
 {
 return delassus_matrix.rows();
 }
 Eigen::DenseIndex cols() const
 {
 return delassus_matrix.cols();
 }

 Matrix matrix() const
 {
 mat_tmp = delassus_matrix;
 mat_tmp += damping.asDiagonal();
 return mat_tmp;
 }

 Matrix inverse() const
 {
 Matrix res = Matrix::Identity(size(), size());
 llt.solveInPlace(res);
 return res;
 }

 protected:
 Matrix delassus_matrix;
 mutable Matrix mat_tmp;
 CholeskyDecomposition llt;
 Vector damping;

 }; // struct DelassusOperatorDenseTpl

} // namespace pinocchio

#endif // ifndef __pinocchio_algorithm_delassus_operator_dense_hpp__