GCC Code Coverage Report

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