| Directory: | ./ |
|---|---|
| File: | include/pinocchio/algorithm/centroidal-derivatives.hxx |
| Date: | 2025-04-30 16:14:33 |
| Exec | Total | Coverage | |
|---|---|---|---|
| Lines: | 200 | 200 | 100.0% |
| Branches: | 253 | 915 | 27.7% |
| Line | Branch | Exec | Source |
|---|---|---|---|
| 1 | // | ||
| 2 | // Copyright (c) 2018-2021 INRIA | ||
| 3 | // | ||
| 4 | |||
| 5 | #ifndef __pinocchio_algorithm_centroidal_derivatives_hxx__ | ||
| 6 | #define __pinocchio_algorithm_centroidal_derivatives_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> class JointCollectionTpl, | ||
| 23 | typename ConfigVectorType, | ||
| 24 | typename TangentVectorType1, | ||
| 25 | typename TangentVectorType2> | ||
| 26 | struct CentroidalDynDerivativesForwardStep | ||
| 27 | : public fusion::JointUnaryVisitorBase<CentroidalDynDerivativesForwardStep< | ||
| 28 | Scalar, | ||
| 29 | Options, | ||
| 30 | JointCollectionTpl, | ||
| 31 | ConfigVectorType, | ||
| 32 | TangentVectorType1, | ||
| 33 | TangentVectorType2>> | ||
| 34 | { | ||
| 35 | typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model; | ||
| 36 | typedef DataTpl<Scalar, Options, JointCollectionTpl> Data; | ||
| 37 | |||
| 38 | typedef typename Model::JointIndex JointIndex; | ||
| 39 | |||
| 40 | typedef boost::fusion::vector< | ||
| 41 | const Model &, | ||
| 42 | Data &, | ||
| 43 | const ConfigVectorType &, | ||
| 44 | const TangentVectorType1 &, | ||
| 45 | const TangentVectorType2 &> | ||
| 46 | ArgsType; | ||
| 47 | |||
| 48 | template<typename JointModel> | ||
| 49 | 218 | static void algo( | |
| 50 | const JointModelBase<JointModel> & jmodel, | ||
| 51 | JointDataBase<typename JointModel::JointDataDerived> & jdata, | ||
| 52 | const Model & model, | ||
| 53 | Data & data, | ||
| 54 | const Eigen::MatrixBase<ConfigVectorType> & q, | ||
| 55 | const Eigen::MatrixBase<TangentVectorType1> & v, | ||
| 56 | const Eigen::MatrixBase<TangentVectorType2> & a) | ||
| 57 | { | ||
| 58 | typedef typename Model::JointIndex JointIndex; | ||
| 59 | typedef typename Data::Motion Motion; | ||
| 60 | |||
| 61 |
1/2✓ Branch 1 taken 109 times.
✗ Branch 2 not taken.
|
218 | const JointIndex & i = jmodel.id(); |
| 62 | 218 | const JointIndex & parent = model.parents[i]; | |
| 63 | 218 | Motion & ov = data.ov[i]; | |
| 64 | 218 | Motion & oa = data.oa[i]; | |
| 65 | |||
| 66 |
1/2✓ Branch 4 taken 109 times.
✗ Branch 5 not taken.
|
218 | jmodel.calc(jdata.derived(), q.derived(), v.derived()); |
| 67 | |||
| 68 |
6/10✓ Branch 1 taken 109 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 104 times.
✓ Branch 5 taken 5 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 104 times.
✓ Branch 9 taken 5 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 104 times.
✗ Branch 13 not taken.
|
218 | data.liMi[i] = model.jointPlacements[i] * jdata.M(); |
| 69 | |||
| 70 |
2/4✓ Branch 1 taken 109 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 109 times.
✗ Branch 6 not taken.
|
218 | data.v[i] = jdata.v(); |
| 71 | |||
| 72 |
2/2✓ Branch 0 taken 105 times.
✓ Branch 1 taken 4 times.
|
218 | if (parent > 0) |
| 73 | { | ||
| 74 |
2/4✓ Branch 3 taken 105 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 105 times.
✗ Branch 8 not taken.
|
210 | data.oMi[i] = data.oMi[parent] * data.liMi[i]; |
| 75 |
2/4✓ Branch 3 taken 105 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 105 times.
✗ Branch 8 not taken.
|
210 | data.v[i] += data.liMi[i].actInv(data.v[parent]); |
| 76 | } | ||
| 77 | else | ||
| 78 |
1/2✓ Branch 3 taken 4 times.
✗ Branch 4 not taken.
|
8 | data.oMi[i] = data.liMi[i]; |
| 79 | |||
| 80 |
5/14✓ Branch 1 taken 109 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 105 times.
✓ Branch 5 taken 4 times.
✗ Branch 6 not taken.
✓ Branch 7 taken 105 times.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✓ Branch 11 taken 105 times.
✗ Branch 12 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 17 not taken.
✗ Branch 18 not taken.
|
436 | data.a[i] = |
| 81 |
7/15✓ Branch 1 taken 109 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 5 taken 109 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 109 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 109 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 109 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 4 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 4 times.
✗ Branch 21 not taken.
|
436 | jdata.S() * jmodel.jointVelocitySelector(a) + jdata.c() + (data.v[i] ^ jdata.v()); |
| 82 |
2/2✓ Branch 0 taken 105 times.
✓ Branch 1 taken 4 times.
|
218 | if (parent > 0) |
| 83 | { | ||
| 84 |
2/4✓ Branch 3 taken 105 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 105 times.
✗ Branch 8 not taken.
|
210 | data.a[i] += data.liMi[i].actInv(data.a[parent]); |
| 85 | } | ||
| 86 | |||
| 87 |
2/4✓ Branch 3 taken 109 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 109 times.
✗ Branch 8 not taken.
|
218 | data.oYcrb[i] = data.oMi[i].act(model.inertias[i]); |
| 88 |
2/4✓ Branch 3 taken 109 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 109 times.
✗ Branch 7 not taken.
|
218 | ov = data.oMi[i].act(data.v[i]); |
| 89 |
2/4✓ Branch 3 taken 109 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 109 times.
✗ Branch 7 not taken.
|
218 | oa = data.oMi[i].act(data.a[i]); |
| 90 | |||
| 91 |
2/4✓ Branch 2 taken 109 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 109 times.
✗ Branch 7 not taken.
|
218 | data.oh[i] = data.oYcrb[i] * ov; |
| 92 |
4/8✓ Branch 2 taken 109 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 109 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 109 times.
✗ Branch 10 not taken.
✓ Branch 13 taken 109 times.
✗ Branch 14 not taken.
|
218 | data.of[i] = data.oYcrb[i] * oa + ov.cross(data.oh[i]); |
| 93 | |||
| 94 | typedef | ||
| 95 | typename SizeDepType<JointModel::NV>::template ColsReturn<typename Data::Matrix6x>::Type | ||
| 96 | ColsBlock; | ||
| 97 |
1/2✓ Branch 1 taken 109 times.
✗ Branch 2 not taken.
|
218 | ColsBlock J_cols = jmodel.jointCols(data.J); |
| 98 |
1/2✓ Branch 1 taken 109 times.
✗ Branch 2 not taken.
|
218 | ColsBlock dJ_cols = jmodel.jointCols(data.dJ); |
| 99 |
1/2✓ Branch 1 taken 109 times.
✗ Branch 2 not taken.
|
218 | ColsBlock dVdq_cols = jmodel.jointCols(data.dVdq); |
| 100 |
1/2✓ Branch 1 taken 109 times.
✗ Branch 2 not taken.
|
218 | ColsBlock dAdq_cols = jmodel.jointCols(data.dAdq); |
| 101 |
1/2✓ Branch 1 taken 109 times.
✗ Branch 2 not taken.
|
218 | ColsBlock dAdv_cols = jmodel.jointCols(data.dAdv); |
| 102 | |||
| 103 |
3/6✓ Branch 2 taken 109 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 109 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 109 times.
✗ Branch 9 not taken.
|
218 | J_cols = data.oMi[i].act(jdata.S()); |
| 104 |
1/2✓ Branch 1 taken 109 times.
✗ Branch 2 not taken.
|
218 | motionSet::motionAction(ov, J_cols, dJ_cols); |
| 105 |
1/2✓ Branch 2 taken 109 times.
✗ Branch 3 not taken.
|
218 | motionSet::motionAction(data.oa[parent], J_cols, dAdq_cols); |
| 106 |
1/2✓ Branch 1 taken 109 times.
✗ Branch 2 not taken.
|
218 | dAdv_cols = dJ_cols; |
| 107 |
2/2✓ Branch 0 taken 105 times.
✓ Branch 1 taken 4 times.
|
218 | if (parent > 0) |
| 108 | { | ||
| 109 |
1/2✓ Branch 2 taken 105 times.
✗ Branch 3 not taken.
|
210 | motionSet::motionAction(data.ov[parent], J_cols, dVdq_cols); |
| 110 |
1/2✓ Branch 2 taken 105 times.
✗ Branch 3 not taken.
|
210 | motionSet::motionAction<ADDTO>(data.ov[parent], dVdq_cols, dAdq_cols); |
| 111 |
2/4✓ Branch 1 taken 105 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 105 times.
✗ Branch 5 not taken.
|
210 | dAdv_cols.noalias() += dVdq_cols; |
| 112 | } | ||
| 113 | else | ||
| 114 | { | ||
| 115 |
1/2✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
|
8 | dVdq_cols.setZero(); |
| 116 | } | ||
| 117 | |||
| 118 | // computes variation of inertias | ||
| 119 |
1/2✓ Branch 2 taken 109 times.
✗ Branch 3 not taken.
|
218 | data.doYcrb[i] = data.oYcrb[i].variation(ov); |
| 120 | |||
| 121 |
1/2✓ Branch 3 taken 109 times.
✗ Branch 4 not taken.
|
218 | addForceCrossMatrix(data.oh[i], data.doYcrb[i]); |
| 122 | } | ||
| 123 | |||
| 124 | template<typename ForceDerived, typename M6> | ||
| 125 | static void | ||
| 126 | 109 | addForceCrossMatrix(const ForceDense<ForceDerived> & f, const Eigen::MatrixBase<M6> & mout) | |
| 127 | { | ||
| 128 | 109 | M6 & mout_ = PINOCCHIO_EIGEN_CONST_CAST(M6, mout); | |
| 129 |
2/4✓ Branch 1 taken 109 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 109 times.
✗ Branch 5 not taken.
|
109 | addSkew( |
| 130 |
1/2✓ Branch 2 taken 109 times.
✗ Branch 3 not taken.
|
109 | -f.linear(), mout_.template block<3, 3>(ForceDerived::LINEAR, ForceDerived::ANGULAR)); |
| 131 |
2/4✓ Branch 1 taken 109 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 109 times.
✗ Branch 5 not taken.
|
109 | addSkew( |
| 132 |
1/2✓ Branch 2 taken 109 times.
✗ Branch 3 not taken.
|
109 | -f.linear(), mout_.template block<3, 3>(ForceDerived::ANGULAR, ForceDerived::LINEAR)); |
| 133 |
2/4✓ Branch 1 taken 109 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 109 times.
✗ Branch 5 not taken.
|
109 | addSkew( |
| 134 |
1/2✓ Branch 2 taken 109 times.
✗ Branch 3 not taken.
|
109 | -f.angular(), mout_.template block<3, 3>(ForceDerived::ANGULAR, ForceDerived::ANGULAR)); |
| 135 | 109 | } | |
| 136 | |||
| 137 | }; // struct CentroidalDynDerivativesForwardStep | ||
| 138 | |||
| 139 | template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl> | ||
| 140 | struct CentroidalDynDerivativesBackwardStep | ||
| 141 | : public fusion::JointUnaryVisitorBase< | ||
| 142 | CentroidalDynDerivativesBackwardStep<Scalar, Options, JointCollectionTpl>> | ||
| 143 | { | ||
| 144 | typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model; | ||
| 145 | typedef DataTpl<Scalar, Options, JointCollectionTpl> Data; | ||
| 146 | |||
| 147 | typedef boost::fusion::vector<const Model &, Data &> ArgsType; | ||
| 148 | |||
| 149 | template<typename JointModel> | ||
| 150 | 218 | static void algo(const JointModelBase<JointModel> & jmodel, const Model & model, Data & data) | |
| 151 | { | ||
| 152 | typedef typename Model::JointIndex JointIndex; | ||
| 153 | |||
| 154 |
1/2✓ Branch 1 taken 109 times.
✗ Branch 2 not taken.
|
218 | const JointIndex & i = jmodel.id(); |
| 155 | 218 | const JointIndex & parent = model.parents[i]; | |
| 156 | |||
| 157 | typedef | ||
| 158 | typename SizeDepType<JointModel::NV>::template ColsReturn<typename Data::Matrix6x>::Type | ||
| 159 | ColsBlock; | ||
| 160 | |||
| 161 |
1/2✓ Branch 1 taken 109 times.
✗ Branch 2 not taken.
|
218 | ColsBlock J_cols = jmodel.jointCols(data.J); |
| 162 |
1/2✓ Branch 1 taken 109 times.
✗ Branch 2 not taken.
|
218 | ColsBlock dVdq_cols = jmodel.jointCols(data.dVdq); |
| 163 |
1/2✓ Branch 1 taken 109 times.
✗ Branch 2 not taken.
|
218 | ColsBlock dAdq_cols = jmodel.jointCols(data.dAdq); |
| 164 |
1/2✓ Branch 1 taken 109 times.
✗ Branch 2 not taken.
|
218 | ColsBlock dAdv_cols = jmodel.jointCols(data.dAdv); |
| 165 |
1/2✓ Branch 1 taken 109 times.
✗ Branch 2 not taken.
|
218 | ColsBlock dHdq_cols = jmodel.jointCols(data.dHdq); |
| 166 |
1/2✓ Branch 1 taken 109 times.
✗ Branch 2 not taken.
|
218 | ColsBlock dFdq_cols = jmodel.jointCols(data.dFdq); |
| 167 |
1/2✓ Branch 1 taken 109 times.
✗ Branch 2 not taken.
|
218 | ColsBlock dFdv_cols = jmodel.jointCols(data.dFdv); |
| 168 |
1/2✓ Branch 1 taken 109 times.
✗ Branch 2 not taken.
|
218 | ColsBlock dFda_cols = jmodel.jointCols(data.dFda); |
| 169 | |||
| 170 | // tau | ||
| 171 |
3/6✓ Branch 1 taken 109 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 109 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 109 times.
✗ Branch 8 not taken.
|
218 | jmodel.jointVelocitySelector(data.tau).noalias() = |
| 172 |
3/6✓ Branch 2 taken 109 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 109 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 109 times.
✗ Branch 9 not taken.
|
218 | J_cols.transpose() * data.of[i].toVector(); |
| 173 | |||
| 174 | // dtau/da similar to data.M | ||
| 175 |
1/2✓ Branch 2 taken 109 times.
✗ Branch 3 not taken.
|
218 | motionSet::inertiaAction(data.oYcrb[i], J_cols, dFda_cols); |
| 176 | |||
| 177 | // dtau/dv | ||
| 178 |
3/6✓ Branch 2 taken 109 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 109 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 109 times.
✗ Branch 9 not taken.
|
218 | dFdv_cols.noalias() = data.doYcrb[i] * J_cols; |
| 179 |
1/2✓ Branch 2 taken 109 times.
✗ Branch 3 not taken.
|
218 | motionSet::inertiaAction<ADDTO>(data.oYcrb[i], dAdv_cols, dFdv_cols); |
| 180 | |||
| 181 | // dtau/dq | ||
| 182 |
2/2✓ Branch 0 taken 105 times.
✓ Branch 1 taken 4 times.
|
218 | if (parent > 0) |
| 183 | { | ||
| 184 |
3/6✓ Branch 2 taken 105 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 105 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 105 times.
✗ Branch 9 not taken.
|
210 | dFdq_cols.noalias() = data.doYcrb[i] * dVdq_cols; |
| 185 |
1/2✓ Branch 2 taken 105 times.
✗ Branch 3 not taken.
|
210 | motionSet::inertiaAction<ADDTO>(data.oYcrb[i], dAdq_cols, dFdq_cols); |
| 186 | } | ||
| 187 | else | ||
| 188 |
1/2✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
|
8 | motionSet::inertiaAction(data.oYcrb[i], dAdq_cols, dFdq_cols); |
| 189 | |||
| 190 |
1/2✓ Branch 2 taken 109 times.
✗ Branch 3 not taken.
|
218 | motionSet::act<ADDTO>(J_cols, data.of[i], dFdq_cols); |
| 191 | |||
| 192 |
1/2✓ Branch 3 taken 109 times.
✗ Branch 4 not taken.
|
218 | data.oYcrb[parent] += data.oYcrb[i]; |
| 193 |
1/2✓ Branch 3 taken 109 times.
✗ Branch 4 not taken.
|
218 | data.doYcrb[parent] += data.doYcrb[i]; |
| 194 |
1/2✓ Branch 3 taken 109 times.
✗ Branch 4 not taken.
|
218 | data.oh[parent] += data.oh[i]; |
| 195 |
1/2✓ Branch 3 taken 109 times.
✗ Branch 4 not taken.
|
218 | data.of[parent] += data.of[i]; |
| 196 | |||
| 197 |
1/2✓ Branch 2 taken 109 times.
✗ Branch 3 not taken.
|
218 | motionSet::act(J_cols, data.oh[i], dHdq_cols); |
| 198 |
1/2✓ Branch 2 taken 109 times.
✗ Branch 3 not taken.
|
218 | motionSet::inertiaAction<ADDTO>(data.oYcrb[i], dVdq_cols, dHdq_cols); |
| 199 | } | ||
| 200 | }; // struct CentroidalDynDerivativesBackwardStep | ||
| 201 | } // namespace impl | ||
| 202 | namespace | ||
| 203 | { | ||
| 204 | // TODO: should be moved to ForceSet | ||
| 205 | template<typename Matrix6xLikeIn, typename Vector3Like, typename Matrix6xLikeOut> | ||
| 206 | 52 | void translateForceSet( | |
| 207 | const Eigen::MatrixBase<Matrix6xLikeIn> & Fin, | ||
| 208 | const Eigen::MatrixBase<Vector3Like> & v3, | ||
| 209 | const Eigen::MatrixBase<Matrix6xLikeOut> & Fout) | ||
| 210 | { | ||
| 211 | EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix6xLikeIn, 6, Eigen::Dynamic) | ||
| 212 | EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3Like, 3) | ||
| 213 | EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix6xLikeOut, 6, Eigen::Dynamic) | ||
| 214 | |||
| 215 |
1/24✗ Branch 2 not taken.
✓ Branch 3 taken 28 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.
|
52 | PINOCCHIO_CHECK_ARGUMENT_SIZE( |
| 216 | Fin.cols(), Fout.cols(), "Fin and Fout do not have the same number of columns"); | ||
| 217 | |||
| 218 |
2/2✓ Branch 1 taken 908 times.
✓ Branch 2 taken 28 times.
|
1740 | for (Eigen::DenseIndex k = 0; k < Fin.cols(); ++k) |
| 219 | { | ||
| 220 | typedef ForceRef<typename Matrix6xLikeIn::ColXpr> ForceTypeIn; | ||
| 221 | typedef ForceRef<typename Matrix6xLikeOut::ColXpr> ForceTypeOut; | ||
| 222 |
2/4✓ Branch 2 taken 908 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 908 times.
✗ Branch 6 not taken.
|
1688 | ForceTypeOut fout(PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLikeOut, Fout).col(k)); |
| 223 |
2/4✓ Branch 2 taken 908 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 908 times.
✗ Branch 6 not taken.
|
1688 | const ForceTypeIn fin(PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLikeIn, Fin).col(k)); |
| 224 |
3/6✓ Branch 1 taken 908 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 908 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 908 times.
✗ Branch 8 not taken.
|
1688 | fout.linear() = fin.linear(); |
| 225 |
7/14✓ Branch 1 taken 908 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 908 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 908 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 908 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 908 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 908 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 908 times.
✗ Branch 20 not taken.
|
1688 | fout.angular().noalias() = fin.angular() - v3.cross(fin.linear()); |
| 226 | } | ||
| 227 | 52 | } | |
| 228 | |||
| 229 | template<typename Matrix6xLike, typename Vector3Like, typename Matrix6xLikeOut> | ||
| 230 | void translateForceSet( | ||
| 231 | const Eigen::MatrixBase<Matrix6xLike> & F, const Eigen::MatrixBase<Vector3Like> & v3) | ||
| 232 | { | ||
| 233 | EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix6xLike, 6, Eigen::Dynamic) | ||
| 234 | EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3Like, 3) | ||
| 235 | |||
| 236 | for (Eigen::DenseIndex k = 0; k < F.cols(); ++k) | ||
| 237 | { | ||
| 238 | typedef ForceRef<typename Matrix6xLike::ColXpr> ForceType; | ||
| 239 | ForceType f(PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLike, F).col(k)); | ||
| 240 | f.angular() -= v3.cross(f.linear()); | ||
| 241 | } | ||
| 242 | } | ||
| 243 | } // namespace | ||
| 244 | namespace impl | ||
| 245 | { | ||
| 246 | template< | ||
| 247 | typename Scalar, | ||
| 248 | int Options, | ||
| 249 | template<typename, int> class JointCollectionTpl, | ||
| 250 | typename ConfigVectorType, | ||
| 251 | typename TangentVectorType1, | ||
| 252 | typename TangentVectorType2, | ||
| 253 | typename Matrix6xLike0, | ||
| 254 | typename Matrix6xLike1, | ||
| 255 | typename Matrix6xLike2, | ||
| 256 | typename Matrix6xLike3> | ||
| 257 | 4 | void computeCentroidalDynamicsDerivatives( | |
| 258 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
| 259 | DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
| 260 | const Eigen::MatrixBase<ConfigVectorType> & q, | ||
| 261 | const Eigen::MatrixBase<TangentVectorType1> & v, | ||
| 262 | const Eigen::MatrixBase<TangentVectorType2> & a, | ||
| 263 | const Eigen::MatrixBase<Matrix6xLike0> & dh_dq, | ||
| 264 | const Eigen::MatrixBase<Matrix6xLike1> & dhdot_dq, | ||
| 265 | const Eigen::MatrixBase<Matrix6xLike2> & dhdot_dv, | ||
| 266 | const Eigen::MatrixBase<Matrix6xLike3> & dhdot_da) | ||
| 267 | { | ||
| 268 |
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( |
| 269 | q.size(), model.nq, "The joint configuration vector is not of right size"); | ||
| 270 |
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( |
| 271 | v.size(), model.nv, "The joint velocity vector is not of right size"); | ||
| 272 |
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( |
| 273 | a.size(), model.nv, "The joint acceleration vector is not of right size"); | ||
| 274 |
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(dh_dq.cols(), model.nv); |
| 275 |
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(dh_dq.rows(), 6); |
| 276 |
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(dhdot_dq.cols(), model.nv); |
| 277 |
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(dhdot_dq.rows(), 6); |
| 278 |
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(dhdot_dv.cols(), model.nv); |
| 279 |
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(dhdot_dv.rows(), 6); |
| 280 |
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(dhdot_da.cols(), model.nv); |
| 281 |
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(dhdot_da.rows(), 6); |
| 282 |
1/2✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
|
4 | assert(model.check(data) && "data is not consistent with model."); |
| 283 |
2/4✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 4 times.
✗ Branch 4 not taken.
|
4 | assert(model.check(MimicChecker()) && "Function does not support mimic joints"); |
| 284 | |||
| 285 | typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model; | ||
| 286 | typedef DataTpl<Scalar, Options, JointCollectionTpl> Data; | ||
| 287 | typedef typename Model::JointIndex JointIndex; | ||
| 288 | typedef typename Data::Force Force; | ||
| 289 | |||
| 290 | typedef CentroidalDynDerivativesForwardStep< | ||
| 291 | Scalar, Options, JointCollectionTpl, ConfigVectorType, TangentVectorType1, | ||
| 292 | TangentVectorType2> | ||
| 293 | Pass1; | ||
| 294 |
2/2✓ Branch 0 taken 109 times.
✓ Branch 1 taken 4 times.
|
113 | for (JointIndex i = 1; i < (JointIndex)model.njoints; ++i) |
| 295 | { | ||
| 296 |
1/2✓ Branch 1 taken 109 times.
✗ Branch 2 not taken.
|
109 | Pass1::run( |
| 297 | 109 | model.joints[i], data.joints[i], | |
| 298 | 218 | typename Pass1::ArgsType(model, data, q.derived(), v.derived(), a.derived())); | |
| 299 | } | ||
| 300 | |||
| 301 | 4 | data.oYcrb[0].setZero(); | |
| 302 | 4 | data.oh[0].setZero(); | |
| 303 | 4 | data.of[0].setZero(); | |
| 304 | typedef CentroidalDynDerivativesBackwardStep<Scalar, Options, JointCollectionTpl> Pass2; | ||
| 305 |
2/2✓ Branch 0 taken 109 times.
✓ Branch 1 taken 4 times.
|
113 | for (JointIndex i = (JointIndex)(model.njoints - 1); i > 0; --i) |
| 306 | { | ||
| 307 |
1/2✓ Branch 3 taken 109 times.
✗ Branch 4 not taken.
|
109 | Pass2::run(model.joints[i], typename Pass2::ArgsType(model, data)); |
| 308 | } | ||
| 309 | |||
| 310 | // expressed all the quantities around the center of mass | ||
| 311 | typedef typename Data::Inertia Inertia; | ||
| 312 | |||
| 313 | 4 | const Inertia & Ytot = data.oYcrb[0]; | |
| 314 | 4 | const typename Inertia::Vector3 & com = Ytot.lever(); | |
| 315 | |||
| 316 | // Mass of the system | ||
| 317 |
0/2✗ Branch 3 not taken.
✗ Branch 4 not taken.
|
4 | data.mass[0] = Ytot.mass(); |
| 318 | |||
| 319 | // Center of mass of the system | ||
| 320 | 4 | data.com[0] = Ytot.lever(); | |
| 321 | |||
| 322 | // Compute the centroidal quantities | ||
| 323 | 4 | data.hg = data.oh[0]; | |
| 324 |
3/6✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 4 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 4 times.
✗ Branch 9 not taken.
|
4 | data.hg.angular() += data.hg.linear().cross(com); |
| 325 | |||
| 326 | 4 | data.dhg = data.of[0]; | |
| 327 |
3/6✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 4 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 4 times.
✗ Branch 9 not taken.
|
4 | data.dhg.angular() += data.dhg.linear().cross(com); |
| 328 | |||
| 329 | // Compute centroidal inertia | ||
| 330 |
0/2✗ Branch 3 not taken.
✗ Branch 4 not taken.
|
4 | data.Ig.mass() = Ytot.mass(); |
| 331 | 4 | data.Ig.lever().setZero(); | |
| 332 | 4 | data.Ig.inertia() = Ytot.inertia(); | |
| 333 | |||
| 334 | // Compute the partial derivatives | ||
| 335 | 4 | translateForceSet(data.dHdq, com, dh_dq.const_cast_derived()); | |
| 336 | 4 | Matrix6xLike0 & dh_dq_ = dh_dq.const_cast_derived(); | |
| 337 |
2/2✓ Branch 0 taken 131 times.
✓ Branch 1 taken 4 times.
|
135 | for (Eigen::DenseIndex k = 0; k < model.nv; ++k) |
| 338 |
5/15✓ Branch 1 taken 131 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 131 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✓ Branch 7 taken 131 times.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
✓ Branch 10 taken 131 times.
✗ Branch 11 not taken.
✗ Branch 12 not taken.
✓ Branch 13 taken 131 times.
✗ Branch 14 not taken.
✗ Branch 15 not taken.
|
131 | dh_dq_.col(k).template segment<3>(Force::ANGULAR) += |
| 339 |
3/9✗ Branch 1 not taken.
✓ Branch 2 taken 131 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 131 times.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✓ Branch 8 taken 131 times.
✗ Branch 9 not taken.
|
262 | data.hg.linear().cross(data.dFda.col(k).template segment<3>(Force::LINEAR)) / Ytot.mass(); |
| 340 | |||
| 341 | 4 | translateForceSet(data.dFdq, com, dhdot_dq.const_cast_derived()); | |
| 342 | 4 | Matrix6xLike1 & dhdot_dq_ = dhdot_dq.const_cast_derived(); | |
| 343 |
2/2✓ Branch 0 taken 131 times.
✓ Branch 1 taken 4 times.
|
135 | for (Eigen::DenseIndex k = 0; k < model.nv; ++k) |
| 344 |
5/15✓ Branch 1 taken 131 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 131 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✓ Branch 7 taken 131 times.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
✓ Branch 10 taken 131 times.
✗ Branch 11 not taken.
✗ Branch 12 not taken.
✓ Branch 13 taken 131 times.
✗ Branch 14 not taken.
✗ Branch 15 not taken.
|
131 | dhdot_dq_.col(k).template segment<3>(Force::ANGULAR) += |
| 345 |
2/6✓ Branch 1 taken 131 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 131 times.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
|
131 | data.dhg.linear().cross(data.dFda.col(k).template segment<3>(Force::LINEAR)) |
| 346 |
1/2✓ Branch 2 taken 131 times.
✗ Branch 3 not taken.
|
262 | / Ytot.mass(); |
| 347 | |||
| 348 | 4 | translateForceSet(data.dFdv, com, dhdot_dv.const_cast_derived()); | |
| 349 | |||
| 350 | 4 | translateForceSet(data.dFda, com, data.Ag); | |
| 351 | 4 | dhdot_da.const_cast_derived() = data.Ag; | |
| 352 | 4 | } | |
| 353 | |||
| 354 | template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl> | ||
| 355 | struct GetCentroidalDynDerivativesBackwardStep | ||
| 356 | : public fusion::JointUnaryVisitorBase< | ||
| 357 | GetCentroidalDynDerivativesBackwardStep<Scalar, Options, JointCollectionTpl>> | ||
| 358 | { | ||
| 359 | typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model; | ||
| 360 | typedef DataTpl<Scalar, Options, JointCollectionTpl> Data; | ||
| 361 | |||
| 362 | typedef boost::fusion::vector<const Model &, Data &> ArgsType; | ||
| 363 | |||
| 364 | template<typename JointModel> | ||
| 365 | 162 | static void algo(const JointModelBase<JointModel> & jmodel, const Model & model, Data & data) | |
| 366 | { | ||
| 367 | typedef typename Model::JointIndex JointIndex; | ||
| 368 | typedef typename Data::Vector3 Vector3; | ||
| 369 | typedef | ||
| 370 | typename SizeDepType<JointModel::NV>::template ColsReturn<typename Data::Matrix6x>::Type | ||
| 371 | ColsBlock; | ||
| 372 | |||
| 373 |
1/2✓ Branch 1 taken 81 times.
✗ Branch 2 not taken.
|
162 | const JointIndex & i = jmodel.id(); |
| 374 | 162 | const JointIndex & parent = model.parents[i]; | |
| 375 | |||
| 376 | 162 | typename Data::Motion & vtmp = data.v[0]; | |
| 377 | 162 | typename Data::Matrix6x & Ftmp = data.Fcrb[0]; | |
| 378 | |||
| 379 |
1/2✓ Branch 1 taken 81 times.
✗ Branch 2 not taken.
|
162 | ColsBlock J_cols = jmodel.jointCols(data.J); |
| 380 |
1/2✓ Branch 1 taken 81 times.
✗ Branch 2 not taken.
|
162 | ColsBlock dVdq_cols = jmodel.jointCols(data.dVdq); |
| 381 |
1/2✓ Branch 1 taken 81 times.
✗ Branch 2 not taken.
|
162 | ColsBlock dHdq_cols = jmodel.jointCols(data.dHdq); |
| 382 |
1/2✓ Branch 1 taken 81 times.
✗ Branch 2 not taken.
|
162 | ColsBlock Ftmp_cols = jmodel.jointCols(Ftmp); |
| 383 | |||
| 384 |
3/6✓ Branch 1 taken 81 times.
✗ Branch 2 not taken.
✓ Branch 6 taken 81 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 81 times.
✗ Branch 10 not taken.
|
162 | const Vector3 mg = data.oYcrb[i].mass() * model.gravity.linear(); |
| 385 |
3/4✓ Branch 1 taken 177 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 96 times.
✓ Branch 4 taken 81 times.
|
354 | for (Eigen::DenseIndex k = 0; k < jmodel.nv(); ++k) |
| 386 | { | ||
| 387 |
2/4✓ Branch 1 taken 96 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 96 times.
✗ Branch 5 not taken.
|
192 | MotionRef<typename ColsBlock::ColXpr> mref(J_cols.col(k)); |
| 388 |
6/12✓ Branch 1 taken 96 times.
✗ Branch 2 not taken.
✓ Branch 6 taken 96 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 96 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 96 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 96 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 96 times.
✗ Branch 19 not taken.
|
192 | vtmp.linear() = mref.linear() + mref.angular().cross(data.oYcrb[i].lever()); |
| 389 | |||
| 390 |
2/4✓ Branch 1 taken 96 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 96 times.
✗ Branch 5 not taken.
|
192 | ForceRef<typename ColsBlock::ColXpr> fout(Ftmp_cols.col(k)); |
| 391 |
4/8✓ Branch 1 taken 96 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 96 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 96 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 96 times.
✗ Branch 11 not taken.
|
192 | fout.angular() += vtmp.linear().cross(mg); |
| 392 | } | ||
| 393 | |||
| 394 |
1/2✓ Branch 3 taken 81 times.
✗ Branch 4 not taken.
|
162 | data.oh[parent] += data.oh[i]; |
| 395 |
2/2✓ Branch 0 taken 3 times.
✓ Branch 1 taken 78 times.
|
162 | if (parent == 0) |
| 396 | { | ||
| 397 |
1/2✓ Branch 3 taken 3 times.
✗ Branch 4 not taken.
|
6 | data.of[0] += data.of[i]; |
| 398 |
1/2✓ Branch 3 taken 3 times.
✗ Branch 4 not taken.
|
6 | data.oYcrb[0] += data.oYcrb[i]; |
| 399 | } | ||
| 400 | |||
| 401 |
1/2✓ Branch 2 taken 81 times.
✗ Branch 3 not taken.
|
162 | motionSet::act(J_cols, data.oh[i], dHdq_cols); |
| 402 |
1/2✓ Branch 2 taken 81 times.
✗ Branch 3 not taken.
|
162 | motionSet::inertiaAction<ADDTO>(data.oYcrb[i], dVdq_cols, dHdq_cols); |
| 403 | } | ||
| 404 | }; // struct GetCentroidalDynDerivativesBackwardStep | ||
| 405 | |||
| 406 | template< | ||
| 407 | typename Scalar, | ||
| 408 | int Options, | ||
| 409 | template<typename, int> class JointCollectionTpl, | ||
| 410 | typename Matrix6xLike0, | ||
| 411 | typename Matrix6xLike1, | ||
| 412 | typename Matrix6xLike2, | ||
| 413 | typename Matrix6xLike3> | ||
| 414 | 3 | void getCentroidalDynamicsDerivatives( | |
| 415 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
| 416 | DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
| 417 | const Eigen::MatrixBase<Matrix6xLike0> & dh_dq, | ||
| 418 | const Eigen::MatrixBase<Matrix6xLike1> & dhdot_dq, | ||
| 419 | const Eigen::MatrixBase<Matrix6xLike2> & dhdot_dv, | ||
| 420 | const Eigen::MatrixBase<Matrix6xLike3> & dhdot_da) | ||
| 421 | { | ||
| 422 |
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(dhdot_dq.cols(), model.nv); |
| 423 |
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(dhdot_dq.rows(), 6); |
| 424 |
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(dhdot_dv.cols(), model.nv); |
| 425 |
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(dhdot_dv.rows(), 6); |
| 426 |
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(dhdot_da.cols(), model.nv); |
| 427 |
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(dhdot_da.rows(), 6); |
| 428 |
1/2✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
|
3 | assert(model.check(data) && "data is not consistent with model."); |
| 429 |
2/4✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 3 times.
✗ Branch 4 not taken.
|
3 | assert(model.check(MimicChecker()) && "Function does not support mimic joints"); |
| 430 | |||
| 431 | typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model; | ||
| 432 | typedef DataTpl<Scalar, Options, JointCollectionTpl> Data; | ||
| 433 | typedef typename Model::JointIndex JointIndex; | ||
| 434 | typedef typename Model::Force Force; | ||
| 435 | |||
| 436 | // compute first data.oh[0] and data.of[0] | ||
| 437 | 3 | data.oh[0].setZero(); | |
| 438 | 3 | data.of[0].setZero(); | |
| 439 | 3 | data.oYcrb[0].setZero(); | |
| 440 | |||
| 441 | 3 | typename Data::Matrix6x & Ftmp = data.Fcrb[0]; | |
| 442 | 3 | Ftmp = data.dFdq; | |
| 443 | |||
| 444 | typedef GetCentroidalDynDerivativesBackwardStep<Scalar, Options, JointCollectionTpl> Pass2; | ||
| 445 |
2/2✓ Branch 0 taken 81 times.
✓ Branch 1 taken 3 times.
|
84 | for (JointIndex i = (JointIndex)(model.njoints - 1); i > 0; --i) |
| 446 | { | ||
| 447 |
1/2✓ Branch 3 taken 81 times.
✗ Branch 4 not taken.
|
81 | Pass2::run(model.joints[i], typename Pass2::ArgsType(model, data)); |
| 448 | } | ||
| 449 | |||
| 450 | typedef typename Data::Inertia Inertia; | ||
| 451 | |||
| 452 | 3 | const Inertia & Ytot = data.oYcrb[0]; | |
| 453 | 3 | const typename Inertia::Vector3 & com = Ytot.lever(); | |
| 454 | |||
| 455 | // Center of mass of the system | ||
| 456 | 3 | data.com[0] = com; | |
| 457 |
0/2✗ Branch 3 not taken.
✗ Branch 4 not taken.
|
3 | data.mass[0] = Ytot.mass(); |
| 458 | |||
| 459 | // Remove the gravity contribution | ||
| 460 |
1/2✓ Branch 3 taken 3 times.
✗ Branch 4 not taken.
|
3 | data.of[0] += Ytot * model.gravity; |
| 461 | |||
| 462 | // Compute the centroidal quantities | ||
| 463 | 3 | data.hg = data.oh[0]; | |
| 464 |
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.hg.angular() += data.hg.linear().cross(com); |
| 465 | |||
| 466 | 3 | data.dhg = data.of[0]; | |
| 467 |
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.dhg.angular() += data.dhg.linear().cross(com); |
| 468 | |||
| 469 | // Compute centroidal inertia | ||
| 470 |
0/2✗ Branch 3 not taken.
✗ Branch 4 not taken.
|
3 | data.Ig.mass() = Ytot.mass(); |
| 471 | 3 | data.Ig.lever().setZero(); | |
| 472 | 3 | data.Ig.inertia() = Ytot.inertia(); | |
| 473 | |||
| 474 | // Retrieve the partial derivatives from RNEA derivatives | ||
| 475 | 3 | translateForceSet(data.dHdq, com, dh_dq.const_cast_derived()); | |
| 476 | 3 | Matrix6xLike0 & dh_dq_ = dh_dq.const_cast_derived(); | |
| 477 |
2/2✓ Branch 0 taken 96 times.
✓ Branch 1 taken 3 times.
|
99 | for (Eigen::DenseIndex k = 0; k < model.nv; ++k) |
| 478 |
5/15✓ Branch 1 taken 96 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 96 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✓ Branch 7 taken 96 times.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
✓ Branch 10 taken 96 times.
✗ Branch 11 not taken.
✗ Branch 12 not taken.
✓ Branch 13 taken 96 times.
✗ Branch 14 not taken.
✗ Branch 15 not taken.
|
96 | dh_dq_.col(k).template segment<3>(Force::ANGULAR) += |
| 479 |
3/9✗ Branch 1 not taken.
✓ Branch 2 taken 96 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 96 times.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✓ Branch 8 taken 96 times.
✗ Branch 9 not taken.
|
192 | data.hg.linear().cross(data.dFda.col(k).template segment<3>(Force::LINEAR)) / Ytot.mass(); |
| 480 | |||
| 481 | 3 | translateForceSet(Ftmp, com, dhdot_dq.const_cast_derived()); | |
| 482 | 3 | Matrix6xLike1 & dhdot_dq_ = dhdot_dq.const_cast_derived(); | |
| 483 |
2/2✓ Branch 0 taken 96 times.
✓ Branch 1 taken 3 times.
|
99 | for (Eigen::DenseIndex k = 0; k < model.nv; ++k) |
| 484 |
5/15✓ Branch 1 taken 96 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 96 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✓ Branch 7 taken 96 times.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
✓ Branch 10 taken 96 times.
✗ Branch 11 not taken.
✗ Branch 12 not taken.
✓ Branch 13 taken 96 times.
✗ Branch 14 not taken.
✗ Branch 15 not taken.
|
96 | dhdot_dq_.col(k).template segment<3>(Force::ANGULAR) += |
| 485 |
2/6✓ Branch 1 taken 96 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 96 times.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
|
96 | data.dhg.linear().cross(data.dFda.col(k).template segment<3>(Force::LINEAR)) |
| 486 |
1/2✓ Branch 2 taken 96 times.
✗ Branch 3 not taken.
|
192 | / Ytot.mass(); |
| 487 | |||
| 488 | 3 | translateForceSet(data.dFdv, com, dhdot_dv.const_cast_derived()); | |
| 489 | |||
| 490 | 3 | translateForceSet(data.dFda, com, data.Ag); | |
| 491 | 3 | dhdot_da.const_cast_derived() = data.Ag; | |
| 492 | 3 | } | |
| 493 | } // namespace impl | ||
| 494 | |||
| 495 | template< | ||
| 496 | typename Scalar, | ||
| 497 | int Options, | ||
| 498 | template<typename, int> class JointCollectionTpl, | ||
| 499 | typename ConfigVectorType, | ||
| 500 | typename TangentVectorType1, | ||
| 501 | typename TangentVectorType2, | ||
| 502 | typename Matrix6xLike0, | ||
| 503 | typename Matrix6xLike1, | ||
| 504 | typename Matrix6xLike2, | ||
| 505 | typename Matrix6xLike3> | ||
| 506 | 4 | void computeCentroidalDynamicsDerivatives( | |
| 507 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
| 508 | DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
| 509 | const Eigen::MatrixBase<ConfigVectorType> & q, | ||
| 510 | const Eigen::MatrixBase<TangentVectorType1> & v, | ||
| 511 | const Eigen::MatrixBase<TangentVectorType2> & a, | ||
| 512 | const Eigen::MatrixBase<Matrix6xLike0> & dh_dq, | ||
| 513 | const Eigen::MatrixBase<Matrix6xLike1> & dhdot_dq, | ||
| 514 | const Eigen::MatrixBase<Matrix6xLike2> & dhdot_dv, | ||
| 515 | const Eigen::MatrixBase<Matrix6xLike3> & dhdot_da) | ||
| 516 | { | ||
| 517 |
7/14✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 4 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 4 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 4 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 4 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 4 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 4 times.
✗ Branch 21 not taken.
|
4 | impl::computeCentroidalDynamicsDerivatives( |
| 518 | model, data, make_const_ref(q), make_const_ref(v), make_const_ref(a), make_ref(dh_dq), | ||
| 519 | make_ref(dhdot_dq), make_ref(dhdot_dv), make_ref(dhdot_da)); | ||
| 520 | 4 | } | |
| 521 | |||
| 522 | template< | ||
| 523 | typename Scalar, | ||
| 524 | int Options, | ||
| 525 | template<typename, int> class JointCollectionTpl, | ||
| 526 | typename Matrix6xLike0, | ||
| 527 | typename Matrix6xLike1, | ||
| 528 | typename Matrix6xLike2, | ||
| 529 | typename Matrix6xLike3> | ||
| 530 | 2 | void getCentroidalDynamicsDerivatives( | |
| 531 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
| 532 | DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
| 533 | const Eigen::MatrixBase<Matrix6xLike0> & dh_dq, | ||
| 534 | const Eigen::MatrixBase<Matrix6xLike1> & dhdot_dq, | ||
| 535 | const Eigen::MatrixBase<Matrix6xLike2> & dhdot_dv, | ||
| 536 | const Eigen::MatrixBase<Matrix6xLike3> & dhdot_da) | ||
| 537 | { | ||
| 538 |
4/8✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 2 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 2 times.
✗ Branch 12 not taken.
|
2 | impl::getCentroidalDynamicsDerivatives( |
| 539 | model, data, make_ref(dh_dq), make_ref(dhdot_dq), make_ref(dhdot_dv), make_ref(dhdot_da)); | ||
| 540 | 2 | } | |
| 541 | |||
| 542 | } // namespace pinocchio | ||
| 543 | |||
| 544 | /// @endcond | ||
| 545 | |||
| 546 | #endif // ifndef __pinocchio_algorithm_centroidal_derivatives_hxx__ | ||
| 547 |