GCC Code Coverage Report

Directory: ./
File: include/pinocchio/algorithm/kinematics-derivatives.hxx
Date: 2024-08-27 18:20:05
Exec Total Coverage
Lines: 502 521 96.4%
Branches: 644 1640 39.3%
Line Branch Exec Source
1 //
2 // Copyright (c) 2017-2020 CNRS INRIA
3 //
4
5 #ifndef __pinocchio_algorithm_kinematics_derivatives_hxx__
6 #define __pinocchio_algorithm_kinematics_derivatives_hxx__
7
8 #include "pinocchio/spatial/classic-acceleration.hpp"
9 #include "pinocchio/multibody/visitor.hpp"
10 #include "pinocchio/algorithm/check.hpp"
11 #include "pinocchio/algorithm/jacobian.hpp"
12
13 namespace pinocchio
14 {
15 namespace impl
16 {
17 template<
18 typename Scalar,
19 int Options,
20 template<typename, int>
21 class JointCollectionTpl,
22 typename ConfigVectorType,
23 typename TangentVectorType1,
24 typename TangentVectorType2>
25 struct ForwardKinematicsDerivativesForwardStep
26 : public fusion::JointUnaryVisitorBase<ForwardKinematicsDerivativesForwardStep<
27 Scalar,
28 Options,
29 JointCollectionTpl,
30 ConfigVectorType,
31 TangentVectorType1,
32 TangentVectorType2>>
33 {
34 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
35 typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
36
37 typedef boost::fusion::vector<
38 const Model &,
39 Data &,
40 const ConfigVectorType &,
41 const TangentVectorType1 &,
42 const TangentVectorType2 &>
43 ArgsType;
44
45 template<typename JointModel>
46 1470 static void algo(
47 const JointModelBase<JointModel> & jmodel,
48 JointDataBase<typename JointModel::JointDataDerived> & jdata,
49 const Model & model,
50 Data & data,
51 const Eigen::MatrixBase<ConfigVectorType> & q,
52 const Eigen::MatrixBase<TangentVectorType1> & v,
53 const Eigen::MatrixBase<TangentVectorType2> & a)
54 {
55 typedef typename Model::JointIndex JointIndex;
56 typedef typename Data::SE3 SE3;
57 typedef typename Data::Motion Motion;
58
59
1/2
✓ Branch 1 taken 735 times.
✗ Branch 2 not taken.
 1470 const JointIndex & i = jmodel.id();
60 1470 const JointIndex & parent = model.parents[i];
61 1470 SE3 & oMi = data.oMi[i];
62 1470 Motion & vi = data.v[i];
63 1470 Motion & ai = data.a[i];
64 1470 Motion & ov = data.ov[i];
65 1470 Motion & oa = data.oa[i];
66
67
1/2
✓ Branch 4 taken 735 times.
✗ Branch 5 not taken.
 1470 jmodel.calc(jdata.derived(), q.derived(), v.derived());
68
69
6/10
✓ Branch 1 taken 735 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 708 times.
✓ Branch 5 taken 27 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 708 times.
✓ Branch 9 taken 27 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 708 times.
✗ Branch 13 not taken.
 1470 data.liMi[i] = model.jointPlacements[i] * jdata.M();
70
71
2/2
✓ Branch 0 taken 707 times.
✓ Branch 1 taken 28 times.
 1470 if (parent > 0)
72
2/4
✓ Branch 3 taken 707 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 707 times.
✗ Branch 7 not taken.
 1414 oMi = data.oMi[parent] * data.liMi[i];
73 else
74
1/2
✓ Branch 2 taken 28 times.
✗ Branch 3 not taken.
 56 oMi = data.liMi[i];
75
76
2/4
✓ Branch 1 taken 735 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 735 times.
✗ Branch 5 not taken.
 1470 vi = jdata.v();
77
2/2
✓ Branch 0 taken 707 times.
✓ Branch 1 taken 28 times.
 1470 if (parent > 0)
78
2/4
✓ Branch 3 taken 707 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 707 times.
✗ Branch 7 not taken.
 1414 vi += data.liMi[i].actInv(data.v[parent]);
79
80
9/18
✓ Branch 1 taken 735 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 735 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 735 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 735 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 735 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 735 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 735 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 735 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 735 times.
✗ Branch 26 not taken.
 1470 ai = jdata.S() * jmodel.jointVelocitySelector(a) + jdata.c() + (vi ^ jdata.v());
81
2/2
✓ Branch 0 taken 707 times.
✓ Branch 1 taken 28 times.
 1470 if (parent > 0)
82
2/4
✓ Branch 3 taken 707 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 707 times.
✗ Branch 7 not taken.
 1414 ai += data.liMi[i].actInv(data.a[parent]);
83
84 typedef
85 typename SizeDepType<JointModel::NV>::template ColsReturn<typename Data::Matrix6x>::Type
86 ColsBlock;
87
1/2
✓ Branch 1 taken 735 times.
✗ Branch 2 not taken.
 1470 ColsBlock dJcols = jmodel.jointCols(data.dJ);
88
1/2
✓ Branch 1 taken 735 times.
✗ Branch 2 not taken.
 1470 ColsBlock Jcols = jmodel.jointCols(data.J);
89
90
3/6
✓ Branch 1 taken 735 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 735 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 735 times.
✗ Branch 8 not taken.
 1470 Jcols = oMi.act(jdata.S());
91
2/4
✓ Branch 1 taken 735 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 735 times.
✗ Branch 5 not taken.
 1470 ov = oMi.act(vi); // Spatial velocity of joint i expressed in the global frame o
92
1/2
✓ Branch 1 taken 735 times.
✗ Branch 2 not taken.
 1470 motionSet::motionAction(ov, Jcols, dJcols);
93
2/4
✓ Branch 1 taken 735 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 735 times.
✗ Branch 5 not taken.
 1470 oa = oMi.act(ai); // Spatial acceleration of joint i expressed in the global frame o
94 }
95 };
96
97 template<
98 typename Scalar,
99 int Options,
100 template<typename, int>
101 class JointCollectionTpl,
102 typename ConfigVectorType,
103 typename TangentVectorType1,
104 typename TangentVectorType2>
105 28 void computeForwardKinematicsDerivatives(
106 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
107 DataTpl<Scalar, Options, JointCollectionTpl> & data,
108 const Eigen::MatrixBase<ConfigVectorType> & q,
109 const Eigen::MatrixBase<TangentVectorType1> & v,
110 const Eigen::MatrixBase<TangentVectorType2> & a)
111 {
112
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 28 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.
 28 PINOCCHIO_CHECK_ARGUMENT_SIZE(
113 q.size(), model.nq, "The configuration vector is not of right size");
114
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 28 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.
 28 PINOCCHIO_CHECK_ARGUMENT_SIZE(v.size(), model.nv, "The velocity vector is not of right size");
115
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 28 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.
 28 PINOCCHIO_CHECK_ARGUMENT_SIZE(
116 a.size(), model.nv, "The acceleration vector is not of right size");
117
1/2
✓ Branch 1 taken 28 times.
✗ Branch 2 not taken.
 28 assert(model.check(data) && "data is not consistent with model.");
118
119 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
120 typedef typename Model::JointIndex JointIndex;
121
122 28 data.v[0].setZero();
123 28 data.a[0].setZero();
124
125 typedef ForwardKinematicsDerivativesForwardStep<
126 Scalar, Options, JointCollectionTpl, ConfigVectorType, TangentVectorType1,
127 TangentVectorType2>
128 Pass1;
129
2/2
✓ Branch 0 taken 735 times.
✓ Branch 1 taken 28 times.
 763 for (JointIndex i = 1; i < (JointIndex)model.njoints; ++i)
130 {
131
1/2
✓ Branch 1 taken 735 times.
✗ Branch 2 not taken.
 735 Pass1::run(
132 735 model.joints[i], data.joints[i],
133 1470 typename Pass1::ArgsType(model, data, q.derived(), v.derived(), a.derived()));
134 }
135 28 }
136
137 template<
138 typename Scalar,
139 int Options,
140 template<typename, int>
141 class JointCollectionTpl,
142 typename Matrix6xOut1,
143 typename Matrix6xOut2>
144 struct JointVelocityDerivativesBackwardStep
145 : public fusion::JointUnaryVisitorBase<JointVelocityDerivativesBackwardStep<
146 Scalar,
147 Options,
148 JointCollectionTpl,
149 Matrix6xOut1,
150 Matrix6xOut2>>
151 {
152 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
153 typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
154
155 typedef boost::fusion::vector<
156 const Model &,
157 const Data &,
158 const typename Model::JointIndex &,
159 const ReferenceFrame &,
160 Matrix6xOut1 &,
161 Matrix6xOut2 &>
162 ArgsType;
163
164 template<typename JointModel>
165 246 static void algo(
166 const JointModelBase<JointModel> & jmodel,
167 const Model & model,
168 const Data & data,
169 const typename Model::JointIndex & jointId,
170 const ReferenceFrame & rf,
171 const Eigen::MatrixBase<Matrix6xOut1> & v_partial_dq,
172 const Eigen::MatrixBase<Matrix6xOut2> & v_partial_dv)
173 {
174 typedef typename Model::JointIndex JointIndex;
175 typedef typename Data::SE3 SE3;
176 typedef typename Data::Motion Motion;
177
178
1/2
✓ Branch 1 taken 123 times.
✗ Branch 2 not taken.
 246 const JointIndex & i = jmodel.id();
179 246 const JointIndex & parent = model.parents[i];
180
1/2
✓ Branch 1 taken 123 times.
✗ Branch 2 not taken.
 246 Motion vtmp; // Temporary variable
181
182 246 const SE3 & oMlast = data.oMi[jointId];
183 246 const Motion & vlast = data.ov[jointId];
184
185 typedef typename SizeDepType<JointModel::NV>::template ColsReturn<
186 typename Data::Matrix6x>::ConstType ColsBlock;
187
1/2
✓ Branch 1 taken 123 times.
✗ Branch 2 not taken.
 246 ColsBlock Jcols = jmodel.jointCols(data.J);
188
189 typedef typename SizeDepType<JointModel::NV>::template ColsReturn<Matrix6xOut1>::Type
190 ColsBlockOut1;
191 246 Matrix6xOut1 & v_partial_dq_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut1, v_partial_dq);
192 typedef typename SizeDepType<JointModel::NV>::template ColsReturn<Matrix6xOut2>::Type
193 ColsBlockOut2;
194 246 Matrix6xOut2 & v_partial_dv_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut2, v_partial_dv);
195
196 // dvec/dv: this result is then needed by dvec/dq
197
1/2
✓ Branch 1 taken 123 times.
✗ Branch 2 not taken.
 246 ColsBlockOut2 v_partial_dv_cols = jmodel.jointCols(v_partial_dv_);
198
3/4
✓ Branch 0 taken 34 times.
✓ Branch 1 taken 20 times.
✓ Branch 2 taken 69 times.
✗ Branch 3 not taken.
 246 switch (rf)
199 {
200 68 case WORLD:
201
1/2
✓ Branch 1 taken 34 times.
✗ Branch 2 not taken.
 68 v_partial_dv_cols = Jcols;
202 68 break;
203 40 case LOCAL_WORLD_ALIGNED:
204
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 40 details::translateJointJacobian(oMlast, Jcols, v_partial_dv_cols);
205 40 break;
206 138 case LOCAL:
207
1/2
✓ Branch 1 taken 69 times.
✗ Branch 2 not taken.
 138 motionSet::se3ActionInverse(oMlast, Jcols, v_partial_dv_cols);
208 138 break;
209 ✗ default:
210 ✗ assert(false && "This must never happened");
211 }
212
213 // dvec/dq
214
1/2
✓ Branch 1 taken 123 times.
✗ Branch 2 not taken.
 246 ColsBlockOut1 v_partial_dq_cols = jmodel.jointCols(v_partial_dq_);
215
3/4
✓ Branch 0 taken 34 times.
✓ Branch 1 taken 20 times.
✓ Branch 2 taken 69 times.
✗ Branch 3 not taken.
 246 switch (rf)
216 {
217 68 case WORLD:
218
2/2
✓ Branch 0 taken 28 times.
✓ Branch 1 taken 6 times.
 68 if (parent > 0)
219
2/4
✓ Branch 2 taken 28 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 28 times.
✗ Branch 6 not taken.
 56 vtmp = data.ov[parent] - vlast;
220 else
221
2/4
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
 12 vtmp = -vlast;
222
1/2
✓ Branch 1 taken 34 times.
✗ Branch 2 not taken.
 68 motionSet::motionAction(vtmp, Jcols, v_partial_dq_cols);
223 68 break;
224 40 case LOCAL_WORLD_ALIGNED:
225
2/2
✓ Branch 0 taken 16 times.
✓ Branch 1 taken 4 times.
 40 if (parent > 0)
226
2/4
✓ Branch 2 taken 16 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 16 times.
✗ Branch 6 not taken.
 32 vtmp = data.ov[parent] - vlast;
227 else
228
2/4
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
 8 vtmp = -vlast;
229
5/10
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 20 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 20 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 20 times.
✗ Branch 14 not taken.
 40 vtmp.linear() += vtmp.angular().cross(oMlast.translation());
230
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 40 motionSet::motionAction(vtmp, v_partial_dv_cols, v_partial_dq_cols);
231 40 break;
232 138 case LOCAL:
233
2/2
✓ Branch 0 taken 58 times.
✓ Branch 1 taken 11 times.
 138 if (parent > 0)
234 {
235
2/4
✓ Branch 2 taken 58 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 58 times.
✗ Branch 6 not taken.
 116 vtmp = oMlast.actInv(data.ov[parent]);
236
1/2
✓ Branch 1 taken 58 times.
✗ Branch 2 not taken.
 116 motionSet::motionAction(vtmp, v_partial_dv_cols, v_partial_dq_cols);
237 }
238 138 break;
239 ✗ default:
240 ✗ assert(false && "This must never happened");
241 }
242 }
243 };
244
245 template<
246 typename Scalar,
247 int Options,
248 template<typename, int>
249 class JointCollectionTpl,
250 typename Matrix6xOut1,
251 typename Matrix6xOut2>
252 21 void getJointVelocityDerivatives(
253 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
254 const DataTpl<Scalar, Options, JointCollectionTpl> & data,
255 const Model::JointIndex jointId,
256 const ReferenceFrame rf,
257 const Eigen::MatrixBase<Matrix6xOut1> & v_partial_dq,
258 const Eigen::MatrixBase<Matrix6xOut2> & v_partial_dv)
259 {
260 EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(Matrix6xOut1, Data::Matrix6x);
261 EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(Matrix6xOut2, Data::Matrix6x);
262
263 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
264
265
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 21 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.
 21 PINOCCHIO_CHECK_ARGUMENT_SIZE(v_partial_dq.cols(), model.nv);
266
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 21 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.
 21 PINOCCHIO_CHECK_ARGUMENT_SIZE(v_partial_dv.cols(), model.nv);
267
1/4
✗ Branch 0 not taken.
✓ Branch 1 taken 21 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
 21 PINOCCHIO_CHECK_INPUT_ARGUMENT((int)jointId < model.njoints, "The joint id is invalid.");
268
1/2
✓ Branch 1 taken 21 times.
✗ Branch 2 not taken.
 21 assert(model.check(data) && "data is not consistent with model.");
269
270 typedef typename Model::JointIndex JointIndex;
271
272 typedef JointVelocityDerivativesBackwardStep<
273 Scalar, Options, JointCollectionTpl, Matrix6xOut1, Matrix6xOut2>
274 Pass1;
275
2/2
✓ Branch 0 taken 123 times.
✓ Branch 1 taken 21 times.
 144 for (JointIndex i = jointId; i > 0; i = model.parents[i])
276 {
277
1/2
✓ Branch 1 taken 123 times.
✗ Branch 2 not taken.
 123 Pass1::run(
278 123 model.joints[i],
279 246 typename Pass1::ArgsType(
280 123 model, data, jointId, rf, PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut1, v_partial_dq),
281 123 PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut2, v_partial_dv)));
282 }
283 21 }
284
285 template<
286 typename Scalar,
287 int Options,
288 template<typename, int>
289 class JointCollectionTpl,
290 typename Matrix6xOut1,
291 typename Matrix6xOut2,
292 typename Matrix6xOut3,
293 typename Matrix6xOut4>
294 struct JointAccelerationDerivativesBackwardStep
295 : public fusion::JointUnaryVisitorBase<JointAccelerationDerivativesBackwardStep<
296 Scalar,
297 Options,
298 JointCollectionTpl,
299 Matrix6xOut1,
300 Matrix6xOut2,
301 Matrix6xOut3,
302 Matrix6xOut4>>
303 {
304 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
305 typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
306
307 typedef boost::fusion::vector<
308 const Model &,
309 const Data &,
310 const typename Model::JointIndex &,
311 const ReferenceFrame &,
312 Matrix6xOut1 &,
313 Matrix6xOut2 &,
314 Matrix6xOut3 &,
315 Matrix6xOut4 &>
316 ArgsType;
317
318 template<typename JointModel>
319 570 static void algo(
320 const JointModelBase<JointModel> & jmodel,
321 const Model & model,
322 const Data & data,
323 const typename Model::JointIndex & jointId,
324 const ReferenceFrame & rf,
325 const Eigen::MatrixBase<Matrix6xOut1> & v_partial_dq,
326 const Eigen::MatrixBase<Matrix6xOut2> & a_partial_dq,
327 const Eigen::MatrixBase<Matrix6xOut3> & a_partial_dv,
328 const Eigen::MatrixBase<Matrix6xOut4> & a_partial_da)
329 {
330 typedef typename Model::JointIndex JointIndex;
331 typedef typename Data::SE3 SE3;
332 typedef typename Data::Motion Motion;
333
334
1/2
✓ Branch 1 taken 285 times.
✗ Branch 2 not taken.
 570 const JointIndex & i = jmodel.id();
335 570 const JointIndex & parent = model.parents[i];
336
1/2
✓ Branch 1 taken 285 times.
✗ Branch 2 not taken.
 570 Motion vtmp; // Temporary variable
337
1/2
✓ Branch 1 taken 285 times.
✗ Branch 2 not taken.
 570 Motion atmp; // Temporary variable
338
339 570 const SE3 & oMlast = data.oMi[jointId];
340 570 const Motion & vlast = data.ov[jointId];
341 570 const Motion & alast = data.oa[jointId];
342
343 typedef typename SizeDepType<JointModel::NV>::template ColsReturn<
344 typename Data::Matrix6x>::ConstType ColsBlock;
345
1/2
✓ Branch 1 taken 285 times.
✗ Branch 2 not taken.
 570 ColsBlock dJcols = jmodel.jointCols(data.dJ);
346
1/2
✓ Branch 1 taken 285 times.
✗ Branch 2 not taken.
 570 ColsBlock Jcols = jmodel.jointCols(data.J);
347
348 typedef typename SizeDepType<JointModel::NV>::template ColsReturn<Matrix6xOut1>::Type
349 ColsBlockOut1;
350 570 Matrix6xOut1 & v_partial_dq_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut1, v_partial_dq);
351 typedef typename SizeDepType<JointModel::NV>::template ColsReturn<Matrix6xOut2>::Type
352 ColsBlockOut2;
353 570 Matrix6xOut2 & a_partial_dq_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut2, a_partial_dq);
354 typedef typename SizeDepType<JointModel::NV>::template ColsReturn<Matrix6xOut3>::Type
355 ColsBlockOut3;
356 570 Matrix6xOut3 & a_partial_dv_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut3, a_partial_dv);
357 typedef typename SizeDepType<JointModel::NV>::template ColsReturn<Matrix6xOut4>::Type
358 ColsBlockOut4;
359 570 Matrix6xOut4 & a_partial_da_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut4, a_partial_da);
360
361
1/2
✓ Branch 1 taken 285 times.
✗ Branch 2 not taken.
 570 ColsBlockOut1 v_partial_dq_cols = jmodel.jointCols(v_partial_dq_);
