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