GCC Code Coverage Report

Directory: ./
File: include/pinocchio/algorithm/aba-derivatives.hxx
Date: 2024-08-27 18:20:05
Exec Total Coverage
Lines: 335 349 96.0%
Branches: 572 1570 36.4%
Line Branch Exec Source
1 //
2 // Copyright (c) 2018-2021 CNRS INRIA
3 //
4
5 #ifndef __pinocchio_algorithm_aba_derivatives_hxx__
6 #define __pinocchio_algorithm_aba_derivatives_hxx__
7
8 #include "pinocchio/multibody/visitor.hpp"
9 #include "pinocchio/algorithm/check.hpp"
10 #include "pinocchio/algorithm/aba.hpp"
11
12 namespace pinocchio
13 {
14 namespace impl
15 {
16 template<
17 typename Scalar,
18 int Options,
19 template<typename, int>
20 class JointCollectionTpl,
21 typename ConfigVectorType,
22 typename TangentVectorType>
23 struct ComputeABADerivativesForwardStep1
24 : public fusion::JointUnaryVisitorBase<ComputeABADerivativesForwardStep1<
25 Scalar,
26 Options,
27 JointCollectionTpl,
28 ConfigVectorType,
29 TangentVectorType>>
30 {
31 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
32 typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
33
34 typedef boost::fusion::
35 vector<const Model &, Data &, const ConfigVectorType &, const TangentVectorType &>
36 ArgsType;
37
38 template<typename JointModel>
39 1848 static void algo(
40 const JointModelBase<JointModel> & jmodel,
41 JointDataBase<typename JointModel::JointDataDerived> & jdata,
42 const Model & model,
43 Data & data,
44 const Eigen::MatrixBase<ConfigVectorType> & q,
45 const Eigen::MatrixBase<TangentVectorType> & v)
46 {
47 typedef typename Model::JointIndex JointIndex;
48
49
1/2
✓ Branch 1 taken 924 times.
✗ Branch 2 not taken.
 1848 const JointIndex i = jmodel.id();
50 1848 const JointIndex parent = model.parents[i];
51 1848 typename Data::Motion & ov = data.ov[i];
52
53
1/2
✓ Branch 4 taken 924 times.
✗ Branch 5 not taken.
 1848 jmodel.calc(jdata.derived(), q.derived(), v.derived());
54
55
6/10
✓ Branch 1 taken 924 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 890 times.
✓ Branch 5 taken 34 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 890 times.
✓ Branch 9 taken 34 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 890 times.
✗ Branch 13 not taken.
 1848 data.liMi[i] = model.jointPlacements[i] * jdata.M();
56
2/2
✓ Branch 0 taken 889 times.
✓ Branch 1 taken 35 times.
 1848 if (parent > 0)
57
2/4
✓ Branch 3 taken 889 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 889 times.
✗ Branch 8 not taken.
 1778 data.oMi[i] = data.oMi[parent] * data.liMi[i];
58 else
59
1/2
✓ Branch 3 taken 35 times.
✗ Branch 4 not taken.
 70 data.oMi[i] = data.liMi[i];
60
61
3/6
✓ Branch 2 taken 924 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 924 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 924 times.
✗ Branch 9 not taken.
 1848 ov = data.oMi[i].act(jdata.v());
62
2/2
✓ Branch 0 taken 889 times.
✓ Branch 1 taken 35 times.
 1848 if (parent > 0)
63
1/2
✓ Branch 2 taken 889 times.
✗ Branch 3 not taken.
 1778 ov += data.ov[parent];
64
65
3/6
✓ Branch 2 taken 924 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 924 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 924 times.
✗ Branch 10 not taken.
 1848 data.oa_gf[i] = data.oMi[i].act(jdata.c());
66
2/2
✓ Branch 0 taken 889 times.
✓ Branch 1 taken 35 times.
 1848 if (parent > 0)
67
2/4
✓ Branch 2 taken 889 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 889 times.
✗ Branch 7 not taken.
 1778 data.oa_gf[i] += (data.ov[parent] ^ ov);
68
69
3/6
✓ Branch 3 taken 924 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 924 times.
✗ Branch 8 not taken.
✓ Branch 11 taken 924 times.
✗ Branch 12 not taken.
 1848 data.oYcrb[i] = data.oinertias[i] = data.oMi[i].act(model.inertias[i]);
70
1/2
✓ Branch 2 taken 924 times.
✗ Branch 3 not taken.
 1848 data.oYaba[i] = data.oYcrb[i].matrix();
71
72
2/4
✓ Branch 2 taken 924 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 924 times.
✗ Branch 7 not taken.
 1848 data.oh[i] = data.oYcrb[i] * ov;
73
2/4
✓ Branch 2 taken 924 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 924 times.
✗ Branch 7 not taken.
 1848 data.of[i] = ov.cross(data.oh[i]);
74
75 typedef
76 typename SizeDepType<JointModel::NV>::template ColsReturn<typename Data::Matrix6x>::Type
77 ColsBlock;
78
1/2
✓ Branch 1 taken 924 times.
✗ Branch 2 not taken.
 1848 ColsBlock J_cols = jmodel.jointCols(data.J);
79
3/6
✓ Branch 2 taken 924 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 924 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 924 times.
✗ Branch 9 not taken.
 1848 J_cols = data.oMi[i].act(jdata.S());
80 }
81 };
82
83 template<
84 typename Scalar,
85 int Options,
86 template<typename, int>
87 class JointCollectionTpl,
88 typename MatrixType>
89 struct ComputeABADerivativesBackwardStep1
90 : public fusion::JointUnaryVisitorBase<
91 ComputeABADerivativesBackwardStep1<Scalar, Options, JointCollectionTpl, MatrixType>>
92 {
93 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
94 typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
95
96 typedef boost::fusion::vector<const Model &, Data &, MatrixType &> ArgsType;
97
98 template<typename JointModel>
99 1848 static void algo(
100 const JointModelBase<JointModel> & jmodel,
101 JointDataBase<typename JointModel::JointDataDerived> & jdata,
102 const Model & model,
103 Data & data,
104 const Eigen::MatrixBase<MatrixType> & Minv)
105 {
106 typedef typename Model::JointIndex JointIndex;
107 typedef typename Data::Inertia Inertia;
108 typedef typename Data::Force Force;
109 typedef typename Data::Matrix6x Matrix6x;
110
111
1/2
✓ Branch 1 taken 924 times.
✗ Branch 2 not taken.
 1848 const JointIndex i = jmodel.id();
112 1848 const JointIndex parent = model.parents[i];
113
114 1848 typename Inertia::Matrix6 & Ia = data.oYaba[i];
115
116 1848 Matrix6x & Fcrb = data.Fcrb[0];
117
118 typedef
119 typename SizeDepType<JointModel::NV>::template ColsReturn<typename Data::Matrix6x>::Type
120 ColsBlock;
121 1848 Force & fi = data.of[i];
122
123 typedef typename SizeDepType<JointModel::NV>::template ColsReturn<Matrix6x>::Type ColBlock;
124
1/2
✓ Branch 1 taken 924 times.
✗ Branch 2 not taken.
 1848 const ColBlock J_cols = jmodel.jointCols(data.J);
125
126
6/12
✓ Branch 1 taken 924 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 924 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 924 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 924 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 924 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 924 times.
✗ Branch 17 not taken.
 1848 jmodel.jointVelocitySelector(data.u).noalias() -= J_cols.transpose() * fi.toVector();
127
128
4/8
✓ Branch 1 taken 924 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 924 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 924 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 924 times.
✗ Branch 11 not taken.
 1848 jdata.U().noalias() = Ia * J_cols;
129
6/12
✓ Branch 1 taken 924 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 924 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 924 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 924 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 924 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 924 times.
✗ Branch 17 not taken.
 1848 jdata.StU().noalias() = J_cols.transpose() * jdata.U();
130
131
4/8
✓ Branch 1 taken 924 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 924 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 924 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 924 times.
✗ Branch 11 not taken.
 1848 jdata.StU().diagonal() += jmodel.jointVelocitySelector(model.armature);
132
133
3/6
✓ Branch 1 taken 924 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 924 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 924 times.
✗ Branch 8 not taken.
 1848 ::pinocchio::internal::PerformStYSInversion<Scalar>::run(jdata.StU(), jdata.Dinv());
134
6/12
✓ Branch 1 taken 924 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 924 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 924 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 924 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 924 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 924 times.
✗ Branch 17 not taken.
 1848 jdata.UDinv().noalias() = jdata.U() * jdata.Dinv();
135
136 1848 MatrixType & Minv_ = PINOCCHIO_EIGEN_CONST_CAST(MatrixType, Minv);
137
138
7/14
✓ Branch 1 taken 924 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 924 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 924 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 924 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 924 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 924 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 924 times.
✗ Branch 20 not taken.
 1848 Minv_.block(jmodel.idx_v(), jmodel.idx_v(), jmodel.nv(), jmodel.nv()) = jdata.Dinv();
139
1/2
✓ Branch 2 taken 924 times.
✗ Branch 3 not taken.
 1848 const int nv_children = data.nvSubtree[i] - jmodel.nv();
140
2/2
✓ Branch 0 taken 787 times.
✓ Branch 1 taken 137 times.
 1848 if (nv_children > 0)
141 {
142
1/2
✓ Branch 1 taken 787 times.
✗ Branch 2 not taken.
 1574 ColsBlock SDinv_cols = jmodel.jointCols(data.SDinv);
143
4/8
✓ Branch 1 taken 787 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 787 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 787 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 787 times.
✗ Branch 11 not taken.
 1574 SDinv_cols.noalias() = J_cols * jdata.Dinv();
144
145
5/10
✓ Branch 1 taken 787 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 787 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 787 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 787 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 787 times.
✗ Branch 14 not taken.
 1574 Minv_.block(jmodel.idx_v(), jmodel.idx_v() + jmodel.nv(), jmodel.nv(), nv_children)
146
4/8
✓ Branch 1 taken 787 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 787 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 787 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 787 times.
✗ Branch 11 not taken.
 3148 .noalias() =
147
4/8
✓ Branch 1 taken 787 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 787 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 787 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 787 times.
✗ Branch 11 not taken.
 1574 -SDinv_cols.transpose() * Fcrb.middleCols(jmodel.idx_v() + jmodel.nv(), nv_children);
148
149
2/2
✓ Branch 0 taken 752 times.
✓ Branch 1 taken 35 times.
 1574 if (parent > 0)
150 {
151
4/8
✓ Branch 2 taken 752 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 752 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 752 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 752 times.
✗ Branch 12 not taken.
 1504 Fcrb.middleCols(jmodel.idx_v(), data.nvSubtree[i]).noalias() +=
152
1/2
✓ Branch 1 taken 752 times.
✗ Branch 2 not taken.
 1504 jdata.U()
153
5/10
✓ Branch 2 taken 752 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 752 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 752 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 752 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 752 times.
✗ Branch 15 not taken.
 4512 * Minv_.block(jmodel.idx_v(), jmodel.idx_v(), jmodel.nv(), data.nvSubtree[i]);
154 ;
155 }
156 }
157 else // This a leaf of the kinematic tree
158 {
159
4/8
✓ Branch 2 taken 137 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 137 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 137 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 137 times.
✗ Branch 12 not taken.
 274 Fcrb.middleCols(jmodel.idx_v(), data.nvSubtree[i]).noalias() =
160
6/12
✓ Branch 2 taken 137 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 137 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 137 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 137 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 137 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 137 times.
✗ Branch 18 not taken.
 548 jdata.U() * Minv_.block(jmodel.idx_v(), jmodel.idx_v(), jmodel.nv(), data.nvSubtree[i]);
161 }
162
163
2/2
✓ Branch 0 taken 889 times.
✓ Branch 1 taken 35 times.
 1848 if (parent > 0)
164 {
165
6/12
✓ Branch 1 taken 889 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 889 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 889 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 889 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 889 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 889 times.
✗ Branch 17 not taken.
 1778 Ia.noalias() -= jdata.UDinv() * jdata.U().transpose();
166
167
4/8
✓ Branch 1 taken 889 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 889 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 889 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 889 times.
✗ Branch 11 not taken.
 1778 fi.toVector().noalias() +=
168
5/10
✓ Branch 1 taken 889 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 889 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 889 times.
✗ Branch 8 not taken.
✓ Branch 11 taken 889 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 889 times.
✗ Branch 15 not taken.
 1778 Ia * data.oa_gf[i].toVector() + jdata.UDinv() * jmodel.jointVelocitySelector(data.u);
169
1/2
✓ Branch 2 taken 889 times.
✗ Branch 3 not taken.
 1778 data.oYaba[parent] += Ia;
170
1/2
✓ Branch 2 taken 889 times.
✗ Branch 3 not taken.
 1778 data.of[parent] += fi;
171 }
172 }
173 };
174
175 template<
176 typename Scalar,
177 int Options,
178 template<typename, int>
179 class JointCollectionTpl,
180 typename MatrixType>
181 struct ComputeABADerivativesForwardStep2
182 : public fusion::JointUnaryVisitorBase<
183 ComputeABADerivativesForwardStep2<Scalar, Options, JointCollectionTpl, MatrixType>>
184 {
185 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
186 typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
187
188 typedef boost::fusion::vector<const Model &, Data &, MatrixType &> ArgsType;
189
190 template<typename JointModel>
191 1848 static void algo(
192 const JointModelBase<JointModel> & jmodel,
193 JointDataBase<typename JointModel::JointDataDerived> & jdata,
194 const Model & model,
195 Data & data,
196 MatrixType & Minv)
197 {
198 typedef typename Model::JointIndex JointIndex;
199 typedef typename Data::Matrix6x Matrix6x;
200
201
1/2
✓ Branch 1 taken 924 times.
✗ Branch 2 not taken.
 1848 const JointIndex i = jmodel.id();
202 1848 const JointIndex parent = model.parents[i];
203
204 1848 typename Data::Motion & ov = data.ov[i];
205 1848 typename Data::Motion & oa = data.oa[i];
206 1848 typename Data::Force & of = data.of[i];
207 1848 typename Data::Motion & oa_gf = data.oa_gf[i];
208
209 typedef typename SizeDepType<JointModel::NV>::template ColsReturn<Matrix6x>::Type ColsBlock;
210
1/2
✓ Branch 1 taken 924 times.
✗ Branch 2 not taken.
 1848 ColsBlock J_cols = jmodel.jointCols(data.J);
211
212
1/2
✓ Branch 2 taken 924 times.
✗ Branch 3 not taken.
 1848 oa_gf += data.oa_gf[parent];
213
4/8
✓ Branch 1 taken 924 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 924 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 924 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 924 times.
✗ Branch 11 not taken.
 1848 jmodel.jointVelocitySelector(data.ddq).noalias() =
214
3/6
✓ Branch 1 taken 924 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 924 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 924 times.
✗ Branch 8 not taken.
 1848 jdata.Dinv() * jmodel.jointVelocitySelector(data.u)
215
4/8
✓ Branch 1 taken 924 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 924 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 924 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 924 times.
✗ Branch 11 not taken.
 1848 - jdata.UDinv().transpose() * oa_gf.toVector();
216
217
5/10
✓ Branch 1 taken 924 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 924 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 924 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 924 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 924 times.
✗ Branch 14 not taken.
 1848 oa_gf.toVector().noalias() += J_cols * jmodel.jointVelocitySelector(data.ddq);
218
2/4
✓ Branch 1 taken 924 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 924 times.
✗ Branch 5 not taken.
 1848 oa = oa_gf + model.gravity;
219
4/8
✓ Branch 2 taken 924 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 924 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 924 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 924 times.
✗ Branch 13 not taken.
 1848 of = data.oYcrb[i] * oa_gf + ov.cross(data.oh[i]);
220
221 1848 MatrixType & Minv_ = PINOCCHIO_EIGEN_CONST_CAST(MatrixType, Minv);
222
223
2/2
✓ Branch 0 taken 889 times.
✓ Branch 1 taken 35 times.
 1848 if (parent > 0)
224 {
225
3/6
✓ Branch 1 taken 889 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 889 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 889 times.
✗ Branch 8 not taken.
 1778 Minv_.middleRows(jmodel.idx_v(), jmodel.nv())
226
2/4
✓ Branch 1 taken 889 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 889 times.
✗ Branch 5 not taken.
 1778 .rightCols(model.nv - jmodel.idx_v())
227
2/4
✓ Branch 1 taken 889 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 889 times.
✗ Branch 5 not taken.
 1778 .noalias() -=
228
5/10
✓ Branch 2 taken 889 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 889 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 889 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 889 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 889 times.
✗ Branch 15 not taken.
 3556 jdata.UDinv().transpose() * data.Fcrb[parent].rightCols(model.nv - jmodel.idx_v());
229 ;
230 }
231
232
4/8
✓ Branch 2 taken 924 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 924 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 924 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 924 times.
✗ Branch 12 not taken.
 1848 data.Fcrb[i].rightCols(model.nv - jmodel.idx_v()).noalias() =
233 J_cols
234
6/12
✓ Branch 1 taken 924 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 924 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 924 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 924 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 924 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 924 times.
✗ Branch 17 not taken.
 1848 * Minv_.middleRows(jmodel.idx_v(), jmodel.nv()).rightCols(model.nv - jmodel.idx_v());
235
2/2
✓ Branch 0 taken 889 times.
✓ Branch 1 taken 35 times.
 1848 if (parent > 0)
236
3/6
✓ Branch 2 taken 889 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 889 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 889 times.
✗ Branch 9 not taken.
 1778 data.Fcrb[i].rightCols(model.nv - jmodel.idx_v()) +=
237
2/4
✓ Branch 2 taken 889 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 889 times.
✗ Branch 6 not taken.
 3556 data.Fcrb[parent].rightCols(model.nv - jmodel.idx_v());
238
239
1/2
✓ Branch 1 taken 924 times.
✗ Branch 2 not taken.
 1848 ColsBlock dJ_cols = jmodel.jointCols(data.dJ);
240
1/2
✓ Branch 1 taken 924 times.
✗ Branch 2 not taken.
 1848 ColsBlock dVdq_cols = jmodel.jointCols(data.dVdq);
241
1/2
✓ Branch 1 taken 924 times.
✗ Branch 2 not taken.
 1848 ColsBlock dAdq_cols = jmodel.jointCols(data.dAdq);
242
1/2
✓ Branch 1 taken 924 times.
✗ Branch 2 not taken.
 1848 ColsBlock dAdv_cols = jmodel.jointCols(data.dAdv);
243
244
1/2
✓ Branch 1 taken 924 times.
✗ Branch 2 not taken.
 1848 motionSet::motionAction(ov, J_cols, dJ_cols);
245
1/2
✓ Branch 2 taken 924 times.
✗ Branch 3 not taken.
 1848 motionSet::motionAction(data.oa_gf[parent], J_cols, dAdq_cols);
246
1/2
✓ Branch 1 taken 924 times.
✗ Branch 2 not taken.
 1848 dAdv_cols = dJ_cols;
247
2/2
✓ Branch 0 taken 889 times.
✓ Branch 1 taken 35 times.
 1848 if (parent > 0)
248 {
249
1/2
✓ Branch 2 taken 889 times.
✗ Branch 3 not taken.
 1778 motionSet::motionAction(data.ov[parent], J_cols, dVdq_cols);
250
1/2
✓ Branch 2 taken 889 times.
✗ Branch 3 not taken.
 1778 motionSet::motionAction<ADDTO>(data.ov[parent], dVdq_cols, dAdq_cols);
251
1/2
✓ Branch 1 taken 889 times.
✗ Branch 2 not taken.
 1778 dAdv_cols += dVdq_cols;
252 }
253 else
254
1/2
✓ Branch 1 taken 35 times.
✗ Branch 2 not taken.
 70 dVdq_cols.setZero();
255
256 // computes variation of inertias
257
1/2
✓ Branch 2 taken 924 times.
✗ Branch 3 not taken.
 1848 data.doYcrb[i] = data.oYcrb[i].variation(ov);
258
1/2
✓ Branch 3 taken 924 times.
✗ Branch 4 not taken.
 1848 addForceCrossMatrix(data.oh[i], data.doYcrb[i]);
259 }
260
261 template<typename ForceDerived, typename M6>
262 static void
263 4116 addForceCrossMatrix(const ForceDense<ForceDerived> & f, const Eigen::MatrixBase<M6> & mout)
264 {
265 4116 M6 & mout_ = PINOCCHIO_EIGEN_CONST_CAST(M6, mout);
266
2/4
✓ Branch 1 taken 2058 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2058 times.
✗ Branch 5 not taken.
 4116 addSkew(
267
1/2
✓ Branch 2 taken 2058 times.
✗ Branch 3 not taken.
 4116 -f.linear(), mout_.template block<3, 3>(ForceDerived::LINEAR, ForceDerived::ANGULAR));
268
2/4
✓ Branch 1 taken 2058 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2058 times.
✗ Branch 5 not taken.
 4116 addSkew(
269
1/2
✓ Branch 2 taken 2058 times.
✗ Branch 3 not taken.
 4116 -f.linear(), mout_.template block<3, 3>(ForceDerived::ANGULAR, ForceDerived::LINEAR));
270
2/4
✓ Branch 1 taken 2058 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2058 times.
✗ Branch 5 not taken.
 4116 addSkew(
271
1/2
✓ Branch 2 taken 2058 times.
✗ Branch 3 not taken.
 4116 -f.angular(), mout_.template block<3, 3>(ForceDerived::ANGULAR, ForceDerived::ANGULAR));
272 4116 }
273 };
274
275 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
276 struct ComputeABADerivativesBackwardStep2
277 : public fusion::JointUnaryVisitorBase<
278 ComputeABADerivativesBackwardStep2<Scalar, Options, JointCollectionTpl>>
279 {
280 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
281 typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
282
283 typedef boost::fusion::vector<const Model &, Data &> ArgsType;
284
285 template<typename JointModel>
286 4116 static void algo(const JointModelBase<JointModel> & jmodel, const Model & model, Data & data)
287 {
288 typedef typename Model::JointIndex JointIndex;
289 typedef Eigen::Matrix<
290 Scalar, JointModel::NV, 6, Options, JointModel::NV == Eigen::Dynamic ? 6 : JointModel::NV,
291 6>
292 MatrixNV6;
293
294
1/2
✓ Branch 1 taken 2058 times.
✗ Branch 2 not taken.
 4116 const JointIndex i = jmodel.id();
295 4116 const JointIndex parent = model.parents[i];
296
297
2/4
✓ Branch 1 taken 2058 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2058 times.
✗ Branch 5 not taken.
 4116 typename PINOCCHIO_EIGEN_PLAIN_ROW_MAJOR_TYPE(MatrixNV6) StdY(jmodel.nv(), 6);
298
299 4116 typename Data::RowMatrixXs & rnea_partial_dq = data.dtau_dq;
300 4116 typename Data::RowMatrixXs & rnea_partial_dv = data.dtau_dv;
301
302 // typename Data::MatrixXs & rnea_partial_dq = data.dtau_dq;
303 // typename Data::MatrixXs & rnea_partial_dv = data.dtau_dv;
304
305 typedef
306 typename SizeDepType<JointModel::NV>::template ColsReturn<typename Data::Matrix6x>::Type
307 ColsBlock;
308
309
1/2
✓ Branch 1 taken 2058 times.
✗ Branch 2 not taken.
 4116 ColsBlock J_cols = jmodel.jointCols(data.J);
310
1/2
✓ Branch 1 taken 2058 times.
✗ Branch 2 not taken.
 4116 ColsBlock dVdq_cols = jmodel.jointCols(data.dVdq);
311
1/2
✓ Branch 1 taken 2058 times.
✗ Branch 2 not taken.
 4116 ColsBlock dAdq_cols = jmodel.jointCols(data.dAdq);
312
1/2
✓ Branch 1 taken 2058 times.
✗ Branch 2 not taken.
 4116 ColsBlock dAdv_cols = jmodel.jointCols(data.dAdv);
313
1/2
✓ Branch 1 taken 2058 times.
✗ Branch 2 not taken.
 4116 ColsBlock dFdq_cols = jmodel.jointCols(data.dFdq);
314
1/2
✓ Branch 1 taken 2058 times.
✗ Branch 2 not taken.
 4116 ColsBlock dFdv_cols = jmodel.jointCols(data.dFdv);
315
1/2
✓ Branch 1 taken 2058 times.
✗ Branch 2 not taken.
 4116 ColsBlock dFda_cols = jmodel.jointCols(data.dFda);
316
317 // dtau/dv
318
1/2
✓ Branch 2 taken 2058 times.
✗ Branch 3 not taken.
 4116 motionSet::inertiaAction(data.oYcrb[i], dAdv_cols, dFdv_cols);
319
3/6
✓ Branch 2 taken 2058 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 2058 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 2058 times.
✗ Branch 9 not taken.
 4116 dFdv_cols.noalias() += data.doYcrb[i] * J_cols;
320
321
4/8
✓ Branch 2 taken 2058 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 2058 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 2058 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 2058 times.
✗ Branch 12 not taken.
 4116 rnea_partial_dv.block(jmodel.idx_v(), jmodel.idx_v(), jmodel.nv(), data.nvSubtree[i])
322
6/12
✓ Branch 2 taken 2058 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 2058 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 2058 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 2058 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 2058 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 2058 times.
✗ Branch 18 not taken.
 8232 .noalias() = J_cols.transpose() * data.dFdv.middleCols(jmodel.idx_v(), data.nvSubtree[i]);
323
324 // dtau/dq
325
1/2
✓ Branch 2 taken 2058 times.
✗ Branch 3 not taken.
 4116 motionSet::inertiaAction(data.oYcrb[i], dAdq_cols, dFdq_cols);
326
2/2
✓ Branch 0 taken 1981 times.
✓ Branch 1 taken 77 times.
 4116 if (parent > 0)
327
2/4
✓ Branch 2 taken 1981 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1981 times.
✗ Branch 6 not taken.
 3962 dFdq_cols += data.doYcrb[i] * dVdq_cols;
328
329
4/8
✓ Branch 2 taken 2058 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 2058 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 2058 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 2058 times.
✗ Branch 12 not taken.
 4116 rnea_partial_dq.block(jmodel.idx_v(), jmodel.idx_v(), jmodel.nv(), data.nvSubtree[i])
330
6/12
✓ Branch 2 taken 2058 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 2058 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 2058 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 2058 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 2058 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 2058 times.
✗ Branch 18 not taken.
 8232 .noalias() = J_cols.transpose() * data.dFdq.middleCols(jmodel.idx_v(), data.nvSubtree[i]);
331
332
1/2
✓ Branch 2 taken 2058 times.
✗ Branch 3 not taken.
 4116 motionSet::act<ADDTO>(J_cols, data.of[i], dFdq_cols);
333
334
1/2
✓ Branch 2 taken 2058 times.
✗ Branch 3 not taken.
 4116 motionSet::inertiaAction(data.oYcrb[i], J_cols, dFda_cols);
335
2/2
✓ Branch 0 taken 1981 times.
✓ Branch 1 taken 77 times.
 4116 if (parent > 0)
336 {
337
3/4
✓ Branch 1 taken 1981 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 18331 times.
✓ Branch 5 taken 1981 times.
 40624 for (int j = data.parents_fromRow[(JointIndex)jmodel.idx_v()]; j >= 0;
338 36662 j = data.parents_fromRow[(JointIndex)j])
339
6/12
✓ Branch 1 taken 18331 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 18331 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 18331 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 18331 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 18331 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 18331 times.
✗ Branch 17 not taken.
 36662 rnea_partial_dq.middleRows(jmodel.idx_v(), jmodel.nv()).col(j).noalias() =
340
3/6
✓ Branch 1 taken 18331 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 18331 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 18331 times.
✗ Branch 8 not taken.
 73324 dFda_cols.transpose() * data.dAdq.col(j);
341
3/4
✓ Branch 1 taken 1981 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 18331 times.
✓ Branch 5 taken 1981 times.
 40624 for (int j = data.parents_fromRow[(JointIndex)jmodel.idx_v()]; j >= 0;
342 36662 j = data.parents_fromRow[(JointIndex)j])
343
6/12
✓ Branch 1 taken 18331 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 18331 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 18331 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 18331 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 18331 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 18331 times.
✗ Branch 17 not taken.
 36662 rnea_partial_dv.middleRows(jmodel.idx_v(), jmodel.nv()).col(j).noalias() =
344
3/6
✓ Branch 1 taken 18331 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 18331 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 18331 times.
✗ Branch 8 not taken.
 73324 dFda_cols.transpose() * data.dAdv.col(j);
345
346
4/8
✓ Branch 2 taken 1981 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1981 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1981 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1981 times.
✗ Branch 12 not taken.
 3962 StdY.noalias() = J_cols.transpose() * data.doYcrb[i];
347
3/4
✓ Branch 1 taken 1981 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 18331 times.
✓ Branch 5 taken 1981 times.
 40624 for (int j = data.parents_fromRow[(JointIndex)jmodel.idx_v()]; j >= 0;
348 36662 j = data.parents_fromRow[(JointIndex)j])
349
5/10
✓ Branch 1 taken 18331 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 18331 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 18331 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 18331 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 18331 times.
✗ Branch 14 not taken.
 36662 rnea_partial_dq.middleRows(jmodel.idx_v(), jmodel.nv()).col(j) +=
350
2/4
✓ Branch 1 taken 18331 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 18331 times.
✗ Branch 5 not taken.
 73324 StdY * data.dVdq.col(j);
351
3/4
✓ Branch 1 taken 1981 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 18331 times.
✓ Branch 5 taken 1981 times.
 40624 for (int j = data.parents_fromRow[(JointIndex)jmodel.idx_v()]; j >= 0;
352 36662 j = data.parents_fromRow[(JointIndex)j])
353
7/14
✓ Branch 1 taken 18331 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 18331 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 18331 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 18331 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 18331 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 18331 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 18331 times.
✗ Branch 20 not taken.
 36662 rnea_partial_dv.middleRows(jmodel.idx_v(), jmodel.nv()).col(j) += StdY * data.J.col(j);
354 }
355
356
2/2
✓ Branch 0 taken 1981 times.
✓ Branch 1 taken 77 times.
 4116 if (parent > 0)
357 {
358
1/2
✓ Branch 3 taken 1981 times.
✗ Branch 4 not taken.
 3962 data.oYcrb[parent] += data.oYcrb[i];
359
1/2
✓ Branch 3 taken 1981 times.
✗ Branch 4 not taken.
 3962 data.doYcrb[parent] += data.doYcrb[i];
360
1/2
✓ Branch 3 taken 1981 times.
✗ Branch 4 not taken.
 3962 data.of[parent] += data.of[i];
361 }
362
363 // Restore the status of dAdq_cols (remove gravity)
364
3/4
✓ Branch 1 taken 4496 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 2438 times.
✓ Branch 4 taken 2058 times.
 8992 for (Eigen::DenseIndex k = 0; k < jmodel.nv(); ++k)
365 {
366
2/4
✓ Branch 1 taken 2438 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2438 times.
✗ Branch 5 not taken.
 4876 MotionRef<typename ColsBlock::ColXpr> m_in(J_cols.col(k));
367
2/4
✓ Branch 1 taken 2438 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2438 times.
✗ Branch 5 not taken.
 4876 MotionRef<typename ColsBlock::ColXpr> m_out(dAdq_cols.col(k));
368
5/10
✓ Branch 1 taken 2438 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2438 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2438 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 2438 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 2438 times.
✗ Branch 14 not taken.
 4876 m_out.linear() += model.gravity.linear().cross(m_in.angular());
369 }
370 }
371 };
372
373 template<
374 typename Scalar,
375 int Options,
376 template<typename, int>
377 class JointCollectionTpl,
378 typename ConfigVectorType,
379 typename TangentVectorType1,
380 typename TangentVectorType2,
381 typename MatrixType1,
382 typename MatrixType2,
383 typename MatrixType3>
384 62 void computeABADerivatives(
385 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
386 DataTpl<Scalar, Options, JointCollectionTpl> & data,
387 const Eigen::MatrixBase<ConfigVectorType> & q,
388 const Eigen::MatrixBase<TangentVectorType1> & v,
389 const Eigen::MatrixBase<TangentVectorType2> & tau,
390 const Eigen::MatrixBase<MatrixType1> & aba_partial_dq,
391 const Eigen::MatrixBase<MatrixType2> & aba_partial_dv,
392 const Eigen::MatrixBase<MatrixType3> & aba_partial_dtau)
393 {
394
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 31 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.
 62 PINOCCHIO_CHECK_ARGUMENT_SIZE(
395 q.size(), model.nq, "The joint configuration vector is not of right size");
396
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 31 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.
 62 PINOCCHIO_CHECK_ARGUMENT_SIZE(
397 v.size(), model.nv, "The joint velocity vector is not of right size");
398
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 31 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.
 62 PINOCCHIO_CHECK_ARGUMENT_SIZE(
399 tau.size(), model.nv, "The joint torque vector is not of right size");
400
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 31 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.
 62 PINOCCHIO_CHECK_ARGUMENT_SIZE(aba_partial_dq.cols(), model.nv);
401
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 31 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.
 62 PINOCCHIO_CHECK_ARGUMENT_SIZE(aba_partial_dq.rows(), model.nv);
402
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 31 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.
 62 PINOCCHIO_CHECK_ARGUMENT_SIZE(aba_partial_dv.cols(), model.nv);
403
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 31 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.
 62 PINOCCHIO_CHECK_ARGUMENT_SIZE(aba_partial_dv.rows(), model.nv);
404
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 31 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.
 62 PINOCCHIO_CHECK_ARGUMENT_SIZE(aba_partial_dtau.cols(), model.nv);
405
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 31 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.
 62 PINOCCHIO_CHECK_ARGUMENT_SIZE(aba_partial_dtau.rows(), model.nv);
406
3/8
✓ Branch 2 taken 31 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 31 times.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✓ Branch 8 taken 31 times.
✗ Branch 11 not taken.
✗ Branch 12 not taken.
 62 PINOCCHIO_CHECK_INPUT_ARGUMENT(
407 isZero(model.gravity.angular()),
408 "The gravity must be a pure force vector, no angular part");
409
1/2
✓ Branch 1 taken 31 times.
✗ Branch 2 not taken.
 62 assert(model.check(data) && "data is not consistent with model.");
410
411 typedef typename ModelTpl<Scalar, Options, JointCollectionTpl>::JointIndex JointIndex;
412
413
1/2
✓ Branch 3 taken 31 times.
✗ Branch 4 not taken.
 62 data.oa_gf[0] = -model.gravity;
414 62 data.u = tau;
415
416 62 MatrixType3 & Minv_ = PINOCCHIO_EIGEN_CONST_CAST(MatrixType3, aba_partial_dtau);
417
1/2
✓ Branch 2 taken 31 times.
✗ Branch 3 not taken.
 62 Minv_.template triangularView<Eigen::Upper>().setZero();
418
419 /// First, compute Minv and a, the joint acceleration vector
420 typedef ComputeABADerivativesForwardStep1<
421 Scalar, Options, JointCollectionTpl, ConfigVectorType, TangentVectorType1>
422 Pass1;
423
2/2
✓ Branch 0 taken 816 times.
✓ Branch 1 taken 31 times.
 1694 for (JointIndex i = 1; i < (JointIndex)model.njoints; ++i)
424 {
425
1/2
✓ Branch 1 taken 816 times.
✗ Branch 2 not taken.
 1632 Pass1::run(
426 1632 model.joints[i], data.joints[i],
427 3264 typename Pass1::ArgsType(model, data, q.derived(), v.derived()));
428 }
429
430 62 data.Fcrb[0].setZero();
431 typedef ComputeABADerivativesBackwardStep1<Scalar, Options, JointCollectionTpl, MatrixType3>
432 Pass2;
433
2/2
✓ Branch 0 taken 816 times.
✓ Branch 1 taken 31 times.
 1694 for (JointIndex i = (JointIndex)(model.njoints - 1); i > 0; --i)
434 {
435
1/2
✓ Branch 4 taken 816 times.
✗ Branch 5 not taken.
 1632 Pass2::run(model.joints[i], data.joints[i], typename Pass2::ArgsType(model, data, Minv_));
436 }
437
438 typedef ComputeABADerivativesForwardStep2<Scalar, Options, JointCollectionTpl, MatrixType3>
439 Pass3;
440
2/2
✓ Branch 0 taken 816 times.
✓ Branch 1 taken 31 times.
 1694 for (JointIndex i = 1; i < (JointIndex)model.njoints; ++i)
441 {
442
1/2
✓ Branch 4 taken 816 times.
✗ Branch 5 not taken.
 1632 Pass3::run(model.joints[i], data.joints[i], typename Pass3::ArgsType(model, data, Minv_));
443 }
444
445 typedef ComputeABADerivativesBackwardStep2<Scalar, Options, JointCollectionTpl> Pass4;
446
2/2
✓ Branch 0 taken 816 times.
✓ Branch 1 taken 31 times.
 1694 for (JointIndex i = (JointIndex)(model.njoints - 1); i > 0; --i)
447 {
448
1/2
✓ Branch 3 taken 816 times.
✗ Branch 4 not taken.
 1632 Pass4::run(model.joints[i], typename Pass4::ArgsType(model, data));
449 }
450
451
2/4
✓ Branch 1 taken 31 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 31 times.
✗ Branch 5 not taken.
 62 Minv_.template triangularView<Eigen::StrictlyLower>() =
452
1/2
✓ Branch 2 taken 31 times.
✗ Branch 3 not taken.
 62 Minv_.transpose().template triangularView<Eigen::StrictlyLower>();
453
454
3/6
✓ Branch 2 taken 31 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 31 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 31 times.
✗ Branch 10 not taken.
 62 PINOCCHIO_EIGEN_CONST_CAST(MatrixType1, aba_partial_dq).noalias() = -Minv_ * data.dtau_dq;
455
3/6
✓ Branch 2 taken 31 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 31 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 31 times.
✗ Branch 10 not taken.
 62 PINOCCHIO_EIGEN_CONST_CAST(MatrixType2, aba_partial_dv).noalias() = -Minv_ * data.dtau_dv;
456 62 }
457
458 template<
459 typename Scalar,
460 int Options,
461 template<typename, int>
462 class JointCollectionTpl,
463 typename ConfigVectorType,
464 typename TangentVectorType1,
465 typename TangentVectorType2,
466 typename MatrixType1,
467 typename MatrixType2,
468 typename MatrixType3>
469 8 void computeABADerivatives(
470 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
471 DataTpl<Scalar, Options, JointCollectionTpl> & data,
472 const Eigen::MatrixBase<ConfigVectorType> & q,
473 const Eigen::MatrixBase<TangentVectorType1> & v,
474 const Eigen::MatrixBase<TangentVectorType2> & tau,
475 const container::aligned_vector<ForceTpl<Scalar, Options>> & fext,
476 const Eigen::MatrixBase<MatrixType1> & aba_partial_dq,
477 const Eigen::MatrixBase<MatrixType2> & aba_partial_dv,
478 const Eigen::MatrixBase<MatrixType3> & aba_partial_dtau)
479 {
480
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 4 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.
 8 PINOCCHIO_CHECK_ARGUMENT_SIZE(
481 q.size(), model.nq, "The joint configuration vector is not of right size");
482
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 4 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.
 8 PINOCCHIO_CHECK_ARGUMENT_SIZE(
483 v.size(), model.nv, "The joint velocity vector is not of right size");
484
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 4 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.
 8 PINOCCHIO_CHECK_ARGUMENT_SIZE(
485 tau.size(), model.nv, "The joint torque vector is not of right size");
486
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 4 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.
 8 PINOCCHIO_CHECK_ARGUMENT_SIZE(
487 fext.size(), (size_t)model.njoints, "The external forces vector is not of right size");
488
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 4 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.
 8 PINOCCHIO_CHECK_ARGUMENT_SIZE(aba_partial_dq.cols(), model.nv);
489
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 4 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.
 8 PINOCCHIO_CHECK_ARGUMENT_SIZE(aba_partial_dq.rows(), model.nv);
490
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 4 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.
 8 PINOCCHIO_CHECK_ARGUMENT_SIZE(aba_partial_dv.cols(), model.nv);
491
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 4 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.
 8 PINOCCHIO_CHECK_ARGUMENT_SIZE(aba_partial_dv.rows(), model.nv);
492
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 4 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.
 8 PINOCCHIO_CHECK_ARGUMENT_SIZE(aba_partial_dtau.cols(), model.nv);
493
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 4 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.
 8 PINOCCHIO_CHECK_ARGUMENT_SIZE(aba_partial_dtau.rows(), model.nv);
494
3/8
✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 4 times.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✓ Branch 8 taken 4 times.
✗ Branch 11 not taken.
✗ Branch 12 not taken.
 8 PINOCCHIO_CHECK_INPUT_ARGUMENT(
495 isZero(model.gravity.angular()),
496 "The gravity must be a pure force vector, no angular part");
497
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 8 assert(model.check(data) && "data is not consistent with model.");
498
499 typedef typename ModelTpl<Scalar, Options, JointCollectionTpl>::JointIndex JointIndex;
500
501
1/2
✓ Branch 3 taken 4 times.
✗ Branch 4 not taken.
 8 data.oa_gf[0] = -model.gravity;
502 8 data.u = tau;
503
504 8 MatrixType3 & Minv_ = PINOCCHIO_EIGEN_CONST_CAST(MatrixType3, aba_partial_dtau);
505
1/2
✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
 8 Minv_.template triangularView<Eigen::Upper>().setZero();
506
507 /// First, compute Minv and a, the joint acceleration vector
508 typedef ComputeABADerivativesForwardStep1<
509 Scalar, Options, JointCollectionTpl, ConfigVectorType, TangentVectorType1>
510 Pass1;
511
2/2
✓ Branch 0 taken 108 times.
✓ Branch 1 taken 4 times.
 224 for (JointIndex i = 1; i < (JointIndex)model.njoints; ++i)
512 {
513
1/2
✓ Branch 1 taken 108 times.
✗ Branch 2 not taken.
 216 Pass1::run(
514 216 model.joints[i], data.joints[i],
515 216 typename Pass1::ArgsType(model, data, q.derived(), v.derived()));
516
1/2
✓ Branch 5 taken 108 times.
✗ Branch 6 not taken.
 216 data.of[i] -= data.oMi[i].act(fext[i]);
517 }
518
519 8 data.Fcrb[0].setZero();
520 typedef ComputeABADerivativesBackwardStep1<Scalar, Options, JointCollectionTpl, MatrixType3>
521 Pass2;
522
2/2
✓ Branch 0 taken 108 times.
✓ Branch 1 taken 4 times.
 224 for (JointIndex i = (JointIndex)(model.njoints - 1); i > 0; --i)
523 {
524
1/2
✓ Branch 4 taken 108 times.
✗ Branch 5 not taken.
 216 Pass2::run(model.joints[i], data.joints[i], typename Pass2::ArgsType(model, data, Minv_));
525 }
526
527 typedef ComputeABADerivativesForwardStep2<Scalar, Options, JointCollectionTpl, MatrixType3>
528 Pass3;
529
2/2
✓ Branch 0 taken 108 times.
✓ Branch 1 taken 4 times.
 224 for (JointIndex i = 1; i < (JointIndex)model.njoints; ++i)
530 {
531
1/2
✓ Branch 4 taken 108 times.
✗ Branch 5 not taken.
 216 Pass3::run(model.joints[i], data.joints[i], typename Pass3::ArgsType(model, data, Minv_));
532
1/2
✓ Branch 5 taken 108 times.
✗ Branch 6 not taken.
 216 data.of[i] -= data.oMi[i].act(fext[i]);
533 }
534
535 typedef ComputeABADerivativesBackwardStep2<Scalar, Options, JointCollectionTpl> Pass4;
536
2/2
✓ Branch 0 taken 108 times.
✓ Branch 1 taken 4 times.
 224 for (JointIndex i = (JointIndex)(model.njoints - 1); i > 0; --i)
537 {
538
1/2
✓ Branch 3 taken 108 times.
✗ Branch 4 not taken.
 216 Pass4::run(model.joints[i], typename Pass4::ArgsType(model, data));
539 }
540
541
2/4
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
 8 Minv_.template triangularView<Eigen::StrictlyLower>() =
542
1/2
✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
 8 Minv_.transpose().template triangularView<Eigen::StrictlyLower>();
543
544
3/6
✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 4 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 4 times.
✗ Branch 10 not taken.
 8 PINOCCHIO_EIGEN_CONST_CAST(MatrixType1, aba_partial_dq).noalias() = -Minv_ * data.dtau_dq;
545
3/6
✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 4 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 4 times.
✗ Branch 10 not taken.
 8 PINOCCHIO_EIGEN_CONST_CAST(MatrixType2, aba_partial_dv).noalias() = -Minv_ * data.dtau_dv;
546 8 }
547
548 namespace optimized
549 {
550 template<
551 typename Scalar,
552 int Options,
553 template<typename, int>
554 class JointCollectionTpl,
555 typename MatrixType>
556 struct ComputeABADerivativesBackwardStep1
557 : public fusion::JointUnaryVisitorBase<
558 ComputeABADerivativesBackwardStep1<Scalar, Options, JointCollectionTpl, MatrixType>>
559 {
560 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
561 typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
562
563 typedef boost::fusion::vector<const Model &, Data &, MatrixType &> ArgsType;
564
565 template<typename JointModel>
566 2268 static void algo(
567 const JointModelBase<JointModel> & jmodel,
568 JointDataBase<typename JointModel::JointDataDerived> & jdata,
569 const Model & model,
570 Data & data,
571 const Eigen::MatrixBase<MatrixType> & Minv)
572 {
573 typedef typename Model::JointIndex JointIndex;
574 typedef typename Data::Matrix6x Matrix6x;
575
576
1/2
✓ Branch 1 taken 1134 times.
✗ Branch 2 not taken.
 2268 const JointIndex i = jmodel.id();
577 2268 const JointIndex parent = model.parents[i];
578
579 2268 Matrix6x & Fcrb = data.Fcrb[0];
580
581 typedef
582 typename SizeDepType<JointModel::NV>::template ColsReturn<typename Data::Matrix6x>::Type
583 ColsBlock;
584
585 typedef
586 typename SizeDepType<JointModel::NV>::template ColsReturn<Matrix6x>::Type ColBlock;
587
1/2
✓ Branch 1 taken 1134 times.
✗ Branch 2 not taken.
 2268 const ColBlock J_cols = jmodel.jointCols(data.J);
588
589 2268 MatrixType & Minv_ = PINOCCHIO_EIGEN_CONST_CAST(MatrixType, Minv);
590
591
7/14
✓ Branch 1 taken 1134 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1134 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1134 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1134 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1134 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1134 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 1134 times.
✗ Branch 20 not taken.
 2268 Minv_.block(jmodel.idx_v(), jmodel.idx_v(), jmodel.nv(), jmodel.nv()) = jdata.Dinv();
592
1/2
✓ Branch 2 taken 1134 times.
✗ Branch 3 not taken.
 2268 const int nv_children = data.nvSubtree[i] - jmodel.nv();
593
2/2
✓ Branch 0 taken 966 times.
✓ Branch 1 taken 168 times.
 2268 if (nv_children > 0)
594 {
595
1/2
✓ Branch 1 taken 966 times.
✗ Branch 2 not taken.
 1932 ColsBlock SDinv_cols = jmodel.jointCols(data.SDinv);
596
4/8
✓ Branch 1 taken 966 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 966 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 966 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 966 times.
✗ Branch 11 not taken.
 1932 SDinv_cols.noalias() = J_cols * jdata.Dinv();
597
598
5/10
✓ Branch 1 taken 966 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 966 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 966 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 966 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 966 times.
✗ Branch 14 not taken.
 1932 Minv_.block(jmodel.idx_v(), jmodel.idx_v() + jmodel.nv(), jmodel.nv(), nv_children)
599
4/8
✓ Branch 1 taken 966 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 966 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 966 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 966 times.
✗ Branch 11 not taken.
 3864 .noalias() =
600
4/8
✓ Branch 1 taken 966 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 966 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 966 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 966 times.
✗ Branch 11 not taken.
 1932 -SDinv_cols.transpose() * Fcrb.middleCols(jmodel.idx_v() + jmodel.nv(), nv_children);
601
602
2/2
✓ Branch 0 taken 924 times.
✓ Branch 1 taken 42 times.
 1932 if (parent > 0)
603 {
604
4/8
✓ Branch 2 taken 924 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 924 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 924 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 924 times.
✗ Branch 12 not taken.
 1848 Fcrb.middleCols(jmodel.idx_v(), data.nvSubtree[i]).noalias() +=
605
1/2
✓ Branch 1 taken 924 times.
✗ Branch 2 not taken.
 1848 jdata.U()
606
5/10
✓ Branch 2 taken 924 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 924 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 924 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 924 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 924 times.
✗ Branch 15 not taken.
 5544 * Minv_.block(jmodel.idx_v(), jmodel.idx_v(), jmodel.nv(), data.nvSubtree[i]);
607 ;
608 }
609 }
610 else // This a leaf of the kinematic tree
611 {
612
4/8
✓ Branch 2 taken 168 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 168 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 168 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 168 times.
✗ Branch 12 not taken.
 336 Fcrb.middleCols(jmodel.idx_v(), data.nvSubtree[i]).noalias() =
613
1/2
✓ Branch 1 taken 168 times.
✗ Branch 2 not taken.
 336 jdata.U()
614
5/10
✓ Branch 2 taken 168 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 168 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 168 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 168 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 168 times.
✗ Branch 15 not taken.
 1008 * Minv_.block(jmodel.idx_v(), jmodel.idx_v(), jmodel.nv(), data.nvSubtree[i]);
615 }
616 }
617 };
618
619 template<
620 typename Scalar,
621 int Options,
622 template<typename, int>
623 class JointCollectionTpl,
624 typename MatrixType>
625 struct ComputeABADerivativesForwardStep2
626 : public fusion::JointUnaryVisitorBase<
627 ComputeABADerivativesForwardStep2<Scalar, Options, JointCollectionTpl, MatrixType>>
628 {
629 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
630 typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
631
632 typedef ::pinocchio::impl::
633 ComputeABADerivativesForwardStep2<Scalar, Options, JointCollectionTpl, MatrixType>
634 SimilarBase;
635
636 typedef boost::fusion::vector<const Model &, Data &, MatrixType &> ArgsType;
637
638 template<typename JointModel>
639 2268 static void algo(
640 const JointModelBase<JointModel> & jmodel,
641 JointDataBase<typename JointModel::JointDataDerived> & jdata,
642 const Model & model,
643 Data & data,
644 MatrixType & Minv)
645 {
646 typedef typename Model::JointIndex JointIndex;
647 typedef typename Data::Matrix6x Matrix6x;
648
649
1/2
✓ Branch 1 taken 1134 times.
✗ Branch 2 not taken.
 2268 const JointIndex i = jmodel.id();
650 2268 const JointIndex parent = model.parents[i];
651
652 2268 const typename Data::Motion & ov = data.ov[i];
653 // typename Data::Motion & oa = data.oa[i];
654 2268 typename Data::Force & of = data.of[i];
655 2268 const typename Data::Motion & oa_gf = data.oa_gf[i];
656
657 typedef
658 typename SizeDepType<JointModel::NV>::template ColsReturn<Matrix6x>::Type ColsBlock;
659
1/2
✓ Branch 1 taken 1134 times.
✗ Branch 2 not taken.
 2268 const ColsBlock J_cols = jmodel.jointCols(data.J);
660
661 // Already done in optimized::aba
662 // oa = oa_gf + model.gravity;
663
1/2
✓ Branch 3 taken 1134 times.
✗ Branch 4 not taken.
 2268 data.oYcrb[i] = data.oinertias[i];
664
4/8
✓ Branch 2 taken 1134 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 1134 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 1134 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 1134 times.
✗ Branch 13 not taken.
 2268 of = data.oinertias[i] * oa_gf + ov.cross(data.oh[i]);
665
666 2268 MatrixType & Minv_ = PINOCCHIO_EIGEN_CONST_CAST(MatrixType, Minv);
667
668
2/2
✓ Branch 0 taken 1092 times.
✓ Branch 1 taken 42 times.
 2268 if (parent > 0)
669 {
670
3/6
✓ Branch 1 taken 1092 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1092 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1092 times.
✗ Branch 8 not taken.
 2184 Minv_.middleRows(jmodel.idx_v(), jmodel.nv())
671
2/4
✓ Branch 1 taken 1092 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1092 times.
✗ Branch 5 not taken.
 2184 .rightCols(model.nv - jmodel.idx_v())
672
2/4
✓ Branch 1 taken 1092 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1092 times.
✗ Branch 5 not taken.
 2184 .noalias() -=
673
5/10
✓ Branch 2 taken 1092 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1092 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1092 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1092 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1092 times.
✗ Branch 15 not taken.
 4368 jdata.UDinv().transpose() * data.Fcrb[parent].rightCols(model.nv - jmodel.idx_v());
674 ;
675 }
676
677
4/8
✓ Branch 2 taken 1134 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1134 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1134 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1134 times.
✗ Branch 12 not taken.
 2268 data.Fcrb[i].rightCols(model.nv - jmodel.idx_v()).noalias() =
678 J_cols
679
6/12
✓ Branch 1 taken 1134 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1134 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1134 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1134 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1134 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1134 times.
✗ Branch 17 not taken.
 2268 * Minv_.middleRows(jmodel.idx_v(), jmodel.nv()).rightCols(model.nv - jmodel.idx_v());
680
2/2
✓ Branch 0 taken 1092 times.
✓ Branch 1 taken 42 times.
 2268 if (parent > 0)
681
3/6
✓ Branch 2 taken 1092 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1092 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1092 times.
✗ Branch 9 not taken.
 2184 data.Fcrb[i].rightCols(model.nv - jmodel.idx_v()) +=
682
2/4
✓ Branch 2 taken 1092 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1092 times.
✗ Branch 6 not taken.
 4368 data.Fcrb[parent].rightCols(model.nv - jmodel.idx_v());
683
684
1/2
✓ Branch 1 taken 1134 times.
✗ Branch 2 not taken.
 2268 ColsBlock dJ_cols = jmodel.jointCols(data.dJ);
685
1/2
✓ Branch 1 taken 1134 times.
✗ Branch 2 not taken.
 2268 ColsBlock dVdq_cols = jmodel.jointCols(data.dVdq);
686
1/2
✓ Branch 1 taken 1134 times.
✗ Branch 2 not taken.
 2268 ColsBlock dAdq_cols = jmodel.jointCols(data.dAdq);
687
1/2
✓ Branch 1 taken 1134 times.
✗ Branch 2 not taken.
 2268 ColsBlock dAdv_cols = jmodel.jointCols(data.dAdv);
688
689
1/2
✓ Branch 1 taken 1134 times.
✗ Branch 2 not taken.
 2268 motionSet::motionAction(ov, J_cols, dJ_cols);
690
1/2
✓ Branch 2 taken 1134 times.
✗ Branch 3 not taken.
 2268 motionSet::motionAction(data.oa_gf[parent], J_cols, dAdq_cols);
691
1/2
✓ Branch 1 taken 1134 times.
✗ Branch 2 not taken.
 2268 dAdv_cols = dJ_cols;
692
2/2
✓ Branch 0 taken 1092 times.
✓ Branch 1 taken 42 times.
 2268 if (parent > 0)
693 {
694
1/2
✓ Branch 2 taken 1092 times.
✗ Branch 3 not taken.
 2184 motionSet::motionAction(data.ov[parent], J_cols, dVdq_cols);
695
1/2
✓ Branch 2 taken 1092 times.
✗ Branch 3 not taken.
 2184 motionSet::motionAction<ADDTO>(data.ov[parent], dVdq_cols, dAdq_cols);
696
1/2
✓ Branch 1 taken 1092 times.
✗ Branch 2 not taken.
 2184 dAdv_cols += dVdq_cols;
697 }
698 else
699
1/2
✓ Branch 1 taken 42 times.
✗ Branch 2 not taken.
 84 dVdq_cols.setZero();
700
701 // computes variation of inertias
702
1/2
✓ Branch 2 taken 1134 times.
✗ Branch 3 not taken.
 2268 data.doYcrb[i] = data.oinertias[i].variation(ov);
703
1/2
✓ Branch 3 taken 1134 times.
✗ Branch 4 not taken.
 2268 SimilarBase::addForceCrossMatrix(data.oh[i], data.doYcrb[i]);
704 }
705 };
706
707 } // namespace optimized
708
709 template<
710 typename Scalar,
711 int Options,
712 template<typename, int>
713 class JointCollectionTpl,
714 typename MatrixType1,
715 typename MatrixType2,
716 typename MatrixType3>
717 typename std::enable_if<
718 !(MatrixType1::IsVectorAtCompileTime || MatrixType2::IsVectorAtCompileTime
719 || MatrixType3::IsVectorAtCompileTime),
720 void>::type
721 42 computeABADerivatives(
722 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
723 DataTpl<Scalar, Options, JointCollectionTpl> & data,
724 const Eigen::MatrixBase<MatrixType1> & aba_partial_dq,
725 const Eigen::MatrixBase<MatrixType2> & aba_partial_dv,
726 const Eigen::MatrixBase<MatrixType3> & aba_partial_dtau)
727 {
728
1/4
✗ Branch 1 not taken.
✓ Branch 2 taken 21 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
 42 PINOCCHIO_CHECK_INPUT_ARGUMENT(aba_partial_dq.cols() == model.nv);
729
1/4
✗ Branch 1 not taken.
✓ Branch 2 taken 21 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
 42 PINOCCHIO_CHECK_INPUT_ARGUMENT(aba_partial_dq.rows() == model.nv);
730
1/4
✗ Branch 1 not taken.
✓ Branch 2 taken 21 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
 42 PINOCCHIO_CHECK_INPUT_ARGUMENT(aba_partial_dv.cols() == model.nv);
731
1/4
✗ Branch 1 not taken.
✓ Branch 2 taken 21 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
 42 PINOCCHIO_CHECK_INPUT_ARGUMENT(aba_partial_dv.rows() == model.nv);
732
1/4
✗ Branch 1 not taken.
✓ Branch 2 taken 21 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
 42 PINOCCHIO_CHECK_INPUT_ARGUMENT(aba_partial_dtau.cols() == model.nv);
733
1/4
✗ Branch 1 not taken.
✓ Branch 2 taken 21 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
 42 PINOCCHIO_CHECK_INPUT_ARGUMENT(aba_partial_dtau.rows() == model.nv);
734
3/8
✓ Branch 2 taken 21 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 21 times.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✓ Branch 8 taken 21 times.
✗ Branch 11 not taken.
✗ Branch 12 not taken.
 42 PINOCCHIO_CHECK_INPUT_ARGUMENT(
735 isZero(model.gravity.angular()),
736 "The gravity must be a pure force vector, no angular part");
737
1/2
✓ Branch 1 taken 21 times.
✗ Branch 2 not taken.
 42 assert(model.check(data) && "data is not consistent with model.");
738
739 typedef typename ModelTpl<Scalar, Options, JointCollectionTpl>::JointIndex JointIndex;
740
741 42 MatrixType3 & Minv_ = PINOCCHIO_EIGEN_CONST_CAST(MatrixType3, aba_partial_dtau);
742
1/2
✓ Branch 2 taken 21 times.
✗ Branch 3 not taken.
 42 Minv_.template triangularView<Eigen::Upper>().setZero();
743
744 42 data.Fcrb[0].setZero();
745 typedef optimized::ComputeABADerivativesBackwardStep1<
746 Scalar, Options, JointCollectionTpl, MatrixType3>
747 Pass2;
748
2/2
✓ Branch 0 taken 567 times.
✓ Branch 1 taken 21 times.
 1176 for (JointIndex i = (JointIndex)(model.njoints - 1); i > 0; --i)
749 {
750
1/2
✓ Branch 4 taken 567 times.
✗ Branch 5 not taken.
 1134 Pass2::run(model.joints[i], data.joints[i], typename Pass2::ArgsType(model, data, Minv_));
751 }
752
753 typedef optimized::ComputeABADerivativesForwardStep2<
754 Scalar, Options, JointCollectionTpl, MatrixType3>
755 Pass3;
756
2/2
✓ Branch 0 taken 567 times.
✓ Branch 1 taken 21 times.
 1176 for (JointIndex i = 1; i < (JointIndex)model.njoints; ++i)
757 {
758
1/2
✓ Branch 4 taken 567 times.
✗ Branch 5 not taken.
 1134 Pass3::run(model.joints[i], data.joints[i], typename Pass3::ArgsType(model, data, Minv_));
759 }
760
761 typedef ComputeABADerivativesBackwardStep2<Scalar, Options, JointCollectionTpl> Pass4;
762
2/2
✓ Branch 0 taken 567 times.
✓ Branch 1 taken 21 times.
 1176 for (JointIndex i = (JointIndex)(model.njoints - 1); i > 0; --i)
763 {
764
1/2
✓ Branch 3 taken 567 times.
✗ Branch 4 not taken.
 1134 Pass4::run(model.joints[i], typename Pass4::ArgsType(model, data));
765 }
766
767
2/4
✓ Branch 1 taken 21 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 21 times.
✗ Branch 5 not taken.
 42 Minv_.template triangularView<Eigen::StrictlyLower>() =
768
1/2
✓ Branch 2 taken 21 times.
✗ Branch 3 not taken.
 42 Minv_.transpose().template triangularView<Eigen::StrictlyLower>();
769
770
3/6
✓ Branch 2 taken 21 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 21 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 21 times.
✗ Branch 10 not taken.
 42 PINOCCHIO_EIGEN_CONST_CAST(MatrixType1, aba_partial_dq).noalias() = -Minv_ * data.dtau_dq;
771
3/6
✓ Branch 2 taken 21 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 21 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 21 times.
✗ Branch 10 not taken.
 42 PINOCCHIO_EIGEN_CONST_CAST(MatrixType2, aba_partial_dv).noalias() = -Minv_ * data.dtau_dv;
772 }
773
774 template<
775 typename Scalar,
776 int Options,
777 template<typename, int>
778 class JointCollectionTpl,
779 typename MatrixType1,
780 typename MatrixType2,
781 typename MatrixType3>
782 42 void computeABADerivatives(
783 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
784 DataTpl<Scalar, Options, JointCollectionTpl> & data,
785 const container::aligned_vector<ForceTpl<Scalar, Options>> & fext,
786 const Eigen::MatrixBase<MatrixType1> & aba_partial_dq,
787 const Eigen::MatrixBase<MatrixType2> & aba_partial_dv,
788 const Eigen::MatrixBase<MatrixType3> & aba_partial_dtau)
789 {
790
1/4
✗ Branch 1 not taken.
✓ Branch 2 taken 21 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
 42 PINOCCHIO_CHECK_INPUT_ARGUMENT(
791 fext.size() == (size_t)model.njoints,
792 "The size of the external forces is not of right size");
793
1/4
✗ Branch 1 not taken.
✓ Branch 2 taken 21 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
 42 PINOCCHIO_CHECK_INPUT_ARGUMENT(aba_partial_dq.cols() == model.nv);
794
1/4
✗ Branch 1 not taken.
✓ Branch 2 taken 21 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
 42 PINOCCHIO_CHECK_INPUT_ARGUMENT(aba_partial_dq.rows() == model.nv);
795
1/4
✗ Branch 1 not taken.
✓ Branch 2 taken 21 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
 42 PINOCCHIO_CHECK_INPUT_ARGUMENT(aba_partial_dv.cols() == model.nv);
796
1/4
✗ Branch 1 not taken.
✓ Branch 2 taken 21 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
 42 PINOCCHIO_CHECK_INPUT_ARGUMENT(aba_partial_dv.rows() == model.nv);
797
1/4
✗ Branch 1 not taken.
✓ Branch 2 taken 21 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
 42 PINOCCHIO_CHECK_INPUT_ARGUMENT(aba_partial_dtau.cols() == model.nv);
798
1/4
✗ Branch 1 not taken.
✓ Branch 2 taken 21 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
 42 PINOCCHIO_CHECK_INPUT_ARGUMENT(aba_partial_dtau.rows() == model.nv);
799
3/8
✓ Branch 2 taken 21 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 21 times.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✓ Branch 8 taken 21 times.
✗ Branch 11 not taken.
✗ Branch 12 not taken.
 42 PINOCCHIO_CHECK_INPUT_ARGUMENT(
800 isZero(model.gravity.angular()),
801 "The gravity must be a pure force vector, no angular part");
802
1/2
✓ Branch 1 taken 21 times.
✗ Branch 2 not taken.
 42 assert(model.check(data) && "data is not consistent with model.");
803
804 typedef typename ModelTpl<Scalar, Options, JointCollectionTpl>::JointIndex JointIndex;
805
806 42 MatrixType3 & Minv_ = PINOCCHIO_EIGEN_CONST_CAST(MatrixType3, aba_partial_dtau);
807
1/2
✓ Branch 2 taken 21 times.
✗ Branch 3 not taken.
 42 Minv_.template triangularView<Eigen::Upper>().setZero();
808
809 /// First, compute Minv and a, the joint acceleration vector
810 // for(JointIndex i=1; i<(JointIndex) model.njoints; ++i)
811 // data.of[i] -= data.oMi[i].act(fext[i]);
812
813 42 data.Fcrb[0].setZero();
814 typedef optimized::ComputeABADerivativesBackwardStep1<
815 Scalar, Options, JointCollectionTpl, MatrixType3>
816 Pass2;
817
2/2
✓ Branch 0 taken 567 times.
✓ Branch 1 taken 21 times.
 1176 for (JointIndex i = (JointIndex)(model.njoints - 1); i > 0; --i)
818 {
819
1/2
✓ Branch 4 taken 567 times.
✗ Branch 5 not taken.
 1134 Pass2::run(model.joints[i], data.joints[i], typename Pass2::ArgsType(model, data, Minv_));
820 }
821
822 typedef optimized::ComputeABADerivativesForwardStep2<
823 Scalar, Options, JointCollectionTpl, MatrixType3>
824 Pass3;
825
2/2
✓ Branch 0 taken 567 times.
✓ Branch 1 taken 21 times.
 1176 for (JointIndex i = 1; i < (JointIndex)model.njoints; ++i)
826 {
827
1/2
✓ Branch 4 taken 567 times.
✗ Branch 5 not taken.
 1134 Pass3::run(model.joints[i], data.joints[i], typename Pass3::ArgsType(model, data, Minv_));
828
1/2
✓ Branch 5 taken 567 times.
✗ Branch 6 not taken.
 1134 data.of[i] -= data.oMi[i].act(fext[i]);
829 }
830
831 typedef ComputeABADerivativesBackwardStep2<Scalar, Options, JointCollectionTpl> Pass4;
832
2/2
✓ Branch 0 taken 567 times.
✓ Branch 1 taken 21 times.
 1176 for (JointIndex i = (JointIndex)(model.njoints - 1); i > 0; --i)
833 {
834
1/2
✓ Branch 3 taken 567 times.
✗ Branch 4 not taken.
 1134 Pass4::run(model.joints[i], typename Pass4::ArgsType(model, data));
835 }
836
837
2/4
✓ Branch 1 taken 21 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 21 times.
✗ Branch 5 not taken.
 42 Minv_.template triangularView<Eigen::StrictlyLower>() =
838
1/2
✓ Branch 2 taken 21 times.
✗ Branch 3 not taken.
 42 Minv_.transpose().template triangularView<Eigen::StrictlyLower>();
839
840
3/6
✓ Branch 2 taken 21 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 21 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 21 times.
✗ Branch 10 not taken.
 42 PINOCCHIO_EIGEN_CONST_CAST(MatrixType1, aba_partial_dq).noalias() = -Minv_ * data.dtau_dq;
841
3/6
✓ Branch 2 taken 21 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 21 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 21 times.
✗ Branch 10 not taken.
 42 PINOCCHIO_EIGEN_CONST_CAST(MatrixType2, aba_partial_dv).noalias() = -Minv_ * data.dtau_dv;
842 }
843
844 template<
845 typename Scalar,
846 int Options,
847 template<typename, int>
848 class JointCollectionTpl,
849 typename ConfigVectorType,
850 typename TangentVectorType1,
851 typename TangentVectorType2>
852 typename std::enable_if<
853 ConfigVectorType::IsVectorAtCompileTime || TangentVectorType1::IsVectorAtCompileTime
854 || TangentVectorType2::IsVectorAtCompileTime,
855 void>::type
856 26 computeABADerivatives(
857 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
858 DataTpl<Scalar, Options, JointCollectionTpl> & data,
859 const Eigen::MatrixBase<ConfigVectorType> & q,
860 const Eigen::MatrixBase<TangentVectorType1> & v,
861 const Eigen::MatrixBase<TangentVectorType2> & tau)
862 {
863 26 ::pinocchio::impl::computeABADerivatives(
864 26 model, data, q, v, tau, data.ddq_dq, data.ddq_dv, data.Minv);
865 26 }
866
867 template<
868 typename Scalar,
869 int Options,
870 template<typename, int>
871 class JointCollectionTpl,
872 typename ConfigVectorType,
873 typename TangentVectorType1,
874 typename TangentVectorType2>
875 2 void computeABADerivatives(
876 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
877 DataTpl<Scalar, Options, JointCollectionTpl> & data,
878 const Eigen::MatrixBase<ConfigVectorType> & q,
879 const Eigen::MatrixBase<TangentVectorType1> & v,
880 const Eigen::MatrixBase<TangentVectorType2> & tau,
881 const container::aligned_vector<ForceTpl<Scalar, Options>> & fext)
882 {
883 2 ::pinocchio::impl::computeABADerivatives(
884 2 model, data, q, v, tau, fext, data.ddq_dq, data.ddq_dv, data.Minv);
885 2 }
886
887 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
888 void computeABADerivatives(
889 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
890 DataTpl<Scalar, Options, JointCollectionTpl> & data)
891 {
892 ::pinocchio::impl::computeABADerivatives(model, data, data.ddq_dq, data.ddq_dv, data.Minv);
893 }
894
895 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
896 void computeABADerivatives(
897 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
898 DataTpl<Scalar, Options, JointCollectionTpl> & data,
899 const container::aligned_vector<ForceTpl<Scalar, Options>> & fext)
900 {
901 ::pinocchio::impl::computeABADerivatives(
902 model, data, fext, data.ddq_dq, data.ddq_dv, data.Minv);
903 }
904
905 } // namespace impl
906
907 template<
908 typename Scalar,
909 int Options,
910 template<typename, int>
911 class JointCollectionTpl,
912 typename ConfigVectorType,
913 typename TangentVectorType1,
914 typename TangentVectorType2,
915 typename MatrixType1,
916 typename MatrixType2,
917 typename MatrixType3>
918 ✗ void computeABADerivatives(
919 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
920 DataTpl<Scalar, Options, JointCollectionTpl> & data,
921 const Eigen::MatrixBase<ConfigVectorType> & q,
922 const Eigen::MatrixBase<TangentVectorType1> & v,
923 const Eigen::MatrixBase<TangentVectorType2> & tau,
924 const Eigen::MatrixBase<MatrixType1> & aba_partial_dq,
925 const Eigen::MatrixBase<MatrixType2> & aba_partial_dv,
926 const Eigen::MatrixBase<MatrixType3> & aba_partial_dtau)
927 {
928 ✗ impl::computeABADerivatives(
929 model, data, make_const_ref(q), make_const_ref(v), make_const_ref(tau),
930 make_ref(aba_partial_dq), make_ref(aba_partial_dv), make_ref(aba_partial_dtau));
931 }
932
933 template<
934 typename Scalar,
935 int Options,
936 template<typename, int>
937 class JointCollectionTpl,
938 typename ConfigVectorType,
939 typename TangentVectorType1,
940 typename TangentVectorType2,
941 typename MatrixType1,
942 typename MatrixType2,
943 typename MatrixType3>
944 ✗ void computeABADerivatives(
945 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
946 DataTpl<Scalar, Options, JointCollectionTpl> & data,
947 const Eigen::MatrixBase<ConfigVectorType> & q,
948 const Eigen::MatrixBase<TangentVectorType1> & v,
949 const Eigen::MatrixBase<TangentVectorType2> & tau,
950 const container::aligned_vector<ForceTpl<Scalar, Options>> & fext,
951 const Eigen::MatrixBase<MatrixType1> & aba_partial_dq,
952 const Eigen::MatrixBase<MatrixType2> & aba_partial_dv,
953 const Eigen::MatrixBase<MatrixType3> & aba_partial_dtau)
954 {
955 ✗ impl::computeABADerivatives(
956 model, data, make_const_ref(q), make_const_ref(v), make_const_ref(tau), fext,
957 make_ref(aba_partial_dq), make_ref(aba_partial_dv), make_ref(aba_partial_dtau));
958 }
959
960 template<
961 typename Scalar,
962 int Options,
963 template<typename, int>
964 class JointCollectionTpl,
965 typename ConfigVectorType,
966 typename TangentVectorType1,
967 typename TangentVectorType2>
968 typename std::enable_if<
969 ConfigVectorType::IsVectorAtCompileTime || TangentVectorType1::IsVectorAtCompileTime
970 || TangentVectorType2::IsVectorAtCompileTime,
971 void>::type
972 2 computeABADerivatives(
973 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
974 DataTpl<Scalar, Options, JointCollectionTpl> & data,
975 const Eigen::MatrixBase<ConfigVectorType> & q,
976 const Eigen::MatrixBase<TangentVectorType1> & v,
977 const Eigen::MatrixBase<TangentVectorType2> & tau)
978 {
979
3/6
✓ 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.
 2 impl::computeABADerivatives(
980 model, data, make_const_ref(q), make_const_ref(v), make_const_ref(tau));
981 2 }
982
983 template<
984 typename Scalar,
985 int Options,
986 template<typename, int>
987 class JointCollectionTpl,
988 typename ConfigVectorType,
989 typename TangentVectorType1,
990 typename TangentVectorType2>
991 1 void computeABADerivatives(
992 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
993 DataTpl<Scalar, Options, JointCollectionTpl> & data,
994 const Eigen::MatrixBase<ConfigVectorType> & q,
995 const Eigen::MatrixBase<TangentVectorType1> & v,
996 const Eigen::MatrixBase<TangentVectorType2> & tau,
997 const container::aligned_vector<ForceTpl<Scalar, Options>> & fext)
998 {
999
3/6
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
 1 impl::computeABADerivatives(
1000 model, data, make_const_ref(q), make_const_ref(v), make_const_ref(tau), fext);
1001 1 }
1002
1003 template<
1004 typename Scalar,
1005 int Options,
1006 template<typename, int>
1007 class JointCollectionTpl,
1008 typename MatrixType1,
1009 typename MatrixType2,
1010 typename MatrixType3>
1011 typename std::enable_if<
1012 !(MatrixType1::IsVectorAtCompileTime || MatrixType2::IsVectorAtCompileTime
1013 || MatrixType3::IsVectorAtCompileTime),
1014 void>::type
1015 ✗ computeABADerivatives(
1016 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
1017 DataTpl<Scalar, Options, JointCollectionTpl> & data,
1018 const Eigen::MatrixBase<MatrixType1> & aba_partial_dq,
1019 const Eigen::MatrixBase<MatrixType2> & aba_partial_dv,
1020 const Eigen::MatrixBase<MatrixType3> & aba_partial_dtau)
1021 {
1022 ✗ impl::computeABADerivatives(
1023 model, data, make_ref(aba_partial_dq), make_ref(aba_partial_dv), make_ref(aba_partial_dtau));
1024 }
1025
1026 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
1027 ✗ void computeABADerivatives(
1028 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
1029 DataTpl<Scalar, Options, JointCollectionTpl> & data)
1030 {
1031 ✗ impl::computeABADerivatives(
1032 ✗ model, data, make_ref(data.ddq_dq), make_ref(data.ddq_dv), make_ref(data.Minv));
1033 }
1034
1035 template<
1036 typename Scalar,
1037 int Options,
1038 template<typename, int>
1039 class JointCollectionTpl,
1040 typename MatrixType1,
1041 typename MatrixType2,
1042 typename MatrixType3>
1043 ✗ void computeABADerivatives(
1044 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
1045 DataTpl<Scalar, Options, JointCollectionTpl> & data,
1046 const container::aligned_vector<ForceTpl<Scalar, Options>> & fext,
1047 const Eigen::MatrixBase<MatrixType1> & aba_partial_dq,
1048 const Eigen::MatrixBase<MatrixType2> & aba_partial_dv,
1049 const Eigen::MatrixBase<MatrixType3> & aba_partial_dtau)
1050 {
1051 ✗ impl::computeABADerivatives(
1052 model, data, fext, make_ref(aba_partial_dq), make_ref(aba_partial_dv),
1053 make_ref(aba_partial_dtau));
1054 }
1055
1056 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
1057 ✗ void computeABADerivatives(
1058 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
1059 DataTpl<Scalar, Options, JointCollectionTpl> & data,
1060 const container::aligned_vector<ForceTpl<Scalar, Options>> & fext)
1061 {
1062 ✗ impl::computeABADerivatives(
1063 ✗ model, data, fext, make_ref(data.ddq_dq), make_ref(data.ddq_dv), make_ref(data.Minv));
1064 }
1065
1066 } // namespace pinocchio
1067
1068 #endif // ifndef __pinocchio_algorithm_aba_derivatives_hxx__
1069