GCC Code Coverage Report

Directory: ./
File: include/pinocchio/algorithm/delassus-operator-sparse.hpp
Date: 2024-08-27 18:20:05
Exec Total Coverage
Lines: 0 62 0.0%
Branches: 0 130 0.0%
Line Branch Exec Source
1 //
2 // Copyright (c) 2024 INRIA
3 //
4
5 #ifndef __pinocchio_algorithm_delassus_operator_sparse_hpp__
6 #define __pinocchio_algorithm_delassus_operator_sparse_hpp__
7
8 #include "pinocchio/algorithm/fwd.hpp"
9 #include "pinocchio/algorithm/delassus-operator-base.hpp"
10
11 namespace pinocchio
12 {
13
14 namespace internal
15 {
16
17 template<typename Derived>
18 struct SimplicialCholeskyWrapper : public Derived
19 {
20 typedef Eigen::SimplicialCholeskyBase<Derived> Base;
21
22 using Base::derived;
23 using Base::m_diag;
24 using Base::m_info;
25 using Base::m_matrix;
26 using Base::m_P;
27 using Base::m_Pinv;
28
29 template<typename Rhs, typename Dest, typename Temporary>
30 ✗ void _solve_impl(
31 const Eigen::MatrixBase<Rhs> & b,
32 Eigen::MatrixBase<Dest> & dest,
33 Eigen::MatrixBase<Temporary> & tmp) const
34 {
35 // eigen_assert(m_factorizationIsOk && "The decomposition is not in a valid state for
36 // solving, you must first call either compute() or symbolic()/numeric()");
37 // eigen_assert(m_matrix.rows()==b.rows());
38
39 ✗ if (m_info != Eigen::Success)
40 ✗ return;
41
42 ✗ if (m_P.size() > 0)
43 ✗ tmp.noalias() = m_P * b;
44 else
45 ✗ tmp = b;
46
47 ✗ if (m_matrix.nonZeros() > 0) // otherwise L==I
48 ✗ derived().matrixL().solveInPlace(tmp);
49
50 ✗ if (m_diag.size() > 0)
51 ✗ tmp = m_diag.asDiagonal().inverse() * tmp;
52
53 ✗ if (m_matrix.nonZeros() > 0) // otherwise U==I
54 ✗ derived().matrixU().solveInPlace(tmp);
55
56 ✗ if (m_P.size() > 0)
57 ✗ dest.noalias() = m_Pinv * tmp;
58 }
59
60 }; // SimplicialCholeskyWrapper
61
62 template<typename SparseCholeskySolver>
63 struct getSparseCholeskySolverBase;
64
65 template<typename SparseCholeskySolver> //, typename Base = typename SparseCholeskySolver::Base>
66 struct SparseSolveInPlaceMethod;
67
68 #ifdef PINOCCHIO_WITH_ACCELERATE_SUPPORT
69 template<typename MatrixType, int UpLo, SparseFactorization_t Solver, bool EnforceSquare>
70 struct SparseSolveInPlaceMethod<Eigen::AccelerateImpl<MatrixType, UpLo, Solver, EnforceSquare>>
71 {
72 typedef Eigen::AccelerateImpl<MatrixType, UpLo, Solver, EnforceSquare> SparseCholeskySolver;
73
74 template<typename Rhs, typename Dest, typename Temporary>
75 static void run(
76 const SparseCholeskySolver & solver,
77 const Eigen::MatrixBase<Rhs> & mat,
78 const Eigen::MatrixBase<Dest> & dest,
79 Eigen::MatrixBase<Temporary> & /*tmp*/)
80 {
81 dest.const_cast_derived() = solver.solve(mat.derived());
82 }
83 };
84 #endif
85
86 template<typename SparseCholeskySolver>
87 struct SparseSolveInPlaceMethod
88 {
89 template<typename Rhs, typename Dest, typename Temporary>
90 ✗ static void run(
91 const SparseCholeskySolver & solver,
92 const Eigen::MatrixBase<Rhs> & mat,
93 const Eigen::MatrixBase<Dest> & dest,
94 Eigen::MatrixBase<Temporary> & tmp)
95 {
96 static_assert(
97 std::is_base_of<
98 Eigen::SimplicialCholeskyBase<SparseCholeskySolver>, SparseCholeskySolver>::value,
99 "The solver is not a base of SimplicialCholeskyBase.");
100 typedef SimplicialCholeskyWrapper<SparseCholeskySolver> CholeskyWrapper;
101
102 ✗ const CholeskyWrapper & wrapper = reinterpret_cast<const CholeskyWrapper &>(solver);
103 ✗ wrapper._solve_impl(mat, dest.const_cast_derived(), tmp.derived());
104 }
105 };
106
107 } // namespace internal
108
109 template<typename _Scalar, int _Options, class _SparseCholeskyDecomposition>
110 struct traits<DelassusOperatorSparseTpl<_Scalar, _Options, _SparseCholeskyDecomposition>>
111 {
112 typedef _SparseCholeskyDecomposition CholeskyDecomposition;
113 typedef typename CholeskyDecomposition::MatrixType SparseMatrix;
114 typedef _Scalar Scalar;
115
116 enum
117 {
118 Options = _Options,
119 RowsAtCompileTime = Eigen::Dynamic
120 };
121
122 typedef Eigen::Matrix<Scalar, Eigen::Dynamic, 1, Options> Vector;
123 typedef Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, Options> DenseMatrix;
124 };
125
126 template<typename _Scalar, int _Options, class SparseCholeskyDecomposition>
127 struct DelassusOperatorSparseTpl
128 : DelassusOperatorBase<DelassusOperatorSparseTpl<_Scalar, _Options, SparseCholeskyDecomposition>>
129 {
130 typedef DelassusOperatorSparseTpl Self;
131 typedef typename traits<Self>::Scalar Scalar;
132 enum
133 {
134 Options = traits<Self>::Options,
135 RowsAtCompileTime = traits<Self>::RowsAtCompileTime
136 };
137
138 typedef typename traits<Self>::SparseMatrix SparseMatrix;
139 typedef typename traits<Self>::Vector Vector;
140 typedef typename traits<Self>::DenseMatrix DenseMatrix;
141 typedef SparseCholeskyDecomposition CholeskyDecomposition;
142 typedef DelassusOperatorBase<Self> Base;
143
144 template<typename MatrixDerived>
145 ✗ explicit DelassusOperatorSparseTpl(const Eigen::SparseMatrixBase<MatrixDerived> & mat)
146 : Base(mat.rows())
147 ✗ , delassus_matrix(mat)
148 ✗ , delassus_matrix_plus_damping(mat)
149 ✗ , llt(mat)
150 ✗ , damping(Vector::Zero(mat.rows()))
151 ✗ , tmp(mat.rows())
152 {
153 ✗ PINOCCHIO_CHECK_ARGUMENT_SIZE(mat.rows(), mat.cols());
154 }
155
156 template<typename VectorLike>
157 ✗ void updateDamping(const Eigen::MatrixBase<VectorLike> & vec)
158 {
159 ✗ for (Eigen::DenseIndex k = 0; k < size(); ++k)
160 {
161 ✗ delassus_matrix_plus_damping.coeffRef(k, k) += -damping[k] + vec[k];
162 }
163 ✗ damping = vec;
164 PINOCCHIO_EIGEN_MALLOC_SAVE_STATUS();
165 PINOCCHIO_EIGEN_MALLOC_ALLOWED();
166 ✗ llt.factorize(delassus_matrix_plus_damping);
167 PINOCCHIO_EIGEN_MALLOC_RESTORE_STATUS();
168 }
169
170 ✗ void updateDamping(const Scalar & mu)
171 {
172 ✗ updateDamping(Vector::Constant(size(), mu));
173 }
174
175 template<typename MatrixLike>
176 ✗ void solveInPlace(const Eigen::MatrixBase<MatrixLike> & mat) const
177 {
178 ✗ internal::SparseSolveInPlaceMethod<CholeskyDecomposition>::run(
179 ✗ llt, mat.derived(), mat.const_cast_derived(), tmp);
180 }
181
182 template<typename MatrixLike>
183 typename PINOCCHIO_EIGEN_PLAIN_TYPE(MatrixLike)
184 ✗ solve(const Eigen::MatrixBase<MatrixLike> & mat) const
185 {
186 ✗ typename PINOCCHIO_EIGEN_PLAIN_TYPE(MatrixLike) res(mat);
187 ✗ solveInPlace(res);
188 ✗ return res;
189 }
190
191 template<typename MatrixDerivedIn, typename MatrixDerivedOut>
192 void solve(
193 const Eigen::MatrixBase<MatrixDerivedIn> & x,
194 const Eigen::MatrixBase<MatrixDerivedOut> & res) const
195 {
196 res.const_cast_derived() = x;
197 llt._solve_impl(x, res.const_cast_derived());
198 }
199
200 template<typename MatrixIn, typename MatrixOut>
201 ✗ void applyOnTheRight(
202 const Eigen::MatrixBase<MatrixIn> & x, const Eigen::MatrixBase<MatrixOut> & res_) const
203 {
204 ✗ MatrixOut & res = res_.const_cast_derived();
205 ✗ res.noalias() = delassus_matrix * x;
206 ✗ res.array() += damping.array() * x.array();
207 }
208
209 template<typename MatrixDerived>
210 typename PINOCCHIO_EIGEN_PLAIN_TYPE(MatrixDerived)
211 ✗ operator*(const Eigen::MatrixBase<MatrixDerived> & x) const
212 {
213 typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(MatrixDerived) ReturnType;
214
215 ✗ PINOCCHIO_CHECK_ARGUMENT_SIZE(x.rows(), size());
216 ✗ ReturnType res(x.rows(), x.cols());
217 ✗ applyOnTheRight(x, res);
218 ✗ return res;
219 }
220
221 ✗ Eigen::DenseIndex size() const
222 {
223 ✗ return delassus_matrix.rows();
224 }
225 ✗ Eigen::DenseIndex rows() const
226 {
227 ✗ return delassus_matrix.rows();
228 }
229 ✗ Eigen::DenseIndex cols() const
230 {
231 ✗ return delassus_matrix.cols();
232 }
233
234 ✗ SparseMatrix matrix() const
235 {
236 ✗ delassus_matrix_plus_damping = delassus_matrix;
237 ✗ delassus_matrix_plus_damping += damping.asDiagonal();
238 ✗ return delassus_matrix_plus_damping;
239 }
240
241 ✗ SparseMatrix inverse() const
242 {
243 ✗ SparseMatrix identity_matrix(size(), size());
244 ✗ identity_matrix.setIdentity();
245 ✗ SparseMatrix res = llt.solve(identity_matrix);
246 ✗ return res;
247 }
248
249 protected:
250 SparseMatrix delassus_matrix;
251 mutable SparseMatrix delassus_matrix_plus_damping;
252 CholeskyDecomposition llt;
253 Vector damping;
254 mutable Vector tmp;
255
256 }; // struct DelassusOperatorSparseTpl
257
258 } // namespace pinocchio
259
260 #endif // ifndef __pinocchio_algorithm_delassus_operator_sparse_hpp__
261