362
1/2
✓ Branch 1 taken 285 times.
✗ Branch 2 not taken.
 570 ColsBlockOut2 a_partial_dq_cols = jmodel.jointCols(a_partial_dq_);
363
1/2
✓ Branch 1 taken 285 times.
✗ Branch 2 not taken.
 570 ColsBlockOut3 a_partial_dv_cols = jmodel.jointCols(a_partial_dv_);
364
1/2
✓ Branch 1 taken 285 times.
✗ Branch 2 not taken.
 570 ColsBlockOut4 a_partial_da_cols = jmodel.jointCols(a_partial_da_);
365
366 // dacc/da
367
3/4
✓ Branch 0 taken 45 times.
✓ Branch 1 taken 62 times.
✓ Branch 2 taken 178 times.
✗ Branch 3 not taken.
 570 switch (rf)
368 {
369 90 case WORLD:
370
1/2
✓ Branch 1 taken 45 times.
✗ Branch 2 not taken.
 90 a_partial_da_cols = Jcols;
371 90 break;
372 124 case LOCAL_WORLD_ALIGNED:
373
1/2
✓ Branch 1 taken 62 times.
✗ Branch 2 not taken.
 124 details::translateJointJacobian(oMlast, Jcols, a_partial_da_cols);
374 124 break;
375 356 case LOCAL:
376
1/2
✓ Branch 1 taken 178 times.
✗ Branch 2 not taken.
 356 motionSet::se3ActionInverse(oMlast, Jcols, a_partial_da_cols);
377 356 break;
378 }
379
380 // dacc/dv
381
3/4
✓ Branch 0 taken 45 times.
✓ Branch 1 taken 62 times.
✓ Branch 2 taken 178 times.
✗ Branch 3 not taken.
 570 switch (rf)
382 {
383 90 case WORLD:
384
2/2
✓ Branch 0 taken 37 times.
✓ Branch 1 taken 8 times.
 90 if (parent > 0)
385
2/4
✓ Branch 2 taken 37 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 37 times.
✗ Branch 6 not taken.
 74 vtmp = data.ov[parent] - vlast;
386 else
387
2/4
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
 16 vtmp = -vlast;
388
389 // also computes dvec/dq
390
1/2
✓ Branch 1 taken 45 times.
✗ Branch 2 not taken.
 90 motionSet::motionAction(vtmp, Jcols, v_partial_dq_cols);
391
392
2/4
✓ Branch 1 taken 45 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 45 times.
✗ Branch 5 not taken.
 90 a_partial_dv_cols = v_partial_dq_cols + dJcols;
393 90 break;
394 124 case LOCAL_WORLD_ALIGNED:
395
2/2
✓ Branch 0 taken 52 times.
✓ Branch 1 taken 10 times.
 124 if (parent > 0)
396
2/4
✓ Branch 2 taken 52 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 52 times.
✗ Branch 6 not taken.
 104 vtmp = data.ov[parent] - vlast;
397 else
398
2/4
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10 times.
✗ Branch 5 not taken.
 20 vtmp = -vlast;
399
5/10
✓ Branch 1 taken 62 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 62 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 62 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 62 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 62 times.
✗ Branch 14 not taken.
 124 vtmp.linear() += vtmp.angular().cross(oMlast.translation());
400
401 // also computes dvec/dq
402
1/2
✓ Branch 1 taken 62 times.
✗ Branch 2 not taken.
 124 motionSet::motionAction(vtmp, a_partial_da_cols, v_partial_dq_cols);
403
404
1/2
✓ Branch 1 taken 62 times.
✗ Branch 2 not taken.
 124 details::translateJointJacobian(oMlast, dJcols, a_partial_dv_cols);
405 // a_partial_dv_cols += v_partial_dq_cols; // dJcols is required later
406 124 break;
407 356 case LOCAL:
408 // also computes dvec/dq
409
2/2
✓ Branch 0 taken 152 times.
✓ Branch 1 taken 26 times.
 356 if (parent > 0)
410 {
411
2/4
✓ Branch 2 taken 152 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 152 times.
✗ Branch 6 not taken.
 304 vtmp = oMlast.actInv(data.ov[parent]);
412
1/2
✓ Branch 1 taken 152 times.
✗ Branch 2 not taken.
 304 motionSet::motionAction(vtmp, a_partial_da_cols, v_partial_dq_cols);
413 }
414
415
2/2
✓ Branch 0 taken 152 times.
✓ Branch 1 taken 26 times.
 356 if (parent > 0)
416
1/2
✓ Branch 2 taken 152 times.
✗ Branch 3 not taken.
 304 vtmp -= data.v[jointId];
417 else
418
2/4
✓ Branch 2 taken 26 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 26 times.
✗ Branch 6 not taken.
 52 vtmp = -data.v[jointId];
419
420
1/2
✓ Branch 1 taken 178 times.
✗ Branch 2 not taken.
 356 motionSet::motionAction(vtmp, a_partial_da_cols, a_partial_dv_cols);
421
1/2
✓ Branch 1 taken 178 times.
✗ Branch 2 not taken.
 356 motionSet::se3ActionInverse<ADDTO>(oMlast, dJcols, a_partial_dv_cols);
422 356 break;
423 }
424
425 // dacc/dq
426
3/4
✓ Branch 0 taken 45 times.
✓ Branch 1 taken 62 times.
✓ Branch 2 taken 178 times.
✗ Branch 3 not taken.
 570 switch (rf)
