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