GCC Code Coverage Report

Directory: ./
File: include/pinocchio/algorithm/rnea-derivatives.hxx
Date: 2025-02-12 21:03:38
Exec Total Coverage
Lines: 246 253 97.2%
Branches: 345 1223 28.2%
Line Branch Exec Source
1 //
2 // Copyright (c) 2017-2021 CNRS INRIA
3 //
4
5 #ifndef __pinocchio_algorithm_rnea_derivatives_hxx__
6 #define __pinocchio_algorithm_rnea_derivatives_hxx__
7
8 #include "pinocchio/multibody/visitor.hpp"
9 #include "pinocchio/algorithm/check.hpp"
10
11 namespace pinocchio
12 {
13 namespace impl
14 {
15 template<
16 typename Scalar,
17 int Options,
18 template<typename, int> class JointCollectionTpl,
19 typename ConfigVectorType>
20 struct ComputeGeneralizedGravityDerivativeForwardStep
21 : public fusion::JointUnaryVisitorBase<ComputeGeneralizedGravityDerivativeForwardStep<
22 Scalar,
23 Options,
24 JointCollectionTpl,
25 ConfigVectorType>>
26 {
27 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
28 typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
29
30 typedef boost::fusion::vector<const Model &, Data &, const ConfigVectorType &> ArgsType;
31
32 template<typename JointModel>
33 270 static void algo(
34 const JointModelBase<JointModel> & jmodel,
35 JointDataBase<typename JointModel::JointDataDerived> & jdata,
36 const Model & model,
37 Data & data,
38 const Eigen::MatrixBase<ConfigVectorType> & q)
39 {
40 typedef typename Model::JointIndex JointIndex;
41 typedef typename Data::Motion Motion;
42
43
1/2
✓ Branch 1 taken 135 times.
✗ Branch 2 not taken.
 270 const JointIndex i = jmodel.id();
44 270 const JointIndex parent = model.parents[i];
45 270 const Motion & minus_gravity = data.oa_gf[0];
46
47
1/2
✓ Branch 3 taken 135 times.
✗ Branch 4 not taken.
 270 jmodel.calc(jdata.derived(), q.derived());
48
49
6/10
✓ Branch 1 taken 135 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 130 times.
✓ Branch 5 taken 5 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 130 times.
✓ Branch 9 taken 5 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 130 times.
✗ Branch 13 not taken.
 270 data.liMi[i] = model.jointPlacements[i] * jdata.M();
50
51
2/2
✓ Branch 0 taken 130 times.
✓ Branch 1 taken 5 times.
 270 if (parent > 0)
52
2/4
✓ Branch 3 taken 130 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 130 times.
✗ Branch 8 not taken.
 260 data.oMi[i] = data.oMi[parent] * data.liMi[i];
53 else
54
1/2
✓ Branch 3 taken 5 times.
✗ Branch 4 not taken.
 10 data.oMi[i] = data.liMi[i];
55
56
3/6
✓ Branch 3 taken 135 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 135 times.
✗ Branch 8 not taken.
✓ Branch 11 taken 135 times.
✗ Branch 12 not taken.
 270 data.oYcrb[i] = data.oinertias[i] = data.oMi[i].act(model.inertias[i]);
57
2/4
✓ Branch 2 taken 135 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 135 times.
✗ Branch 7 not taken.
 270 data.of[i] = data.oYcrb[i] * minus_gravity;
58
59 typedef
60 typename SizeDepType<JointModel::NV>::template ColsReturn<typename Data::Matrix6x>::Type
61 ColsBlock;
62
1/2
✓ Branch 1 taken 135 times.
✗ Branch 2 not taken.
 270 ColsBlock J_cols = jmodel.jointCols(data.J);
63
1/2
✓ Branch 1 taken 135 times.
✗ Branch 2 not taken.
 270 ColsBlock dAdq_cols = jmodel.jointCols(data.dAdq);
64
3/6
✓ Branch 2 taken 135 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 135 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 135 times.
✗ Branch 9 not taken.
 270 J_cols = data.oMi[i].act(jdata.S());
65
1/2
✓ Branch 1 taken 135 times.
✗ Branch 2 not taken.
 270 motionSet::motionAction(minus_gravity, J_cols, dAdq_cols);
66 }
67 };
68
69 template<
70 typename Scalar,
71 int Options,
72 template<typename, int> class JointCollectionTpl,
73 typename ReturnMatrixType>
74 struct ComputeGeneralizedGravityDerivativeBackwardStep
75 : public fusion::JointUnaryVisitorBase<ComputeGeneralizedGravityDerivativeBackwardStep<
76 Scalar,
77 Options,
78 JointCollectionTpl,
79 ReturnMatrixType>>
80 {
81 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
82 typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
83
84 typedef boost::fusion::
85 vector<const Model &, Data &, typename Data::VectorXs &, ReturnMatrixType &>
86 ArgsType;
87
88 template<typename JointModel>
89 270 static void algo(
90 const JointModelBase<JointModel> & jmodel,
91 const Model & model,
92 Data & data,
93 typename Data::VectorXs & g,
94 const Eigen::MatrixBase<ReturnMatrixType> & gravity_partial_dq)
95 {
96 typedef typename Model::JointIndex JointIndex;
97 typedef Eigen::Matrix<
98 Scalar, JointModel::NV, 6, Options, JointModel::NV == Eigen::Dynamic ? 6 : JointModel::NV,
99 6>
100 MatrixNV6;
101
102
1/2
✓ Branch 1 taken 135 times.
✗ Branch 2 not taken.
 270 const JointIndex i = jmodel.id();
103 270 const JointIndex parent = model.parents[i];
104
105
2/4
✓ Branch 1 taken 135 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 135 times.
✗ Branch 5 not taken.
 270 typename PINOCCHIO_EIGEN_PLAIN_ROW_MAJOR_TYPE(MatrixNV6) YS(jmodel.nv(), 6);
106
107 typedef
108 typename SizeDepType<JointModel::NV>::template ColsReturn<typename Data::Matrix6x>::Type
109 ColsBlock;
110
111
1/2
✓ Branch 1 taken 135 times.
✗ Branch 2 not taken.
 270 ColsBlock J_cols = jmodel.jointCols(data.J);
112
1/2
✓ Branch 1 taken 135 times.
✗ Branch 2 not taken.
 270 ColsBlock dAdq_cols = jmodel.jointCols(data.dAdq);
113
1/2
✓ Branch 1 taken 135 times.
✗ Branch 2 not taken.
 270 ColsBlock dFdq_cols = jmodel.jointCols(data.dFdq);
114
1/2
✓ Branch 1 taken 135 times.
✗ Branch 2 not taken.
 270 ColsBlock Ag_cols = jmodel.jointCols(data.Ag);
115
116
1/2
✓ Branch 2 taken 135 times.
✗ Branch 3 not taken.
 270 motionSet::inertiaAction(data.oYcrb[i], dAdq_cols, dFdq_cols);
117
118 ReturnMatrixType & gravity_partial_dq_ =
119 270 PINOCCHIO_EIGEN_CONST_CAST(ReturnMatrixType, gravity_partial_dq);
120
4/8
✓ Branch 2 taken 135 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 135 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 135 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 135 times.
✗ Branch 12 not taken.
 270 gravity_partial_dq_.block(jmodel.idx_v(), jmodel.idx_v(), jmodel.nv(), data.nvSubtree[i])
121
6/12
✓ Branch 2 taken 135 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 135 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 135 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 135 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 135 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 135 times.
✗ Branch 18 not taken.
 540 .noalias() = J_cols.transpose() * data.dFdq.middleCols(jmodel.idx_v(), data.nvSubtree[i]);
122
123
1/2
✓ Branch 2 taken 135 times.
✗ Branch 3 not taken.
 270 motionSet::act<ADDTO>(J_cols, data.of[i], dFdq_cols);
124
125
1/2
✓ Branch 2 taken 135 times.
✗ Branch 3 not taken.
 270 motionSet::inertiaAction(data.oYcrb[i], J_cols, Ag_cols);
126
3/4
✓ Branch 1 taken 135 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1205 times.
✓ Branch 5 taken 135 times.
 2680 for (int j = data.parents_fromRow[(typename Model::Index)jmodel.idx_v()]; j >= 0;
127 2410 j = data.parents_fromRow[(typename Model::Index)j])
128
6/12
✓ Branch 1 taken 1205 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1205 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1205 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1205 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1205 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1205 times.
✗ Branch 17 not taken.
 2410 gravity_partial_dq_.middleRows(jmodel.idx_v(), jmodel.nv()).col(j).noalias() =
129
3/6
✓ Branch 1 taken 1205 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1205 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1205 times.
✗ Branch 8 not taken.
 4820 Ag_cols.transpose() * data.dAdq.col(j);
130
131
6/12
✓ Branch 2 taken 135 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 135 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 135 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 135 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 135 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 135 times.
✗ Branch 18 not taken.
 270 jmodel.jointVelocitySelector(g).noalias() = J_cols.transpose() * data.of[i].toVector();
132
2/2
✓ Branch 0 taken 130 times.
✓ Branch 1 taken 5 times.
 270 if (parent > 0)
133 {
134
1/2
✓ Branch 3 taken 130 times.
✗ Branch 4 not taken.
 260 data.oYcrb[parent] += data.oYcrb[i];
135
1/2
✓ Branch 3 taken 130 times.
✗ Branch 4 not taken.
 260 data.of[parent] += data.of[i];
136 }
137 }
138 };
139
140 template<
141 typename Scalar,
142 int Options,
143 template<typename, int> class JointCollectionTpl,
144 typename ConfigVectorType,
145 typename ReturnMatrixType>
146 3 void computeGeneralizedGravityDerivatives(
147 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
148 DataTpl<Scalar, Options, JointCollectionTpl> & data,
149 const Eigen::MatrixBase<ConfigVectorType> & q,
150 const Eigen::MatrixBase<ReturnMatrixType> & gravity_partial_dq)
151 {
152
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(
153 q.size(), model.nq, "The configuration vector is not of right size");
154
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(gravity_partial_dq.cols(), model.nv);
155
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(gravity_partial_dq.rows(), model.nv);
156
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 assert(model.check(data) && "data is not consistent with model.");
157
158 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
159 typedef typename Model::JointIndex JointIndex;
160
161
1/2
✓ Branch 3 taken 3 times.
✗ Branch 4 not taken.
 3 data.oa_gf[0] = -model.gravity; // minus_gravity used in the two Passes
162
163 typedef ComputeGeneralizedGravityDerivativeForwardStep<
164 Scalar, Options, JointCollectionTpl, ConfigVectorType>
165 Pass1;
166
2/2
✓ Branch 0 taken 81 times.
✓ Branch 1 taken 3 times.
 84 for (JointIndex i = 1; i < (JointIndex)model.njoints; ++i)
167 {
168
1/2
✓ Branch 1 taken 81 times.
✗ Branch 2 not taken.
 81 Pass1::run(
169 162 model.joints[i], data.joints[i], typename Pass1::ArgsType(model, data, q.derived()));
170 }
171
172 typedef ComputeGeneralizedGravityDerivativeBackwardStep<
173 Scalar, Options, JointCollectionTpl, ReturnMatrixType>
174 Pass2;
175 ReturnMatrixType & gravity_partial_dq_ =
176 3 PINOCCHIO_EIGEN_CONST_CAST(ReturnMatrixType, gravity_partial_dq);
177
2/2
✓ Branch 0 taken 81 times.
✓ Branch 1 taken 3 times.
 84 for (JointIndex i = (JointIndex)(model.njoints - 1); i > 0; --i)
178 {
179
1/2
✓ Branch 1 taken 81 times.
✗ Branch 2 not taken.
 81 Pass2::run(
180 162 model.joints[i], typename Pass2::ArgsType(model, data, data.g, gravity_partial_dq_));
181 }
182 3 }
183
184 template<
185 typename Scalar,
186 int Options,
187 template<typename, int> class JointCollectionTpl,
188 typename ConfigVectorType,
189 typename ReturnMatrixType>
190 2 void computeStaticTorqueDerivatives(
191 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
192 DataTpl<Scalar, Options, JointCollectionTpl> & data,
193 const Eigen::MatrixBase<ConfigVectorType> & q,
194 const container::aligned_vector<ForceTpl<Scalar, Options>> & fext,
195 const Eigen::MatrixBase<ReturnMatrixType> & static_torque_partial_dq)
196 {
197
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(
198 q.size(), model.nq, "The configuration vector is not of right size");
199
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(static_torque_partial_dq.cols(), model.nv);
200
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(static_torque_partial_dq.rows(), model.nv);
201
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(
202 fext.size(), (size_t)model.njoints, "The size of the external forces is not of right size");
203
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 assert(model.check(data) && "data is not consistent with model.");
204
205 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
206 typedef typename Model::JointIndex JointIndex;
207
208
1/2
✓ Branch 3 taken 2 times.
✗ Branch 4 not taken.
 2 data.oa_gf[0] = -model.gravity; // minus_gravity used in the two Passes
209
210 typedef ComputeGeneralizedGravityDerivativeForwardStep<
211 Scalar, Options, JointCollectionTpl, ConfigVectorType>
212 Pass1;
213
2/2
✓ Branch 0 taken 54 times.
✓ Branch 1 taken 2 times.
 56 for (JointIndex i = 1; i < (JointIndex)model.njoints; ++i)
214 {
215
1/2
✓ Branch 1 taken 54 times.
✗ Branch 2 not taken.
 54 Pass1::run(
216 54 model.joints[i], data.joints[i], typename Pass1::ArgsType(model, data, q.derived()));
217
1/2
✓ Branch 5 taken 54 times.
✗ Branch 6 not taken.
 54 data.of[i] -= data.oMi[i].act(fext[i]);
218 }
219
220 typedef ComputeGeneralizedGravityDerivativeBackwardStep<
221 Scalar, Options, JointCollectionTpl, ReturnMatrixType>
222 Pass2;
223 ReturnMatrixType & static_torque_partial_dq_ =
224 2 PINOCCHIO_EIGEN_CONST_CAST(ReturnMatrixType, static_torque_partial_dq);
225
2/2
✓ Branch 0 taken 54 times.
✓ Branch 1 taken 2 times.
 56 for (JointIndex i = (JointIndex)(model.njoints - 1); i > 0; --i)
226 {
227
1/2
✓ Branch 1 taken 54 times.
✗ Branch 2 not taken.
 54 Pass2::run(
228 54 model.joints[i],
229 108 typename Pass2::ArgsType(model, data, data.tau, static_torque_partial_dq_));
230 }
231 2 }
232
233 template<
234 typename Scalar,
235 int Options,
236 template<typename, int> class JointCollectionTpl,
237 typename ConfigVectorType,
238 typename TangentVectorType1,
239 typename TangentVectorType2>
240 struct ComputeRNEADerivativesForwardStep
241 : public fusion::JointUnaryVisitorBase<ComputeRNEADerivativesForwardStep<
242 Scalar,
243 Options,
244 JointCollectionTpl,
245 ConfigVectorType,
246 TangentVectorType1,
247 TangentVectorType2>>
248 {
249 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
250 typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
251
252 typedef boost::fusion::vector<
253 const Model &,
254 Data &,
255 const ConfigVectorType &,
256 const TangentVectorType1 &,
257 const TangentVectorType2 &>
258 ArgsType;
259
260 template<typename JointModel>
261 9734 static void algo(
262 const JointModelBase<JointModel> & jmodel,
263 JointDataBase<typename JointModel::JointDataDerived> & jdata,
264 const Model & model,
265 Data & data,
266 const Eigen::MatrixBase<ConfigVectorType> & q,
267 const Eigen::MatrixBase<TangentVectorType1> & v,
268 const Eigen::MatrixBase<TangentVectorType2> & a)
269 {
270 typedef typename Model::JointIndex JointIndex;
271 typedef typename Data::Motion Motion;
272
273
1/2
✓ Branch 1 taken 4867 times.
✗ Branch 2 not taken.
 9734 const JointIndex i = jmodel.id();
274 9734 const JointIndex parent = model.parents[i];
275 9734 Motion & ov = data.ov[i];
276 9734 Motion & oa = data.oa[i];
277 9734 Motion & oa_gf = data.oa_gf[i];
278
279
1/2
✓ Branch 4 taken 4867 times.
✗ Branch 5 not taken.
 9734 jmodel.calc(jdata.derived(), q.derived(), v.derived());
280
281
6/10
✓ Branch 1 taken 4867 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4686 times.
✓ Branch 5 taken 181 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 4686 times.
✓ Branch 9 taken 181 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 4686 times.
✗ Branch 13 not taken.
 9734 data.liMi[i] = model.jointPlacements[i] * jdata.M();
282
283
2/4
✓ Branch 1 taken 4867 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 4867 times.
✗ Branch 6 not taken.
 9734 data.v[i] = jdata.v();
284
285
2/2
✓ Branch 0 taken 4686 times.
✓ Branch 1 taken 181 times.
 9734 if (parent > 0)
286 {
287
2/4
✓ Branch 3 taken 4686 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 4686 times.
✗ Branch 8 not taken.
 9372 data.oMi[i] = data.oMi[parent] * data.liMi[i];
288
2/4
✓ Branch 3 taken 4686 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 4686 times.
✗ Branch 8 not taken.
 9372 data.v[i] += data.liMi[i].actInv(data.v[parent]);
289 }
290 else
291
1/2
✓ Branch 3 taken 181 times.
✗ Branch 4 not taken.
 362 data.oMi[i] = data.liMi[i];
292
293
5/9
✓ Branch 1 taken 4867 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4687 times.
✓ Branch 5 taken 180 times.
✗ Branch 6 not taken.
✓ Branch 7 taken 4687 times.
✗ Branch 8 not taken.
✓ Branch 11 taken 4687 times.
✗ Branch 12 not taken.
 19468 data.a[i] =
294
7/14
✓ Branch 1 taken 4867 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 4867 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 4867 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 4867 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 4867 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 180 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 180 times.
✗ Branch 21 not taken.
 19522 jdata.S() * jmodel.jointVelocitySelector(a) + jdata.c() + (data.v[i] ^ jdata.v());
295
2/2
✓ Branch 0 taken 4686 times.
✓ Branch 1 taken 181 times.
 9734 if (parent > 0)
296 {
297
2/4
✓ Branch 3 taken 4686 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 4686 times.
✗ Branch 8 not taken.
 9372 data.a[i] += data.liMi[i].actInv(data.a[parent]);
298 }
299
300
3/6
✓ Branch 3 taken 4867 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 4867 times.
✗ Branch 8 not taken.
✓ Branch 11 taken 4867 times.
✗ Branch 12 not taken.
 9734 data.oYcrb[i] = data.oinertias[i] = data.oMi[i].act(model.inertias[i]);
301
2/4
✓ Branch 3 taken 4867 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 4867 times.
✗ Branch 7 not taken.
 9734 ov = data.oMi[i].act(data.v[i]);
302
2/4
✓ Branch 3 taken 4867 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 4867 times.
✗ Branch 7 not taken.
 9734 oa = data.oMi[i].act(data.a[i]);
303
2/4
✓ Branch 1 taken 4867 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4867 times.
✗ Branch 5 not taken.
 9734 oa_gf = oa - model.gravity; // add gravity contribution
304
305
2/4
✓ Branch 2 taken 4867 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 4867 times.
✗ Branch 7 not taken.
 9734 data.oh[i] = data.oYcrb[i] * ov;
306
4/8
✓ Branch 2 taken 4867 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 4867 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 4867 times.
✗ Branch 10 not taken.
✓ Branch 13 taken 4867 times.
✗ Branch 14 not taken.
 9734 data.of[i] = data.oYcrb[i] * oa_gf + ov.cross(data.oh[i]);
307
308 typedef
309 typename SizeDepType<JointModel::NV>::template ColsReturn<typename Data::Matrix6x>::Type
310 ColsBlock;
311
1/2
✓ Branch 1 taken 4867 times.
✗ Branch 2 not taken.
 9734 ColsBlock J_cols = jmodel.jointCols(data.J);
312
1/2
✓ Branch 1 taken 4867 times.
✗ Branch 2 not taken.
 9734 ColsBlock dJ_cols = jmodel.jointCols(data.dJ);
313
1/2
✓ Branch 1 taken 4867 times.
✗ Branch 2 not taken.
 9734 ColsBlock dVdq_cols = jmodel.jointCols(data.dVdq);
314
1/2
✓ Branch 1 taken 4867 times.
✗ Branch 2 not taken.
 9734 ColsBlock dAdq_cols = jmodel.jointCols(data.dAdq);
315
1/2
✓ Branch 1 taken 4867 times.
✗ Branch 2 not taken.
 9734 ColsBlock dAdv_cols = jmodel.jointCols(data.dAdv);
316
317
3/6
✓ Branch 2 taken 4867 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 4867 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 4867 times.
✗ Branch 9 not taken.
 9734 J_cols = data.oMi[i].act(jdata.S());
318
1/2
✓ Branch 1 taken 4867 times.
✗ Branch 2 not taken.
 9734 motionSet::motionAction(ov, J_cols, dJ_cols);
319
1/2
✓ Branch 2 taken 4867 times.
✗ Branch 3 not taken.
 9734 motionSet::motionAction(data.oa_gf[parent], J_cols, dAdq_cols);
320
1/2
✓ Branch 1 taken 4867 times.
✗ Branch 2 not taken.
 9734 dAdv_cols = dJ_cols;
321
2/2
✓ Branch 0 taken 4686 times.
✓ Branch 1 taken 181 times.
 9734 if (parent > 0)
322 {
323
1/2
✓ Branch 2 taken 4686 times.
✗ Branch 3 not taken.
 9372 motionSet::motionAction(data.ov[parent], J_cols, dVdq_cols);
324
1/2
✓ Branch 2 taken 4686 times.
✗ Branch 3 not taken.
 9372 motionSet::motionAction<ADDTO>(data.ov[parent], dVdq_cols, dAdq_cols);
325
2/4
✓ Branch 1 taken 4686 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4686 times.
✗ Branch 5 not taken.
 9372 dAdv_cols.noalias() += dVdq_cols;
326 }
327 else
328 {
329
1/2
✓ Branch 1 taken 181 times.
✗ Branch 2 not taken.
 362 dVdq_cols.setZero();
330 }
331
332 // computes variation of inertias
333
1/2
✓ Branch 2 taken 4867 times.
✗ Branch 3 not taken.
 9734 data.doYcrb[i] = data.oYcrb[i].variation(ov);
334
335
1/2
✓ Branch 3 taken 4867 times.
✗ Branch 4 not taken.
 9734 addForceCrossMatrix(data.oh[i], data.doYcrb[i]);
336 }
337
338 template<typename ForceDerived, typename M6>
339 static void
340 5461 addForceCrossMatrix(const ForceDense<ForceDerived> & f, const Eigen::MatrixBase<M6> & mout)
341 {
342 5461 M6 & mout_ = PINOCCHIO_EIGEN_CONST_CAST(M6, mout);
343
2/4
✓ Branch 1 taken 5461 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5461 times.
✗ Branch 5 not taken.
 5461 addSkew(
344
1/2
✓ Branch 2 taken 5461 times.
✗ Branch 3 not taken.
 5461 -f.linear(), mout_.template block<3, 3>(ForceDerived::LINEAR, ForceDerived::ANGULAR));
345
2/4
✓ Branch 1 taken 5461 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5461 times.
✗ Branch 5 not taken.
 5461 addSkew(
346
1/2
✓ Branch 2 taken 5461 times.
✗ Branch 3 not taken.
 5461 -f.linear(), mout_.template block<3, 3>(ForceDerived::ANGULAR, ForceDerived::LINEAR));
347
2/4
✓ Branch 1 taken 5461 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5461 times.
✗ Branch 5 not taken.
 5461 addSkew(
348
1/2
✓ Branch 2 taken 5461 times.
✗ Branch 3 not taken.
 5461 -f.angular(), mout_.template block<3, 3>(ForceDerived::ANGULAR, ForceDerived::ANGULAR));
349 5461 }
350 };
351
352 template<
353 typename Scalar,
354 int Options,
355 template<typename, int> class JointCollectionTpl,
356 typename MatrixType1,
357 typename MatrixType2,
358 typename MatrixType3>
359 struct ComputeRNEADerivativesBackwardStep
360 : public fusion::JointUnaryVisitorBase<ComputeRNEADerivativesBackwardStep<
361 Scalar,
362 Options,
363 JointCollectionTpl,
364 MatrixType1,
365 MatrixType2,
366 MatrixType3>>
367 {
368 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
369 typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
370
371 typedef boost::fusion::
372 vector<const Model &, Data &, const MatrixType1 &, const MatrixType2 &, const MatrixType3 &>
373 ArgsType;
374
375 template<typename JointModel>
376 9734 static void algo(
377 const JointModelBase<JointModel> & jmodel,
378 const Model & model,
379 Data & data,
380 const Eigen::MatrixBase<MatrixType1> & rnea_partial_dq,
381 const Eigen::MatrixBase<MatrixType2> & rnea_partial_dv,
382 const Eigen::MatrixBase<MatrixType3> & rnea_partial_da)
383 {
384 typedef typename Model::JointIndex JointIndex;
385 typedef typename Data::Matrix6x Matrix6x;
386
387
1/2
✓ Branch 1 taken 4867 times.
✗ Branch 2 not taken.
 9734 const JointIndex i = jmodel.id();
388 9734 const JointIndex parent = model.parents[i];
389
390 typedef typename SizeDepType<JointModel::NV>::template ColsReturn<Matrix6x>::Type ColsBlock;
391
392 9734 Matrix6x & dYtJ = data.Fcrb[0];
393
1/2
✓ Branch 1 taken 4867 times.
✗ Branch 2 not taken.
 9734 ColsBlock J_cols = jmodel.jointCols(data.J);
394
1/2
✓ Branch 1 taken 4867 times.
✗ Branch 2 not taken.
 9734 ColsBlock dVdq_cols = jmodel.jointCols(data.dVdq);
395
1/2
✓ Branch 1 taken 4867 times.
✗ Branch 2 not taken.
 9734 ColsBlock dAdq_cols = jmodel.jointCols(data.dAdq);
396
1/2
✓ Branch 1 taken 4867 times.
✗ Branch 2 not taken.
 9734 ColsBlock dAdv_cols = jmodel.jointCols(data.dAdv);
397
1/2
✓ Branch 1 taken 4867 times.
✗ Branch 2 not taken.
 9734 ColsBlock dFdq_cols = jmodel.jointCols(data.dFdq);
398
1/2
✓ Branch 1 taken 4867 times.
✗ Branch 2 not taken.
 9734 ColsBlock dFdv_cols = jmodel.jointCols(data.dFdv);
399
1/2
✓ Branch 1 taken 4867 times.
✗ Branch 2 not taken.
 9734 ColsBlock dFda_cols = jmodel.jointCols(data.dFda); // Also equals to Ag_cols
400
1/2
✓ Branch 1 taken 4867 times.
✗ Branch 2 not taken.
 9734 ColsBlock dYtJ_cols = jmodel.jointCols(dYtJ);
401
402 9734 MatrixType1 & rnea_partial_dq_ = PINOCCHIO_EIGEN_CONST_CAST(MatrixType1, rnea_partial_dq);
403 9734 MatrixType2 & rnea_partial_dv_ = PINOCCHIO_EIGEN_CONST_CAST(MatrixType2, rnea_partial_dv);
404 9734 MatrixType3 & rnea_partial_da_ = PINOCCHIO_EIGEN_CONST_CAST(MatrixType3, rnea_partial_da);
405
406 // tau
407
3/6
✓ Branch 1 taken 4867 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4867 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4867 times.
✗ Branch 8 not taken.
 9734 jmodel.jointVelocitySelector(data.tau).noalias() =
408
3/6
✓ Branch 2 taken 4867 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 4867 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 4867 times.
✗ Branch 9 not taken.
 9734 J_cols.transpose() * data.of[i].toVector();
409
410 9734 const Eigen::DenseIndex nv_subtree = data.nvSubtree[i];
411
1/2
✓ Branch 1 taken 4867 times.
✗ Branch 2 not taken.
 9734 const Eigen::DenseIndex nv = jmodel.nv();
412
1/2
✓ Branch 1 taken 4867 times.
✗ Branch 2 not taken.
 9734 const Eigen::DenseIndex idx_v = jmodel.idx_v();
413 9734 const Eigen::DenseIndex idx_v_plus = idx_v + nv;
414 9734 const Eigen::DenseIndex nv_subtree_plus = nv_subtree - nv;
415
416 // dtau/da similar to data.M
417
1/2
✓ Branch 2 taken 4867 times.
✗ Branch 3 not taken.
 9734 motionSet::inertiaAction(data.oYcrb[i], J_cols, dFda_cols);
418
3/6
✓ Branch 1 taken 4867 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4867 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4867 times.
✗ Branch 8 not taken.
 9734 rnea_partial_da_.block(idx_v, idx_v, nv, nv_subtree).noalias() =
419
3/6
✓ Branch 1 taken 4867 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4867 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4867 times.
✗ Branch 8 not taken.
 9734 J_cols.transpose() * data.dFda.middleCols(idx_v, nv_subtree);
420
421 // dtau/dq
422
2/2
✓ Branch 0 taken 4686 times.
✓ Branch 1 taken 181 times.
 9734 if (parent > 0)
423 {
424
3/6
✓ Branch 2 taken 4686 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 4686 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 4686 times.
✗ Branch 9 not taken.
 9372 dFdq_cols.noalias() = data.doYcrb[i] * dVdq_cols;
425
1/2
✓ Branch 2 taken 4686 times.
✗ Branch 3 not taken.
 9372 motionSet::inertiaAction<ADDTO>(data.oYcrb[i], dAdq_cols, dFdq_cols);
426 }
427 else
428
1/2
✓ Branch 2 taken 181 times.
✗ Branch 3 not taken.
 362 motionSet::inertiaAction(data.oYcrb[i], dAdq_cols, dFdq_cols);
429
430
5/10
✓ Branch 2 taken 4867 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 4867 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 4867 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 4867 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 4867 times.
✗ Branch 15 not taken.
 9734 dYtJ_cols.transpose().noalias() = J_cols.transpose() * data.doYcrb[i];
431
4/8
✓ Branch 1 taken 4867 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4867 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4867 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 4867 times.
✗ Branch 11 not taken.
 9734 rnea_partial_dq_.block(idx_v_plus, idx_v, nv_subtree_plus, nv).noalias() =
432
3/6
✓ Branch 1 taken 4867 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4867 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4867 times.
✗ Branch 8 not taken.
 9734 data.dFda.middleCols(idx_v_plus, nv_subtree_plus).transpose() * dAdq_cols
433
3/6
✓ Branch 1 taken 4867 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4867 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4867 times.
✗ Branch 8 not taken.
 9734 + dYtJ.middleCols(idx_v_plus, nv_subtree_plus).transpose() * dVdq_cols;
434
435
3/6
✓ Branch 1 taken 4867 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4867 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4867 times.
✗ Branch 8 not taken.
 9734 rnea_partial_dq_.block(idx_v, idx_v, nv, nv_subtree).noalias() =
436
3/6
✓ Branch 1 taken 4867 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4867 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4867 times.
✗ Branch 8 not taken.
 9734 J_cols.transpose() * data.dFdq.middleCols(idx_v, nv_subtree);
437
438
1/2
✓ Branch 2 taken 4867 times.
✗ Branch 3 not taken.
 9734 motionSet::act<ADDTO>(J_cols, data.of[i], dFdq_cols);
439
440 // dtau/dv
441
3/6
✓ Branch 2 taken 4867 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 4867 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 4867 times.
✗ Branch 9 not taken.
 9734 dFdv_cols.noalias() = data.doYcrb[i] * J_cols;
442
1/2
✓ Branch 2 taken 4867 times.
✗ Branch 3 not taken.
 9734 motionSet::inertiaAction<ADDTO>(data.oYcrb[i], dAdv_cols, dFdv_cols);
443
444
4/8
✓ Branch 1 taken 4867 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4867 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4867 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 4867 times.
✗ Branch 11 not taken.
 9734 rnea_partial_dv_.block(idx_v_plus, idx_v, nv_subtree_plus, nv).noalias() =
445
3/6
✓ Branch 1 taken 4867 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4867 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4867 times.
✗ Branch 8 not taken.
 9734 data.dFda.middleCols(idx_v_plus, nv_subtree_plus).transpose() * dAdv_cols
446
3/6
✓ Branch 1 taken 4867 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4867 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4867 times.
✗ Branch 8 not taken.
 9734 + dYtJ.middleCols(idx_v_plus, nv_subtree_plus).transpose() * J_cols;
447
448
3/6
✓ Branch 1 taken 4867 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4867 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4867 times.
✗ Branch 8 not taken.
 9734 rnea_partial_dv_.block(idx_v, idx_v, nv, nv_subtree).noalias() =
449
3/6
✓ Branch 1 taken 4867 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4867 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4867 times.
✗ Branch 8 not taken.
 9734 J_cols.transpose() * data.dFdv.middleCols(idx_v, nv_subtree);
450
451
2/2
✓ Branch 0 taken 4686 times.
✓ Branch 1 taken 181 times.
 9734 if (parent > 0)
452 {
453
1/2
✓ Branch 3 taken 4686 times.
✗ Branch 4 not taken.
 9372 data.oYcrb[parent] += data.oYcrb[i];
454
1/2
✓ Branch 3 taken 4686 times.
✗ Branch 4 not taken.
 9372 data.doYcrb[parent] += data.doYcrb[i];
455
1/2
✓ Branch 3 taken 4686 times.
✗ Branch 4 not taken.
 9372 data.of[parent] += data.of[i];
456 }
457 }
458 };
459
460 template<
461 typename Scalar,
462 int Options,
463 template<typename, int> class JointCollectionTpl,
464 typename ConfigVectorType,
465 typename TangentVectorType1,
466 typename TangentVectorType2,
467 typename MatrixType1,
468 typename MatrixType2,
469 typename MatrixType3>
470 347 void computeRNEADerivatives(
471 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
472 DataTpl<Scalar, Options, JointCollectionTpl> & data,
473 const Eigen::MatrixBase<ConfigVectorType> & q,
474 const Eigen::MatrixBase<TangentVectorType1> & v,
475 const Eigen::MatrixBase<TangentVectorType2> & a,
476 const Eigen::MatrixBase<MatrixType1> & rnea_partial_dq,
477 const Eigen::MatrixBase<MatrixType2> & rnea_partial_dv,
478 const Eigen::MatrixBase<MatrixType3> & rnea_partial_da)
479 {
480
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 174 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.
 347 PINOCCHIO_CHECK_ARGUMENT_SIZE(
481 q.size(), model.nq, "The joint configuration vector is not of right size");
482
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 174 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.
 347 PINOCCHIO_CHECK_ARGUMENT_SIZE(
483 v.size(), model.nv, "The joint velocity vector is not of right size");
484
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 174 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.
 347 PINOCCHIO_CHECK_ARGUMENT_SIZE(
485 a.size(), model.nv, "The joint acceleration vector is not of right size");
486
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 174 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.
 347 PINOCCHIO_CHECK_ARGUMENT_SIZE(rnea_partial_dq.cols(), model.nv);
487
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 174 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.
 347 PINOCCHIO_CHECK_ARGUMENT_SIZE(rnea_partial_dq.rows(), model.nv);
488
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 174 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.
 347 PINOCCHIO_CHECK_ARGUMENT_SIZE(rnea_partial_dv.cols(), model.nv);
489
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 174 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.
 347 PINOCCHIO_CHECK_ARGUMENT_SIZE(rnea_partial_dv.rows(), model.nv);
490
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 174 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.
 347 PINOCCHIO_CHECK_ARGUMENT_SIZE(rnea_partial_da.cols(), model.nv);
491
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 174 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.
 347 PINOCCHIO_CHECK_ARGUMENT_SIZE(rnea_partial_da.rows(), model.nv);
492
4/10
✓ Branch 2 taken 174 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 174 times.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✓ Branch 8 taken 173 times.
✓ Branch 9 taken 1 times.
✗ Branch 11 not taken.
✗ Branch 12 not taken.
✗ Branch 13 not taken.
 347 PINOCCHIO_CHECK_INPUT_ARGUMENT(
493 isZero(model.gravity.angular()),
494 "The gravity must be a pure force vector, no angular part");
495
1/2
✓ Branch 1 taken 174 times.
✗ Branch 2 not taken.
 347 assert(model.check(data) && "data is not consistent with model.");
496
497 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
498 typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
499 typedef typename Model::JointIndex JointIndex;
500
501
1/2
✓ Branch 3 taken 174 times.
✗ Branch 4 not taken.
 347 data.oa_gf[0] = -model.gravity;
502
503 typedef ComputeRNEADerivativesForwardStep<
504 Scalar, Options, JointCollectionTpl, ConfigVectorType, TangentVectorType1,
505 TangentVectorType2>
506 Pass1;
507
2/2
✓ Branch 0 taken 4678 times.
✓ Branch 1 taken 174 times.
 9676 for (JointIndex i = 1; i < (JointIndex)model.njoints; ++i)
508 {
509
1/2
✓ Branch 1 taken 4678 times.
✗ Branch 2 not taken.
 9329 Pass1::run(
510 9329 model.joints[i], data.joints[i],
511 18658 typename Pass1::ArgsType(model, data, q.derived(), v.derived(), a.derived()));
512 }
513
514 typedef ComputeRNEADerivativesBackwardStep<
515 Scalar, Options, JointCollectionTpl, MatrixType1, MatrixType2, MatrixType3>
516 Pass2;
517
2/2
✓ Branch 0 taken 4678 times.
✓ Branch 1 taken 174 times.
 9676 for (JointIndex i = (JointIndex)(model.njoints - 1); i > 0; --i)
518 {
519
1/2
✓ Branch 1 taken 4678 times.
✗ Branch 2 not taken.
 9329 Pass2::run(
520 18658 model.joints[i], typename Pass2::ArgsType(
521 9329 model, data, PINOCCHIO_EIGEN_CONST_CAST(MatrixType1, rnea_partial_dq),
522 9329 PINOCCHIO_EIGEN_CONST_CAST(MatrixType2, rnea_partial_dv),
523 9329 PINOCCHIO_EIGEN_CONST_CAST(MatrixType3, rnea_partial_da)));
524 }
525
526 // Restore the status of dAdq_cols (remove gravity)
527
2/2
✓ Branch 0 taken 5545 times.
✓ Branch 1 taken 174 times.
 11405 for (Eigen::DenseIndex k = 0; k < model.nv; ++k)
528 {
529
2/4
✓ Branch 1 taken 5545 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5545 times.
✗ Branch 5 not taken.
 11058 MotionRef<typename Data::Matrix6x::ColXpr> m_in(data.J.col(k));
530
2/4
✓ Branch 1 taken 5545 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5545 times.
✗ Branch 5 not taken.
 11058 MotionRef<typename Data::Matrix6x::ColXpr> m_out(data.dAdq.col(k));
531
5/10
✓ Branch 1 taken 5545 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5545 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 5545 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 5545 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 5545 times.
✗ Branch 14 not taken.
 11058 m_out.linear() += model.gravity.linear().cross(m_in.angular());
532 }
533
534 // Add armature contribution
535
3/6
✓ Branch 1 taken 174 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 174 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 174 times.
✗ Branch 8 not taken.
 347 data.tau.array() +=
536
1/2
✓ Branch 2 taken 174 times.
✗ Branch 3 not taken.
 347 model.armature.array() * a.array(); // TODO: check if there is memory allocation
537
1/2
✓ Branch 3 taken 174 times.
✗ Branch 4 not taken.
 347 PINOCCHIO_EIGEN_CONST_CAST(MatrixType3, rnea_partial_da).diagonal() += model.armature;
538 347 }
539
540 template<
541 typename Scalar,
542 int Options,
543 template<typename, int> class JointCollectionTpl,
544 typename ConfigVectorType,
545 typename TangentVectorType1,
546 typename TangentVectorType2,
547 typename MatrixType1,
548 typename MatrixType2,
549 typename MatrixType3>
550 14 void computeRNEADerivatives(
551 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
552 DataTpl<Scalar, Options, JointCollectionTpl> & data,
553 const Eigen::MatrixBase<ConfigVectorType> & q,
554 const Eigen::MatrixBase<TangentVectorType1> & v,
555 const Eigen::MatrixBase<TangentVectorType2> & a,
556 const container::aligned_vector<ForceTpl<Scalar, Options>> & fext,
557 const Eigen::MatrixBase<MatrixType1> & rnea_partial_dq,
558 const Eigen::MatrixBase<MatrixType2> & rnea_partial_dv,
559 const Eigen::MatrixBase<MatrixType3> & rnea_partial_da)
560 {
561
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 7 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.
 14 PINOCCHIO_CHECK_ARGUMENT_SIZE(
562 q.size(), model.nq, "The joint configuration vector is not of right size");
563
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 7 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.
 14 PINOCCHIO_CHECK_ARGUMENT_SIZE(
564 v.size(), model.nv, "The joint velocity vector is not of right size");
565
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 7 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.
 14 PINOCCHIO_CHECK_ARGUMENT_SIZE(
566 a.size(), model.nv, "The joint acceleration vector is not of right size");
567
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 7 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.
 14 PINOCCHIO_CHECK_ARGUMENT_SIZE(
568 fext.size(), (size_t)model.njoints, "The size of the external forces is not of right size");
569
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 7 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.
 14 PINOCCHIO_CHECK_ARGUMENT_SIZE(rnea_partial_dq.cols(), model.nv);
570
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 7 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.
 14 PINOCCHIO_CHECK_ARGUMENT_SIZE(rnea_partial_dq.rows(), model.nv);
571
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 7 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.
 14 PINOCCHIO_CHECK_ARGUMENT_SIZE(rnea_partial_dv.cols(), model.nv);
572
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 7 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.
 14 PINOCCHIO_CHECK_ARGUMENT_SIZE(rnea_partial_dv.rows(), model.nv);
573
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 7 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.
 14 PINOCCHIO_CHECK_ARGUMENT_SIZE(rnea_partial_da.cols(), model.nv);
574
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 7 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.
 14 PINOCCHIO_CHECK_ARGUMENT_SIZE(rnea_partial_da.rows(), model.nv);
575
1/2
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
 14 assert(model.check(data) && "data is not consistent with model.");
576
577 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
578 typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
579 typedef typename Model::JointIndex JointIndex;
580
581
1/2
✓ Branch 3 taken 7 times.
✗ Branch 4 not taken.
 14 data.oa_gf[0] = -model.gravity;
582
583 typedef ComputeRNEADerivativesForwardStep<
584 Scalar, Options, JointCollectionTpl, ConfigVectorType, TangentVectorType1,
585 TangentVectorType2>
586 Pass1;
587
2/2
✓ Branch 0 taken 189 times.
✓ Branch 1 taken 7 times.
 392 for (JointIndex i = 1; i < (JointIndex)model.njoints; ++i)
588 {
589
1/2
✓ Branch 1 taken 189 times.
✗ Branch 2 not taken.
 378 Pass1::run(
590 378 model.joints[i], data.joints[i],
591 378 typename Pass1::ArgsType(model, data, q.derived(), v.derived(), a.derived()));
592
1/2
✓ Branch 5 taken 189 times.
✗ Branch 6 not taken.
 378 data.of[i] -= data.oMi[i].act(fext[i]);
593 }
594
595 typedef ComputeRNEADerivativesBackwardStep<
596 Scalar, Options, JointCollectionTpl, MatrixType1, MatrixType2, MatrixType3>
597 Pass2;
598
2/2
✓ Branch 0 taken 189 times.
✓ Branch 1 taken 7 times.
 392 for (JointIndex i = (JointIndex)(model.njoints - 1); i > 0; --i)
599 {
600
1/2
✓ Branch 1 taken 189 times.
✗ Branch 2 not taken.
 378 Pass2::run(
601 756 model.joints[i], typename Pass2::ArgsType(
602 378 model, data, PINOCCHIO_EIGEN_CONST_CAST(MatrixType1, rnea_partial_dq),
603 378 PINOCCHIO_EIGEN_CONST_CAST(MatrixType2, rnea_partial_dv),
604 378 PINOCCHIO_EIGEN_CONST_CAST(MatrixType3, rnea_partial_da)));
605 }
606
607 // Restore the status of dAdq_cols (remove gravity)
608
2/2
✓ Branch 0 taken 224 times.
✓ Branch 1 taken 7 times.
 462 for (Eigen::DenseIndex k = 0; k < model.nv; ++k)
609 {
610
2/4
✓ Branch 1 taken 224 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 224 times.
✗ Branch 5 not taken.
 448 MotionRef<typename Data::Matrix6x::ColXpr> m_in(data.J.col(k));
611
2/4
✓ Branch 1 taken 224 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 224 times.
✗ Branch 5 not taken.
 448 MotionRef<typename Data::Matrix6x::ColXpr> m_out(data.dAdq.col(k));
612
5/10
✓ Branch 1 taken 224 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 224 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 224 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 224 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 224 times.
✗ Branch 14 not taken.
 448 m_out.linear() += model.gravity.linear().cross(m_in.angular());
613 }
614
615 // Add armature contribution
616
3/6
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 7 times.
✗ Branch 8 not taken.
 14 data.tau.array() +=
617
1/2
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
 14 model.armature.array() * a.array(); // TODO: check if there is memory allocation
618
1/2
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
 14 data.M.diagonal() += model.armature;
619 }
620
621 template<
622 typename Scalar,
623 int Options,
624 template<typename, int> class JointCollectionTpl,
625 typename ConfigVectorType,
626 typename TangentVectorType1,
627 typename TangentVectorType2>
628 ✗ void computeRNEADerivatives(
629 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
630 DataTpl<Scalar, Options, JointCollectionTpl> & data,
631 const Eigen::MatrixBase<ConfigVectorType> & q,
632 const Eigen::MatrixBase<TangentVectorType1> & v,
633 const Eigen::MatrixBase<TangentVectorType2> & a)
634 {
635 ✗ impl::computeRNEADerivatives(
636 ✗ model, data, q.derived(), v.derived(), a.derived(), data.dtau_dq, data.dtau_dv, data.M);
637 }
638
639 template<
640 typename Scalar,
641 int Options,
642 template<typename, int> class JointCollectionTpl,
643 typename ConfigVectorType,
644 typename TangentVectorType1,
645 typename TangentVectorType2>
646 ✗ void computeRNEADerivatives(
647 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
648 DataTpl<Scalar, Options, JointCollectionTpl> & data,
649 const Eigen::MatrixBase<ConfigVectorType> & q,
650 const Eigen::MatrixBase<TangentVectorType1> & v,
651 const Eigen::MatrixBase<TangentVectorType2> & a,
652 const container::aligned_vector<ForceTpl<Scalar, Options>> & fext)
653 {
654 ✗ impl::computeRNEADerivatives(
655 ✗ model, data, q.derived(), v.derived(), a.derived(), fext, data.dtau_dq, data.dtau_dv,
656 ✗ data.M);
657 }
658
659 } // namespace impl
660
661 template<
662 typename Scalar,
663 int Options,
664 template<typename, int> class JointCollectionTpl,
665 typename ConfigVectorType,
666 typename ReturnMatrixType>
667 3 void computeGeneralizedGravityDerivatives(
668 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
669 DataTpl<Scalar, Options, JointCollectionTpl> & data,
670 const Eigen::MatrixBase<ConfigVectorType> & q,
671 const Eigen::MatrixBase<ReturnMatrixType> & gravity_partial_dq)
672 {
673
2/4
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
 3 impl::computeGeneralizedGravityDerivatives(
674 model, data, make_const_ref(q), make_ref(gravity_partial_dq));
675 3 }
676
677 template<
678 typename Scalar,
679 int Options,
680 template<typename, int> class JointCollectionTpl,
681 typename ConfigVectorType,
682 typename ReturnMatrixType>
683 2 void computeStaticTorqueDerivatives(
684 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
685 DataTpl<Scalar, Options, JointCollectionTpl> & data,
686 const Eigen::MatrixBase<ConfigVectorType> & q,
687 const container::aligned_vector<ForceTpl<Scalar, Options>> & fext,
688 const Eigen::MatrixBase<ReturnMatrixType> & static_torque_partial_dq)
689 {
690
2/4
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
 2 impl::computeStaticTorqueDerivatives(
691 model, data, make_const_ref(q), fext, make_ref(static_torque_partial_dq));
692 2 }
693
694 template<
695 typename Scalar,
696 int Options,
697 template<typename, int> class JointCollectionTpl,
698 typename ConfigVectorType,
699 typename TangentVectorType1,
700 typename TangentVectorType2,
701 typename MatrixType1,
702 typename MatrixType2,
703 typename MatrixType3>
704 274 void computeRNEADerivatives(
705 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
706 DataTpl<Scalar, Options, JointCollectionTpl> & data,
707 const Eigen::MatrixBase<ConfigVectorType> & q,
708 const Eigen::MatrixBase<TangentVectorType1> & v,
709 const Eigen::MatrixBase<TangentVectorType2> & a,
710 const Eigen::MatrixBase<MatrixType1> & rnea_partial_dq,
711 const Eigen::MatrixBase<MatrixType2> & rnea_partial_dv,
712 const Eigen::MatrixBase<MatrixType3> & rnea_partial_da)
713 {
714
6/12
✓ Branch 2 taken 138 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 138 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 138 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 138 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 138 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 138 times.
✗ Branch 18 not taken.
 274 impl::computeRNEADerivatives(
715 model, data, make_const_ref(q), make_const_ref(v), make_const_ref(a),
716 make_ref(rnea_partial_dq), make_ref(rnea_partial_dv), make_ref(rnea_partial_da));
717 274 }
718
719 template<
720 typename Scalar,
721 int Options,
722 template<typename, int> class JointCollectionTpl,
723 typename ConfigVectorType,
724 typename TangentVectorType1,
725 typename TangentVectorType2,
726 typename MatrixType1,
727 typename MatrixType2,
728 typename MatrixType3>
729 1 void computeRNEADerivatives(
730 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
731 DataTpl<Scalar, Options, JointCollectionTpl> & data,
732 const Eigen::MatrixBase<ConfigVectorType> & q,
733 const Eigen::MatrixBase<TangentVectorType1> & v,
734 const Eigen::MatrixBase<TangentVectorType2> & a,
735 const container::aligned_vector<ForceTpl<Scalar, Options>> & fext,
736 const Eigen::MatrixBase<MatrixType1> & rnea_partial_dq,
737 const Eigen::MatrixBase<MatrixType2> & rnea_partial_dv,
738 const Eigen::MatrixBase<MatrixType3> & rnea_partial_da)
739 {
740
6/12
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
 1 impl::computeRNEADerivatives(
741 model, data, make_const_ref(q), make_const_ref(v), make_const_ref(a), fext,
742 make_ref(rnea_partial_dq), make_ref(rnea_partial_dv), make_ref(rnea_partial_da));
743 1 }
744
745 template<
746 typename Scalar,
747 int Options,
748 template<typename, int> class JointCollectionTpl,
749 typename ConfigVectorType,
750 typename TangentVectorType1,
751 typename TangentVectorType2>
752 36 void computeRNEADerivatives(
753 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
754 DataTpl<Scalar, Options, JointCollectionTpl> & data,
755 const Eigen::MatrixBase<ConfigVectorType> & q,
756 const Eigen::MatrixBase<TangentVectorType1> & v,
757 const Eigen::MatrixBase<TangentVectorType2> & a)
758 {
759
4/8
✓ Branch 1 taken 36 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 36 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 36 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 36 times.
✗ Branch 11 not taken.
 36 impl::computeRNEADerivatives(
760
1/2
✓ Branch 1 taken 36 times.
✗ Branch 2 not taken.
 36 model, data, make_const_ref(q), make_const_ref(v), make_const_ref(a), make_ref(data.dtau_dq),
761
1/2
✓ Branch 2 taken 36 times.
✗ Branch 3 not taken.
 36 make_ref(data.dtau_dv), make_ref(data.M));
762 36 }
763
764 template<
765 typename Scalar,
766 int Options,
767 template<typename, int> class JointCollectionTpl,
768 typename ConfigVectorType,
769 typename TangentVectorType1,
770 typename TangentVectorType2>
771 6 void computeRNEADerivatives(
772 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
773 DataTpl<Scalar, Options, JointCollectionTpl> & data,
774 const Eigen::MatrixBase<ConfigVectorType> & q,
775 const Eigen::MatrixBase<TangentVectorType1> & v,
776 const Eigen::MatrixBase<TangentVectorType2> & a,
777 const container::aligned_vector<ForceTpl<Scalar, Options>> & fext)
778 {
779
4/8
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 6 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 6 times.
✗ Branch 11 not taken.
 6 impl::computeRNEADerivatives(
780 model, data, make_const_ref(q), make_const_ref(v), make_const_ref(a), fext,
781
2/4
✓ Branch 2 taken 6 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 6 times.
✗ Branch 6 not taken.
 6 make_ref(data.dtau_dq), make_ref(data.dtau_dv), make_ref(data.M));
782 6 }
783
784 } // namespace pinocchio
785
786 #endif // ifndef __pinocchio_algorithm_rnea_derivatives_hxx__
787