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 |
|
|
|