GCC Code Coverage Report

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