GCC Code Coverage Report

Directory: ./
File: include/pinocchio/algorithm/centroidal-derivatives.hxx
Date: 2025-02-12 21:03:38
Exec Total Coverage
Lines: 198 198 100.0%
Branches: 249 901 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> 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/9
✓ 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 11 taken 105 times.
✗ Branch 12 not taken.
 436 data.a[i] =
81
7/14
✓ Branch 1 taken 109 times.
✗ Branch 2 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
284 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
285 typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
286 typedef typename Model::JointIndex JointIndex;
287 typedef typename Data::Force Force;
288
289 typedef CentroidalDynDerivativesForwardStep<
290 Scalar, Options, JointCollectionTpl, ConfigVectorType, TangentVectorType1,
291 TangentVectorType2>
292 Pass1;
293
2/2
✓ Branch 0 taken 109 times.
✓ Branch 1 taken 4 times.
 113 for (JointIndex i = 1; i < (JointIndex)model.njoints; ++i)
294 {
295
1/2
✓ Branch 1 taken 109 times.
✗ Branch 2 not taken.
 109 Pass1::run(
296 109 model.joints[i], data.joints[i],
297 218 typename Pass1::ArgsType(model, data, q.derived(), v.derived(), a.derived()));
298 }
299
300 4 data.oYcrb[0].setZero();
301 4 data.oh[0].setZero();
302 4 data.of[0].setZero();
303 typedef CentroidalDynDerivativesBackwardStep<Scalar, Options, JointCollectionTpl> Pass2;
304
2/2
✓ Branch 0 taken 109 times.
✓ Branch 1 taken 4 times.
 113 for (JointIndex i = (JointIndex)(model.njoints - 1); i > 0; --i)
305 {
306
1/2
✓ Branch 3 taken 109 times.
✗ Branch 4 not taken.
 109 Pass2::run(model.joints[i], typename Pass2::ArgsType(model, data));
307 }
308
309 // expressed all the quantities around the center of mass
310 typedef typename Data::Inertia Inertia;
311
312 4 const Inertia & Ytot = data.oYcrb[0];
313 4 const typename Inertia::Vector3 & com = Ytot.lever();
314
315 // Mass of the system
316
0/2
✗ Branch 3 not taken.
✗ Branch 4 not taken.
 4 data.mass[0] = Ytot.mass();
317
318 // Center of mass of the system
319 4 data.com[0] = Ytot.lever();
320
321 // Compute the centroidal quantities
322 4 data.hg = data.oh[0];
323
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);
324
325 4 data.dhg = data.of[0];
326
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);
327
328 // Compute centroidal inertia
329
0/2
✗ Branch 3 not taken.
✗ Branch 4 not taken.
 4 data.Ig.mass() = Ytot.mass();
330 4 data.Ig.lever().setZero();
331 4 data.Ig.inertia() = Ytot.inertia();
332
333 // Compute the partial derivatives
334 4 translateForceSet(data.dHdq, com, dh_dq.const_cast_derived());
335 4 Matrix6xLike0 & dh_dq_ = dh_dq.const_cast_derived();
336
2/2
✓ Branch 0 taken 131 times.
✓ Branch 1 taken 4 times.
 135 for (Eigen::DenseIndex k = 0; k < model.nv; ++k)
337
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) +=
338
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();
339
340 4 translateForceSet(data.dFdq, com, dhdot_dq.const_cast_derived());
341 4 Matrix6xLike1 & dhdot_dq_ = dhdot_dq.const_cast_derived();
342
2/2
✓ Branch 0 taken 131 times.
✓ Branch 1 taken 4 times.
 135 for (Eigen::DenseIndex k = 0; k < model.nv; ++k)
343
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) +=
344
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))
345
1/2
✓ Branch 2 taken 131 times.
✗ Branch 3 not taken.
 262 / Ytot.mass();
