GCC Code Coverage Report

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