Line |
Branch |
Exec |
Source |
1 |
|
|
// |
2 |
|
|
// Copyright (c) 2024 INRIA |
3 |
|
|
// |
4 |
|
|
|
5 |
|
|
#ifndef __pinocchio_algorithm_delassus_operator_base_hpp__ |
6 |
|
|
#define __pinocchio_algorithm_delassus_operator_base_hpp__ |
7 |
|
|
|
8 |
|
|
#include "pinocchio/algorithm/fwd.hpp" |
9 |
|
|
#include "pinocchio/math/eigenvalues.hpp" |
10 |
|
|
|
11 |
|
|
namespace pinocchio |
12 |
|
|
{ |
13 |
|
|
|
14 |
|
|
template<typename DelassusOperatorDerived> |
15 |
|
|
struct DelassusOperatorBase |
16 |
|
|
{ |
17 |
|
|
typedef typename traits<DelassusOperatorDerived>::Scalar Scalar; |
18 |
|
|
typedef typename traits<DelassusOperatorDerived>::Vector Vector; |
19 |
|
|
typedef PowerIterationAlgoTpl<Vector> PowerIterationAlgo; |
20 |
|
|
|
21 |
|
✗ |
DelassusOperatorDerived & derived() |
22 |
|
|
{ |
23 |
|
✗ |
return static_cast<DelassusOperatorDerived &>(*this); |
24 |
|
|
} |
25 |
|
✗ |
const DelassusOperatorDerived & derived() const |
26 |
|
|
{ |
27 |
|
✗ |
return static_cast<const DelassusOperatorDerived &>(*this); |
28 |
|
|
} |
29 |
|
|
|
30 |
|
2915 |
explicit DelassusOperatorBase(const Eigen::DenseIndex size) |
31 |
|
2915 |
: power_iteration_algo(size) |
32 |
|
|
{ |
33 |
|
2915 |
} |
34 |
|
|
|
35 |
|
✗ |
Scalar computeLargestEigenValue( |
36 |
|
|
const bool reset = true, const int max_it = 10, const Scalar rel_tol = Scalar(1e-8)) const |
37 |
|
|
{ |
38 |
|
✗ |
power_iteration_algo.max_it = max_it; |
39 |
|
✗ |
power_iteration_algo.rel_tol = rel_tol; |
40 |
|
✗ |
if (reset) |
41 |
|
✗ |
power_iteration_algo.reset(); |
42 |
|
|
|
43 |
|
✗ |
power_iteration_algo.run(derived()); |
44 |
|
|
|
45 |
|
✗ |
return power_iteration_algo.largest_eigen_value; |
46 |
|
|
} |
47 |
|
|
|
48 |
|
|
template<typename VectorLike> |
49 |
|
|
Scalar computeLargestEigenValue( |
50 |
|
|
const Eigen::PlainObjectBase<VectorLike> & largest_eigenvector_est, |
51 |
|
|
const bool reset = true, |
52 |
|
|
const int max_it = 10, |
53 |
|
|
const Scalar rel_tol = Scalar(1e-8)) const |
54 |
|
|
{ |
55 |
|
|
PINOCCHIO_CHECK_ARGUMENT_SIZE(largest_eigenvector_est.size(), size()); |
56 |
|
|
power_iteration_algo.max_it = max_it; |
57 |
|
|
power_iteration_algo.rel_tol = rel_tol; |
58 |
|
|
if (reset) |
59 |
|
|
power_iteration_algo.reset(); |
60 |
|
|
power_iteration_algo.principal_eigen_vector = largest_eigenvector_est; |
61 |
|
|
|
62 |
|
|
power_iteration_algo.run(derived()); |
63 |
|
|
|
64 |
|
|
return power_iteration_algo.largest_eigen_value; |
65 |
|
|
} |
66 |
|
|
|
67 |
|
✗ |
Scalar computeLowestEigenValue( |
68 |
|
|
const bool reset = true, |
69 |
|
|
const bool compute_largest = true, |
70 |
|
|
const int max_it = 10, |
71 |
|
|
const Scalar rel_tol = Scalar(1e-8)) const |
72 |
|
|
{ |
73 |
|
✗ |
power_iteration_algo.max_it = max_it; |
74 |
|
✗ |
power_iteration_algo.rel_tol = rel_tol; |
75 |
|
✗ |
if (reset) |
76 |
|
✗ |
power_iteration_algo.reset(); |
77 |
|
|
|
78 |
|
✗ |
power_iteration_algo.lowest(derived(), compute_largest); |
79 |
|
|
|
80 |
|
✗ |
return power_iteration_algo.lowest_eigen_value; |
81 |
|
|
} |
82 |
|
|
|
83 |
|
|
template<typename VectorLike1, typename VectorLike2> |
84 |
|
|
Scalar computeLowestEigenValue( |
85 |
|
|
const Eigen::PlainObjectBase<VectorLike1> & largest_eigenvector_est, |
86 |
|
|
const Eigen::PlainObjectBase<VectorLike2> & lowest_eigenvector_est, |
87 |
|
|
const bool reset = true, |
88 |
|
|
const bool compute_largest = true, |
89 |
|
|
const int max_it = 10, |
90 |
|
|
const Scalar rel_tol = Scalar(1e-8)) const |
91 |
|
|
{ |
92 |
|
|
PINOCCHIO_CHECK_ARGUMENT_SIZE(largest_eigenvector_est.size(), size()); |
93 |
|
|
PINOCCHIO_CHECK_ARGUMENT_SIZE(lowest_eigenvector_est.size(), size()); |
94 |
|
|
|
95 |
|
|
power_iteration_algo.max_it = max_it; |
96 |
|
|
power_iteration_algo.rel_tol = rel_tol; |
97 |
|
|
if (reset) |
98 |
|
|
power_iteration_algo.reset(); |
99 |
|
|
power_iteration_algo.principal_eigen_vector = largest_eigenvector_est; |
100 |
|
|
power_iteration_algo.lowest_eigen_vector = lowest_eigenvector_est; |
101 |
|
|
|
102 |
|
|
power_iteration_algo.lowest(derived(), compute_largest); |
103 |
|
|
|
104 |
|
|
return power_iteration_algo.lowest_eigen_value; |
105 |
|
|
} |
106 |
|
|
|
107 |
|
|
template<typename VectorLike> |
108 |
|
|
void updateDamping(const Eigen::MatrixBase<VectorLike> & vec) |
109 |
|
|
{ |
110 |
|
|
derived().updateDamping(vec.derived()); |
111 |
|
|
} |
112 |
|
|
|
113 |
|
|
void updateDamping(const Scalar mu) |
114 |
|
|
{ |
115 |
|
|
derived().updateDamping(mu); |
116 |
|
|
} |
117 |
|
|
|
118 |
|
|
template<typename MatrixLike> |
119 |
|
|
void solveInPlace(const Eigen::MatrixBase<MatrixLike> & mat) const |
120 |
|
|
{ |
121 |
|
|
derived().solveInPlace(mat.const_cast_derived()); |
122 |
|
|
} |
123 |
|
|
|
124 |
|
|
template<typename MatrixLike> |
125 |
|
|
typename PINOCCHIO_EIGEN_PLAIN_TYPE(MatrixLike) |
126 |
|
|
solve(const Eigen::MatrixBase<MatrixLike> & mat) const |
127 |
|
|
{ |
128 |
|
|
return derived().solve(mat); |
129 |
|
|
} |
130 |
|
|
|
131 |
|
|
template<typename MatrixDerivedIn, typename MatrixDerivedOut> |
132 |
|
|
void solve( |
133 |
|
|
const Eigen::MatrixBase<MatrixDerivedIn> & x, |
134 |
|
|
const Eigen::MatrixBase<MatrixDerivedOut> & res) const |
135 |
|
|
{ |
136 |
|
|
derived().solve(x.derived(), res.const_cast_derived()); |
137 |
|
|
} |
138 |
|
|
|
139 |
|
|
template<typename MatrixIn, typename MatrixOut> |
140 |
|
|
void applyOnTheRight( |
141 |
|
|
const Eigen::MatrixBase<MatrixIn> & x, const Eigen::MatrixBase<MatrixOut> & res) const |
142 |
|
|
{ |
143 |
|
|
derived().applyOnTheRight(x.derived(), res.const_cast_derived()); |
144 |
|
|
} |
145 |
|
|
|
146 |
|
|
template<typename MatrixDerived> |
147 |
|
|
typename PINOCCHIO_EIGEN_PLAIN_TYPE(MatrixDerived) |
148 |
|
|
operator*(const Eigen::MatrixBase<MatrixDerived> & x) const |
149 |
|
|
{ |
150 |
|
|
return derived() * x.derived(); |
151 |
|
|
} |
152 |
|
|
|
153 |
|
|
Eigen::DenseIndex size() const |
154 |
|
|
{ |
155 |
|
|
return derived().size(); |
156 |
|
|
} |
157 |
|
|
Eigen::DenseIndex rows() const |
158 |
|
|
{ |
159 |
|
|
return derived().rows(); |
160 |
|
|
} |
161 |
|
|
Eigen::DenseIndex cols() const |
162 |
|
|
{ |
163 |
|
|
return derived().cols(); |
164 |
|
|
} |
165 |
|
|
|
166 |
|
|
PowerIterationAlgo & getPowerIterationAlgo() |
167 |
|
|
{ |
168 |
|
|
return power_iteration_algo; |
169 |
|
|
} |
170 |
|
|
|
171 |
|
|
const PowerIterationAlgo & getPowerIterationAlgo() const |
172 |
|
|
{ |
173 |
|
|
return power_iteration_algo; |
174 |
|
|
} |
175 |
|
|
|
176 |
|
|
protected: |
177 |
|
|
mutable PowerIterationAlgo power_iteration_algo; |
178 |
|
|
|
179 |
|
|
}; // struct DelassusOperatorBase |
180 |
|
|
|
181 |
|
|
} // namespace pinocchio |
182 |
|
|
|
183 |
|
|
#endif // ifndef __pinocchio_algorithm_delassus_operator_base_hpp__ |
184 |
|
|
|