346
347 4 translateForceSet(data.dFdv, com, dhdot_dv.const_cast_derived());
348
349 4 translateForceSet(data.dFda, com, data.Ag);
350 4 dhdot_da.const_cast_derived() = data.Ag;
351 4 }
352
353 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
354 struct GetCentroidalDynDerivativesBackwardStep
355 : public fusion::JointUnaryVisitorBase<
356 GetCentroidalDynDerivativesBackwardStep<Scalar, Options, JointCollectionTpl>>
357 {
358 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
359 typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
360
361 typedef boost::fusion::vector<const Model &, Data &> ArgsType;
362
363 template<typename JointModel>
364 162 static void algo(const JointModelBase<JointModel> & jmodel, const Model & model, Data & data)
365 {
366 typedef typename Model::JointIndex JointIndex;
367 typedef typename Data::Vector3 Vector3;
368 typedef
369 typename SizeDepType<JointModel::NV>::template ColsReturn<typename Data::Matrix6x>::Type
370 ColsBlock;
371
372
1/2
✓ Branch 1 taken 81 times.
✗ Branch 2 not taken.
 162 const JointIndex & i = jmodel.id();
373 162 const JointIndex & parent = model.parents[i];
374
375 162 typename Data::Motion & vtmp = data.v[0];
376 162 typename Data::Matrix6x & Ftmp = data.Fcrb[0];
377
378
1/2
✓ Branch 1 taken 81 times.
✗ Branch 2 not taken.
 162 ColsBlock J_cols = jmodel.jointCols(data.J);
379
1/2
✓ Branch 1 taken 81 times.
✗ Branch 2 not taken.
 162 ColsBlock dVdq_cols = jmodel.jointCols(data.dVdq);
380
1/2
✓ Branch 1 taken 81 times.
✗ Branch 2 not taken.
 162 ColsBlock dHdq_cols = jmodel.jointCols(data.dHdq);
381
1/2
✓ Branch 1 taken 81 times.
✗ Branch 2 not taken.
 162 ColsBlock Ftmp_cols = jmodel.jointCols(Ftmp);
382
383
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();
384
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)
385 {
386
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));
387
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());
388
389
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));
390
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);
391 }
392
393
1/2
✓ Branch 3 taken 81 times.
✗ Branch 4 not taken.
 162 data.oh[parent] += data.oh[i];
394
2/2
✓ Branch 0 taken 3 times.
✓ Branch 1 taken 78 times.
 162 if (parent == 0)
395 {
396
1/2
✓ Branch 3 taken 3 times.
✗ Branch 4 not taken.
 6 data.of[0] += data.of[i];
397
1/2
✓ Branch 3 taken 3 times.
✗ Branch 4 not taken.
 6 data.oYcrb[0] += data.oYcrb[i];
398 }
399
400
1/2
✓ Branch 2 taken 81 times.
✗ Branch 3 not taken.
 162 motionSet::act(J_cols, data.oh[i], dHdq_cols);
401
1/2
✓ Branch 2 taken 81 times.
✗ Branch 3 not taken.
 162 motionSet::inertiaAction<ADDTO>(data.oYcrb[i], dVdq_cols, dHdq_cols);
402 }
403 }; // struct GetCentroidalDynDerivativesBackwardStep
404
405 template<
406 typename Scalar,
407 int Options,
408 template<typename, int> class JointCollectionTpl,
409 typename Matrix6xLike0,
410 typename Matrix6xLike1,
411 typename Matrix6xLike2,
412 typename Matrix6xLike3>
413 3 void getCentroidalDynamicsDerivatives(
414 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
415 DataTpl<Scalar, Options, JointCollectionTpl> & data,
416 const Eigen::MatrixBase<Matrix6xLike0> & dh_dq,
417 const Eigen::MatrixBase<Matrix6xLike1> & dhdot_dq,
418 const Eigen::MatrixBase<Matrix6xLike2> & dhdot_dv,
419 const Eigen::MatrixBase<Matrix6xLike3> & dhdot_da)
420 {
421
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);
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.rows(), 6);
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_dv.cols(), model.nv);
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.rows(), 6);
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_da.cols(), model.nv);
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.rows(), 6);
427
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 assert(model.check(data) && "data is not consistent with model.");
428
429 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
430 typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
431 typedef typename Model::JointIndex JointIndex;
432 typedef typename Model::Force Force;
433
434 // compute first data.oh[0] and data.of[0]
435 3 data.oh[0].setZero();
436 3 data.of[0].setZero();
437 3 data.oYcrb[0].setZero();
438
439 3 typename Data::Matrix6x & Ftmp = data.Fcrb[0];
440 3 Ftmp = data.dFdq;
441
442 typedef GetCentroidalDynDerivativesBackwardStep<Scalar, Options, JointCollectionTpl> Pass2;
443
2/2
✓ Branch 0 taken 81 times.
✓ Branch 1 taken 3 times.
 84 for (JointIndex i = (JointIndex)(model.njoints - 1); i > 0; --i)
444 {
445
1/2
✓ Branch 3 taken 81 times.
✗ Branch 4 not taken.
 81 Pass2::run(model.joints[i], typename Pass2::ArgsType(model, data));
446 }
447
448 typedef typename Data::Inertia Inertia;
449
450 3 const Inertia & Ytot = data.oYcrb[0];
451 3 const typename Inertia::Vector3 & com = Ytot.lever();
452
453 // Center of mass of the system
454 3 data.com[0] = com;
455
0/2
✗ Branch 3 not taken.
✗ Branch 4 not taken.
 3 data.mass[0] = Ytot.mass();
