GCC Code Coverage Report

Directory: ./
File: include/pinocchio/algorithm/aba.hxx
Date: 2025-04-30 16:14:33
Exec Total Coverage
Lines: 321 321 100.0%
Branches: 512 1140 44.9%
Line Branch Exec Source
1 //
2 // Copyright (c) 2016-2021 CNRS INRIA
3 //
4
5 #ifndef __pinocchio_algorithm_aba_hxx__
6 #define __pinocchio_algorithm_aba_hxx__
7
8 #include "pinocchio/spatial/act-on-set.hpp"
9 #include "pinocchio/multibody/visitor.hpp"
10 #include "pinocchio/algorithm/check.hpp"
11
12 /// @cond DEV
13
14 namespace pinocchio
15 {
16 namespace impl
17 {
18 namespace internal
19 {
20
21 template<typename Scalar>
22 struct SE3actOn
23 {
24 template<int Options, typename Matrix6Type>
25 static typename PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix6Type)
26 5696 run(const SE3Tpl<Scalar, Options> & M, const Eigen::MatrixBase<Matrix6Type> & I)
27 {
28 typedef SE3Tpl<Scalar, Options> SE3;
29 typedef typename SE3::Matrix3 Matrix3;
30 typedef typename SE3::Vector3 Vector3;
31
32 typedef const Eigen::Block<Matrix6Type, 3, 3> constBlock3;
33
34 typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix6Type) ReturnType;
35 typedef Eigen::Block<ReturnType, 3, 3> Block3;
36
37 5696 Matrix6Type & I_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix6Type, I);
38
1/2
✓ Branch 1 taken 5644 times.
✗ Branch 2 not taken.
 5696 const constBlock3 & Ai = I_.template block<3, 3>(Inertia::LINEAR, Inertia::LINEAR);
39
1/2
✓ Branch 1 taken 5644 times.
✗ Branch 2 not taken.
 5696 const constBlock3 & Bi = I_.template block<3, 3>(Inertia::LINEAR, Inertia::ANGULAR);
40
1/2
✓ Branch 1 taken 5644 times.
✗ Branch 2 not taken.
 5696 const constBlock3 & Di = I_.template block<3, 3>(Inertia::ANGULAR, Inertia::ANGULAR);
41
42
1/2
✓ Branch 1 taken 5644 times.
✗ Branch 2 not taken.
 5696 const Matrix3 & R = M.rotation();
43
1/2
✓ Branch 1 taken 5644 times.
✗ Branch 2 not taken.
 5696 const Vector3 & t = M.translation();
44
45
1/2
✓ Branch 1 taken 5644 times.
✗ Branch 2 not taken.
 5696 ReturnType res;
46
1/2
✓ Branch 1 taken 5644 times.
✗ Branch 2 not taken.
 5696 Block3 Ao = res.template block<3, 3>(Inertia::LINEAR, Inertia::LINEAR);
47
1/2
✓ Branch 1 taken 5644 times.
✗ Branch 2 not taken.
 5696 Block3 Bo = res.template block<3, 3>(Inertia::LINEAR, Inertia::ANGULAR);
48
1/2
✓ Branch 1 taken 5644 times.
✗ Branch 2 not taken.
 5696 Block3 Co = res.template block<3, 3>(Inertia::ANGULAR, Inertia::LINEAR);
49
1/2
✓ Branch 1 taken 5644 times.
✗ Branch 2 not taken.
 5696 Block3 Do = res.template block<3, 3>(Inertia::ANGULAR, Inertia::ANGULAR);
50
51
3/6
✓ Branch 1 taken 5644 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5644 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 5644 times.
✗ Branch 8 not taken.
 5696 Do.noalias() = R * Ai; // tmp variable
52
4/8
✓ Branch 1 taken 5644 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5644 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 5644 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 5644 times.
✗ Branch 11 not taken.
 5696 Ao.noalias() = Do * R.transpose();
53
54
3/6
✓ Branch 1 taken 5644 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5644 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 5644 times.
✗ Branch 8 not taken.
 5696 Do.noalias() = R * Bi; // tmp variable
55
4/8
✓ Branch 1 taken 5644 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5644 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 5644 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 5644 times.
✗ Branch 11 not taken.
 5696 Bo.noalias() = Do * R.transpose();
56
57
3/6
✓ Branch 1 taken 5644 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5644 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 5644 times.
✗ Branch 8 not taken.
 5696 Co.noalias() = R * Di; // tmp variable
58
4/8
✓ Branch 1 taken 5644 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5644 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 5644 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 5644 times.
✗ Branch 11 not taken.
 5696 Do.noalias() = Co * R.transpose();
59
60
4/8
✓ Branch 1 taken 5644 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5644 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 5644 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 5644 times.
✗ Branch 11 not taken.
 5696 Do.row(0) += t.cross(Bo.col(0));
61
4/8
✓ Branch 1 taken 5644 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5644 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 5644 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 5644 times.
✗ Branch 11 not taken.
 5696 Do.row(1) += t.cross(Bo.col(1));
62
4/8
✓ Branch 1 taken 5644 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5644 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 5644 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 5644 times.
✗ Branch 11 not taken.
 5696 Do.row(2) += t.cross(Bo.col(2));
63
64
4/8
✓ Branch 1 taken 5644 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5644 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 5644 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 5644 times.
✗ Branch 11 not taken.
 5696 Co.col(0) = t.cross(Ao.col(0));
65
4/8
✓ Branch 1 taken 5644 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5644 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 5644 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 5644 times.
✗ Branch 11 not taken.
 5696 Co.col(1) = t.cross(Ao.col(1));
66
4/8
✓ Branch 1 taken 5644 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5644 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 5644 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 5644 times.
✗ Branch 11 not taken.
 5696 Co.col(2) = t.cross(Ao.col(2));
67
2/4
✓ Branch 1 taken 5644 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5644 times.
✗ Branch 5 not taken.
 5696 Co += Bo.transpose();
68
69
2/4
✓ Branch 1 taken 5644 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5644 times.
✗ Branch 5 not taken.
 5696 Bo = Co.transpose();
70
4/8
✓ Branch 1 taken 5644 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5644 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 5644 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 5644 times.
✗ Branch 11 not taken.
 5696 Do.col(0) += t.cross(Bo.col(0));
71
4/8
✓ Branch 1 taken 5644 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5644 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 5644 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 5644 times.
✗ Branch 11 not taken.
 5696 Do.col(1) += t.cross(Bo.col(1));
72
4/8
✓ Branch 1 taken 5644 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5644 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 5644 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 5644 times.
✗ Branch 11 not taken.
 5696 Do.col(2) += t.cross(Bo.col(2));
73
74 11392 return res;
75 }
76 };
77 } // namespace internal
78
79 template<
80 typename Scalar,
81 int Options,
82 template<typename, int> class JointCollectionTpl,
83 typename ConfigVectorType,
84 typename TangentVectorType>
85 struct AbaWorldConventionForwardStep1
86 : public fusion::JointUnaryVisitorBase<AbaWorldConventionForwardStep1<
87 Scalar,
88 Options,
89 JointCollectionTpl,
90 ConfigVectorType,
91 TangentVectorType>>
92 {
93 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
94 typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
95
96 typedef boost::fusion::
97 vector<const Model &, Data &, const ConfigVectorType &, const TangentVectorType &>
98 ArgsType;
99
100 template<typename JointModel>
101 910 static void algo(
102 const pinocchio::JointModelBase<JointModel> & jmodel,
103 pinocchio::JointDataBase<typename JointModel::JointDataDerived> & jdata,
104 const Model & model,
105 Data & data,
106 const Eigen::MatrixBase<ConfigVectorType> & q,
107 const Eigen::MatrixBase<TangentVectorType> & v)
108 {
109 typedef typename Model::JointIndex JointIndex;
110 typedef typename Data::Motion Motion;
111
112 910 const JointIndex i = jmodel.id();
113 910 Motion & ov = data.ov[i];
114 910 jmodel.calc(jdata.derived(), q.derived(), v.derived());
115
116 910 const JointIndex & parent = model.parents[i];
117
3/7
✗ Branch 3 not taken.
✓ Branch 4 taken 427 times.
✓ Branch 5 taken 28 times.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✓ Branch 8 taken 427 times.
✗ Branch 9 not taken.
 910 data.liMi[i] = model.jointPlacements[i] * jdata.M();
118
2/2
✓ Branch 0 taken 417 times.
✓ Branch 1 taken 38 times.
 910 if (parent > 0)
119
1/2
✓ Branch 5 taken 417 times.
✗ Branch 6 not taken.
 834 data.oMi[i] = data.oMi[parent] * data.liMi[i];
120 else
121 76 data.oMi[i] = data.liMi[i];
122
123
2/4
✓ Branch 4 taken 455 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 455 times.
✗ Branch 8 not taken.
 910 jmodel.jointCols(data.J) = data.oMi[i].act(jdata.S());
124
125
1/5
✗ Branch 3 not taken.
✓ Branch 4 taken 455 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
 910 ov = data.oMi[i].act(jdata.v());
126
2/2
✓ Branch 0 taken 417 times.
✓ Branch 1 taken 38 times.
 910 if (parent > 0)
127 834 ov += data.ov[parent];
128
129
1/6
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 455 times.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
 910 data.oa_gf[i] = data.oMi[i].act(jdata.c());
130
2/2
✓ Branch 0 taken 417 times.
✓ Branch 1 taken 38 times.
 910 if (parent > 0)
131
1/2
✓ Branch 4 taken 417 times.
✗ Branch 5 not taken.
 834 data.oa_gf[i] += (data.ov[parent] ^ ov);
132
133
2/4
✓ Branch 5 taken 455 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 455 times.
✗ Branch 10 not taken.
 910 data.oinertias[i] = data.oYcrb[i] = data.oMi[i].act(model.inertias[i]);
134 910 data.oYaba[i] = data.oYcrb[i].matrix();
135
136
1/2
✓ Branch 4 taken 455 times.
✗ Branch 5 not taken.
 910 data.oh[i] = data.oYcrb[i] * ov; // necessary for ABA derivatives
137
1/2
✓ Branch 4 taken 455 times.
✗ Branch 5 not taken.
 910 data.of[i] = ov.cross(data.oh[i]);
138 }
139 };
140
141 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
142 struct AbaWorldConventionBackwardStep
143 : public fusion::JointUnaryVisitorBase<
144 AbaWorldConventionBackwardStep<Scalar, Options, JointCollectionTpl>>
145 {
146 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
147 typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
148
149 typedef boost::fusion::vector<const Model &, Data &> ArgsType;
150
151 template<typename JointModel>
152 910 static void algo(
153 const JointModelBase<JointModel> & jmodel,
154 JointDataBase<typename JointModel::JointDataDerived> & jdata,
155 const Model & model,
156 Data & data)
157 {
158 typedef typename Model::JointIndex JointIndex;
159 typedef typename Data::Inertia Inertia;
160 typedef typename Data::Force Force;
161 typedef typename Data::Matrix6x Matrix6x;
162
163
1/2
✓ Branch 1 taken 455 times.
✗ Branch 2 not taken.
 910 const JointIndex i = jmodel.id();
164 910 const JointIndex parent = model.parents[i];
165 910 typename Inertia::Matrix6 & Ia = data.oYaba[i];
166
167 typedef typename SizeDepType<JointModel::NV>::template ColsReturn<Matrix6x>::Type ColBlock;
168
1/2
✓ Branch 1 taken 455 times.
✗ Branch 2 not taken.
 910 ColBlock Jcols = jmodel.jointCols(data.J);
169
170 910 Force & fi = data.of[i];
171
172
6/12
✓ Branch 1 taken 455 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 455 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 455 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 455 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 455 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 455 times.
✗ Branch 17 not taken.
 910 jmodel.jointVelocitySelector(data.u).noalias() -= Jcols.transpose() * fi.toVector();
173
174
4/8
✓ Branch 1 taken 455 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 455 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 455 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 455 times.
✗ Branch 11 not taken.
 910 jdata.U().noalias() = Ia * Jcols;
175
6/12
✓ Branch 1 taken 455 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 455 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 455 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 455 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 455 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 455 times.
✗ Branch 17 not taken.
 910 jdata.StU().noalias() = Jcols.transpose() * jdata.U();
176
177 // Account for the rotor inertia contribution
178
4/8
✓ Branch 1 taken 455 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 455 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 455 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 455 times.
✗ Branch 11 not taken.
 910 jdata.StU().diagonal() += jmodel.jointVelocitySelector(model.armature);
179
180
3/6
✓ Branch 1 taken 455 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 455 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 455 times.
✗ Branch 8 not taken.
 910 ::pinocchio::internal::PerformStYSInversion<Scalar>::run(jdata.StU(), jdata.Dinv());
181
6/12
✓ Branch 1 taken 455 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 455 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 455 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 455 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 455 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 455 times.
✗ Branch 17 not taken.
 910 jdata.UDinv().noalias() = jdata.U() * jdata.Dinv();
182
183
2/2
✓ Branch 0 taken 417 times.
✓ Branch 1 taken 38 times.
 910 if (parent > 0)
184 {
185
6/12
✓ Branch 1 taken 417 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 417 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 417 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 417 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 417 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 417 times.
✗ Branch 17 not taken.
 834 Ia.noalias() -= jdata.UDinv() * jdata.U().transpose();
186
187
4/8
✓ Branch 1 taken 417 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 417 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 417 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 417 times.
✗ Branch 11 not taken.
 834 fi.toVector().noalias() +=
188
5/10
✓ Branch 1 taken 417 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 417 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 417 times.
✗ Branch 8 not taken.
✓ Branch 11 taken 417 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 417 times.
✗ Branch 15 not taken.
 834 Ia * data.oa_gf[i].toVector() + jdata.UDinv() * jmodel.jointVelocitySelector(data.u);
189
1/2
✓ Branch 2 taken 417 times.
✗ Branch 3 not taken.
 834 data.oYaba[parent] += Ia;
190
1/2
✓ Branch 2 taken 417 times.
✗ Branch 3 not taken.
 834 data.of[parent] += fi;
191 }
192 }
193 };
194
195 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
196 struct AbaWorldConventionForwardStep2
197 : public fusion::JointUnaryVisitorBase<
198 AbaWorldConventionForwardStep2<Scalar, Options, JointCollectionTpl>>
199 {
200 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
201 typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
202
203 typedef boost::fusion::vector<const Model &, Data &> ArgsType;
204
205 template<typename JointModel>
206 910 static void algo(
207 const pinocchio::JointModelBase<JointModel> & jmodel,
208 pinocchio::JointDataBase<typename JointModel::JointDataDerived> & jdata,
209 const Model & model,
210 Data & data)
211 {
212
213 typedef typename Model::JointIndex JointIndex;
214 typedef typename Data::Matrix6x Matrix6x;
215
216 typedef typename SizeDepType<JointModel::NV>::template ColsReturn<Matrix6x>::Type ColBlock;
217
1/2
✓ Branch 1 taken 455 times.
✗ Branch 2 not taken.
 910 ColBlock J_cols = jmodel.jointCols(data.J);
218
219
1/2
✓ Branch 1 taken 455 times.
✗ Branch 2 not taken.
 910 const JointIndex i = jmodel.id();
220 910 const JointIndex parent = model.parents[i];
221
222
1/2
✓ Branch 3 taken 455 times.
✗ Branch 4 not taken.
 910 data.oa_gf[i] += data.oa_gf[parent]; // does take into account the gravity field
223
4/8
✓ Branch 1 taken 455 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 455 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 455 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 455 times.
✗ Branch 11 not taken.
 910 jmodel.jointVelocitySelector(data.ddq).noalias() =
224
3/6
✓ Branch 1 taken 455 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 455 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 455 times.
✗ Branch 8 not taken.
 910 jdata.Dinv() * jmodel.jointVelocitySelector(data.u)
225
4/8
✓ Branch 2 taken 455 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 455 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 455 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 455 times.
✗ Branch 12 not taken.
 910 - jdata.UDinv().transpose() * data.oa_gf[i].toVector();
226
5/10
✓ Branch 1 taken 455 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 455 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 455 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 455 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 455 times.
✗ Branch 15 not taken.
 910 data.oa_gf[i].toVector().noalias() += J_cols * jmodel.jointVelocitySelector(data.ddq);
227
228 // Handle consistent output
229
2/4
✓ Branch 2 taken 455 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 455 times.
✗ Branch 7 not taken.
 910 data.oa[i] = data.oa_gf[i] + model.gravity;
230
4/8
✓ Branch 3 taken 455 times.
✗ Branch 4 not taken.
✓ Branch 8 taken 455 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 455 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 455 times.
✗ Branch 16 not taken.
 910 data.of[i] = data.oinertias[i] * data.oa_gf[i] + data.ov[i].cross(data.oh[i]);
231 }
232 };
233
234 template<
235 typename Scalar,
236 int Options,
237 template<typename, int> class JointCollectionTpl,
238 typename ConfigVectorType,
239 typename TangentVectorType1,
240 typename TangentVectorType2>
241 const typename DataTpl<Scalar, Options, JointCollectionTpl>::TangentVectorType &
242 51 abaWorldConvention(
243 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
244 DataTpl<Scalar, Options, JointCollectionTpl> & data,
245 const Eigen::MatrixBase<ConfigVectorType> & q,
246 const Eigen::MatrixBase<TangentVectorType1> & v,
247 const Eigen::MatrixBase<TangentVectorType2> & tau)
248 {
249
1/2
✓ Branch 1 taken 35 times.
✗ Branch 2 not taken.
 51 assert(model.check(data) && "data is not consistent with model.");
250
2/4
✓ Branch 1 taken 35 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 35 times.
✗ Branch 4 not taken.
 51 assert(model.check(MimicChecker()) && "Function does not support mimic joints");
251
1/4
✗ Branch 1 not taken.
✓ Branch 2 taken 35 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
 51 PINOCCHIO_CHECK_INPUT_ARGUMENT(
252 q.size() == model.nq, "The joint configuration vector is not of right size");
253
1/4
✗ Branch 1 not taken.
✓ Branch 2 taken 35 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
 51 PINOCCHIO_CHECK_INPUT_ARGUMENT(
254 v.size() == model.nv, "The joint velocity vector is not of right size");
255
1/4
✗ Branch 1 not taken.
✓ Branch 2 taken 35 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
 51 PINOCCHIO_CHECK_INPUT_ARGUMENT(
256 tau.size() == model.nv, "The joint acceleration vector is not of right size");
257
258 typedef typename ModelTpl<Scalar, Options, JointCollectionTpl>::JointIndex JointIndex;
259
260
1/2
✓ Branch 3 taken 35 times.
✗ Branch 4 not taken.
 51 data.oa_gf[0] = -model.gravity;
261 51 data.of[0].setZero();
262 51 data.u = tau;
263
264 typedef AbaWorldConventionForwardStep1<
265 Scalar, Options, JointCollectionTpl, ConfigVectorType, TangentVectorType1>
266 Pass1;
267
2/2
✓ Branch 0 taken 374 times.
✓ Branch 1 taken 35 times.
 545 for (JointIndex i = 1; i < (JointIndex)model.njoints; ++i)
268 {
269
1/2
✓ Branch 1 taken 374 times.
✗ Branch 2 not taken.
 494 Pass1::run(
270 494 model.joints[i], data.joints[i],
271 988 typename Pass1::ArgsType(model, data, q.derived(), v.derived()));
272 }
273
274 typedef AbaWorldConventionBackwardStep<Scalar, Options, JointCollectionTpl> Pass2;
275
2/2
✓ Branch 0 taken 374 times.
✓ Branch 1 taken 35 times.
 545 for (JointIndex i = (JointIndex)model.njoints - 1; i > 0; --i)
276 {
277
1/2
✓ Branch 4 taken 374 times.
✗ Branch 5 not taken.
 494 Pass2::run(model.joints[i], data.joints[i], typename Pass2::ArgsType(model, data));
278 }
279
280 typedef AbaWorldConventionForwardStep2<Scalar, Options, JointCollectionTpl> Pass3;
281
2/2
✓ Branch 0 taken 374 times.
✓ Branch 1 taken 35 times.
 545 for (JointIndex i = 1; i < (JointIndex)model.njoints; ++i)
282 {
283
1/2
✓ Branch 4 taken 374 times.
✗ Branch 5 not taken.
 494 Pass3::run(model.joints[i], data.joints[i], typename Pass3::ArgsType(model, data));
284 }
285
286
2/2
✓ Branch 0 taken 374 times.
✓ Branch 1 taken 35 times.
 545 for (JointIndex i = (JointIndex)model.njoints - 1; i > 0; --i)
287 {
288 494 const JointIndex parent = model.parents[i];
289 494 data.of[parent] += data.of[i];
290 }
291
292 51 return data.ddq;
293 }
294
295 template<
296 typename Scalar,
297 int Options,
298 template<typename, int> class JointCollectionTpl,
299 typename ConfigVectorType,
300 typename TangentVectorType1,
301 typename TangentVectorType2,
302 typename ForceDerived>
303 const typename DataTpl<Scalar, Options, JointCollectionTpl>::TangentVectorType &
304 3 abaWorldConvention(
305 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
306 DataTpl<Scalar, Options, JointCollectionTpl> & data,
307 const Eigen::MatrixBase<ConfigVectorType> & q,
308 const Eigen::MatrixBase<TangentVectorType1> & v,
309 const Eigen::MatrixBase<TangentVectorType2> & tau,
310 const container::aligned_vector<ForceDerived> & fext)
311
312 {
313
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 assert(model.check(data) && "data is not consistent with model.");
314
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");
315
316
1/4
✗ Branch 1 not taken.
✓ Branch 2 taken 3 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
 3 PINOCCHIO_CHECK_INPUT_ARGUMENT(
317 q.size() == model.nq, "The joint configuration vector is not of right size");
318
1/4
✗ Branch 1 not taken.
✓ Branch 2 taken 3 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
 3 PINOCCHIO_CHECK_INPUT_ARGUMENT(
319 v.size() == model.nv, "The joint velocity vector is not of right size");
320
1/4
✗ Branch 1 not taken.
✓ Branch 2 taken 3 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
 3 PINOCCHIO_CHECK_INPUT_ARGUMENT(
321 tau.size() == model.nv, "The joint acceleration vector is not of right size");
322
323 typedef typename ModelTpl<Scalar, Options, JointCollectionTpl>::JointIndex JointIndex;
324
325
1/2
✓ Branch 3 taken 3 times.
✗ Branch 4 not taken.
 3 data.oa_gf[0] = -model.gravity;
326 3 data.u = tau;
327
328 typedef AbaWorldConventionForwardStep1<
329 Scalar, Options, JointCollectionTpl, ConfigVectorType, TangentVectorType1>
330 Pass1;
331
2/2
✓ Branch 0 taken 81 times.
✓ Branch 1 taken 3 times.
 84 for (JointIndex i = 1; i < (JointIndex)model.njoints; ++i)
332 {
333
1/2
✓ Branch 1 taken 81 times.
✗ Branch 2 not taken.
 81 Pass1::run(
334 81 model.joints[i], data.joints[i],
335 81 typename Pass1::ArgsType(model, data, q.derived(), v.derived()));
336
1/2
✓ Branch 5 taken 81 times.
✗ Branch 6 not taken.
 81 data.of[i] -= data.oMi[i].act(fext[i]);
337 }
338
339 typedef AbaWorldConventionBackwardStep<Scalar, Options, JointCollectionTpl> Pass2;
340
2/2
✓ Branch 0 taken 81 times.
✓ Branch 1 taken 3 times.
 84 for (JointIndex i = (JointIndex)model.njoints - 1; i > 0; --i)
341 {
342
1/2
✓ Branch 4 taken 81 times.
✗ Branch 5 not taken.
 81 Pass2::run(model.joints[i], data.joints[i], typename Pass2::ArgsType(model, data));
343 }
344
345 typedef AbaWorldConventionForwardStep2<Scalar, Options, JointCollectionTpl> Pass3;
346
2/2
✓ Branch 0 taken 81 times.
✓ Branch 1 taken 3 times.
 84 for (JointIndex i = 1; i < (JointIndex)model.njoints; ++i)
347 {
348
1/2
✓ Branch 4 taken 81 times.
✗ Branch 5 not taken.
 81 Pass3::run(model.joints[i], data.joints[i], typename Pass3::ArgsType(model, data));
349 }
350
351 3 return data.ddq;
352 }
353
354 template<
355 typename Scalar,
356 int Options,
357 template<typename, int> class JointCollectionTpl,
358 typename ConfigVectorType,
359 typename TangentVectorType>
360 struct AbaLocalConventionForwardStep1
361 : public fusion::JointUnaryVisitorBase<AbaLocalConventionForwardStep1<
362 Scalar,
363 Options,
364 JointCollectionTpl,
365 ConfigVectorType,
366 TangentVectorType>>
367 {
368 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
369 typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
370
371 typedef boost::fusion::
372 vector<const Model &, Data &, const ConfigVectorType &, const TangentVectorType &>
373 ArgsType;
374
375 template<typename JointModel>
376 12048 static void algo(
377 const pinocchio::JointModelBase<JointModel> & jmodel,
378 pinocchio::JointDataBase<typename JointModel::JointDataDerived> & jdata,
379 const Model & model,
380 Data & data,
381 const Eigen::MatrixBase<ConfigVectorType> & q,
382 const Eigen::MatrixBase<TangentVectorType> & v)
383 {
384 typedef typename Model::JointIndex JointIndex;
385
386 12048 const JointIndex i = jmodel.id();
387 12048 jmodel.calc(jdata.derived(), q.derived(), v.derived());
388
389 12048 const JointIndex & parent = model.parents[i];
390
3/7
✗ Branch 3 not taken.
✓ Branch 4 taken 5647 times.
✓ Branch 5 taken 377 times.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✓ Branch 8 taken 5647 times.
✗ Branch 9 not taken.
 12048 data.liMi[i] = model.jointPlacements[i] * jdata.M();
391
392
0/2
✗ Branch 3 not taken.
✗ Branch 4 not taken.
 12048 data.v[i] = jdata.v();
393
2/2
✓ Branch 0 taken 5644 times.
✓ Branch 1 taken 380 times.
 12048 if (parent > 0)
394
1/2
✓ Branch 5 taken 5644 times.
✗ Branch 6 not taken.
 11288 data.v[i] += data.liMi[i].actInv(data.v[parent]);
395
396
2/12
✗ Branch 3 not taken.
✓ Branch 4 taken 6024 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✓ Branch 9 taken 6024 times.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 12 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
 12048 data.a_gf[i] = jdata.c() + (data.v[i] ^ jdata.v());
397
398 12048 data.Yaba[i] = model.inertias[i].matrix();
399
1/2
✓ Branch 5 taken 6024 times.
✗ Branch 6 not taken.
 12048 data.h[i] = model.inertias[i] * data.v[i];
400
1/2
✓ Branch 5 taken 6024 times.
✗ Branch 6 not taken.
 12048 data.f[i] = data.v[i].cross(data.h[i]); // -f_ext
401 }
402 };
403
404 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
405 struct AbaLocalConventionBackwardStep
406 : public fusion::JointUnaryVisitorBase<
407 AbaLocalConventionBackwardStep<Scalar, Options, JointCollectionTpl>>
408 {
409 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
410 typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
411
412 typedef boost::fusion::vector<const Model &, Data &> ArgsType;
413
414 template<typename JointModel>
415 12048 static void algo(
416 const JointModelBase<JointModel> & jmodel,
417 JointDataBase<typename JointModel::JointDataDerived> & jdata,
418 const Model & model,
419 Data & data)
420 {
421 typedef typename Model::JointIndex JointIndex;
422 typedef typename Data::Inertia Inertia;
423 typedef typename Data::Force Force;
424
425 12048 const JointIndex i = jmodel.id();
426 12048 const JointIndex parent = model.parents[i];
427 12048 typename Inertia::Matrix6 & Ia = data.Yaba[i];
428
429
3/6
✓ Branch 4 taken 6024 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 6024 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 6024 times.
✗ Branch 11 not taken.
 12048 jmodel.jointVelocitySelector(data.u) -= jdata.S().transpose() * data.f[i];
430
1/2
✓ Branch 1 taken 6024 times.
✗ Branch 2 not taken.
 12048 jmodel.calc_aba(
431 12048 jdata.derived(), jmodel.jointVelocitySelector(model.armature), Ia, parent > 0);
432
433
2/2
✓ Branch 0 taken 5644 times.
✓ Branch 1 taken 380 times.
 12048 if (parent > 0)
434 {
435 11288 Force & pa = data.f[i];
436
4/8
✓ Branch 1 taken 5644 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5644 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 5644 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 5644 times.
✗ Branch 11 not taken.
 11288 pa.toVector().noalias() +=
437
4/8
✓ Branch 2 taken 5644 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 5644 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 5644 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 5644 times.
✗ Branch 13 not taken.
 11288 Ia * data.a_gf[i].toVector() + jdata.UDinv() * jmodel.jointVelocitySelector(data.u);
438
1/2
✓ Branch 4 taken 5644 times.
✗ Branch 5 not taken.
 11288 data.Yaba[parent] += internal::SE3actOn<Scalar>::run(data.liMi[i], Ia);
439
1/2
✓ Branch 4 taken 5644 times.
✗ Branch 5 not taken.
 11288 data.f[parent] += data.liMi[i].act(pa);
440 }
441 }
442 };
443
444 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
445 struct AbaLocalConventionForwardStep2
446 : public fusion::JointUnaryVisitorBase<
447 AbaLocalConventionForwardStep2<Scalar, Options, JointCollectionTpl>>
448 {
449 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
450 typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
451
452 typedef boost::fusion::vector<const Model &, Data &> ArgsType;
453
454 template<typename JointModel>
455 12048 static void algo(
456 const pinocchio::JointModelBase<JointModel> & jmodel,
457 pinocchio::JointDataBase<typename JointModel::JointDataDerived> & jdata,
458 const Model & model,
459 Data & data)
460 {
461 typedef typename Model::JointIndex JointIndex;
462
463 12048 const JointIndex i = jmodel.id();
464 12048 const JointIndex parent = model.parents[i];
465
466
1/2
✓ Branch 5 taken 6024 times.
✗ Branch 6 not taken.
 12048 data.a_gf[i] += data.liMi[i].actInv(data.a_gf[parent]);
467
4/8
✓ Branch 1 taken 6024 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6024 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 6024 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 6024 times.
✗ Branch 11 not taken.
 12048 jmodel.jointVelocitySelector(data.ddq).noalias() =
468
3/6
✓ Branch 1 taken 6024 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6024 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 6024 times.
✗ Branch 8 not taken.
 12048 jdata.Dinv() * jmodel.jointVelocitySelector(data.u)
469
1/5
✗ Branch 4 not taken.
✓ Branch 5 taken 6024 times.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
 12048 - jdata.UDinv().transpose() * data.a_gf[i].toVector();
470
3/6
✓ Branch 2 taken 6024 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 6024 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 6024 times.
✗ Branch 10 not taken.
 12048 data.a_gf[i] += jdata.S() * jmodel.jointVelocitySelector(data.ddq);
471
472 12048 data.a[i] = data.a_gf[i];
473
6/12
✓ Branch 3 taken 6024 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 6024 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 6024 times.
✗ Branch 10 not taken.
✓ Branch 13 taken 6024 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 6024 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 6024 times.
✗ Branch 20 not taken.
 12048 data.a[i].linear().noalias() += data.oMi[i].rotation().transpose() * model.gravity.linear();
474
3/6
✓ Branch 6 taken 6024 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 6024 times.
✗ Branch 10 not taken.
✓ Branch 13 taken 6024 times.
✗ Branch 14 not taken.
 12048 data.f[i] = model.inertias[i] * data.a_gf[i] + data.v[i].cross(data.h[i]);
475 }
476 };
477
478 template<
479 typename Scalar,
480 int Options,
481 template<typename, int> class JointCollectionTpl,
482 typename ConfigVectorType,
483 typename TangentVectorType1,
484 typename TangentVectorType2>
485 const typename DataTpl<Scalar, Options, JointCollectionTpl>::TangentVectorType &
486 279 abaLocalConvention(
487 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
488 DataTpl<Scalar, Options, JointCollectionTpl> & data,
489 const Eigen::MatrixBase<ConfigVectorType> & q,
490 const Eigen::MatrixBase<TangentVectorType1> & v,
491 const Eigen::MatrixBase<TangentVectorType2> & tau)
492 {
493
1/2
✓ Branch 1 taken 276 times.
✗ Branch 2 not taken.
 279 assert(model.check(data) && "data is not consistent with model.");
494
2/4
✓ Branch 1 taken 276 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 276 times.
✗ Branch 4 not taken.
 279 assert(model.check(MimicChecker()) && "Function does not support mimic joints");
495
496
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 276 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.
 279 PINOCCHIO_CHECK_ARGUMENT_SIZE(
497 q.size(), model.nq, "The joint configuration vector is not of right size");
498
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 276 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.
 279 PINOCCHIO_CHECK_ARGUMENT_SIZE(
499 v.size(), model.nv, "The joint velocity vector is not of right size");
500
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 276 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.
 279 PINOCCHIO_CHECK_ARGUMENT_SIZE(
501 tau.size(), model.nv, "The joint torque vector is not of right size");
502 ;
503
504 typedef typename ModelTpl<Scalar, Options, JointCollectionTpl>::JointIndex JointIndex;
505
506 279 data.v[0].setZero();
507
1/2
✓ Branch 3 taken 276 times.
✗ Branch 4 not taken.
 279 data.a_gf[0] = -model.gravity;
508 279 data.f[0].setZero();
509 279 data.u = tau;
510
511 typedef AbaLocalConventionForwardStep1<
512 Scalar, Options, JointCollectionTpl, ConfigVectorType, TangentVectorType1>
513 Pass1;
514
2/2
✓ Branch 0 taken 3216 times.
✓ Branch 1 taken 276 times.
 3576 for (JointIndex i = 1; i < (JointIndex)model.njoints; ++i)
515 {
516
1/2
✓ Branch 1 taken 3216 times.
✗ Branch 2 not taken.
 3297 Pass1::run(
517 3297 model.joints[i], data.joints[i],
518 6594 typename Pass1::ArgsType(model, data, q.derived(), v.derived()));
519 }
520
521 typedef AbaLocalConventionBackwardStep<Scalar, Options, JointCollectionTpl> Pass2;
522
2/2
✓ Branch 0 taken 3216 times.
✓ Branch 1 taken 276 times.
 3576 for (JointIndex i = (JointIndex)model.njoints - 1; i > 0; --i)
523 {
524
1/2
✓ Branch 4 taken 3216 times.
✗ Branch 5 not taken.
 3297 Pass2::run(model.joints[i], data.joints[i], typename Pass2::ArgsType(model, data));
525 }
526
527 typedef AbaLocalConventionForwardStep2<Scalar, Options, JointCollectionTpl> Pass3;
528
2/2
✓ Branch 0 taken 3216 times.
✓ Branch 1 taken 276 times.
 3576 for (JointIndex i = 1; i < (JointIndex)model.njoints; ++i)
529 {
530
1/2
✓ Branch 4 taken 3216 times.
✗ Branch 5 not taken.
 3297 Pass3::run(model.joints[i], data.joints[i], typename Pass3::ArgsType(model, data));
531 }
532
533
2/2
✓ Branch 0 taken 3216 times.
✓ Branch 1 taken 276 times.
 3576 for (JointIndex i = (JointIndex)model.njoints - 1; i > 0; --i)
534 {
535 3297 const JointIndex parent = model.parents[i];
536
1/2
✓ Branch 5 taken 3216 times.
✗ Branch 6 not taken.
 3297 data.f[parent] += data.liMi[i].act(data.f[i]);
537 }
538
539 279 return data.ddq;
540 }
541
542 template<
543 typename Scalar,
544 int Options,
545 template<typename, int> class JointCollectionTpl,
546 typename ConfigVectorType,
547 typename TangentVectorType1,
548 typename TangentVectorType2,
549 typename ForceDerived>
550 const typename DataTpl<Scalar, Options, JointCollectionTpl>::TangentVectorType &
551 104 abaLocalConvention(
552 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
553 DataTpl<Scalar, Options, JointCollectionTpl> & data,
554 const Eigen::MatrixBase<ConfigVectorType> & q,
555 const Eigen::MatrixBase<TangentVectorType1> & v,
556 const Eigen::MatrixBase<TangentVectorType2> & tau,
557 const container::aligned_vector<ForceDerived> & fext)
558
559 {
560
1/2
✓ Branch 1 taken 104 times.
✗ Branch 2 not taken.
 104 assert(model.check(data) && "data is not consistent with model.");
561
2/4
✓ Branch 1 taken 104 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 104 times.
✗ Branch 4 not taken.
 104 assert(model.check(MimicChecker()) && "Function does not support mimic joints");
562
563
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 104 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.
 104 PINOCCHIO_CHECK_ARGUMENT_SIZE(
564 q.size(), model.nq, "The joint configuration vector is not of right size");
565
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 104 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.
 104 PINOCCHIO_CHECK_ARGUMENT_SIZE(
566 v.size(), model.nv, "The joint velocity vector is not of right size");
567
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 104 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.
 104 PINOCCHIO_CHECK_ARGUMENT_SIZE(
568 tau.size(), model.nv, "The joint torque vector is not of right size");
569
570 typedef typename ModelTpl<Scalar, Options, JointCollectionTpl>::JointIndex JointIndex;
571
572 104 data.v[0].setZero();
573
1/2
✓ Branch 3 taken 104 times.
✗ Branch 4 not taken.
 104 data.a_gf[0] = -model.gravity;
574 104 data.u = tau;
575
576 typedef AbaLocalConventionForwardStep1<
577 Scalar, Options, JointCollectionTpl, ConfigVectorType, TangentVectorType1>
578 Pass1;
579
2/2
✓ Branch 0 taken 2808 times.
✓ Branch 1 taken 104 times.
 2912 for (JointIndex i = 1; i < (JointIndex)model.njoints; ++i)
580 {
581
1/2
✓ Branch 1 taken 2808 times.
✗ Branch 2 not taken.
 2808 Pass1::run(
582 2808 model.joints[i], data.joints[i],
583 2808 typename Pass1::ArgsType(model, data, q.derived(), v.derived()));
584 2808 data.f[i] -= fext[i];
585 }
586
587 typedef AbaLocalConventionBackwardStep<Scalar, Options, JointCollectionTpl> Pass2;
588
2/2
✓ Branch 0 taken 2808 times.
✓ Branch 1 taken 104 times.
 2912 for (JointIndex i = (JointIndex)model.njoints - 1; i > 0; --i)
589 {
590
1/2
✓ Branch 4 taken 2808 times.
✗ Branch 5 not taken.
 2808 Pass2::run(model.joints[i], data.joints[i], typename Pass2::ArgsType(model, data));
591 }
592
593 typedef AbaLocalConventionForwardStep2<Scalar, Options, JointCollectionTpl> Pass3;
594
2/2
✓ Branch 0 taken 2808 times.
✓ Branch 1 taken 104 times.
 2912 for (JointIndex i = 1; i < (JointIndex)model.njoints; ++i)
595 {
596
1/2
✓ Branch 4 taken 2808 times.
✗ Branch 5 not taken.
 2808 Pass3::run(model.joints[i], data.joints[i], typename Pass3::ArgsType(model, data));
597 }
598
599
2/2
✓ Branch 0 taken 2808 times.
✓ Branch 1 taken 104 times.
 2912 for (JointIndex i = (JointIndex)model.njoints - 1; i > 0; --i)
600 {
601 2808 const JointIndex parent = model.parents[i];
602
1/2
✓ Branch 5 taken 2808 times.
✗ Branch 6 not taken.
 2808 data.f[parent] += data.liMi[i].act(data.f[i]);
603 }
604
605 104 return data.ddq;
606 }
607
608 template<
609 typename Scalar,
610 int Options,
611 template<typename, int> class JointCollectionTpl,
612 typename ConfigVectorType>
613 struct ComputeMinverseForwardStep1
614 : public fusion::JointUnaryVisitorBase<
615 ComputeMinverseForwardStep1<Scalar, Options, JointCollectionTpl, ConfigVectorType>>
616 {
617 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
618 typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
619
620 typedef boost::fusion::vector<const Model &, Data &, const ConfigVectorType &> ArgsType;
621
622 template<typename JointModel>
623 1458 static void algo(
624 const pinocchio::JointModelBase<JointModel> & jmodel,
625 pinocchio::JointDataBase<typename JointModel::JointDataDerived> & jdata,
626 const Model & model,
627 Data & data,
628 const Eigen::MatrixBase<ConfigVectorType> & q)
629 {
630 typedef typename Model::JointIndex JointIndex;
631
632
1/2
✓ Branch 1 taken 729 times.
✗ Branch 2 not taken.
 1458 const JointIndex & i = jmodel.id();
633
1/2
✓ Branch 3 taken 729 times.
✗ Branch 4 not taken.
 1458 jmodel.calc(jdata.derived(), q.derived());
634
635 1458 const JointIndex & parent = model.parents[i];
636
6/10
✓ Branch 1 taken 729 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 702 times.
✓ Branch 5 taken 27 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 702 times.
✓ Branch 9 taken 27 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 702 times.
✗ Branch 13 not taken.
 1458 data.liMi[i] = model.jointPlacements[i] * jdata.M();
637
638
2/2
✓ Branch 0 taken 702 times.
✓ Branch 1 taken 27 times.
 1458 if (parent > 0)
639
2/4
✓ Branch 3 taken 702 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 702 times.
✗ Branch 8 not taken.
 1404 data.oMi[i] = data.oMi[parent] * data.liMi[i];
640 else
641
1/2
✓ Branch 3 taken 27 times.
✗ Branch 4 not taken.
 54 data.oMi[i] = data.liMi[i];
642
643 typedef
644 typename SizeDepType<JointModel::NV>::template ColsReturn<typename Data::Matrix6x>::Type
645 ColsBlock;
646
1/2
✓ Branch 1 taken 729 times.
✗ Branch 2 not taken.
 1458 ColsBlock J_cols = jmodel.jointCols(data.J);
647
3/6
✓ Branch 2 taken 729 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 729 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 729 times.
✗ Branch 9 not taken.
 1458 J_cols = data.oMi[i].act(jdata.S());
648
649
2/4
✓ Branch 3 taken 729 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 729 times.
✗ Branch 8 not taken.
 1458 data.oYcrb[i] = data.oMi[i].act(model.inertias[i]);
650
1/2
✓ Branch 2 taken 729 times.
✗ Branch 3 not taken.
 1458 data.oYaba[i] = data.oYcrb[i].matrix();
651 }
652 };
653
654 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
655 struct ComputeMinverseBackwardStep
656 : public fusion::JointUnaryVisitorBase<
657 ComputeMinverseBackwardStep<Scalar, Options, JointCollectionTpl>>
658 {
659 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
660 typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
661
662 typedef boost::fusion::vector<const Model &, Data &> ArgsType;
663
664 template<typename JointModel>
665 1458 static void algo(
666 const JointModelBase<JointModel> & jmodel,
667 JointDataBase<typename JointModel::JointDataDerived> & jdata,
668 const Model & model,
669 Data & data)
670 {
671 typedef typename Model::JointIndex JointIndex;
672 typedef typename Data::Inertia Inertia;
673
674
1/2
✓ Branch 1 taken 729 times.
✗ Branch 2 not taken.
 1458 const JointIndex i = jmodel.id();
675 1458 const JointIndex parent = model.parents[i];
676
677 1458 typename Inertia::Matrix6 & Ia = data.oYaba[i];
678 1458 typename Data::RowMatrixXs & Minv = data.Minv;
679 1458 typename Data::Matrix6x & Fcrb = data.Fcrb[0];
680 typedef
681 typename SizeDepType<JointModel::NV>::template ColsReturn<typename Data::Matrix6x>::Type
682 ColsBlock;
683
684
1/2
✓ Branch 1 taken 729 times.
✗ Branch 2 not taken.
 1458 ColsBlock J_cols = jmodel.jointCols(data.J);
685
686
4/8
✓ Branch 1 taken 729 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 729 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 729 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 729 times.
✗ Branch 11 not taken.
 1458 jdata.U().noalias() = Ia * J_cols;
687
6/12
✓ Branch 1 taken 729 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 729 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 729 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 729 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 729 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 729 times.
✗ Branch 17 not taken.
 1458 jdata.StU().noalias() = J_cols.transpose() * jdata.U();
688
689 // Account for the rotor inertia contribution
690
4/8
✓ Branch 1 taken 729 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 729 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 729 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 729 times.
✗ Branch 11 not taken.
 1458 jdata.StU().diagonal() += jmodel.jointVelocitySelector(model.armature);
691
692
3/6
✓ Branch 1 taken 729 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 729 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 729 times.
✗ Branch 8 not taken.
 1458 ::pinocchio::internal::PerformStYSInversion<Scalar>::run(jdata.StU(), jdata.Dinv());
693
6/12
✓ Branch 1 taken 729 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 729 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 729 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 729 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 729 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 729 times.
✗ Branch 17 not taken.
 1458 jdata.UDinv().noalias() = jdata.U() * jdata.Dinv();
694
695
7/14
✓ Branch 1 taken 729 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 729 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 729 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 729 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 729 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 729 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 729 times.
✗ Branch 20 not taken.
 1458 Minv.block(jmodel.idx_v(), jmodel.idx_v(), jmodel.nv(), jmodel.nv()) = jdata.Dinv();
696
1/2
✓ Branch 2 taken 729 times.
✗ Branch 3 not taken.
 1458 const int nv_children = data.nvSubtree[i] - jmodel.nv();
697
2/2
✓ Branch 0 taken 621 times.
✓ Branch 1 taken 108 times.
 1458 if (nv_children > 0)
698 {
699
1/2
✓ Branch 1 taken 621 times.
✗ Branch 2 not taken.
 1242 ColsBlock SDinv_cols = jmodel.jointCols(data.SDinv);
700
4/8
✓ Branch 1 taken 621 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 621 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 621 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 621 times.
✗ Branch 11 not taken.
 1242 SDinv_cols.noalias() = J_cols * jdata.Dinv();
701
702
5/10
✓ Branch 1 taken 621 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 621 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 621 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 621 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 621 times.
✗ Branch 14 not taken.
 1242 Minv.block(jmodel.idx_v(), jmodel.idx_v() + jmodel.nv(), jmodel.nv(), nv_children)
703
4/8
✓ Branch 1 taken 621 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 621 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 621 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 621 times.
✗ Branch 11 not taken.
 2484 .noalias() =
704
4/8
✓ Branch 1 taken 621 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 621 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 621 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 621 times.
✗ Branch 11 not taken.
 1242 -SDinv_cols.transpose() * Fcrb.middleCols(jmodel.idx_v() + jmodel.nv(), nv_children);
705
706
2/2
✓ Branch 0 taken 594 times.
✓ Branch 1 taken 27 times.
 1242 if (parent > 0)
707 {
708
4/8
✓ Branch 2 taken 594 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 594 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 594 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 594 times.
✗ Branch 12 not taken.
 1188 Fcrb.middleCols(jmodel.idx_v(), data.nvSubtree[i]).noalias() +=
709
1/2
✓ Branch 1 taken 594 times.
✗ Branch 2 not taken.
 1188 jdata.U()
710
5/12
✓ Branch 2 taken 594 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 594 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 594 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 594 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 594 times.
✗ Branch 15 not taken.
✗ Branch 17 not taken.
✗ Branch 18 not taken.
 3564 * Minv.block(jmodel.idx_v(), jmodel.idx_v(), jmodel.nv(), data.nvSubtree[i]);
711 ;
712 }
713 }
714 else
715 {
716
4/8
✓ Branch 2 taken 108 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 108 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 108 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 108 times.
✗ Branch 12 not taken.
 216 Fcrb.middleCols(jmodel.idx_v(), data.nvSubtree[i]).noalias() =
717
6/12
✓ Branch 2 taken 108 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 108 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 108 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 108 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 108 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 108 times.
✗ Branch 18 not taken.
 432 jdata.U() * Minv.block(jmodel.idx_v(), jmodel.idx_v(), jmodel.nv(), data.nvSubtree[i]);
718 }
719
720
2/2
✓ Branch 0 taken 702 times.
✓ Branch 1 taken 27 times.
 1458 if (parent > 0)
721 {
722
6/12
✓ Branch 1 taken 702 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 702 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 702 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 702 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 702 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 702 times.
✗ Branch 17 not taken.
 1404 Ia.noalias() -= jdata.UDinv() * jdata.U().transpose();
723
1/2
✓ Branch 2 taken 702 times.
✗ Branch 3 not taken.
 1404 data.oYaba[parent] += Ia;
724 }
725 }
726 };
727
728 namespace optimized
729 {
730 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
731 struct ComputeMinverseBackwardStep
732 : public fusion::JointUnaryVisitorBase<
733 ComputeMinverseBackwardStep<Scalar, Options, JointCollectionTpl>>
734 {
735 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
736 typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
737
738 typedef boost::fusion::vector<const Model &, Data &> ArgsType;
739
740 template<typename JointModel>
741 54 static void algo(
742 const JointModelBase<JointModel> & jmodel,
743 JointDataBase<typename JointModel::JointDataDerived> & jdata,
744 const Model & model,
745 Data & data)
746 {
747 typedef typename Model::JointIndex JointIndex;
748
749
1/2
✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
 54 const JointIndex i = jmodel.id();
750 54 const JointIndex parent = model.parents[i];
751
752 54 typename Data::RowMatrixXs & Minv = data.Minv;
753 54 typename Data::Matrix6x & Fcrb = data.Fcrb[0];
754 typedef
755 typename SizeDepType<JointModel::NV>::template ColsReturn<typename Data::Matrix6x>::Type
756 ColsBlock;
757
758
1/2
✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
 54 const ColsBlock J_cols = jmodel.jointCols(data.J);
759
760
7/14
✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 27 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 27 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 27 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 27 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 27 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 27 times.
✗ Branch 20 not taken.
 54 Minv.block(jmodel.idx_v(), jmodel.idx_v(), jmodel.nv(), jmodel.nv()) = jdata.Dinv();
761
1/2
✓ Branch 2 taken 27 times.
✗ Branch 3 not taken.
 54 const int nv_children = data.nvSubtree[i] - jmodel.nv();
762
2/2
✓ Branch 0 taken 23 times.
✓ Branch 1 taken 4 times.
 54 if (nv_children > 0)
763 {
764
1/2
✓ Branch 1 taken 23 times.
✗ Branch 2 not taken.
 46 ColsBlock SDinv_cols = jmodel.jointCols(data.SDinv);
765
4/8
✓ Branch 1 taken 23 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 23 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 23 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 23 times.
✗ Branch 11 not taken.
 46 SDinv_cols.noalias() = J_cols * jdata.Dinv();
766
5/10
✓ Branch 1 taken 23 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 23 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 23 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 23 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 23 times.
✗ Branch 14 not taken.
 46 Minv.block(jmodel.idx_v(), jmodel.idx_v() + jmodel.nv(), jmodel.nv(), nv_children)
767
4/8
✓ Branch 1 taken 23 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 23 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 23 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 23 times.
✗ Branch 11 not taken.
 92 .noalias() =
768
4/8
✓ Branch 1 taken 23 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 23 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 23 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 23 times.
✗ Branch 11 not taken.
 46 -SDinv_cols.transpose() * Fcrb.middleCols(jmodel.idx_v() + jmodel.nv(), nv_children);
769
770
2/2
✓ Branch 0 taken 22 times.
✓ Branch 1 taken 1 times.
 46 if (parent > 0)
771 {
772
4/8
✓ Branch 2 taken 22 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 22 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 22 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 22 times.
✗ Branch 12 not taken.
 44 Fcrb.middleCols(jmodel.idx_v(), data.nvSubtree[i]).noalias() +=
773
1/2
✓ Branch 1 taken 22 times.
✗ Branch 2 not taken.
 44 jdata.U()
774
5/12
✓ Branch 2 taken 22 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 22 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 22 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 22 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 22 times.
✗ Branch 15 not taken.
✗ Branch 17 not taken.
✗ Branch 18 not taken.
 132 * Minv.block(jmodel.idx_v(), jmodel.idx_v(), jmodel.nv(), data.nvSubtree[i]);
775 ;
776 }
777 }
778 else
779 {
780
4/8
✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 4 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 4 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 4 times.
✗ Branch 12 not taken.
 8 Fcrb.middleCols(jmodel.idx_v(), data.nvSubtree[i]).noalias() =
781
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 8 jdata.U()
782
5/12
✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 4 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 4 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 4 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 4 times.
✗ Branch 15 not taken.
✗ Branch 17 not taken.
✗ Branch 18 not taken.
 24 * Minv.block(jmodel.idx_v(), jmodel.idx_v(), jmodel.nv(), data.nvSubtree[i]);
783 }
784 }
785 };
786 } // namespace optimized
787
788 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
789 struct ComputeMinverseForwardStep2
790 : public fusion::JointUnaryVisitorBase<
791 ComputeMinverseForwardStep2<Scalar, Options, JointCollectionTpl>>
792 {
793 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
794 typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
795
796 typedef boost::fusion::vector<const Model &, Data &> ArgsType;
797
798 template<typename JointModel>
799 1512 static void algo(
800 const pinocchio::JointModelBase<JointModel> & jmodel,
801 pinocchio::JointDataBase<typename JointModel::JointDataDerived> & jdata,
802 const Model & model,
803 Data & data)
804 {
805 typedef typename Model::JointIndex JointIndex;
806
807
1/2
✓ Branch 1 taken 756 times.
✗ Branch 2 not taken.
 1512 const JointIndex i = jmodel.id();
808 1512 const JointIndex parent = model.parents[i];
809 1512 typename Data::RowMatrixXs & Minv = data.Minv;
810
811 typedef
812 typename SizeDepType<JointModel::NV>::template ColsReturn<typename Data::Matrix6x>::Type
813 ColsBlock;
814
1/2
✓ Branch 1 taken 756 times.
✗ Branch 2 not taken.
 1512 ColsBlock J_cols = jmodel.jointCols(data.J);
815
816
2/2
✓ Branch 0 taken 728 times.
✓ Branch 1 taken 28 times.
 1512 if (parent > 0)
817 {
818
3/6
✓ Branch 1 taken 728 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 728 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 728 times.
✗ Branch 8 not taken.
 1456 Minv.middleRows(jmodel.idx_v(), jmodel.nv())
819
2/4
✓ Branch 1 taken 728 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 728 times.
✗ Branch 5 not taken.
 1456 .rightCols(model.nv - jmodel.idx_v())
820
2/4
✓ Branch 1 taken 728 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 728 times.
✗ Branch 5 not taken.
 1456 .noalias() -=
821
5/10
✓ Branch 2 taken 728 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 728 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 728 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 728 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 728 times.
✗ Branch 15 not taken.
 2912 jdata.UDinv().transpose() * data.Fcrb[parent].rightCols(model.nv - jmodel.idx_v());
822 }
823
824
4/8
✓ Branch 2 taken 756 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 756 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 756 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 756 times.
✗ Branch 12 not taken.
 1512 data.Fcrb[i].rightCols(model.nv - jmodel.idx_v()).noalias() =
825 J_cols
826
6/12
✓ Branch 1 taken 756 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 756 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 756 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 756 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 756 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 756 times.
✗ Branch 17 not taken.
 1512 * Minv.middleRows(jmodel.idx_v(), jmodel.nv()).rightCols(model.nv - jmodel.idx_v());
827
2/2
✓ Branch 0 taken 728 times.
✓ Branch 1 taken 28 times.
 1512 if (parent > 0)
828
3/6
✓ Branch 2 taken 728 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 728 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 728 times.
✗ Branch 9 not taken.
 1456 data.Fcrb[i].rightCols(model.nv - jmodel.idx_v()) +=
829
2/4
✓ Branch 2 taken 728 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 728 times.
✗ Branch 6 not taken.
 2912 data.Fcrb[parent].rightCols(model.nv - jmodel.idx_v());
830 }
831 };
832
833 template<
834 typename Scalar,
835 int Options,
836 template<typename, int> class JointCollectionTpl,
837 typename ConfigVectorType>
838 27 const typename DataTpl<Scalar, Options, JointCollectionTpl>::RowMatrixXs & computeMinverse(
839 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
840 DataTpl<Scalar, Options, JointCollectionTpl> & data,
841 const Eigen::MatrixBase<ConfigVectorType> & q)
842 {
843
1/2
✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
 27 assert(model.check(data) && "data is not consistent with model.");
844
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");
845
846
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(
847 q.size(), model.nq, "The joint configuration vector is not of right size");
848
849 typedef typename ModelTpl<Scalar, Options, JointCollectionTpl>::JointIndex JointIndex;
850
1/2
✓ Branch 2 taken 27 times.
✗ Branch 3 not taken.
 27 data.Minv.template triangularView<Eigen::Upper>().setZero();
851
852 typedef ComputeMinverseForwardStep1<Scalar, Options, JointCollectionTpl, ConfigVectorType>
853 Pass1;
854
2/2
✓ Branch 0 taken 729 times.
✓ Branch 1 taken 27 times.
 756 for (JointIndex i = 1; i < (JointIndex)model.njoints; ++i)
855 {
856
1/2
✓ Branch 1 taken 729 times.
✗ Branch 2 not taken.
 729 Pass1::run(
857 1458 model.joints[i], data.joints[i], typename Pass1::ArgsType(model, data, q.derived()));
858 }
859
860 27 data.Fcrb[0].setZero();
861 typedef ComputeMinverseBackwardStep<Scalar, Options, JointCollectionTpl> Pass2;
862
2/2
✓ Branch 0 taken 729 times.
✓ Branch 1 taken 27 times.
 756 for (JointIndex i = (JointIndex)model.njoints - 1; i > 0; --i)
863 {
864
1/2
✓ Branch 4 taken 729 times.
✗ Branch 5 not taken.
 729 Pass2::run(model.joints[i], data.joints[i], typename Pass2::ArgsType(model, data));
865 }
866
867 typedef ComputeMinverseForwardStep2<Scalar, Options, JointCollectionTpl> Pass3;
868
2/2
✓ Branch 0 taken 729 times.
✓ Branch 1 taken 27 times.
 756 for (JointIndex i = 1; i < (JointIndex)model.njoints; ++i)
869 {
870
1/2
✓ Branch 4 taken 729 times.
✗ Branch 5 not taken.
 729 Pass3::run(model.joints[i], data.joints[i], typename Pass3::ArgsType(model, data));
871 }
872
873 27 return data.Minv;
874 }
875 } // namespace impl
876
877 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
878 1 const typename DataTpl<Scalar, Options, JointCollectionTpl>::RowMatrixXs & computeMinverse(
879 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
880 DataTpl<Scalar, Options, JointCollectionTpl> & data)
881 {
882
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 assert(model.check(data) && "data is not consistent with model.");
883
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 1 assert(model.check(MimicChecker()) && "Function does not support mimic joints");
884
885 typedef typename ModelTpl<Scalar, Options, JointCollectionTpl>::JointIndex JointIndex;
886
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 data.Minv.template triangularView<Eigen::Upper>().setZero();
887
888 1 data.Fcrb[0].setZero();
889 typedef impl::optimized::ComputeMinverseBackwardStep<Scalar, Options, JointCollectionTpl> Pass2;
890
2/2
✓ Branch 0 taken 27 times.
✓ Branch 1 taken 1 times.
 28 for (JointIndex i = (JointIndex)model.njoints - 1; i > 0; --i)
891 {
892
1/2
✓ Branch 4 taken 27 times.
✗ Branch 5 not taken.
 27 Pass2::run(model.joints[i], data.joints[i], typename Pass2::ArgsType(model, data));
893 }
894
895 typedef impl::ComputeMinverseForwardStep2<Scalar, Options, JointCollectionTpl> Pass3;
896
2/2
✓ Branch 0 taken 27 times.
✓ Branch 1 taken 1 times.
 28 for (JointIndex i = 1; i < (JointIndex)model.njoints; ++i)
897 {
898
1/2
✓ Branch 4 taken 27 times.
✗ Branch 5 not taken.
 27 Pass3::run(model.joints[i], data.joints[i], typename Pass3::ArgsType(model, data));
899 }
900
901 1 return data.Minv;
902 }
903
904 // --- CHECKER ---------------------------------------------------------------
905 // --- CHECKER ---------------------------------------------------------------
906 // --- CHECKER ---------------------------------------------------------------
907
908 // Check whether all masses are nonzero and diagonal of inertia is nonzero
909 // The second test is overconstraining.
910 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
911 inline bool
912 3 ABAChecker::checkModel_impl(const ModelTpl<Scalar, Options, JointCollectionTpl> & model) const
913 {
914 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
915 typedef typename Model::JointIndex JointIndex;
916
917
2/2
✓ Branch 0 taken 55 times.
✓ Branch 1 taken 2 times.
 57 for (JointIndex j = 1; j < (JointIndex)model.njoints; j++)
918 55 if (
919
2/2
✓ Branch 6 taken 54 times.
✓ Branch 7 taken 1 times.
 110 (model.inertias[j].mass() < 1e-5) || (model.inertias[j].inertia().data()[0] < 1e-5)
920
1/2
✓ Branch 4 taken 54 times.
✗ Branch 5 not taken.
 54 || (model.inertias[j].inertia().data()[2] < 1e-5)
921
4/6
✓ Branch 0 taken 55 times.
✗ Branch 1 not taken.
✗ Branch 6 not taken.
✓ Branch 7 taken 54 times.
✓ Branch 8 taken 1 times.
✓ Branch 9 taken 54 times.
 110 || (model.inertias[j].inertia().data()[5] < 1e-5))
922 1 return false;
923 2 return true;
924 }
925
926 template<
927 typename Scalar,
928 int Options,
929 template<typename, int> class JointCollectionTpl,
930 typename ConfigVectorType,
931 typename TangentVectorType1,
932 typename TangentVectorType2>
933 330 const typename DataTpl<Scalar, Options, JointCollectionTpl>::TangentVectorType & aba(
934 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
935 DataTpl<Scalar, Options, JointCollectionTpl> & data,
936 const Eigen::MatrixBase<ConfigVectorType> & q,
937 const Eigen::MatrixBase<TangentVectorType1> & v,
938 const Eigen::MatrixBase<TangentVectorType2> & tau,
939 const Convention convention)
940 {
941
2/3
✓ Branch 0 taken 276 times.
✓ Branch 1 taken 35 times.
✗ Branch 2 not taken.
 330 switch (convention)
942 {
943 279 case Convention::LOCAL:
944 279 return impl::abaLocalConvention(model, data, q, v, tau);
945 51 case Convention::WORLD:
946 51 return impl::abaWorldConvention(model, data, q, v, tau);
947 }
948 }
949
950 template<
951 typename Scalar,
952 int Options,
953 template<typename, int> class JointCollectionTpl,
954 typename ConfigVectorType,
955 typename TangentVectorType1,
956 typename TangentVectorType2,
957 typename ForceDerived>
958 107 const typename DataTpl<Scalar, Options, JointCollectionTpl>::TangentVectorType & aba(
959 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
960 DataTpl<Scalar, Options, JointCollectionTpl> & data,
961 const Eigen::MatrixBase<ConfigVectorType> & q,
962 const Eigen::MatrixBase<TangentVectorType1> & v,
963 const Eigen::MatrixBase<TangentVectorType2> & tau,
964 const container::aligned_vector<ForceDerived> & fext,
965 const Convention convention)
966 {
967
2/3
✓ Branch 0 taken 104 times.
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 107 switch (convention)
968 {
969 104 case Convention::LOCAL:
970 104 return impl::abaLocalConvention(model, data, q, v, tau, fext);
971 3 case Convention::WORLD:
972 3 return impl::abaWorldConvention(model, data, q, v, tau, fext);
973 }
974 }
975
976 template<
977 typename Scalar,
978 int Options,
979 template<typename, int> class JointCollectionTpl,
980 typename ConfigVectorType>
981 27 const typename DataTpl<Scalar, Options, JointCollectionTpl>::RowMatrixXs & computeMinverse(
982 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
983 DataTpl<Scalar, Options, JointCollectionTpl> & data,
984 const Eigen::MatrixBase<ConfigVectorType> & q)
985 {
986
1/2
✓ Branch 2 taken 27 times.
✗ Branch 3 not taken.
 27 return impl::computeMinverse(model, data, make_const_ref(q));
987 }
988
989 } // namespace pinocchio
990
991 /// @endcond
992
993 #endif // ifndef __pinocchio_algorithm_aba_hxx__
994