| Directory: | ./ |
|---|---|
| File: | include/pinocchio/algorithm/crba.hxx |
| Date: | 2024-08-27 18:20:05 |
| Exec | Total | Coverage | |
|---|---|---|---|
| Lines: | 40 | 83 | 48.2% |
| Branches: | 59 | 253 | 23.3% |
| Line | Branch | Exec | Source |
|---|---|---|---|
| 1 | // | ||
| 2 | // Copyright (c) 2015-2021 CNRS INRIA | ||
| 3 | // | ||
| 4 | |||
| 5 | #ifndef __pinocchio_crba_hxx__ | ||
| 6 | #define __pinocchio_crba_hxx__ | ||
| 7 | |||
| 8 | #include "pinocchio/multibody/visitor.hpp" | ||
| 9 | #include "pinocchio/spatial/act-on-set.hpp" | ||
| 10 | #include "pinocchio/algorithm/kinematics.hpp" | ||
| 11 | #include "pinocchio/algorithm/check.hpp" | ||
| 12 | |||
| 13 | /// @cond DEV | ||
| 14 | |||
| 15 | namespace pinocchio | ||
| 16 | { | ||
| 17 | namespace impl | ||
| 18 | { | ||
| 19 | template< | ||
| 20 | typename Scalar, | ||
| 21 | int Options, | ||
| 22 | template<typename, int> | ||
| 23 | class JointCollectionTpl, | ||
| 24 | typename ConfigVectorType> | ||
| 25 | struct CrbaWorldConventionForwardStep | ||
| 26 | : public fusion::JointUnaryVisitorBase< | ||
| 27 | CrbaWorldConventionForwardStep<Scalar, Options, JointCollectionTpl, ConfigVectorType>> | ||
| 28 | { | ||
| 29 | typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model; | ||
| 30 | typedef DataTpl<Scalar, Options, JointCollectionTpl> Data; | ||
| 31 | |||
| 32 | typedef boost::fusion::vector<const Model &, Data &, const ConfigVectorType &> ArgsType; | ||
| 33 | |||
| 34 | template<typename JointModel> | ||
| 35 | 216 | static void algo( | |
| 36 | const JointModelBase<JointModel> & jmodel, | ||
| 37 | JointDataBase<typename JointModel::JointDataDerived> & jdata, | ||
| 38 | const Model & model, | ||
| 39 | Data & data, | ||
| 40 | const Eigen::MatrixBase<ConfigVectorType> & q) | ||
| 41 | { | ||
| 42 | typedef typename Model::JointIndex JointIndex; | ||
| 43 | |||
| 44 |
1/2✓ Branch 1 taken 108 times.
✗ Branch 2 not taken.
|
216 | const JointIndex & i = jmodel.id(); |
| 45 |
1/2✓ Branch 3 taken 108 times.
✗ Branch 4 not taken.
|
216 | jmodel.calc(jdata.derived(), q.derived()); |
| 46 | |||
| 47 |
6/10✓ Branch 1 taken 108 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 104 times.
✓ Branch 5 taken 4 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 104 times.
✓ Branch 9 taken 4 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 104 times.
✗ Branch 13 not taken.
|
216 | data.liMi[i] = model.jointPlacements[i] * jdata.M(); |
| 48 | |||
| 49 | 216 | const JointIndex & parent = model.parents[i]; | |
| 50 |
2/2✓ Branch 0 taken 104 times.
✓ Branch 1 taken 4 times.
|
216 | if (parent > 0) |
| 51 |
2/4✓ Branch 3 taken 104 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 104 times.
✗ Branch 8 not taken.
|
208 | data.oMi[i] = data.oMi[parent] * data.liMi[i]; |
| 52 | else | ||
| 53 |
1/2✓ Branch 3 taken 4 times.
✗ Branch 4 not taken.
|
8 | data.oMi[i] = data.liMi[i]; |
| 54 | |||
| 55 |
4/8✓ Branch 2 taken 108 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 108 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 108 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 108 times.
✗ Branch 12 not taken.
|
216 | jmodel.jointCols(data.J) = data.oMi[i].act(jdata.S()); |
| 56 | |||
| 57 |
2/4✓ Branch 3 taken 108 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 108 times.
✗ Branch 8 not taken.
|
216 | data.oYcrb[i] = data.oMi[i].act(model.inertias[i]); |
| 58 | } | ||
| 59 | }; | ||
| 60 | |||
| 61 | template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl> | ||
| 62 | struct CrbaWorldConventionBackwardStep | ||
| 63 | : public fusion::JointUnaryVisitorBase< | ||
| 64 | CrbaWorldConventionBackwardStep<Scalar, Options, JointCollectionTpl>> | ||
| 65 | { | ||
| 66 | typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model; | ||
| 67 | typedef DataTpl<Scalar, Options, JointCollectionTpl> Data; | ||
| 68 | |||
| 69 | typedef boost::fusion::vector<const Model &, Data &> ArgsType; | ||
| 70 | |||
| 71 | template<typename JointModel> | ||
| 72 | 216 | static void algo(const JointModelBase<JointModel> & jmodel, const Model & model, Data & data) | |
| 73 | { | ||
| 74 | typedef typename Model::JointIndex JointIndex; | ||
| 75 | typedef | ||
| 76 | typename SizeDepType<JointModel::NV>::template ColsReturn<typename Data::Matrix6x>::Type | ||
| 77 | ColsBlock; | ||
| 78 |
1/2✓ Branch 1 taken 108 times.
✗ Branch 2 not taken.
|
216 | const JointIndex & i = jmodel.id(); |
| 79 | |||
| 80 | // Centroidal momentum map | ||
| 81 |
1/2✓ Branch 1 taken 108 times.
✗ Branch 2 not taken.
|
216 | ColsBlock Ag_cols = jmodel.jointCols(data.Ag); |
| 82 |
1/2✓ Branch 1 taken 108 times.
✗ Branch 2 not taken.
|
216 | ColsBlock J_cols = jmodel.jointCols(data.J); |
| 83 |
1/2✓ Branch 2 taken 108 times.
✗ Branch 3 not taken.
|
216 | motionSet::inertiaAction(data.oYcrb[i], J_cols, Ag_cols); |
| 84 | |||
| 85 | // Joint Space Inertia Matrix | ||
| 86 |
6/12✓ Branch 2 taken 108 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 108 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 108 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 108 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 108 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 108 times.
✗ Branch 18 not taken.
|
216 | data.M.block(jmodel.idx_v(), jmodel.idx_v(), jmodel.nv(), data.nvSubtree[i]).noalias() = |
| 87 |
4/8✓ Branch 2 taken 108 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 108 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 108 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 108 times.
✗ Branch 12 not taken.
|
216 | J_cols.transpose() * data.Ag.middleCols(jmodel.idx_v(), data.nvSubtree[i]); |
| 88 | |||
| 89 | 216 | const JointIndex & parent = model.parents[i]; | |
| 90 |
1/2✓ Branch 3 taken 108 times.
✗ Branch 4 not taken.
|
216 | data.oYcrb[parent] += data.oYcrb[i]; |
| 91 | } | ||
| 92 | }; | ||
| 93 | |||
| 94 | template< | ||
| 95 | typename Scalar, | ||
| 96 | int Options, | ||
| 97 | template<typename, int> | ||
| 98 | class JointCollectionTpl, | ||
| 99 | typename ConfigVectorType> | ||
| 100 | struct CrbaLocalConventionForwardStep | ||
| 101 | : public fusion::JointUnaryVisitorBase< | ||
| 102 | CrbaLocalConventionForwardStep<Scalar, Options, JointCollectionTpl, ConfigVectorType>> | ||
| 103 | { | ||
| 104 | typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model; | ||
| 105 | typedef DataTpl<Scalar, Options, JointCollectionTpl> Data; | ||
| 106 | |||
| 107 | typedef boost::fusion::vector<const Model &, Data &, const ConfigVectorType &> ArgsType; | ||
| 108 | |||
| 109 | template<typename JointModel> | ||
| 110 | ✗ | static void algo( | |
| 111 | const JointModelBase<JointModel> & jmodel, | ||
| 112 | JointDataBase<typename JointModel::JointDataDerived> & jdata, | ||
| 113 | const Model & model, | ||
| 114 | Data & data, | ||
| 115 | const Eigen::MatrixBase<ConfigVectorType> & q) | ||
| 116 | { | ||
| 117 | typedef typename Model::JointIndex JointIndex; | ||
| 118 | |||
| 119 | ✗ | const JointIndex & i = jmodel.id(); | |
| 120 | ✗ | jmodel.calc(jdata.derived(), q.derived()); | |
| 121 | |||
| 122 | ✗ | data.liMi[i] = model.jointPlacements[i] * jdata.M(); | |
| 123 | ✗ | data.Ycrb[i] = model.inertias[i]; | |
| 124 | } | ||
| 125 | }; | ||
| 126 | |||
| 127 | template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl> | ||
| 128 | struct CrbaLocalConventionBackwardStep | ||
| 129 | : public fusion::JointUnaryVisitorBase< | ||
| 130 | CrbaLocalConventionBackwardStep<Scalar, Options, JointCollectionTpl>> | ||
| 131 | { | ||
| 132 | typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model; | ||
| 133 | typedef DataTpl<Scalar, Options, JointCollectionTpl> Data; | ||
| 134 | |||
| 135 | typedef boost::fusion::vector<const Model &, Data &> ArgsType; | ||
| 136 | |||
| 137 | template<typename JointModel> | ||
| 138 | ✗ | static void algo( | |
| 139 | const JointModelBase<JointModel> & jmodel, | ||
| 140 | JointDataBase<typename JointModel::JointDataDerived> & jdata, | ||
| 141 | const Model & model, | ||
| 142 | Data & data) | ||
| 143 | { | ||
| 144 | /* | ||
| 145 | * F[1:6,i] = Y*S | ||
| 146 | * M[i,SUBTREE] = S'*F[1:6,SUBTREE] | ||
| 147 | * if li>0 | ||
| 148 | * Yli += liXi Yi | ||
| 149 | * F[1:6,SUBTREE] = liXi F[1:6,SUBTREE] | ||
| 150 | */ | ||
| 151 | |||
| 152 | typedef typename Model::JointIndex JointIndex; | ||
| 153 | typedef typename Data::Matrix6x::ColsBlockXpr Block; | ||
| 154 | ✗ | const JointIndex & i = jmodel.id(); | |
| 155 | |||
| 156 | /* F[1:6,i] = Y*S */ | ||
| 157 | // data.Fcrb[i].block<6,JointModel::NV>(0,jmodel.idx_v()) = data.Ycrb[i] * jdata.S(); | ||
| 158 | ✗ | jmodel.jointCols(data.Fcrb[i]) = data.Ycrb[i] * jdata.S(); | |
| 159 | |||
| 160 | /* M[i,SUBTREE] = S'*F[1:6,SUBTREE] */ | ||
| 161 | ✗ | data.M.block(jmodel.idx_v(), jmodel.idx_v(), jmodel.nv(), data.nvSubtree[i]).noalias() = | |
| 162 | ✗ | jdata.S().transpose() * data.Fcrb[i].middleCols(jmodel.idx_v(), data.nvSubtree[i]); | |
| 163 | |||
| 164 | ✗ | const JointIndex & parent = model.parents[i]; | |
| 165 | ✗ | if (parent > 0) | |
| 166 | { | ||
| 167 | /* Yli += liXi Yi */ | ||
| 168 | ✗ | data.Ycrb[parent] += data.liMi[i].act(data.Ycrb[i]); | |
| 169 | |||
| 170 | /* F[1:6,SUBTREE] = liXi F[1:6,SUBTREE] */ | ||
| 171 | ✗ | Block jF = data.Fcrb[parent].middleCols(jmodel.idx_v(), data.nvSubtree[i]); | |
| 172 | ✗ | Block iF = data.Fcrb[i].middleCols(jmodel.idx_v(), data.nvSubtree[i]); | |
| 173 | ✗ | forceSet::se3Action(data.liMi[i], iF, jF); | |
| 174 | } | ||
| 175 | } | ||
| 176 | }; | ||
| 177 | |||
| 178 | template< | ||
| 179 | typename Scalar, | ||
| 180 | int Options, | ||
| 181 | template<typename, int> | ||
| 182 | class JointCollectionTpl, | ||
| 183 | typename ConfigVectorType> | ||
| 184 | ✗ | const typename DataTpl<Scalar, Options, JointCollectionTpl>::MatrixXs & crbaLocalConvention( | |
| 185 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
| 186 | DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
| 187 | const Eigen::MatrixBase<ConfigVectorType> & q) | ||
| 188 | { | ||
| 189 | ✗ | assert(model.check(data) && "data is not consistent with model."); | |
| 190 | ✗ | PINOCCHIO_CHECK_ARGUMENT_SIZE( | |
| 191 | q.size(), model.nq, "The configuration vector is not of right size"); | ||
| 192 | |||
| 193 | typedef typename ModelTpl<Scalar, Options, JointCollectionTpl>::JointIndex JointIndex; | ||
| 194 | |||
| 195 | typedef CrbaLocalConventionForwardStep<Scalar, Options, JointCollectionTpl, ConfigVectorType> | ||
| 196 | Pass1; | ||
| 197 | ✗ | for (JointIndex i = 1; i < (JointIndex)(model.njoints); ++i) | |
| 198 | { | ||
| 199 | ✗ | Pass1::run( | |
| 200 | ✗ | model.joints[i], data.joints[i], typename Pass1::ArgsType(model, data, q.derived())); | |
| 201 | } | ||
| 202 | |||
| 203 | typedef CrbaLocalConventionBackwardStep<Scalar, Options, JointCollectionTpl> Pass2; | ||
| 204 | ✗ | for (JointIndex i = (JointIndex)(model.njoints - 1); i > 0; --i) | |
| 205 | { | ||
| 206 | ✗ | Pass2::run(model.joints[i], data.joints[i], typename Pass2::ArgsType(model, data)); | |
| 207 | } | ||
| 208 | |||
| 209 | // Add the armature contribution | ||
| 210 | ✗ | data.M.diagonal() += model.armature; | |
| 211 | |||
| 212 | ✗ | return data.M; | |
| 213 | } | ||
| 214 | |||
| 215 | template< | ||
| 216 | typename Scalar, | ||
| 217 | int Options, | ||
| 218 | template<typename, int> | ||
| 219 | class JointCollectionTpl, | ||
| 220 | typename ConfigVectorType> | ||
| 221 | 4 | const typename DataTpl<Scalar, Options, JointCollectionTpl>::MatrixXs & crbaWorldConvention( | |
| 222 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
| 223 | DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
| 224 | const Eigen::MatrixBase<ConfigVectorType> & q) | ||
| 225 | { | ||
| 226 |
2/4✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 4 times.
✗ Branch 4 not taken.
|
4 | assert(model.check(data) && "data is not consistent with model."); |
| 227 |
1/24✗ Branch 1 not taken.
✓ Branch 2 taken 4 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.
|
4 | PINOCCHIO_CHECK_ARGUMENT_SIZE( |
| 228 | q.size(), model.nq, "The configuration vector is not of right size"); | ||
| 229 | |||
| 230 | typedef typename ModelTpl<Scalar, Options, JointCollectionTpl>::JointIndex JointIndex; | ||
| 231 | |||
| 232 |
1/2✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
|
4 | data.oYcrb[0].setZero(); |
| 233 | typedef CrbaWorldConventionForwardStep<Scalar, Options, JointCollectionTpl, ConfigVectorType> | ||
| 234 | Pass1; | ||
| 235 |
2/2✓ Branch 0 taken 108 times.
✓ Branch 1 taken 4 times.
|
112 | for (JointIndex i = 1; i < (JointIndex)(model.njoints); ++i) |
| 236 | { | ||
| 237 |
1/2✓ Branch 1 taken 108 times.
✗ Branch 2 not taken.
|
108 | Pass1::run( |
| 238 |
1/2✓ Branch 2 taken 108 times.
✗ Branch 3 not taken.
|
216 | model.joints[i], data.joints[i], typename Pass1::ArgsType(model, data, q.derived())); |
| 239 | } | ||
| 240 | |||
| 241 | typedef CrbaWorldConventionBackwardStep<Scalar, Options, JointCollectionTpl> Pass2; | ||
| 242 |
2/2✓ Branch 0 taken 108 times.
✓ Branch 1 taken 4 times.
|
112 | for (JointIndex i = (JointIndex)(model.njoints - 1); i > 0; --i) |
| 243 | { | ||
| 244 |
2/4✓ Branch 1 taken 108 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 108 times.
✗ Branch 6 not taken.
|
108 | Pass2::run(model.joints[i], typename Pass2::ArgsType(model, data)); |
| 245 | } | ||
| 246 | |||
| 247 | // Add the armature contribution | ||
| 248 |
2/4✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
|
4 | data.M.diagonal() += model.armature; |
| 249 | |||
| 250 | // Retrieve the Centroidal Momemtum map | ||
| 251 | typedef DataTpl<Scalar, Options, JointCollectionTpl> Data; | ||
| 252 | typedef typename Data::Force Force; | ||
| 253 | typedef Eigen::Block<typename Data::Matrix6x, 3, -1> Block3x; | ||
| 254 | |||
| 255 | 4 | data.mass[0] = data.oYcrb[0].mass(); | |
| 256 |
1/2✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
|
4 | data.com[0] = data.oYcrb[0].lever(); |
| 257 | |||
| 258 |
1/2✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
|
4 | const Block3x Ag_lin = data.Ag.template middleRows<3>(Force::LINEAR); |
| 259 |
1/2✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
|
4 | Block3x Ag_ang = data.Ag.template middleRows<3>(Force::ANGULAR); |
| 260 |
2/2✓ Branch 0 taken 128 times.
✓ Branch 1 taken 4 times.
|
132 | for (long i = 0; i < model.nv; ++i) |
| 261 |
4/8✓ Branch 1 taken 128 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 128 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 128 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 128 times.
✗ Branch 12 not taken.
|
128 | Ag_ang.col(i) += Ag_lin.col(i).cross(data.com[0]); |
| 262 | |||
| 263 | 4 | return data.M; | |
| 264 | } | ||
| 265 | } // namespace impl | ||
| 266 | // --- CHECKER --------------------------------------------------------------- | ||
| 267 | // --- CHECKER --------------------------------------------------------------- | ||
| 268 | // --- CHECKER --------------------------------------------------------------- | ||
| 269 | |||
| 270 | namespace internal | ||
| 271 | { | ||
| 272 | template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl> | ||
| 273 | ✗ | inline bool isDescendant( | |
| 274 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
| 275 | const typename ModelTpl<Scalar, Options, JointCollectionTpl>::JointIndex j, | ||
| 276 | const typename ModelTpl<Scalar, Options, JointCollectionTpl>::JointIndex root) | ||
| 277 | { | ||
| 278 | typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model; | ||
| 279 | typedef typename Model::JointIndex JointIndex; | ||
| 280 | |||
| 281 | ✗ | if (j >= (JointIndex)model.njoints) | |
| 282 | ✗ | return false; | |
| 283 | ✗ | if (j == 0) | |
| 284 | ✗ | return root == 0; | |
| 285 | ✗ | return (j == root) || isDescendant(model, model.parents[j], root); | |
| 286 | } | ||
| 287 | } // namespace internal | ||
| 288 | |||
| 289 | template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl> | ||
| 290 | inline bool | ||
| 291 | ✗ | CRBAChecker::checkModel_impl(const ModelTpl<Scalar, Options, JointCollectionTpl> & model) const | |
| 292 | { | ||
| 293 | typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model; | ||
| 294 | typedef typename Model::JointIndex JointIndex; | ||
| 295 | |||
| 296 | // For CRBA, the tree must be "compact", i.e. all descendants of a node i are stored | ||
| 297 | // immediately after i in the "parents" map, i.e. forall joint i, the interval i+1..n-1 | ||
| 298 | // can be separated in two intervals [i+1..k] and [k+1..n-1], where any [i+1..k] is a descendant | ||
| 299 | // of i and none of [k+1..n-1] is a descendant of i. | ||
| 300 | ✗ | for (JointIndex i = 1; i < (JointIndex)(model.njoints - 1); | |
| 301 | ++i) // no need to check joints 0 and N-1 | ||
| 302 | { | ||
| 303 | ✗ | JointIndex k = i + 1; | |
| 304 | ✗ | while (internal::isDescendant(model, k, i)) | |
| 305 | ✗ | ++k; | |
| 306 | ✗ | for (; int(k) < model.njoints; ++k) | |
| 307 | ✗ | if (internal::isDescendant(model, k, i)) | |
| 308 | ✗ | return false; | |
| 309 | } | ||
| 310 | ✗ | return true; | |
| 311 | } | ||
| 312 | |||
| 313 | template< | ||
| 314 | typename Scalar, | ||
| 315 | int Options, | ||
| 316 | template<typename, int> | ||
| 317 | class JointCollectionTpl, | ||
| 318 | typename ConfigVectorType> | ||
| 319 | 4 | const typename DataTpl<Scalar, Options, JointCollectionTpl>::MatrixXs & crba( | |
| 320 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
| 321 | DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
| 322 | const Eigen::MatrixBase<ConfigVectorType> & q, | ||
| 323 | const Convention convention) | ||
| 324 | { | ||
| 325 |
1/3✗ Branch 0 not taken.
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
|
4 | switch (convention) |
| 326 | { | ||
| 327 | ✗ | case Convention::LOCAL: | |
| 328 | ✗ | return ::pinocchio::impl::crbaLocalConvention(model, data, make_const_ref(q)); | |
| 329 | 4 | case Convention::WORLD: | |
| 330 |
1/2✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
|
4 | return ::pinocchio::impl::crbaWorldConvention(model, data, make_const_ref(q)); |
| 331 | } | ||
| 332 | } | ||
| 333 | |||
| 334 | } // namespace pinocchio | ||
| 335 | |||
| 336 | /// @endcond | ||
| 337 | |||
| 338 | #endif // ifndef __pinocchio_crba_hxx__ | ||
| 339 |