456
457 // Remove the gravity contribution
458
1/2
✓ Branch 3 taken 3 times.
✗ Branch 4 not taken.
 3 data.of[0] += Ytot * model.gravity;
459
460 // Compute the centroidal quantities
461 3 data.hg = data.oh[0];
462
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);
463
464 3 data.dhg = data.of[0];
465
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);
466
467 // Compute centroidal inertia
468
0/2
✗ Branch 3 not taken.
✗ Branch 4 not taken.
 3 data.Ig.mass() = Ytot.mass();
469 3 data.Ig.lever().setZero();
470 3 data.Ig.inertia() = Ytot.inertia();
471
472 // Retrieve the partial derivatives from RNEA derivatives
473 3 translateForceSet(data.dHdq, com, dh_dq.const_cast_derived());
474 3 Matrix6xLike0 & dh_dq_ = dh_dq.const_cast_derived();
475
2/2
✓ Branch 0 taken 96 times.
✓ Branch 1 taken 3 times.
 99 for (Eigen::DenseIndex k = 0; k < model.nv; ++k)
476
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) +=
477
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();
478
479 3 translateForceSet(Ftmp, com, dhdot_dq.const_cast_derived());
480 3 Matrix6xLike1 & dhdot_dq_ = dhdot_dq.const_cast_derived();
481
2/2
✓ Branch 0 taken 96 times.
✓ Branch 1 taken 3 times.
 99 for (Eigen::DenseIndex k = 0; k < model.nv; ++k)
482
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) +=
483
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))
484
1/2
✓ Branch 2 taken 96 times.
✗ Branch 3 not taken.
 192 / Ytot.mass();
485
486 3 translateForceSet(data.dFdv, com, dhdot_dv.const_cast_derived());
487
488 3 translateForceSet(data.dFda, com, data.Ag);
489 3 dhdot_da.const_cast_derived() = data.Ag;
490 3 }
491 } // namespace impl
492
493 template<
494 typename Scalar,
495 int Options,
496 template<typename, int> class JointCollectionTpl,
497 typename ConfigVectorType,
498 typename TangentVectorType1,
499 typename TangentVectorType2,
500 typename Matrix6xLike0,
501 typename Matrix6xLike1,
502 typename Matrix6xLike2,
503 typename Matrix6xLike3>
504 4 void computeCentroidalDynamicsDerivatives(
505 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
506 DataTpl<Scalar, Options, JointCollectionTpl> & data,
507 const Eigen::MatrixBase<ConfigVectorType> & q,
508 const Eigen::MatrixBase<TangentVectorType1> & v,
509 const Eigen::MatrixBase<TangentVectorType2> & a,
510 const Eigen::MatrixBase<Matrix6xLike0> & dh_dq,
511 const Eigen::MatrixBase<Matrix6xLike1> & dhdot_dq,
512 const Eigen::MatrixBase<Matrix6xLike2> & dhdot_dv,
513 const Eigen::MatrixBase<Matrix6xLike3> & dhdot_da)
514 {
515
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(
516 model, data, make_const_ref(q), make_const_ref(v), make_const_ref(a), make_ref(dh_dq),
517 make_ref(dhdot_dq), make_ref(dhdot_dv), make_ref(dhdot_da));
518 4 }
519
520 template<
521 typename Scalar,
522 int Options,
523 template<typename, int> class JointCollectionTpl,
524 typename Matrix6xLike0,
525 typename Matrix6xLike1,
526 typename Matrix6xLike2,
527 typename Matrix6xLike3>
528 2 void getCentroidalDynamicsDerivatives(
529 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
530 DataTpl<Scalar, Options, JointCollectionTpl> & data,
531 const Eigen::MatrixBase<Matrix6xLike0> & dh_dq,
532 const Eigen::MatrixBase<Matrix6xLike1> & dhdot_dq,
533 const Eigen::MatrixBase<Matrix6xLike2> & dhdot_dv,
534 const Eigen::MatrixBase<Matrix6xLike3> & dhdot_da)
535 {
536
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(
537 model, data, make_ref(dh_dq), make_ref(dhdot_dq), make_ref(dhdot_dv), make_ref(dhdot_da));
538 2 }
539
540 } // namespace pinocchio
541
542 /// @endcond
543
544 #endif // ifndef __pinocchio_algorithm_centroidal_derivatives_hxx__
545