427 {
428 90 case WORLD:
429
2/2
✓ Branch 0 taken 37 times.
✓ Branch 1 taken 8 times.
 90 if (parent > 0)
430
2/4
✓ Branch 2 taken 37 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 37 times.
✗ Branch 6 not taken.
 74 atmp = data.oa[parent] - alast;
431 else
432
2/4
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
 16 atmp = -alast;
433
1/2
✓ Branch 1 taken 45 times.
✗ Branch 2 not taken.
 90 motionSet::motionAction(atmp, Jcols, a_partial_dq_cols);
434
435
2/2
✓ Branch 0 taken 37 times.
✓ Branch 1 taken 8 times.
 90 if (parent > 0)
436
1/2
✓ Branch 1 taken 37 times.
✗ Branch 2 not taken.
 74 motionSet::motionAction<ADDTO>(vtmp, dJcols, a_partial_dq_cols);
437 90 break;
438 124 case LOCAL_WORLD_ALIGNED:
439
2/2
✓ Branch 0 taken 52 times.
✓ Branch 1 taken 10 times.
 124 if (parent > 0)
440
2/4
✓ Branch 2 taken 52 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 52 times.
✗ Branch 6 not taken.
 104 atmp = data.oa[parent] - alast;
441 else
442
2/4
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10 times.
✗ Branch 5 not taken.
 20 atmp = -alast;
443
444
5/10
✓ Branch 1 taken 62 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 62 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 62 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 62 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 62 times.
✗ Branch 14 not taken.
 124 atmp.linear() += atmp.angular().cross(oMlast.translation());
445
1/2
✓ Branch 1 taken 62 times.
✗ Branch 2 not taken.
 124 motionSet::motionAction(atmp, a_partial_da_cols, a_partial_dq_cols);
446
447
2/2
✓ Branch 0 taken 52 times.
✓ Branch 1 taken 10 times.
 124 if (parent > 0)
448
1/2
✓ Branch 1 taken 52 times.
✗ Branch 2 not taken.
 104 motionSet::motionAction<ADDTO>(vtmp, a_partial_dv_cols, a_partial_dq_cols);
449
450
1/2
✓ Branch 1 taken 62 times.
✗ Branch 2 not taken.
 124 a_partial_dv_cols += v_partial_dq_cols;
451 124 break;
452 356 case LOCAL:
453
2/2
✓ Branch 0 taken 152 times.
✓ Branch 1 taken 26 times.
 356 if (parent > 0)
454 {
455
2/4
✓ Branch 2 taken 152 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 152 times.
✗ Branch 6 not taken.
 304 atmp = oMlast.actInv(data.oa[parent]);
456
1/2
✓ Branch 1 taken 152 times.
✗ Branch 2 not taken.
 304 motionSet::motionAction(atmp, a_partial_da_cols, a_partial_dq_cols);
457
458
1/2
✓ Branch 1 taken 152 times.
✗ Branch 2 not taken.
 304 motionSet::motionAction<ADDTO>(vtmp, v_partial_dq_cols, a_partial_dq_cols);
459 }
460
461 356 break;
462 }
463 }
464 };
465
466 template<
467 typename Scalar,
468 int Options,
469 template<typename, int>
470 class JointCollectionTpl,
471 typename Matrix6xOut1,
472 typename Matrix6xOut2,
473 typename Matrix6xOut3,
474 typename Matrix6xOut4>
475 44 void getJointAccelerationDerivatives(
476 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
477 const DataTpl<Scalar, Options, JointCollectionTpl> & data,
478 const Model::JointIndex jointId,
479 const ReferenceFrame rf,
480 const Eigen::MatrixBase<Matrix6xOut1> & v_partial_dq,
481 const Eigen::MatrixBase<Matrix6xOut2> & a_partial_dq,
482 const Eigen::MatrixBase<Matrix6xOut3> & a_partial_dv,
483 const Eigen::MatrixBase<Matrix6xOut4> & a_partial_da)
484 {
485
486 EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(Matrix6xOut1, Data::Matrix6x);
487 EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(Matrix6xOut2, Data::Matrix6x);
488 EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(Matrix6xOut3, Data::Matrix6x);
489 EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(Matrix6xOut4, Data::Matrix6x);
490
491 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
492
493
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 44 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.
 44 PINOCCHIO_CHECK_ARGUMENT_SIZE(v_partial_dq.cols(), model.nv);
494
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 44 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.
 44 PINOCCHIO_CHECK_ARGUMENT_SIZE(a_partial_dq.cols(), model.nv);
495
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 44 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.
 44 PINOCCHIO_CHECK_ARGUMENT_SIZE(a_partial_dv.cols(), model.nv);
496
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 44 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.
 44 PINOCCHIO_CHECK_ARGUMENT_SIZE(a_partial_da.cols(), model.nv);
497
1/4
✗ Branch 0 not taken.
✓ Branch 1 taken 44 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
 44 PINOCCHIO_CHECK_INPUT_ARGUMENT((int)jointId < model.njoints, "The joint id is invalid.");
498
1/2
✓ Branch 1 taken 44 times.
✗ Branch 2 not taken.
 44 assert(model.check(data) && "data is not consistent with model.");
499
500 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
501 typedef typename Model::JointIndex JointIndex;
502
503 typedef JointAccelerationDerivativesBackwardStep<
504 Scalar, Options, JointCollectionTpl, Matrix6xOut1, Matrix6xOut2, Matrix6xOut3, Matrix6xOut4>
505 Pass1;
506
2/2
✓ Branch 0 taken 285 times.
✓ Branch 1 taken 44 times.
 329 for (JointIndex i = jointId; i > 0; i = model.parents[i])
507 {
508
1/2
✓ Branch 1 taken 285 times.
✗ Branch 2 not taken.
 285 Pass1::run(
509 285 model.joints[i],
510 570 typename Pass1::ArgsType(
511 285 model, data, jointId, rf, PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut1, v_partial_dq),
512 285 PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut2, a_partial_dq),
513 285 PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut3, a_partial_dv),
514 285 PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut4, a_partial_da)));
515 }
516 44 }
517
518 template<
519 typename Scalar,
520 int Options,
521 template<typename, int>
522 class JointCollectionTpl,
523 typename Matrix6xOut1,
524 typename Matrix6xOut2,
525 typename Matrix6xOut3,
526 typename Matrix6xOut4,
527 typename Matrix6xOut5>
528 2 void getJointAccelerationDerivatives(
529 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
530 const DataTpl<Scalar, Options, JointCollectionTpl> & data,
531 const Model::JointIndex jointId,
532 const ReferenceFrame rf,
533 const Eigen::MatrixBase<Matrix6xOut1> & v_partial_dq,
534 const Eigen::MatrixBase<Matrix6xOut2> & v_partial_dv,
535 const Eigen::MatrixBase<Matrix6xOut3> & a_partial_dq,
536 const Eigen::MatrixBase<Matrix6xOut4> & a_partial_dv,
537 const Eigen::MatrixBase<Matrix6xOut5> & a_partial_da)
538 {
539 2 impl::getJointAccelerationDerivatives(
540 2 model, data, jointId, rf, PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut1, v_partial_dq),
541 2 PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut3, a_partial_dq),
542 2 PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut4, a_partial_dv),
543 2 PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut5, a_partial_da));
544
545 2 PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut2, v_partial_dv) = a_partial_da;
546 2 }
547
548 template<
549 typename Scalar,
550 int Options,
551 template<typename, int>
552 class JointCollectionTpl,
553 typename Matrix3xOut1,
554 typename Matrix3xOut2>
555 struct PointVelocityDerivativesBackwardStep
556 : public fusion::JointUnaryVisitorBase<PointVelocityDerivativesBackwardStep<
557 Scalar,
558 Options,
559 JointCollectionTpl,
560 Matrix3xOut1,
561 Matrix3xOut2>>
562 {
563 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
564 typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
565
566 typedef boost::fusion::vector<
567 const Model &,
568 const Data &,
569 const typename Data::SE3 &,
570 const typename Data::Motion &,
571 const ReferenceFrame &,
572 Matrix3xOut1 &,
573 Matrix3xOut2 &>
574 ArgsType;
575
576 template<typename JointModel>
577 28 static void algo(
578 const JointModelBase<JointModel> & jmodel,
579 const Model & model,
580 const Data & data,
581 const typename Data::SE3 & oMpoint,
582 const typename Data::Motion & spatial_point_velocity,
583 const ReferenceFrame & rf,
584 const Eigen::MatrixBase<Matrix3xOut1> & v_partial_dq,
585 const Eigen::MatrixBase<Matrix3xOut2> & v_partial_dv)
586 {
587 typedef typename Model::JointIndex JointIndex;
588 typedef typename Data::SE3 SE3;
589 typedef typename Data::Motion Motion;
590
591
1/2
✓ Branch 1 taken 14 times.
✗ Branch 2 not taken.
 28 const JointIndex i = jmodel.id();
592 28 const JointIndex parent = model.parents[i];
593
1/2
✓ Branch 1 taken 14 times.
✗ Branch 2 not taken.
 28 Motion vtmp; // Temporary variable
594
595 28 const SE3 & oMlast = oMpoint;
596
597 typedef typename SizeDepType<JointModel::NV>::template ColsReturn<
598 typename Data::Matrix6x>::ConstType ColsBlock;
599
1/2
✓ Branch 1 taken 14 times.
✗ Branch 2 not taken.
 28 ColsBlock Jcols = jmodel.jointCols(data.J);
600
601 typedef typename SizeDepType<JointModel::NV>::template ColsReturn<Matrix3xOut1>::Type
602 ColsBlockOut1;
603 28 Matrix3xOut1 & v_partial_dq_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix3xOut1, v_partial_dq);
604 typedef typename SizeDepType<JointModel::NV>::template ColsReturn<Matrix3xOut2>::Type
605 ColsBlockOut2;
606 28 Matrix3xOut2 & v_partial_dv_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix3xOut2, v_partial_dv);
607
608
1/2
✓ Branch 1 taken 14 times.
✗ Branch 2 not taken.
 28 ColsBlockOut1 v_partial_dq_cols = jmodel.jointCols(v_partial_dq_);
609
1/2
✓ Branch 1 taken 14 times.
✗ Branch 2 not taken.
 28 ColsBlockOut2 v_partial_dv_cols = jmodel.jointCols(v_partial_dv_);
610
611
1/2
✓ Branch 1 taken 14 times.
✗ Branch 2 not taken.
 28 const int nv = jmodel.nv();
612
1/2
✓ Branch 1 taken 14 times.
✗ Branch 2 not taken.
 28 Eigen::Matrix<Scalar, 6, JointModel::NV, Options> v_spatial_partial_dv_cols(6, nv);
