Directory: | ./ |
---|---|
File: | include/pinocchio/algorithm/contact-dynamics.hxx |
Date: | 2024-08-27 18:20:05 |
Exec | Total | Coverage | |
---|---|---|---|
Lines: | 72 | 90 | 80.0% |
Branches: | 57 | 384 | 14.8% |
Line | Branch | Exec | Source |
---|---|---|---|
1 | // | ||
2 | // Copyright (c) 2016-2020 CNRS INRIA | ||
3 | // | ||
4 | |||
5 | #ifndef __pinocchio_algorithm_constrained_dynamics_hxx__ | ||
6 | #define __pinocchio_algorithm_constrained_dynamics_hxx__ | ||
7 | |||
8 | #include "pinocchio/algorithm/compute-all-terms.hpp" | ||
9 | #include "pinocchio/algorithm/cholesky.hpp" | ||
10 | #include "pinocchio/algorithm/crba.hpp" | ||
11 | #include "pinocchio/algorithm/check.hpp" | ||
12 | #include "pinocchio/math/matrix.hpp" | ||
13 | |||
14 | #include <Eigen/Cholesky> | ||
15 | |||
16 | namespace pinocchio | ||
17 | { | ||
18 | |||
19 | namespace internal | ||
20 | { | ||
21 | template<typename Scalar, bool is_floating_point = pinocchio::is_floating_point<Scalar>::value> | ||
22 | struct PerformCholeskySolveInPlace | ||
23 | { | ||
24 | template<typename MatrixIn, typename MatrixLLT, typename MatrixOut> | ||
25 | static void run( | ||
26 | const Eigen::MatrixBase<MatrixIn> & mat, | ||
27 | Eigen::LLT<MatrixLLT> & llt, | ||
28 | const Eigen::MatrixBase<MatrixOut> & res, | ||
29 | const bool compute) | ||
30 | { | ||
31 | if (compute) | ||
32 | llt.compute(mat); | ||
33 | llt.solveInPlace(res.const_cast_derived()); | ||
34 | } | ||
35 | |||
36 | template<typename MatrixIn, typename MatrixLLT, typename MatrixOut> | ||
37 | 13 | static void run( | |
38 | const Eigen::MatrixBase<MatrixIn> & /*mat*/, | ||
39 | const Eigen::LLT<MatrixLLT> & llt, | ||
40 | const Eigen::MatrixBase<MatrixOut> & res) | ||
41 | { | ||
42 | 13 | llt.solveInPlace(res.const_cast_derived()); | |
43 | 13 | } | |
44 | }; | ||
45 | |||
46 | template<typename Scalar> | ||
47 | struct PerformCholeskySolveInPlace<Scalar, false> | ||
48 | { | ||
49 | template<typename MatrixIn, typename MatrixLLT, typename MatrixOut> | ||
50 | static void run( | ||
51 | const Eigen::MatrixBase<MatrixIn> & mat, | ||
52 | const Eigen::LLT<MatrixLLT> & /*llt*/, | ||
53 | const Eigen::MatrixBase<MatrixOut> & res) | ||
54 | { | ||
55 | typename PINOCCHIO_EIGEN_PLAIN_TYPE(MatrixIn) mat_inv(mat.rows(), mat.cols()); | ||
56 | inverse(mat, mat_inv); | ||
57 | res.const_cast_derived() = mat_inv * res; | ||
58 | } | ||
59 | }; | ||
60 | |||
61 | template<typename Scalar, bool is_floating_point = pinocchio::is_floating_point<Scalar>::value> | ||
62 | struct PerformCholeskyCompute | ||
63 | { | ||
64 | template<typename MatrixIn, typename MatrixLLT> | ||
65 | 12 | static void run(const Eigen::MatrixBase<MatrixIn> & mat, Eigen::LLT<MatrixLLT> & llt) | |
66 | { | ||
67 | 12 | llt.compute(mat); | |
68 | 12 | } | |
69 | }; | ||
70 | |||
71 | template<typename Scalar> | ||
72 | struct PerformCholeskyCompute<Scalar, false> | ||
73 | { | ||
74 | template<typename MatrixIn, typename MatrixLLT> | ||
75 | static void run(const Eigen::MatrixBase<MatrixIn> &, Eigen::LLT<MatrixLLT> &) | ||
76 | { | ||
77 | } | ||
78 | }; | ||
79 | |||
80 | } // namespace internal | ||
81 | |||
82 | template< | ||
83 | typename Scalar, | ||
84 | int Options, | ||
85 | template<typename, int> | ||
86 | class JointCollectionTpl, | ||
87 | typename TangentVectorType, | ||
88 | typename ConstraintMatrixType, | ||
89 | typename DriftVectorType> | ||
90 | inline const typename DataTpl<Scalar, Options, JointCollectionTpl>::TangentVectorType & | ||
91 | 9 | forwardDynamics( | |
92 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
93 | DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
94 | const Eigen::MatrixBase<TangentVectorType> & tau, | ||
95 | const Eigen::MatrixBase<ConstraintMatrixType> & J, | ||
96 | const Eigen::MatrixBase<DriftVectorType> & gamma, | ||
97 | const Scalar inv_damping) | ||
98 | { | ||
99 |
1/24✗ Branch 1 not taken.
✓ Branch 2 taken 9 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 17 not taken.
✗ Branch 18 not taken.
✗ Branch 20 not taken.
✗ Branch 21 not taken.
✗ Branch 23 not taken.
✗ Branch 24 not taken.
✗ Branch 26 not taken.
✗ Branch 27 not taken.
✗ Branch 29 not taken.
✗ Branch 30 not taken.
✗ Branch 33 not taken.
✗ Branch 34 not taken.
✗ Branch 36 not taken.
✗ Branch 37 not taken.
|
9 | PINOCCHIO_CHECK_ARGUMENT_SIZE(tau.size(), model.nv); |
100 |
1/24✗ Branch 1 not taken.
✓ Branch 2 taken 9 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 17 not taken.
✗ Branch 18 not taken.
✗ Branch 20 not taken.
✗ Branch 21 not taken.
✗ Branch 23 not taken.
✗ Branch 24 not taken.
✗ Branch 26 not taken.
✗ Branch 27 not taken.
✗ Branch 29 not taken.
✗ Branch 30 not taken.
✗ Branch 33 not taken.
✗ Branch 34 not taken.
✗ Branch 36 not taken.
✗ Branch 37 not taken.
|
9 | PINOCCHIO_CHECK_ARGUMENT_SIZE(J.cols(), model.nv); |
101 |
1/24✗ Branch 2 not taken.
✓ Branch 3 taken 9 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
✗ Branch 12 not taken.
✗ Branch 13 not taken.
✗ Branch 15 not taken.
✗ Branch 16 not taken.
✗ Branch 19 not taken.
✗ Branch 20 not taken.
✗ Branch 22 not taken.
✗ Branch 23 not taken.
✗ Branch 25 not taken.
✗ Branch 26 not taken.
✗ Branch 28 not taken.
✗ Branch 29 not taken.
✗ Branch 31 not taken.
✗ Branch 32 not taken.
✗ Branch 35 not taken.
✗ Branch 36 not taken.
✗ Branch 38 not taken.
✗ Branch 39 not taken.
|
9 | PINOCCHIO_CHECK_ARGUMENT_SIZE(J.rows(), gamma.size()); |
102 |
1/2✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
|
9 | assert(model.check(data) && "data is not consistent with model."); |
103 | |||
104 | typedef DataTpl<Scalar, Options, JointCollectionTpl> Data; | ||
105 | |||
106 | 9 | typename Data::TangentVectorType & a = data.ddq; | |
107 | 9 | typename Data::VectorXs & lambda_c = data.lambda_c; | |
108 | |||
109 | // Compute the UDUt decomposition of data.M | ||
110 | 9 | cholesky::decompose(model, data); | |
111 | |||
112 | // Compute the dynamic drift (control - nle) | ||
113 |
1/2✓ Branch 2 taken 9 times.
✗ Branch 3 not taken.
|
9 | data.torque_residual = tau - data.nle; |
114 | 9 | cholesky::solve(model, data, data.torque_residual); | |
115 | |||
116 |
1/2✓ Branch 2 taken 9 times.
✗ Branch 3 not taken.
|
9 | data.sDUiJt = J.transpose(); |
117 | // Compute U^-1 * J.T | ||
118 | 9 | cholesky::Uiv(model, data, data.sDUiJt); | |
119 |
2/2✓ Branch 0 taken 274 times.
✓ Branch 1 taken 9 times.
|
283 | for (Eigen::DenseIndex k = 0; k < model.nv; ++k) |
120 |
2/4✓ Branch 2 taken 274 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 274 times.
✗ Branch 6 not taken.
|
274 | data.sDUiJt.row(k) /= sqrt(data.D[k]); |
121 | |||
122 |
3/6✓ Branch 2 taken 9 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 9 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 9 times.
✗ Branch 9 not taken.
|
9 | data.JMinvJt.noalias() = data.sDUiJt.transpose() * data.sDUiJt; |
123 | |||
124 |
2/4✓ Branch 2 taken 9 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 9 times.
✗ Branch 6 not taken.
|
9 | data.JMinvJt.diagonal().array() += inv_damping; |
125 | |||
126 | // Compute the Lagrange Multipliers | ||
127 |
4/8✓ Branch 2 taken 9 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 9 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 9 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 9 times.
✗ Branch 12 not taken.
|
9 | lambda_c.noalias() = -J * data.torque_residual - gamma; |
128 | // data.llt_JMinvJt.compute(data.JMinvJt); | ||
129 | // data.llt_JMinvJt.solveInPlace(lambda_c); | ||
130 | 9 | internal::PerformCholeskyCompute<Scalar>::run(data.JMinvJt, data.llt_JMinvJt); | |
131 | 9 | internal::PerformCholeskySolveInPlace<Scalar>::run(data.JMinvJt, data.llt_JMinvJt, lambda_c); | |
132 | |||
133 | // Compute the joint acceleration | ||
134 |
3/6✓ Branch 2 taken 9 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 9 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 9 times.
✗ Branch 9 not taken.
|
9 | a.noalias() = J.transpose() * lambda_c; |
135 | 9 | cholesky::solve(model, data, a); | |
136 | 9 | a += data.torque_residual; | |
137 | |||
138 | 9 | return a; | |
139 | } | ||
140 | |||
141 | template< | ||
142 | typename Scalar, | ||
143 | int Options, | ||
144 | template<typename, int> | ||
145 | class JointCollectionTpl, | ||
146 | typename ConfigVectorType, | ||
147 | typename TangentVectorType1, | ||
148 | typename TangentVectorType2, | ||
149 | typename ConstraintMatrixType, | ||
150 | typename DriftVectorType> | ||
151 | inline const typename DataTpl<Scalar, Options, JointCollectionTpl>::TangentVectorType & | ||
152 | 9 | forwardDynamics( | |
153 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
154 | DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
155 | const Eigen::MatrixBase<ConfigVectorType> & q, | ||
156 | const Eigen::MatrixBase<TangentVectorType1> & v, | ||
157 | const Eigen::MatrixBase<TangentVectorType2> & tau, | ||
158 | const Eigen::MatrixBase<ConstraintMatrixType> & J, | ||
159 | const Eigen::MatrixBase<DriftVectorType> & gamma, | ||
160 | const Scalar inv_damping) | ||
161 | { | ||
162 |
1/24✗ Branch 1 not taken.
✓ Branch 2 taken 9 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 17 not taken.
✗ Branch 18 not taken.
✗ Branch 20 not taken.
✗ Branch 21 not taken.
✗ Branch 23 not taken.
✗ Branch 24 not taken.
✗ Branch 26 not taken.
✗ Branch 27 not taken.
✗ Branch 29 not taken.
✗ Branch 30 not taken.
✗ Branch 33 not taken.
✗ Branch 34 not taken.
✗ Branch 36 not taken.
✗ Branch 37 not taken.
|
9 | PINOCCHIO_CHECK_ARGUMENT_SIZE(q.size(), model.nq); |
163 |
1/24✗ Branch 1 not taken.
✓ Branch 2 taken 9 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 17 not taken.
✗ Branch 18 not taken.
✗ Branch 20 not taken.
✗ Branch 21 not taken.
✗ Branch 23 not taken.
✗ Branch 24 not taken.
✗ Branch 26 not taken.
✗ Branch 27 not taken.
✗ Branch 29 not taken.
✗ Branch 30 not taken.
✗ Branch 33 not taken.
✗ Branch 34 not taken.
✗ Branch 36 not taken.
✗ Branch 37 not taken.
|
9 | PINOCCHIO_CHECK_ARGUMENT_SIZE(v.size(), model.nv); |
164 | |||
165 | 9 | computeAllTerms(model, data, q, v); | |
166 | |||
167 | 9 | return forwardDynamics(model, data, tau, J, gamma, inv_damping); | |
168 | } | ||
169 | |||
170 | template< | ||
171 | typename Scalar, | ||
172 | int Options, | ||
173 | template<typename, int> | ||
174 | class JointCollectionTpl, | ||
175 | typename ConfigVectorType, | ||
176 | typename ConstraintMatrixType, | ||
177 | typename KKTMatrixType> | ||
178 | 1 | void computeKKTContactDynamicMatrixInverse( | |
179 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
180 | DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
181 | const Eigen::MatrixBase<ConfigVectorType> & q, | ||
182 | const Eigen::MatrixBase<ConstraintMatrixType> & J, | ||
183 | const Eigen::MatrixBase<KKTMatrixType> & KKTMatrix_inv, | ||
184 | const Scalar & inv_damping) | ||
185 | { | ||
186 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 1 times.
|
1 | assert(model.check(data)); |
187 |
2/6✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
|
1 | PINOCCHIO_CHECK_INPUT_ARGUMENT( |
188 | check_expression_if_real<Scalar>(inv_damping >= 0.), "mu must be positive."); | ||
189 | |||
190 | // Compute the mass matrix. | ||
191 | 1 | crba(model, data, q, Convention::WORLD); | |
192 | |||
193 | // Compute the UDUt decomposition of data.M. | ||
194 | 1 | cholesky::decompose(model, data); | |
195 | |||
196 | using std::sqrt; | ||
197 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
1 | data.sDUiJt = J.transpose(); |
198 | // Compute U^-1 * J.T | ||
199 | 1 | cholesky::Uiv(model, data, data.sDUiJt); | |
200 |
2/2✓ Branch 0 taken 32 times.
✓ Branch 1 taken 1 times.
|
33 | for (Eigen::DenseIndex k = 0; k < model.nv; ++k) |
201 |
2/4✓ Branch 2 taken 32 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 32 times.
✗ Branch 6 not taken.
|
32 | data.sDUiJt.row(k) /= sqrt(data.D[k]); |
202 | |||
203 |
3/6✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
|
1 | data.JMinvJt.noalias() = data.sDUiJt.transpose() * data.sDUiJt; |
204 | |||
205 |
2/4✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
|
1 | data.JMinvJt.diagonal().array() += inv_damping; |
206 | // data.llt_JMinvJt.compute(data.JMinvJt); | ||
207 | 1 | internal::PerformCholeskyCompute<Scalar>::run(data.JMinvJt, data.llt_JMinvJt); | |
208 | |||
209 | PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH | ||
210 | PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS | ||
211 | 1 | getKKTContactDynamicMatrixInverse(model, data, J, KKTMatrix_inv.const_cast_derived()); | |
212 | PINOCCHIO_COMPILER_DIAGNOSTIC_POP | ||
213 | 1 | } | |
214 | |||
215 | template< | ||
216 | typename Scalar, | ||
217 | int Options, | ||
218 | template<typename, int> | ||
219 | class JointCollectionTpl, | ||
220 | typename ConstraintMatrixType, | ||
221 | typename KKTMatrixType> | ||
222 | ✗ | void getKKTContactDynamicMatrixInverse( | |
223 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
224 | const DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
225 | const Eigen::MatrixBase<ConstraintMatrixType> & J, | ||
226 | const Eigen::MatrixBase<KKTMatrixType> & KKTMatrix_inv) | ||
227 | { | ||
228 | ✗ | assert(model.check(data)); | |
229 | ✗ | PINOCCHIO_CHECK_ARGUMENT_SIZE(KKTMatrix_inv.cols(), data.JMinvJt.cols() + model.nv); | |
230 | ✗ | PINOCCHIO_CHECK_ARGUMENT_SIZE(KKTMatrix_inv.rows(), data.JMinvJt.rows() + model.nv); | |
231 | |||
232 | typedef DataTpl<Scalar, Options, JointCollectionTpl> Data; | ||
233 | ✗ | const Eigen::DenseIndex nc = data.JMinvJt.cols(); | |
234 | |||
235 | ✗ | KKTMatrixType & KKTMatrix_inv_ = PINOCCHIO_EIGEN_CONST_CAST(KKTMatrixType, KKTMatrix_inv); | |
236 | |||
237 | typedef Eigen::Block<KKTMatrixType> BlockType; | ||
238 | ✗ | BlockType topLeft = KKTMatrix_inv_.topLeftCorner(model.nv, model.nv); | |
239 | ✗ | BlockType topRight = KKTMatrix_inv_.topRightCorner(model.nv, nc); | |
240 | ✗ | BlockType bottomLeft = KKTMatrix_inv_.bottomLeftCorner(nc, model.nv); | |
241 | ✗ | BlockType bottomRight = KKTMatrix_inv_.bottomRightCorner(nc, nc); | |
242 | |||
243 | ✗ | bottomRight = -Data::MatrixXs::Identity(nc, nc); | |
244 | // data.llt_JMinvJt.solveInPlace(bottomRight); | ||
245 | ✗ | internal::PerformCholeskySolveInPlace<Scalar>::run(data.JMinvJt, data.llt_JMinvJt, bottomRight); | |
246 | ✗ | topLeft.setIdentity(); | |
247 | ✗ | cholesky::solve(model, data, topLeft); | |
248 | |||
249 | ✗ | bottomLeft.noalias() = J * topLeft; | |
250 | ✗ | topRight.noalias() = bottomLeft.transpose() * (-bottomRight); | |
251 | ✗ | topLeft.noalias() -= topRight * bottomLeft; | |
252 | ✗ | bottomLeft = topRight.transpose(); | |
253 | } | ||
254 | |||
255 | template< | ||
256 | typename Scalar, | ||
257 | int Options, | ||
258 | template<typename, int> | ||
259 | class JointCollectionTpl, | ||
260 | typename ConfigVectorType, | ||
261 | typename TangentVectorType, | ||
262 | typename ConstraintMatrixType> | ||
263 | inline const typename DataTpl<Scalar, Options, JointCollectionTpl>::TangentVectorType & | ||
264 | 3 | impulseDynamics( | |
265 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
266 | DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
267 | const Eigen::MatrixBase<ConfigVectorType> & q, | ||
268 | const Eigen::MatrixBase<TangentVectorType> & v_before, | ||
269 | const Eigen::MatrixBase<ConstraintMatrixType> & J, | ||
270 | const Scalar r_coeff, | ||
271 | const Scalar inv_damping) | ||
272 | { | ||
273 |
1/24✗ Branch 1 not taken.
✓ Branch 2 taken 3 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 17 not taken.
✗ Branch 18 not taken.
✗ Branch 20 not taken.
✗ Branch 21 not taken.
✗ Branch 23 not taken.
✗ Branch 24 not taken.
✗ Branch 26 not taken.
✗ Branch 27 not taken.
✗ Branch 29 not taken.
✗ Branch 30 not taken.
✗ Branch 33 not taken.
✗ Branch 34 not taken.
✗ Branch 36 not taken.
✗ Branch 37 not taken.
|
3 | PINOCCHIO_CHECK_ARGUMENT_SIZE(q.size(), model.nq); |
274 | |||
275 | // Compute the mass matrix | ||
276 | 3 | crba(model, data, q, Convention::WORLD); | |
277 | |||
278 | 3 | return impulseDynamics(model, data, v_before, J, r_coeff, inv_damping); | |
279 | } | ||
280 | |||
281 | template< | ||
282 | typename Scalar, | ||
283 | int Options, | ||
284 | template<typename, int> | ||
285 | class JointCollectionTpl, | ||
286 | typename TangentVectorType, | ||
287 | typename ConstraintMatrixType> | ||
288 | inline const typename DataTpl<Scalar, Options, JointCollectionTpl>::TangentVectorType & | ||
289 | 3 | impulseDynamics( | |
290 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
291 | DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
292 | const Eigen::MatrixBase<TangentVectorType> & v_before, | ||
293 | const Eigen::MatrixBase<ConstraintMatrixType> & J, | ||
294 | const Scalar r_coeff, | ||
295 | const Scalar inv_damping) | ||
296 | { | ||
297 |
1/24✗ Branch 1 not taken.
✓ Branch 2 taken 3 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 17 not taken.
✗ Branch 18 not taken.
✗ Branch 20 not taken.
✗ Branch 21 not taken.
✗ Branch 23 not taken.
✗ Branch 24 not taken.
✗ Branch 26 not taken.
✗ Branch 27 not taken.
✗ Branch 29 not taken.
✗ Branch 30 not taken.
✗ Branch 33 not taken.
✗ Branch 34 not taken.
✗ Branch 36 not taken.
✗ Branch 37 not taken.
|
3 | PINOCCHIO_CHECK_ARGUMENT_SIZE(v_before.size(), model.nv); |
298 |
1/24✗ Branch 1 not taken.
✓ Branch 2 taken 3 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 17 not taken.
✗ Branch 18 not taken.
✗ Branch 20 not taken.
✗ Branch 21 not taken.
✗ Branch 23 not taken.
✗ Branch 24 not taken.
✗ Branch 26 not taken.
✗ Branch 27 not taken.
✗ Branch 29 not taken.
✗ Branch 30 not taken.
✗ Branch 33 not taken.
✗ Branch 34 not taken.
✗ Branch 36 not taken.
✗ Branch 37 not taken.
|
3 | PINOCCHIO_CHECK_ARGUMENT_SIZE(J.cols(), model.nv); |
299 |
1/2✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
|
3 | assert(model.check(data) && "data is not consistent with model."); |
300 | |||
301 | typedef DataTpl<Scalar, Options, JointCollectionTpl> Data; | ||
302 | |||
303 | 3 | typename Data::VectorXs & impulse_c = data.impulse_c; | |
304 | 3 | typename Data::TangentVectorType & dq_after = data.dq_after; | |
305 | |||
306 | // Compute the UDUt decomposition of data.M | ||
307 | 3 | cholesky::decompose(model, data); | |
308 | |||
309 |
1/2✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
|
3 | data.sDUiJt = J.transpose(); |
310 | // Compute U^-1 * J.T | ||
311 | 3 | cholesky::Uiv(model, data, data.sDUiJt); | |
312 |
2/2✓ Branch 0 taken 96 times.
✓ Branch 1 taken 3 times.
|
99 | for (int k = 0; k < model.nv; ++k) |
313 |
2/4✓ Branch 2 taken 96 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 96 times.
✗ Branch 6 not taken.
|
96 | data.sDUiJt.row(k) /= sqrt(data.D[k]); |
314 | |||
315 |
3/6✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 3 times.
✗ Branch 9 not taken.
|
3 | data.JMinvJt.noalias() = data.sDUiJt.transpose() * data.sDUiJt; |
316 |
2/4✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
|
3 | data.JMinvJt.diagonal().array() += inv_damping; |
317 | |||
318 | // Compute the Lagrange Multipliers related to the contact impulses | ||
319 |
3/6✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 3 times.
✗ Branch 9 not taken.
|
3 | impulse_c.noalias() = (-r_coeff - 1.) * (J * v_before); |
320 | // data.llt_JMinvJt.compute(data.JMinvJt); | ||
321 | // data.llt_JMinvJt.solveInPlace(impulse_c); | ||
322 | 3 | internal::PerformCholeskyCompute<Scalar>::run(data.JMinvJt, data.llt_JMinvJt); | |
323 | 3 | internal::PerformCholeskySolveInPlace<Scalar>::run(data.JMinvJt, data.llt_JMinvJt, impulse_c); | |
324 | |||
325 | // Compute the joint velocity after impacts | ||
326 |
3/6✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 3 times.
✗ Branch 9 not taken.
|
3 | dq_after.noalias() = J.transpose() * impulse_c; |
327 | 3 | cholesky::solve(model, data, dq_after); | |
328 | 3 | dq_after += v_before; | |
329 | |||
330 | 3 | return dq_after; | |
331 | } | ||
332 | } // namespace pinocchio | ||
333 | |||
334 | #endif // ifndef __pinocchio_algorithm_constrained_dynamics_hxx__ | ||
335 |