GCC Code Coverage Report

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