613
614 #define FOR_NV() for (Eigen::DenseIndex j = 0; j < nv; ++j)
615 #define GET_LINEAR(vec6) vec6.template segment<3>(Motion::LINEAR)
616 #define GET_ANGULAR(vec6) vec6.template segment<3>(Motion::ANGULAR)
617
618 // dvec/dv
619
1/2
✓ Branch 1 taken 14 times.
✗ Branch 2 not taken.
 28 motionSet::se3ActionInverse(oMlast, Jcols, v_spatial_partial_dv_cols);
620
2/4
✓ Branch 1 taken 14 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 14 times.
✗ Branch 5 not taken.
 28 v_partial_dv_cols = v_spatial_partial_dv_cols.template middleRows<3>(Motion::LINEAR);
621
622 // dvec/dq
623
2/2
✓ Branch 0 taken 12 times.
✓ Branch 1 taken 2 times.
 28 if (parent > 0)
624 {
625
2/4
✓ Branch 2 taken 12 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 12 times.
✗ Branch 6 not taken.
 24 vtmp = oMlast.actInv(data.ov[parent]);
626
2/2
✓ Branch 0 taken 12 times.
✓ Branch 1 taken 12 times.
 48 FOR_NV()
627
6/12
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 12 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 12 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 12 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 12 times.
✗ Branch 17 not taken.
 48 v_partial_dq_cols.col(j).noalias() =
628
3/6
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 12 times.
✗ Branch 8 not taken.
 24 vtmp.angular().cross(GET_LINEAR(v_spatial_partial_dv_cols.col(j)))
629
3/6
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 12 times.
✗ Branch 8 not taken.
 48 + vtmp.linear().cross(GET_ANGULAR(v_spatial_partial_dv_cols.col(j)));
630 }
631 else
632
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 4 v_partial_dq_cols.setZero();
633
634
2/2
✓ Branch 0 taken 7 times.
✓ Branch 1 taken 7 times.
 28 if (rf == LOCAL_WORLD_ALIGNED)
635 {
636
2/2
✓ Branch 0 taken 12 times.
✓ Branch 1 taken 7 times.
 38 FOR_NV()
637
6/12
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 12 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 12 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 12 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 12 times.
✗ Branch 17 not taken.
 48 v_partial_dq_cols.col(j) = oMlast.rotation()
638
1/2
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
 24 * (v_partial_dq_cols.col(j)
639
2/4
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12 times.
✗ Branch 5 not taken.
 24 + GET_ANGULAR(v_spatial_partial_dv_cols.col(j))
640
1/2
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
 48 .cross(spatial_point_velocity.linear()));
641
2/2
✓ Branch 0 taken 12 times.
✓ Branch 1 taken 7 times.
 38 FOR_NV()
642
5/10
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 12 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 12 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 12 times.
✗ Branch 14 not taken.
 24 v_partial_dv_cols.col(j) = oMlast.rotation() * v_partial_dv_cols.col(j);
643 }
644
645 #undef FOR_NV
646 #undef GET_LINEAR
647 #undef GET_ANGULAR
648 }
649 };
650
651 template<
652 typename Scalar,
653 int Options,
654 template<typename, int>
655 class JointCollectionTpl,
656 typename Matrix3xOut1,
657 typename Matrix3xOut2>
658 2 void getPointVelocityDerivatives(
659 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
660 const DataTpl<Scalar, Options, JointCollectionTpl> & data,
661 const Model::JointIndex joint_id,
662 const SE3Tpl<Scalar, Options> & placement,
663 const ReferenceFrame rf,
664 const Eigen::MatrixBase<Matrix3xOut1> & v_point_partial_dq,
665 const Eigen::MatrixBase<Matrix3xOut2> & v_point_partial_dv)
666 {
667 EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(Matrix3xOut1, Data::Matrix3x);
668 EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(Matrix3xOut2, Data::Matrix3x);
669
670 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
671 typedef typename Model::JointIndex JointIndex;
672 typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
673
674
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(v_point_partial_dq.cols(), model.nv);
675
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(v_point_partial_dv.cols(), model.nv);
676
1/4
✗ Branch 0 not taken.
✓ Branch 1 taken 2 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
 2 PINOCCHIO_CHECK_INPUT_ARGUMENT((int)joint_id < model.njoints, "The joint id is invalid.");
677
3/6
✓ Branch 0 taken 1 times.
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 1 times.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
 2 PINOCCHIO_CHECK_INPUT_ARGUMENT(
678 rf == LOCAL || rf == LOCAL_WORLD_ALIGNED,
679 "The reference frame is not valid, expected LOCAL or LOCAL_WORLD_ALIGNED");
680
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 2 times.
✗ Branch 4 not taken.
 2 assert(model.check(data) && "data is not consistent with model.");
681
682 typedef typename Data::SE3 SE3;
683 typedef typename Data::Motion Motion;
684
685
1/2
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
 2 const SE3 oMpoint = data.oMi[joint_id] * placement;
686
1/2
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
 2 const Motion spatial_velocity = oMpoint.actInv(data.ov[joint_id]);
687
688 typedef PointVelocityDerivativesBackwardStep<
689 Scalar, Options, JointCollectionTpl, Matrix3xOut1, Matrix3xOut2>
690 Pass1;
691
2/2
✓ Branch 0 taken 14 times.
✓ Branch 1 taken 2 times.
 16 for (JointIndex i = joint_id; i > 0; i = model.parents[i])
692 {
693
1/2
✓ Branch 1 taken 14 times.
✗ Branch 2 not taken.
 14 Pass1::run(
694
1/2
✓ Branch 1 taken 14 times.
✗ Branch 2 not taken.
 28 model.joints[i], typename Pass1::ArgsType(
695 model, data, oMpoint, spatial_velocity, rf,
696 14 PINOCCHIO_EIGEN_CONST_CAST(Matrix3xOut1, v_point_partial_dq),
697 14 PINOCCHIO_EIGEN_CONST_CAST(Matrix3xOut2, v_point_partial_dv)));
698 }
699 2 }
700
701 template<
702 typename Scalar,
703 int Options,
704 template<typename, int>
705 class JointCollectionTpl,
706 typename Matrix3xOut1,
707 typename Matrix3xOut2,
708 typename Matrix3xOut3,
709 typename Matrix3xOut4>
710 struct PointClassicAccelerationDerivativesBackwardStep
711 : public fusion::JointUnaryVisitorBase<PointClassicAccelerationDerivativesBackwardStep<
712 Scalar,
713 Options,
714 JointCollectionTpl,
715 Matrix3xOut1,
716 Matrix3xOut2,
717 Matrix3xOut3,
718 Matrix3xOut4>>
719 {
720 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
721 typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
722
723 typedef boost::fusion::vector<
724 const Model &,
725 const Data &,
726 const typename Data::SE3 &,
727 const typename Data::Motion &,
728 const typename Data::Motion::Vector3 &,
729 const ReferenceFrame &,
730 Matrix3xOut1 &,
731 Matrix3xOut2 &,
732 Matrix3xOut3 &,
733 Matrix3xOut4 &>
734 ArgsType;
735
736 template<typename JointModel>
737 396 static void algo(
738 const JointModelBase<JointModel> & jmodel,
739 const Model & model,
740 const Data & data,
741 const typename Data::SE3 & oMpoint,
742 const typename Data::Motion & spatial_point_velocity,
743 const typename Data::Motion::Vector3 & point_classic_acceleration,
744 const ReferenceFrame & rf,
745 const Eigen::MatrixBase<Matrix3xOut1> & v_partial_dq,
746 const Eigen::MatrixBase<Matrix3xOut2> & a_partial_dq,
747 const Eigen::MatrixBase<Matrix3xOut3> & a_partial_dv,
748 const Eigen::MatrixBase<Matrix3xOut4> & a_partial_da)
749 {
750 typedef typename Model::JointIndex JointIndex;
751 typedef typename Data::SE3 SE3;
752 typedef typename Data::Motion Motion;
753
754
1/2
✓ Branch 1 taken 198 times.
✗ Branch 2 not taken.
 396 const JointIndex i = jmodel.id();
755 396 const JointIndex parent = model.parents[i];
756
1/2
✓ Branch 1 taken 198 times.
✗ Branch 2 not taken.
 396 Motion vtmp; // Temporary variable
757
1/2
✓ Branch 1 taken 198 times.
✗ Branch 2 not taken.
 396 Motion atmp; // Temporary variable
758
759 396 const SE3 & oMlast = oMpoint;
760 396 const Motion & v_last = spatial_point_velocity;
761
762 typedef typename SizeDepType<JointModel::NV>::template ColsReturn<
763 typename Data::Matrix6x>::ConstType ColsBlock;
764
1/2
✓ Branch 1 taken 198 times.
✗ Branch 2 not taken.
 396 ColsBlock dJcols = jmodel.jointCols(data.dJ);
765
1/2
✓ Branch 1 taken 198 times.
✗ Branch 2 not taken.
 396 ColsBlock Jcols = jmodel.jointCols(data.J);
766
767 typedef typename SizeDepType<JointModel::NV>::template ColsReturn<Matrix3xOut1>::Type
768 ColsBlockOut1;
769 396 Matrix3xOut1 & v_partial_dq_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix3xOut1, v_partial_dq);
770 typedef typename SizeDepType<JointModel::NV>::template ColsReturn<Matrix3xOut2>::Type
771 ColsBlockOut2;
772 396 Matrix3xOut2 & a_partial_dq_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix3xOut2, a_partial_dq);
773 typedef typename SizeDepType<JointModel::NV>::template ColsReturn<Matrix3xOut3>::Type
774 ColsBlockOut3;
775 396 Matrix3xOut3 & a_partial_dv_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix3xOut3, a_partial_dv);
776 typedef typename SizeDepType<JointModel::NV>::template ColsReturn<Matrix3xOut4>::Type
777 ColsBlockOut4;
778 396 Matrix3xOut4 & a_partial_da_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix3xOut4, a_partial_da);
779
780
1/2
✓ Branch 1 taken 198 times.
✗ Branch 2 not taken.
 396 ColsBlockOut1 v_partial_dq_cols = jmodel.jointCols(v_partial_dq_);
781
1/2
✓ Branch 1 taken 198 times.
✗ Branch 2 not taken.
 396 ColsBlockOut2 a_partial_dq_cols = jmodel.jointCols(a_partial_dq_);
782
1/2
✓ Branch 1 taken 198 times.
✗ Branch 2 not taken.
 396 ColsBlockOut3 a_partial_dv_cols = jmodel.jointCols(a_partial_dv_);
783
1/2
✓ Branch 1 taken 198 times.
✗ Branch 2 not taken.
 396 ColsBlockOut4 a_partial_da_cols = jmodel.jointCols(a_partial_da_);
784
785
1/2
✓ Branch 1 taken 198 times.
✗ Branch 2 not taken.
 396 const int nv = jmodel.nv();
786
1/2
✓ Branch 1 taken 198 times.
✗ Branch 2 not taken.
 396 Eigen::Matrix<Scalar, 6, JointModel::NV, Options> a_spatial_partial_da_cols(6, nv);
787
1/2
✓ Branch 1 taken 198 times.
✗ Branch 2 not taken.
 396 Eigen::Matrix<Scalar, 6, JointModel::NV, Options> v_spatial_partial_dq_cols(6, nv);
788
789 #define FOR_NV() for (Eigen::DenseIndex j = 0; j < nv; ++j)
790 #define GET_LINEAR(vec6) vec6.template segment<3>(Motion::LINEAR)
791 #define GET_ANGULAR(vec6) vec6.template segment<3>(Motion::ANGULAR)
792
793 // dacc/da
794
1/2
✓ Branch 1 taken 198 times.
✗ Branch 2 not taken.
 396 motionSet::se3ActionInverse(oMlast, Jcols, a_spatial_partial_da_cols);
795
2/4
✓ Branch 1 taken 198 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 198 times.
✗ Branch 5 not taken.
 396 a_partial_da_cols = a_spatial_partial_da_cols.template middleRows<3>(Motion::LINEAR);
796
797 // dacc/dv
798 // also computes dvec/dq
799
2/2
✓ Branch 0 taken 171 times.
✓ Branch 1 taken 27 times.
 396 if (parent > 0)
800 {
801
2/4
✓ Branch 2 taken 171 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 171 times.
✗ Branch 6 not taken.
 342 vtmp = oMlast.actInv(data.ov[parent]);
802
1/2
✓ Branch 1 taken 171 times.
✗ Branch 2 not taken.
 342 motionSet::motionAction(vtmp, a_spatial_partial_da_cols, v_spatial_partial_dq_cols);
803
2/4
✓ Branch 1 taken 171 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 171 times.
✗ Branch 5 not taken.
 342 v_partial_dq_cols = v_spatial_partial_dq_cols.template middleRows<3>(Motion::LINEAR);
804 }
805 else
806
1/2
✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
 54 v_partial_dq_cols.setZero();
807
808
2/2
✓ Branch 0 taken 171 times.
✓ Branch 1 taken 27 times.
 396 if (parent > 0)
809
1/2
✓ Branch 1 taken 171 times.
✗ Branch 2 not taken.
 342 vtmp -= v_last;
810 else
811
2/4
✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 27 times.
✗ Branch 5 not taken.
 54 vtmp = -v_last;
812
813 // motionSet::motionAction(vtmp,a_partial_da_cols,a_partial_dv_cols);
814
2/2
✓ Branch 0 taken 333 times.
✓ Branch 1 taken 198 times.
 1062 FOR_NV()
815
6/12
✓ Branch 1 taken 333 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 333 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 333 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 333 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 333 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 333 times.
✗ Branch 17 not taken.
 1332 a_partial_dv_cols.col(j).noalias() =
816
3/6
✓ Branch 1 taken 333 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 333 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 333 times.
✗ Branch 8 not taken.
 666 vtmp.angular().cross(GET_LINEAR(a_spatial_partial_da_cols.col(j)))
817
3/6
✓ Branch 1 taken 333 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 333 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 333 times.
✗ Branch 8 not taken.
 1332 + vtmp.linear().cross(GET_ANGULAR(a_spatial_partial_da_cols.col(j)));
818 // motionSet::se3ActionInverse<ADDTO>(oMlast,dJcols,a_partial_dv_cols);
819
2/2
✓ Branch 0 taken 333 times.
✓ Branch 1 taken 198 times.
 1062 FOR_NV()
820
4/8
✓ Branch 1 taken 333 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 333 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 333 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 333 times.
✗ Branch 11 not taken.
 1332 a_partial_dv_cols.col(j) +=
821
2/4
✓ Branch 1 taken 333 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 333 times.
✗ Branch 5 not taken.
 666 oMlast.rotation().transpose()
822
6/12
✓ Branch 1 taken 333 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 333 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 333 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 333 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 333 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 333 times.
✗ Branch 17 not taken.
 1332 * (GET_LINEAR(dJcols.col(j)) + GET_ANGULAR(dJcols.col(j)).cross(oMlast.translation()));
823 // wxv
824
2/2
✓ Branch 0 taken 333 times.
✓ Branch 1 taken 198 times.
 1062 FOR_NV()
825
5/10
✓ Branch 1 taken 333 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 333 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 333 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 333 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 333 times.
✗ Branch 14 not taken.
 1332 a_partial_dv_cols.col(j) +=
826
3/6
✓ Branch 1 taken 333 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 333 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 333 times.
✗ Branch 8 not taken.
 666 v_last.angular().cross(GET_LINEAR(a_spatial_partial_da_cols.col(j)))
827
3/6
✓ Branch 1 taken 333 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 333 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 333 times.
✗ Branch 8 not taken.
 1332 - v_last.linear().cross(GET_ANGULAR(a_spatial_partial_da_cols.col(j)));
828
829 // dacc/dq
830
2/2
✓ Branch 0 taken 171 times.
✓ Branch 1 taken 27 times.
 396 if (parent > 0)
831 {
832
2/4
✓ Branch 2 taken 171 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 171 times.
✗ Branch 6 not taken.
 342 atmp = oMlast.actInv(data.oa[parent]);
833 // motionSet::motionAction(atmp,a_partial_da_cols,a_partial_dq_cols);
834
2/2
✓ Branch 0 taken 171 times.
✓ Branch 1 taken 171 times.
 684 FOR_NV()
835
6/12
✓ Branch 1 taken 171 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 171 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 171 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 171 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 171 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 171 times.
✗ Branch 17 not taken.
 684 a_partial_dq_cols.col(j).noalias() =
836
3/6
✓ Branch 1 taken 171 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 171 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 171 times.
✗ Branch 8 not taken.
 342 atmp.angular().cross(GET_LINEAR(a_spatial_partial_da_cols.col(j)))
837
3/6
✓ Branch 1 taken 171 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 171 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 171 times.
✗ Branch 8 not taken.
 684 + atmp.linear().cross(GET_ANGULAR(a_spatial_partial_da_cols.col(j)));
838
839 // motionSet::motionAction<ADDTO>(vtmp,v_partial_dq_cols,a_partial_dq_cols);
840
2/2
✓ Branch 0 taken 171 times.
✓ Branch 1 taken 171 times.
 684 FOR_NV()
841
5/10
✓ Branch 1 taken 171 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 171 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 171 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 171 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 171 times.
✗ Branch 14 not taken.
 684 a_partial_dq_cols.col(j) +=
842
3/6
✓ Branch 1 taken 171 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 171 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 171 times.
✗ Branch 8 not taken.
 342 vtmp.angular().cross(GET_LINEAR(v_spatial_partial_dq_cols.col(j)))
843
3/6
✓ Branch 1 taken 171 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 171 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 171 times.
✗ Branch 8 not taken.
 684 + vtmp.linear().cross(GET_ANGULAR(v_spatial_partial_dq_cols.col(j)));
844
845 // wxv
846
2/2
✓ Branch 0 taken 171 times.
✓ Branch 1 taken 171 times.
 684 FOR_NV()
847
5/10
✓ Branch 1 taken 171 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 171 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 171 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 171 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 171 times.
✗ Branch 14 not taken.
 684 a_partial_dq_cols.col(j) +=
848
3/6
✓ Branch 1 taken 171 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 171 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 171 times.
✗ Branch 8 not taken.
 342 v_last.angular().cross(GET_LINEAR(v_spatial_partial_dq_cols.col(j)))
849
3/6
✓ Branch 1 taken 171 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 171 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 171 times.
✗ Branch 8 not taken.
 684 - v_last.linear().cross(GET_ANGULAR(v_spatial_partial_dq_cols.col(j)));
850 }
851 else
852
1/2
✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
 54 a_partial_dq_cols.setZero();
853
854
2/2
✓ Branch 0 taken 53 times.
✓ Branch 1 taken 145 times.
 396 if (rf == LOCAL_WORLD_ALIGNED)
855 {
856
857
2/2
✓ Branch 0 taken 88 times.
✓ Branch 1 taken 53 times.
 282 FOR_NV()
858
6/12
✓ Branch 1 taken 88 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 88 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 88 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 88 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 88 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 88 times.
✗ Branch 17 not taken.
 352 v_partial_dq_cols.col(j) = oMlast.rotation()
859
1/2
✓ Branch 1 taken 88 times.
✗ Branch 2 not taken.
 176 * (v_partial_dq_cols.col(j)
860
2/4
✓ Branch 1 taken 88 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 88 times.
✗ Branch 5 not taken.
 176 + GET_ANGULAR(a_spatial_partial_da_cols.col(j))
861
1/2
✓ Branch 1 taken 88 times.
✗ Branch 2 not taken.
 352 .cross(spatial_point_velocity.linear()));
862
2/2
✓ Branch 0 taken 88 times.
✓ Branch 1 taken 53 times.
 282 FOR_NV()
863
4/8
✓ Branch 1 taken 88 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 88 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 88 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 88 times.
✗ Branch 11 not taken.
 528 a_partial_dq_cols.col(j) =
864
2/4
✓ Branch 1 taken 88 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 88 times.
✗ Branch 5 not taken.
 176 oMlast.rotation()
865
3/6
✓ Branch 1 taken 88 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 88 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 88 times.
✗ Branch 8 not taken.
 528 * (a_partial_dq_cols.col(j) + GET_ANGULAR(a_spatial_partial_da_cols.col(j)).cross(point_classic_acceleration));
866
2/2
✓ Branch 0 taken 88 times.
✓ Branch 1 taken 53 times.
 282 FOR_NV()
867
5/10
✓ Branch 1 taken 88 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 88 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 88 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 88 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 88 times.
✗ Branch 14 not taken.
 176 a_partial_dv_cols.col(j) = oMlast.rotation() * a_partial_dv_cols.col(j);
868
2/2
✓ Branch 0 taken 88 times.
✓ Branch 1 taken 53 times.
 282 FOR_NV()
869
5/10
✓ Branch 1 taken 88 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 88 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 88 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 88 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 88 times.
✗ Branch 14 not taken.
 176 a_partial_da_cols.col(j) = oMlast.rotation() * a_partial_da_cols.col(j);
870 }
871
872 #undef FOR_NV
873 #undef GET_LINEAR
874 #undef GET_ANGULAR
875 }
876 };
877
878 template<
879 typename Scalar,
880 int Options,
881 template<typename, int>
882 class JointCollectionTpl,
883 typename Matrix3xOut1,
884 typename Matrix3xOut2,
885 typename Matrix3xOut3,
886 typename Matrix3xOut4>
887 27 void getPointClassicAccelerationDerivatives(
888 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
889 const DataTpl<Scalar, Options, JointCollectionTpl> & data,
890 const Model::JointIndex joint_id,
891 const SE3Tpl<Scalar, Options> & placement,
892 const ReferenceFrame rf,
893 const Eigen::MatrixBase<Matrix3xOut1> & v_point_partial_dq,
894 const Eigen::MatrixBase<Matrix3xOut2> & a_point_partial_dq,
895 const Eigen::MatrixBase<Matrix3xOut3> & a_point_partial_dv,
896 const Eigen::MatrixBase<Matrix3xOut4> & a_point_partial_da)
897 {
898 EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(Matrix3xOut1, Data::Matrix3x);
899 EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(Matrix3xOut2, Data::Matrix3x);
900 EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(Matrix3xOut3, Data::Matrix3x);
901 EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(Matrix3xOut4, Data::Matrix3x);
902
903 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
904 typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
905 typedef typename Data::SE3 SE3;
906 typedef typename Data::Motion Motion;
907 typedef typename SE3::Vector3 Vector3;
908 typedef typename Model::JointIndex JointIndex;
909
910
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 27 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.
 27 PINOCCHIO_CHECK_ARGUMENT_SIZE(v_point_partial_dq.cols(), model.nv);
911
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 27 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.
 27 PINOCCHIO_CHECK_ARGUMENT_SIZE(a_point_partial_dq.cols(), model.nv);
912
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 27 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.
 27 PINOCCHIO_CHECK_ARGUMENT_SIZE(a_point_partial_dv.cols(), model.nv);
913
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 27 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.
 27 PINOCCHIO_CHECK_ARGUMENT_SIZE(a_point_partial_da.cols(), model.nv);
914
1/4
✗ Branch 0 not taken.
✓ Branch 1 taken 27 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
 27 PINOCCHIO_CHECK_INPUT_ARGUMENT((int)joint_id < model.njoints, "The joint id is invalid.");
915
3/6
✓ Branch 0 taken 7 times.
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 7 times.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
 27 PINOCCHIO_CHECK_INPUT_ARGUMENT(
916 rf == LOCAL || rf == LOCAL_WORLD_ALIGNED,
917 "The reference frame is not valid, expected LOCAL or LOCAL_WORLD_ALIGNED");
918
2/4
✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 27 times.
✗ Branch 4 not taken.
 27 assert(model.check(data) && "data is not consistent with model.");
919
920
1/2
✓ Branch 2 taken 27 times.
✗ Branch 3 not taken.
 27 const SE3 oMpoint = data.oMi[joint_id] * placement;
921
1/2
✓ Branch 2 taken 27 times.
✗ Branch 3 not taken.
 27 const Motion spatial_velocity = oMpoint.actInv(data.ov[joint_id]);
922
1/2
✓ Branch 2 taken 27 times.
✗ Branch 3 not taken.
 27 const Motion spatial_acceleration = oMpoint.actInv(data.oa[joint_id]);
923
1/2
✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
 27 const Vector3 point_acc = classicAcceleration(spatial_velocity, spatial_acceleration);
924
925 typedef PointClassicAccelerationDerivativesBackwardStep<
926 Scalar, Options, JointCollectionTpl, Matrix3xOut1, Matrix3xOut2, Matrix3xOut3, Matrix3xOut4>
927 Pass1;
928
2/2
✓ Branch 0 taken 198 times.
✓ Branch 1 taken 27 times.
 225 for (JointIndex i = joint_id; i > 0; i = model.parents[i])
929 {
930
1/2
✓ Branch 1 taken 198 times.
✗ Branch 2 not taken.
 198 Pass1::run(
931
1/2
✓ Branch 1 taken 198 times.
✗ Branch 2 not taken.
 396 model.joints[i], typename Pass1::ArgsType(
932 model, data, oMpoint, spatial_velocity, point_acc, rf,
933 198 PINOCCHIO_EIGEN_CONST_CAST(Matrix3xOut1, v_point_partial_dq),
934 198 PINOCCHIO_EIGEN_CONST_CAST(Matrix3xOut2, a_point_partial_dq),
935 198 PINOCCHIO_EIGEN_CONST_CAST(Matrix3xOut3, a_point_partial_dv),
936 198 PINOCCHIO_EIGEN_CONST_CAST(Matrix3xOut4, a_point_partial_da)));
937 }
938 27 }
939
940 template<
941 typename Scalar,
942 int Options,
943 template<typename, int>
944 class JointCollectionTpl,
945 typename Matrix3xOut1,
946 typename Matrix3xOut2,
947 typename Matrix3xOut3,
948 typename Matrix3xOut4,
949 typename Matrix3xOut5>
950 3 void getPointClassicAccelerationDerivatives(
951 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
952 const DataTpl<Scalar, Options, JointCollectionTpl> & data,
953 const Model::JointIndex joint_id,
954 const SE3Tpl<Scalar, Options> & placement,
955 const ReferenceFrame rf,
956 const Eigen::MatrixBase<Matrix3xOut1> & v_point_partial_dq,
957 const Eigen::MatrixBase<Matrix3xOut2> & v_point_partial_dv,
958 const Eigen::MatrixBase<Matrix3xOut3> & a_point_partial_dq,
959 const Eigen::MatrixBase<Matrix3xOut4> & a_point_partial_dv,
960 const Eigen::MatrixBase<Matrix3xOut5> & a_point_partial_da)
961 {
962 EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(Matrix3xOut2, Data::Matrix3x);
963
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(v_point_partial_dv.cols(), model.nv);
964 3 impl::getPointClassicAccelerationDerivatives(
965 model, data, joint_id, placement, rf,
966 3 PINOCCHIO_EIGEN_CONST_CAST(Matrix3xOut1, v_point_partial_dq),
967 3 PINOCCHIO_EIGEN_CONST_CAST(Matrix3xOut3, a_point_partial_dq),
968 3 PINOCCHIO_EIGEN_CONST_CAST(Matrix3xOut4, a_point_partial_dv),
969 3 PINOCCHIO_EIGEN_CONST_CAST(Matrix3xOut5, a_point_partial_da));
970
971 3 PINOCCHIO_EIGEN_CONST_CAST(Matrix3xOut2, v_point_partial_dv) = a_point_partial_da;
972 3 }
973
974 } // namespace impl
975 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
976 2 void computeJointKinematicHessians(
977 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
978 DataTpl<Scalar, Options, JointCollectionTpl> & data)
979 {
980
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 assert(model.check(data) && "data is not consistent with model.");
981
982 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
983 typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
984
985 typedef MotionRef<const typename Data::Matrix6x::ConstColXpr> MotionIn;
986
987 typedef typename Data::Motion Motion;
988 typedef Eigen::Map<typename Motion::Vector6> MapVector6;
989 typedef MotionRef<MapVector6> MotionOut;
990
991 2 const typename Data::Matrix6x & J = data.J;
992 2 typename Data::Tensor3x & kinematic_hessians = data.kinematic_hessians;
993 2 const Eigen::DenseIndex slice_matrix_size = 6 * model.nv;
994
995
2/2
✓ Branch 0 taken 54 times.
✓ Branch 1 taken 2 times.
 56 for (size_t joint_id = 1; joint_id < (size_t)model.njoints; ++joint_id)
996 {
997 54 const std::vector<typename Model::JointIndex> & subtree = model.subtrees[joint_id];
998 54 const std::vector<typename Model::JointIndex> & support = model.supports[joint_id];
999
1000 54 const int nv = model.nvs[joint_id];
1001 54 const int idx_v = model.idx_vs[joint_id];
1002
1003
2/2
✓ Branch 0 taken 64 times.
✓ Branch 1 taken 54 times.
 118 for (int joint_row = 0; joint_row < nv; ++joint_row)
1004 {
1005 64 const Eigen::DenseIndex outer_row_id = idx_v + joint_row;
1006
1007
2/2
✓ Branch 1 taken 286 times.
✓ Branch 2 taken 64 times.
 350 for (size_t support_id = 0; support_id < support.size() - 1; ++support_id)
1008 {
1009 286 const typename Model::JointIndex joint_id_support = support[support_id];
1010
1011 286 const int inner_nv = model.nvs[joint_id_support];
1012 286 const int inner_idx_v = model.idx_vs[joint_id_support];
1013
2/2
✓ Branch 0 taken 482 times.
✓ Branch 1 taken 286 times.
 768 for (int inner_joint_row = 0; inner_joint_row < inner_nv; ++inner_joint_row)
1014 {
1015 482 const Eigen::DenseIndex inner_row_id = inner_idx_v + inner_joint_row;
1016
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 482 times.
 482 assert(inner_row_id < outer_row_id);
1017
1018
1/2
✓ Branch 1 taken 482 times.
✗ Branch 2 not taken.
 482 MapVector6 motion_vec_in(
1019
2/4
✓ Branch 1 taken 482 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 482 times.
✗ Branch 5 not taken.
 482 kinematic_hessians.data() + inner_row_id * slice_matrix_size + outer_row_id * 6);
1020
1/2
✓ Branch 1 taken 482 times.
✗ Branch 2 not taken.
 482 MapVector6 motion_vec_out(
1021
2/4
✓ Branch 1 taken 482 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 482 times.
✗ Branch 5 not taken.
 482 kinematic_hessians.data() + outer_row_id * slice_matrix_size + inner_row_id * 6);
1022
1023
2/4
✓ Branch 1 taken 482 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 482 times.
✗ Branch 5 not taken.
 482 motion_vec_out = -motion_vec_in;
1024 }
1025 }
1026
1027
2/4
✓ Branch 1 taken 64 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 64 times.
✗ Branch 5 not taken.
 64 const MotionIn S1(J.col(outer_row_id));
1028
1029 // Computations already done
1030
2/2
✓ Branch 0 taken 30 times.
✓ Branch 1 taken 64 times.
 94 for (int inner_joint_row = 0; inner_joint_row < joint_row; ++inner_joint_row)
1031 {
1032 30 const Eigen::DenseIndex inner_row_id = idx_v + inner_joint_row;
1033
1/2
✓ Branch 1 taken 30 times.
✗ Branch 2 not taken.
 30 MapVector6 motion_vec_in(
1034
2/4
✓ Branch 1 taken 30 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 30 times.
✗ Branch 5 not taken.
 30 kinematic_hessians.data() + inner_row_id * slice_matrix_size + outer_row_id * 6);
1035
1/2
✓ Branch 1 taken 30 times.
✗ Branch 2 not taken.
 30 MapVector6 motion_vec_out(
1036
2/4
✓ Branch 1 taken 30 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 30 times.
✗ Branch 5 not taken.
 30 kinematic_hessians.data() + outer_row_id * slice_matrix_size + inner_row_id * 6);
1037
1038
2/4
✓ Branch 1 taken 30 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 30 times.
✗ Branch 5 not taken.
 30 motion_vec_out = -motion_vec_in;
1039 }
1040
1041
2/2
✓ Branch 0 taken 30 times.
✓ Branch 1 taken 64 times.
 94 for (int inner_joint_row = joint_row + 1; inner_joint_row < nv; ++inner_joint_row)
1042 {
1043 30 const Eigen::DenseIndex inner_row_id = idx_v + inner_joint_row;
1044
2/4
✓ Branch 1 taken 30 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 30 times.
✗ Branch 5 not taken.
 30 const MotionIn S2(J.col(inner_row_id));
1045
1046
1/2
✓ Branch 1 taken 30 times.
✗ Branch 2 not taken.
 30 MapVector6 motion_vec_out(
1047
2/4
✓ Branch 1 taken 30 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 30 times.
✗ Branch 5 not taken.
 30 kinematic_hessians.data() + outer_row_id * slice_matrix_size + inner_row_id * 6);
1048
2/4
✓ Branch 1 taken 30 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 30 times.
✗ Branch 5 not taken.
 30 MotionOut S1xS2(motion_vec_out);
1049
1050
2/4
✓ Branch 1 taken 30 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 30 times.
✗ Branch 5 not taken.
 30 S1xS2 = S1.cross(S2);
1051 }
1052
1053
2/2
✓ Branch 1 taken 482 times.
✓ Branch 2 taken 64 times.
 546 for (size_t subtree_id = 1; subtree_id < subtree.size(); ++subtree_id)
1054 {
1055 482 const typename Model::JointIndex joint_id_subtree = subtree[subtree_id];
1056
1057 482 const int inner_nv = model.nvs[joint_id_subtree];
1058 482 const int inner_idx_v = model.idx_vs[joint_id_subtree];
1059
2/2
✓ Branch 0 taken 482 times.
✓ Branch 1 taken 482 times.
 964 for (int inner_joint_row = 0; inner_joint_row < inner_nv; ++inner_joint_row)
1060 {
1061 482 const Eigen::DenseIndex inner_row_id = inner_idx_v + inner_joint_row;
1062
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 482 times.
 482 assert(inner_row_id > outer_row_id);
1063
2/4
✓ Branch 1 taken 482 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 482 times.
✗ Branch 5 not taken.
 482 const MotionIn S2(J.col(inner_row_id));
1064
1065
1/2
✓ Branch 1 taken 482 times.
✗ Branch 2 not taken.
 482 MapVector6 motion_vec_out(
1066
2/4
✓ Branch 1 taken 482 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 482 times.
✗ Branch 5 not taken.
 482 kinematic_hessians.data() + outer_row_id * slice_matrix_size + inner_row_id * 6);
1067
2/4
✓ Branch 1 taken 482 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 482 times.
✗ Branch 5 not taken.
 482 MotionOut S1xS2(motion_vec_out);
1068
1069
2/4
✓ Branch 1 taken 482 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 482 times.
✗ Branch 5 not taken.
 482 S1xS2 = S1.cross(S2);
1070 }
1071 }
1072 }
1073 }
1074 2 }
1075
1076 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
1077 3 void getJointKinematicHessian(
1078 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
1079 const DataTpl<Scalar, Options, JointCollectionTpl> & data,
1080 const JointIndex joint_id,
1081 const ReferenceFrame rf,
1082 Tensor<Scalar, 3, Options> & kinematic_hessian)
1083 {
1084
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 assert(model.check(data) && "data is not consistent with model.");
1085
2/4
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 3 times.
✗ Branch 4 not taken.
 3 assert(
1086 joint_id < model.joints.size() && joint_id > 0
1087 && "joint_id is outside the valid index for a joint in model.joints");
1088
1089 typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
1090 typedef typename Data::SE3 SE3;
1091 typedef typename Data::Motion Motion;
1092
1093 3 const typename Data::Matrix6x & J = data.J;
1094 3 const typename Data::Tensor3x & kinematic_hessians = data.kinematic_hessians;
1095
1096 // Allocate memory
1097
1/26
✗ 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 16 not taken.
✗ Branch 17 not taken.
✗ Branch 19 not taken.
✗ Branch 20 not taken.
✗ Branch 22 not taken.
✗ Branch 23 not taken.
✗ Branch 25 not taken.
✗ Branch 26 not taken.
✗ Branch 28 not taken.
✗ Branch 29 not taken.
✗ Branch 31 not taken.
✗ Branch 32 not taken.
✗ Branch 35 not taken.
✗ Branch 36 not taken.
✗ Branch 38 not taken.
✗ Branch 39 not taken.
 3 PINOCCHIO_CHECK_ARGUMENT_SIZE(
1098 kinematic_hessian.dimension(0), 6, "The result tensor is not of the right dimension.");
1099
1/26
✗ 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 16 not taken.
✗ Branch 17 not taken.
✗ Branch 19 not taken.
✗ Branch 20 not taken.
✗ Branch 22 not taken.
✗ Branch 23 not taken.
✗ Branch 25 not taken.
✗ Branch 26 not taken.
✗ Branch 28 not taken.
✗ Branch 29 not taken.
✗ Branch 31 not taken.
✗ Branch 32 not taken.
✗ Branch 35 not taken.
✗ Branch 36 not taken.
✗ Branch 38 not taken.
✗ Branch 39 not taken.
 3 PINOCCHIO_CHECK_ARGUMENT_SIZE(
1100 kinematic_hessian.dimension(1), model.nv, "The result tensor is not of the right dimension.");
1101
1/26
✗ 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 16 not taken.
✗ Branch 17 not taken.
✗ Branch 19 not taken.
✗ Branch 20 not taken.
✗ Branch 22 not taken.
✗ Branch 23 not taken.
✗ Branch 25 not taken.
✗ Branch 26 not taken.
✗ Branch 28 not taken.
✗ Branch 29 not taken.
✗ Branch 31 not taken.
✗ Branch 32 not taken.
✗ Branch 35 not taken.
✗ Branch 36 not taken.
✗ Branch 38 not taken.
✗ Branch 39 not taken.
 3 PINOCCHIO_CHECK_ARGUMENT_SIZE(
1102 kinematic_hessian.dimension(2), model.nv, "The result tensor is not of the right dimension.");
1103
1104 3 const int idx_vj = model.joints[joint_id].idx_v();
1105 3 const int nvj = model.joints[joint_id].nv();
1106 3 const Eigen::DenseIndex slice_matrix_size = 6 * model.nv;
1107
1108 typedef std::vector<int> IndexVector;
1109 3 const Eigen::DenseIndex last_idx = idx_vj + nvj - 1;
1110 const std::vector<int> & supporting_indexes =
1111 3 data.supports_fromRow[(size_t)(last_idx)]; // until the last element of the joint size (nvj)
1112
1113 typedef Eigen::Map<typename Motion::Vector6> MapVector6;
1114 typedef MotionRef<MapVector6> MotionOut;
1115 typedef Eigen::Map<const typename Motion::Vector6> ConstMapVector6;
1116 typedef MotionRef<ConstMapVector6> MotionIn;
1117
1118
3/4
✓ Branch 0 taken 1 times.
✓ Branch 1 taken 1 times.
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 3 switch (rf)
1119 {
1120 1 case WORLD: {
1121
2/2
✓ Branch 1 taken 10 times.
✓ Branch 2 taken 1 times.
 11 for (size_t i = 0; i < supporting_indexes.size(); ++i)
1122 {
1123 10 const Eigen::DenseIndex outer_row_id = supporting_indexes[i];
1124
1125 // Take into account parent indexes of the current joint motion subspace
1126 25 for (int subspace_idx = data.start_idx_v_fromRow[(size_t)outer_row_id];
1127
2/2
✓ Branch 0 taken 15 times.
✓ Branch 1 taken 10 times.
 25 subspace_idx < outer_row_id; ++subspace_idx)
1128 {
1129
1/2
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
 15 ConstMapVector6 vec_in(
1130
2/4
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 15 times.
✗ Branch 5 not taken.
 15 kinematic_hessians.data() + outer_row_id * slice_matrix_size + subspace_idx * 6);
1131
1132
1/2
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
 15 MapVector6 vec_out(
1133
2/4
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 15 times.
✗ Branch 5 not taken.
 15 kinematic_hessian.data() + outer_row_id * slice_matrix_size + subspace_idx * 6);
1134
1135
1/2
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
 15 vec_out = vec_in;
1136 }
1137
1138
2/2
✓ Branch 1 taken 45 times.
✓ Branch 2 taken 10 times.
 55 for (size_t j = i + 1; j < supporting_indexes.size(); ++j)
1139 {
1140 45 const Eigen::DenseIndex inner_row_id = supporting_indexes[j];
1141
1142
1/2
✓ Branch 1 taken 45 times.
✗ Branch 2 not taken.
 45 ConstMapVector6 vec_in(
1143
2/4
✓ Branch 1 taken 45 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 45 times.
✗ Branch 5 not taken.
 45 kinematic_hessians.data() + outer_row_id * slice_matrix_size + inner_row_id * 6);
1144
1145
1/2
✓ Branch 1 taken 45 times.
✗ Branch 2 not taken.
 45 MapVector6 vec_out(
1146
2/4
✓ Branch 1 taken 45 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 45 times.
✗ Branch 5 not taken.
 45 kinematic_hessian.data() + outer_row_id * slice_matrix_size + inner_row_id * 6);
1147
1148
1/2
✓ Branch 1 taken 45 times.
✗ Branch 2 not taken.
 45 vec_out = vec_in;
1149 }
1150 }
1151 1 break;
1152 }
1153 1 case LOCAL_WORLD_ALIGNED: {
1154 typedef MotionRef<const typename Data::Matrix6x::ConstColXpr> MotionColRef;
1155 1 const SE3 & oMlast = data.oMi[joint_id];
1156
1157
2/2
✓ Branch 1 taken 10 times.
✓ Branch 2 taken 1 times.
 11 for (size_t i = 0; i < supporting_indexes.size(); ++i)
1158 {
1159 10 const Eigen::DenseIndex outer_row_id = supporting_indexes[i];
1160
2/4
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10 times.
✗ Branch 5 not taken.
 10 const MotionColRef S1(J.col(outer_row_id));
1161
1162
1/2
✓ Branch 1 taken 40 times.
✗ Branch 2 not taken.
 40 for (size_t j = 0; j < supporting_indexes.size(); ++j)
1163 {
1164 40 const Eigen::DenseIndex inner_row_id = supporting_indexes[j];
1165
2/2
✓ Branch 1 taken 10 times.
✓ Branch 2 taken 30 times.
 40 if (inner_row_id >= data.start_idx_v_fromRow[(size_t)outer_row_id])
1166 10 break;
1167
1168
2/4
✓ Branch 1 taken 30 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 30 times.
✗ Branch 5 not taken.
 30 MotionColRef S2(J.col(inner_row_id));
1169
1170
1/2
✓ Branch 1 taken 30 times.
✗ Branch 2 not taken.
 30 ConstMapVector6 vec_in(
1171
2/4
✓ Branch 1 taken 30 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 30 times.
✗ Branch 5 not taken.
 30 kinematic_hessians.data() + outer_row_id * slice_matrix_size + inner_row_id * 6);
1172
2/4
✓ Branch 1 taken 30 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 30 times.
✗ Branch 5 not taken.
 30 MotionIn S1xS2(vec_in);
1173
1174
1/2
✓ Branch 1 taken 30 times.
✗ Branch 2 not taken.
 30 MapVector6 vec_out(
1175
2/4
✓ Branch 1 taken 30 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 30 times.
✗ Branch 5 not taken.
 30 kinematic_hessian.data() + outer_row_id * slice_matrix_size + inner_row_id * 6);
1176
2/4
✓ Branch 1 taken 30 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 30 times.
✗ Branch 5 not taken.
 30 MotionOut m_out(vec_out);
1177
1178
6/12
✓ Branch 1 taken 30 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 30 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 30 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 30 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 30 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 30 times.
✗ Branch 17 not taken.
 90 m_out.linear() =
1179
4/8
✓ Branch 1 taken 30 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 30 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 30 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 30 times.
✗ Branch 11 not taken.
 120 -(S1.linear() - oMlast.translation().cross(S1.angular())).cross(S2.angular());
1180 }
1181
1182 // Take into account parent indexes of the current joint motion subspace
1183 10 for (int inner_row_id = data.start_idx_v_fromRow[(size_t)outer_row_id];
1184
2/2
✓ Branch 0 taken 15 times.
✓ Branch 1 taken 10 times.
 25 inner_row_id < outer_row_id; ++inner_row_id)
1185 {
1186
2/4
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 15 times.
✗ Branch 5 not taken.
 15 MotionColRef S2(J.col(inner_row_id));
1187
1188
1/2
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
 15 ConstMapVector6 vec_in(
1189
2/4
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 15 times.
✗ Branch 5 not taken.
 15 kinematic_hessians.data() + outer_row_id * slice_matrix_size + inner_row_id * 6);
1190
2/4
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 15 times.
✗ Branch 5 not taken.
 15 MotionIn S1xS2(vec_in);
1191
1192
1/2
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
 15 MapVector6 vec_out(
1193
2/4
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 15 times.
✗ Branch 5 not taken.
 15 kinematic_hessian.data() + outer_row_id * slice_matrix_size + inner_row_id * 6);
1194
2/4
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 15 times.
✗ Branch 5 not taken.
 15 MotionOut m_out(vec_out);
1195
1196
1/2
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
 15 vec_out = vec_in;
1197
7/14
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 15 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 15 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 15 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 15 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 15 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 15 times.
✗ Branch 20 not taken.
 60 m_out.linear() -=
1198
4/8
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 15 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 15 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 15 times.
✗ Branch 11 not taken.
 60 (S1.linear() - oMlast.translation().cross(S1.angular())).cross(S2.angular())
1199
2/4
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 15 times.
✗ Branch 5 not taken.
 15 + oMlast.translation().cross(S1xS2.angular());
1200 }
1201
1202 // case: outer_row_id == inner_row_id
1203 {
1204
1/2
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
 10 MapVector6 vec_out(
1205
2/4
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10 times.
✗ Branch 5 not taken.
 10 kinematic_hessian.data() + outer_row_id * slice_matrix_size + outer_row_id * 6);
1206
2/4
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10 times.
✗ Branch 5 not taken.
 10 MotionOut m_out(vec_out);
1207
1208
6/12
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 10 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 10 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 10 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 10 times.
✗ Branch 17 not taken.
 30 m_out.linear() =
1209
4/8
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 10 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 10 times.
✗ Branch 11 not taken.
 30 -(S1.linear() - oMlast.translation().cross(S1.angular())).cross(S1.angular());
1210 }
1211
1212
2/2
✓ Branch 1 taken 45 times.
✓ Branch 2 taken 10 times.
 55 for (size_t j = i + 1; j < supporting_indexes.size(); ++j)
1213 {
1214 45 const Eigen::DenseIndex inner_row_id = supporting_indexes[j];
1215
2/4
✓ Branch 1 taken 45 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 45 times.
✗ Branch 5 not taken.
 45 MotionColRef S2(J.col(inner_row_id));
1216
1217
1/2
✓ Branch 1 taken 45 times.
✗ Branch 2 not taken.
 45 ConstMapVector6 vec_in(
1218
2/4
✓ Branch 1 taken 45 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 45 times.
✗ Branch 5 not taken.
 45 kinematic_hessians.data() + outer_row_id * slice_matrix_size + inner_row_id * 6);
1219
2/4
✓ Branch 1 taken 45 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 45 times.
✗ Branch 5 not taken.
 45 MotionIn S1xS2(vec_in);
1220
1221
1/2
✓ Branch 1 taken 45 times.
✗ Branch 2 not taken.
 45 MapVector6 vec_out(
1222
2/4
✓ Branch 1 taken 45 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 45 times.
✗ Branch 5 not taken.
 45 kinematic_hessian.data() + outer_row_id * slice_matrix_size + inner_row_id * 6);
1223
2/4
✓ Branch 1 taken 45 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 45 times.
✗ Branch 5 not taken.
 45 MotionOut m_out(vec_out);
1224
1225
1/2
✓ Branch 1 taken 45 times.
✗ Branch 2 not taken.
 45 vec_out = vec_in;
1226
7/14
✓ Branch 1 taken 45 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 45 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 45 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 45 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 45 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 45 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 45 times.
✗ Branch 20 not taken.
 180 m_out.linear() -=
1227
4/8
✓ Branch 1 taken 45 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 45 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 45 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 45 times.
✗ Branch 11 not taken.
 180 (S1.linear() - oMlast.translation().cross(S1.angular())).cross(S2.angular())
1228
2/4
✓ Branch 1 taken 45 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 45 times.
✗ Branch 5 not taken.
 45 + oMlast.translation().cross(S1xS2.angular());
1229 }
1230 }
1231 1 break;
1232 }
1233 1 case LOCAL: {
1234 1 const SE3 & oMlast = data.oMi[joint_id];
1235
1236 1 for (IndexVector::const_reverse_iterator rit = supporting_indexes.rbegin();
1237
3/4
✓ Branch 3 taken 11 times.
✗ Branch 4 not taken.
✓ Branch 5 taken 10 times.
✓ Branch 6 taken 1 times.
 11 rit != supporting_indexes.rend(); ++rit)
1238 {
1239 10 const Eigen::DenseIndex outer_row_id = *rit;
1240
1241 // This corresponds to the joint connected to the world, we can skip
1242
2/2
✓ Branch 2 taken 6 times.
✓ Branch 3 taken 4 times.
 10 if (data.parents_fromRow[(size_t)data.start_idx_v_fromRow[(size_t)outer_row_id]] < 0)
1243 6 continue;
1244
1245 // Take into account current joint motion subspace
1246 4 for (int subspace_idx = data.end_idx_v_fromRow[(size_t)outer_row_id];
1247
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 4 times.
 4 subspace_idx > outer_row_id; --subspace_idx)
1248 {
1249 ✗ ConstMapVector6 vec_in(
1250 ✗ kinematic_hessians.data() + subspace_idx * slice_matrix_size + outer_row_id * 6);
1251 ✗ MotionIn m_in(vec_in);
1252
1253 ✗ MapVector6 vec_out(
1254 ✗ kinematic_hessian.data() + outer_row_id * slice_matrix_size + subspace_idx * 6);
1255 ✗ MotionOut m_out(vec_out);
1256
1257 ✗ m_out = oMlast.actInv(m_in);
1258 }
1259
1260 4 IndexVector::const_reverse_iterator inner_rit = rit;
1261
3/4
✓ Branch 3 taken 34 times.
✗ Branch 4 not taken.
✓ Branch 5 taken 30 times.
✓ Branch 6 taken 4 times.
 34 for (++inner_rit; inner_rit != supporting_indexes.rend(); ++inner_rit)
1262 {
1263 30 const Eigen::DenseIndex inner_row_id = *inner_rit;
1264
1265
1/2
✓ Branch 1 taken 30 times.
✗ Branch 2 not taken.
 30 ConstMapVector6 vec_in(
1266
2/4
✓ Branch 1 taken 30 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 30 times.
✗ Branch 5 not taken.
 30 kinematic_hessians.data() + inner_row_id * slice_matrix_size + outer_row_id * 6);
1267
1268
2/4
✓ Branch 1 taken 30 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 30 times.
✗ Branch 5 not taken.
 30 MotionIn m_in(vec_in);
1269
1270
1/2
✓ Branch 1 taken 30 times.
✗ Branch 2 not taken.
 30 MapVector6 vec_out(
1271
2/4
✓ Branch 1 taken 30 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 30 times.
✗ Branch 5 not taken.
 30 kinematic_hessian.data() + outer_row_id * slice_matrix_size + inner_row_id * 6);
1272
2/4
✓ Branch 1 taken 30 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 30 times.
✗ Branch 5 not taken.
 30 MotionOut m_out(vec_out);
1273
1274
2/4
✓ Branch 1 taken 30 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 30 times.
✗ Branch 5 not taken.
 30 m_out = oMlast.actInv(m_in);
1275 }
1276 }
1277
1278 1 break;
1279 }
1280 ✗ default:
1281 ✗ assert(false && "must never happened");
1282 break;
1283 }
1284 3 }
1285
1286 template<
1287 typename Scalar,
1288 int Options,
1289 template<typename, int>
1290 class JointCollectionTpl,
1291 typename ConfigVectorType,
1292 typename TangentVectorType1,
1293 typename TangentVectorType2>
1294 3 void computeForwardKinematicsDerivatives(
1295 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
1296 DataTpl<Scalar, Options, JointCollectionTpl> & data,
1297 const Eigen::MatrixBase<ConfigVectorType> & q,
1298 const Eigen::MatrixBase<TangentVectorType1> & v,
1299 const Eigen::MatrixBase<TangentVectorType2> & a)
1300 {
1301
3/6
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 3 times.
✗ Branch 9 not taken.
 3 impl::computeForwardKinematicsDerivatives(
1302 model, data, make_const_ref(q), make_const_ref(v), make_const_ref(a));
1303 3 }
1304
1305 template<
1306 typename Scalar,
1307 int Options,
1308 template<typename, int>
1309 class JointCollectionTpl,
1310 typename Matrix6xOut1,
1311 typename Matrix6xOut2>
1312 14 void getJointVelocityDerivatives(
1313 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
1314 const DataTpl<Scalar, Options, JointCollectionTpl> & data,
1315 const Model::JointIndex jointId,
1316 const ReferenceFrame rf,
1317 const Eigen::MatrixBase<Matrix6xOut1> & v_partial_dq,
1318 const Eigen::MatrixBase<Matrix6xOut2> & v_partial_dv)
1319 {
1320
2/4
✓ Branch 2 taken 12 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 12 times.
✗ Branch 6 not taken.
 14 impl::getJointVelocityDerivatives(
1321 model, data, jointId, rf, make_ref(v_partial_dq), make_ref(v_partial_dv));
1322 14 }
1323
1324 template<
1325 typename Scalar,
1326 int Options,
1327 template<typename, int>
1328 class JointCollectionTpl,
1329 typename Matrix6xOut1,
1330 typename Matrix6xOut2,
1331 typename Matrix6xOut3,
1332 typename Matrix6xOut4>
1333 29 void getJointAccelerationDerivatives(
1334 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
1335 const DataTpl<Scalar, Options, JointCollectionTpl> & data,
1336 const Model::JointIndex jointId,
1337 const ReferenceFrame rf,
1338 const Eigen::MatrixBase<Matrix6xOut1> & v_partial_dq,
1339 const Eigen::MatrixBase<Matrix6xOut2> & a_partial_dq,
1340 const Eigen::MatrixBase<Matrix6xOut3> & a_partial_dv,
1341 const Eigen::MatrixBase<Matrix6xOut4> & a_partial_da)
1342 {
1343
4/8
✓ Branch 2 taken 29 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 29 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 29 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 29 times.
✗ Branch 12 not taken.
 29 impl::getJointAccelerationDerivatives(
1344 model, data, jointId, rf, make_ref(v_partial_dq), make_ref(a_partial_dq),
1345 make_ref(a_partial_dv), make_ref(a_partial_da));
1346 29 }
1347
1348 template<
1349 typename Scalar,
1350 int Options,
1351 template<typename, int>
1352 class JointCollectionTpl,
1353 typename Matrix6xOut1,
1354 typename Matrix6xOut2,
1355 typename Matrix6xOut3,
1356 typename Matrix6xOut4,
1357 typename Matrix6xOut5>
1358 ✗ void getJointAccelerationDerivatives(
1359 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
1360 const DataTpl<Scalar, Options, JointCollectionTpl> & data,
1361 const Model::JointIndex jointId,
1362 const ReferenceFrame rf,
1363 const Eigen::MatrixBase<Matrix6xOut1> & v_partial_dq,
1364 const Eigen::MatrixBase<Matrix6xOut2> & v_partial_dv,
1365 const Eigen::MatrixBase<Matrix6xOut3> & a_partial_dq,
1366 const Eigen::MatrixBase<Matrix6xOut4> & a_partial_dv,
1367 const Eigen::MatrixBase<Matrix6xOut5> & a_partial_da)
1368 {
1369 ✗ impl::getJointAccelerationDerivatives(
1370 model, data, jointId, rf, make_ref(v_partial_dq), make_ref(v_partial_dv),
1371 make_ref(a_partial_dq), make_ref(a_partial_dv), make_ref(a_partial_da));
1372 }
1373
1374 template<
1375 typename Scalar,
1376 int Options,
1377 template<typename, int>
1378 class JointCollectionTpl,
1379 typename Matrix3xOut1,
1380 typename Matrix3xOut2>
1381 ✗ void getPointVelocityDerivatives(
1382 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
1383 const DataTpl<Scalar, Options, JointCollectionTpl> & data,
1384 const Model::JointIndex joint_id,
1385 const SE3Tpl<Scalar, Options> & placement,
1386 const ReferenceFrame rf,
1387 const Eigen::MatrixBase<Matrix3xOut1> & v_point_partial_dq,
1388 const Eigen::MatrixBase<Matrix3xOut2> & v_point_partial_dv)
1389 {
1390 ✗ impl::getPointVelocityDerivatives(
1391 model, data, joint_id, placement, rf, make_ref(v_point_partial_dq),
1392 make_ref(v_point_partial_dv));
1393 }
1394
1395 template<
1396 typename Scalar,
1397 int Options,
1398 template<typename, int>
1399 class JointCollectionTpl,
1400 typename Matrix3xOut1,
1401 typename Matrix3xOut2,
1402 typename Matrix3xOut3,
1403 typename Matrix3xOut4>
1404 20 void getPointClassicAccelerationDerivatives(
1405 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
1406 const DataTpl<Scalar, Options, JointCollectionTpl> & data,
1407 const Model::JointIndex joint_id,
1408 const SE3Tpl<Scalar, Options> & placement,
1409 const ReferenceFrame rf,
1410 const Eigen::MatrixBase<Matrix3xOut1> & v_point_partial_dq,
1411 const Eigen::MatrixBase<Matrix3xOut2> & a_point_partial_dq,
1412 const Eigen::MatrixBase<Matrix3xOut3> & a_point_partial_dv,
1413 const Eigen::MatrixBase<Matrix3xOut4> & a_point_partial_da)
1414 {
1415
4/8
✓ Branch 2 taken 20 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 20 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 20 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 20 times.
✗ Branch 12 not taken.
 20 impl::getPointClassicAccelerationDerivatives(
1416 model, data, joint_id, placement, rf, make_ref(v_point_partial_dq),
1417 make_ref(a_point_partial_dq), make_ref(a_point_partial_dv), make_ref(a_point_partial_da));
1418 20 }
1419
1420 template<
1421 typename Scalar,
1422 int Options,
1423 template<typename, int>
1424 class JointCollectionTpl,
1425 typename Matrix3xOut1,
1426 typename Matrix3xOut2,
1427 typename Matrix3xOut3,
1428 typename Matrix3xOut4,
1429 typename Matrix3xOut5>
1430 ✗ void getPointClassicAccelerationDerivatives(
1431 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
1432 const DataTpl<Scalar, Options, JointCollectionTpl> & data,
1433 const Model::JointIndex joint_id,
1434 const SE3Tpl<Scalar, Options> & placement,
1435 const ReferenceFrame rf,
1436 const Eigen::MatrixBase<Matrix3xOut1> & v_point_partial_dq,
1437 const Eigen::MatrixBase<Matrix3xOut2> & v_point_partial_dv,
1438 const Eigen::MatrixBase<Matrix3xOut3> & a_point_partial_dq,
1439 const Eigen::MatrixBase<Matrix3xOut4> & a_point_partial_dv,
1440 const Eigen::MatrixBase<Matrix3xOut5> & a_point_partial_da)
1441 {
1442 ✗ impl::getPointClassicAccelerationDerivatives(
1443 model, data, joint_id, placement, rf, make_ref(v_point_partial_dq),
1444 make_ref(v_point_partial_dv), make_ref(a_point_partial_dq), make_ref(a_point_partial_dv),
1445 make_ref(a_point_partial_da));
1446 }
1447
1448 } // namespace pinocchio
1449
1450 #endif // ifndef __pinocchio_algorithm_kinematics_derivatives_hxx__
1451