GCC Code Coverage Report

Directory: ./
File: include/pinocchio/algorithm/aba.hxx
Date: 2025-02-12 21:03:38
Exec Total Coverage
Lines: 315 315 100.0%
Branches: 500 1090 45.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/5
✓ Branch 4 taken 427 times.
✓ Branch 5 taken 28 times.
✗ Branch 6 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/2
✓ Branch 4 taken 455 times.
✗ Branch 5 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/2
✓ Branch 5 taken 455 times.
✗ Branch 6 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
1/4
✗ Branch 1 not taken.
✓ Branch 2 taken 35 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
 51 PINOCCHIO_CHECK_INPUT_ARGUMENT(
251 q.size() == model.nq, "The joint configuration vector is not of right size");
252
1/4
✗ Branch 1 not taken.
✓ Branch 2 taken 35 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
 51 PINOCCHIO_CHECK_INPUT_ARGUMENT(
253 v.size() == model.nv, "The joint velocity vector is not of right size");
254
1/4
✗ Branch 1 not taken.
✓ Branch 2 taken 35 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
 51 PINOCCHIO_CHECK_INPUT_ARGUMENT(
255 tau.size() == model.nv, "The joint acceleration vector is not of right size");
256
257 typedef typename ModelTpl<Scalar, Options, JointCollectionTpl>::JointIndex JointIndex;
258
259
1/2
✓ Branch 3 taken 35 times.
✗ Branch 4 not taken.
 51 data.oa_gf[0] = -model.gravity;
260 51 data.of[0].setZero();
261 51 data.u = tau;
262
263 typedef AbaWorldConventionForwardStep1<
264 Scalar, Options, JointCollectionTpl, ConfigVectorType, TangentVectorType1>
265 Pass1;
266
2/2
✓ Branch 0 taken 374 times.
✓ Branch 1 taken 35 times.
 545 for (JointIndex i = 1; i < (JointIndex)model.njoints; ++i)
267 {
268
1/2
✓ Branch 1 taken 374 times.
✗ Branch 2 not taken.
 494 Pass1::run(
269 494 model.joints[i], data.joints[i],
270 988 typename Pass1::ArgsType(model, data, q.derived(), v.derived()));
271 }
272
273 typedef AbaWorldConventionBackwardStep<Scalar, Options, JointCollectionTpl> Pass2;
274
2/2
✓ Branch 0 taken 374 times.
✓ Branch 1 taken 35 times.
 545 for (JointIndex i = (JointIndex)model.njoints - 1; i > 0; --i)
275 {
276
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));
277 }
278
279 typedef AbaWorldConventionForwardStep2<Scalar, Options, JointCollectionTpl> Pass3;
280
2/2
✓ Branch 0 taken 374 times.
✓ Branch 1 taken 35 times.
 545 for (JointIndex i = 1; i < (JointIndex)model.njoints; ++i)
281 {
282
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));
283 }
284
285
2/2
✓ Branch 0 taken 374 times.
✓ Branch 1 taken 35 times.
 545 for (JointIndex i = (JointIndex)model.njoints - 1; i > 0; --i)
286 {
287 494 const JointIndex parent = model.parents[i];
288 494 data.of[parent] += data.of[i];
289 }
290
291 51 return data.ddq;
292 }
293
294 template<
295 typename Scalar,
296 int Options,
297 template<typename, int> class JointCollectionTpl,
298 typename ConfigVectorType,
299 typename TangentVectorType1,
300 typename TangentVectorType2,
301 typename ForceDerived>
302 const typename DataTpl<Scalar, Options, JointCollectionTpl>::TangentVectorType &
303 3 abaWorldConvention(
304 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
305 DataTpl<Scalar, Options, JointCollectionTpl> & data,
306 const Eigen::MatrixBase<ConfigVectorType> & q,
307 const Eigen::MatrixBase<TangentVectorType1> & v,
308 const Eigen::MatrixBase<TangentVectorType2> & tau,
309 const container::aligned_vector<ForceDerived> & fext)
310
311 {
312
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 assert(model.check(data) && "data is not consistent with model.");
313
1/4
✗ Branch 1 not taken.
✓ Branch 2 taken 3 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
 3 PINOCCHIO_CHECK_INPUT_ARGUMENT(
314 q.size() == model.nq, "The joint configuration vector is not of right size");
315
1/4
✗ Branch 1 not taken.
✓ Branch 2 taken 3 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
 3 PINOCCHIO_CHECK_INPUT_ARGUMENT(
316 v.size() == model.nv, "The joint velocity vector is not of right size");
317
1/4
✗ Branch 1 not taken.
✓ Branch 2 taken 3 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
 3 PINOCCHIO_CHECK_INPUT_ARGUMENT(
318 tau.size() == model.nv, "The joint acceleration vector is not of right size");
319
320 typedef typename ModelTpl<Scalar, Options, JointCollectionTpl>::JointIndex JointIndex;
321
322
1/2
✓ Branch 3 taken 3 times.
✗ Branch 4 not taken.
 3 data.oa_gf[0] = -model.gravity;
323 3 data.u = tau;
324
325 typedef AbaWorldConventionForwardStep1<
326 Scalar, Options, JointCollectionTpl, ConfigVectorType, TangentVectorType1>
327 Pass1;
328
2/2
✓ Branch 0 taken 81 times.
✓ Branch 1 taken 3 times.
 84 for (JointIndex i = 1; i < (JointIndex)model.njoints; ++i)
329 {
330
1/2
✓ Branch 1 taken 81 times.
✗ Branch 2 not taken.
 81 Pass1::run(
331 81 model.joints[i], data.joints[i],
332 81 typename Pass1::ArgsType(model, data, q.derived(), v.derived()));
333
1/2
✓ Branch 5 taken 81 times.
✗ Branch 6 not taken.
 81 data.of[i] -= data.oMi[i].act(fext[i]);
334 }
335
336 typedef AbaWorldConventionBackwardStep<Scalar, Options, JointCollectionTpl> Pass2;
337
2/2
✓ Branch 0 taken 81 times.
✓ Branch 1 taken 3 times.
 84 for (JointIndex i = (JointIndex)model.njoints - 1; i > 0; --i)
338 {
339
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));
340 }
341
342 typedef AbaWorldConventionForwardStep2<Scalar, Options, JointCollectionTpl> Pass3;
343
2/2
✓ Branch 0 taken 81 times.
✓ Branch 1 taken 3 times.
 84 for (JointIndex i = 1; i < (JointIndex)model.njoints; ++i)
344 {
345
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));
346 }
347
348 3 return data.ddq;
349 }
350
351 template<
352 typename Scalar,
353 int Options,
354 template<typename, int> class JointCollectionTpl,
355 typename ConfigVectorType,
356 typename TangentVectorType>
357 struct AbaLocalConventionForwardStep1
358 : public fusion::JointUnaryVisitorBase<AbaLocalConventionForwardStep1<
359 Scalar,
360 Options,
361 JointCollectionTpl,
362 ConfigVectorType,
363 TangentVectorType>>
364 {
365 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
366 typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
367
368 typedef boost::fusion::
369 vector<const Model &, Data &, const ConfigVectorType &, const TangentVectorType &>
370 ArgsType;
371
372 template<typename JointModel>
373 12048 static void algo(
374 const pinocchio::JointModelBase<JointModel> & jmodel,
375 pinocchio::JointDataBase<typename JointModel::JointDataDerived> & jdata,
376 const Model & model,
377 Data & data,
378 const Eigen::MatrixBase<ConfigVectorType> & q,
379 const Eigen::MatrixBase<TangentVectorType> & v)
380 {
381 typedef typename Model::JointIndex JointIndex;
382
383 12048 const JointIndex i = jmodel.id();
384 12048 jmodel.calc(jdata.derived(), q.derived(), v.derived());
385
386 12048 const JointIndex & parent = model.parents[i];
387
3/5
✓ Branch 4 taken 5647 times.
✓ Branch 5 taken 377 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 5647 times.
✗ Branch 9 not taken.
 12048 data.liMi[i] = model.jointPlacements[i] * jdata.M();
388
389 12048 data.v[i] = jdata.v();
390
2/2
✓ Branch 0 taken 5644 times.
✓ Branch 1 taken 380 times.
 12048 if (parent > 0)
391
1/2
✓ Branch 5 taken 5644 times.
✗ Branch 6 not taken.
 11288 data.v[i] += data.liMi[i].actInv(data.v[parent]);
392
393
2/8
✓ Branch 4 taken 6024 times.
✗ Branch 5 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.
 12048 data.a_gf[i] = jdata.c() + (data.v[i] ^ jdata.v());
394
395 12048 data.Yaba[i] = model.inertias[i].matrix();
396
1/2
✓ Branch 5 taken 6024 times.
✗ Branch 6 not taken.
 12048 data.h[i] = model.inertias[i] * data.v[i];
397
1/2
✓ Branch 5 taken 6024 times.
✗ Branch 6 not taken.
 12048 data.f[i] = data.v[i].cross(data.h[i]); // -f_ext
398 }
399 };
400
401 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
402 struct AbaLocalConventionBackwardStep
403 : public fusion::JointUnaryVisitorBase<
404 AbaLocalConventionBackwardStep<Scalar, Options, JointCollectionTpl>>
405 {
406 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
407 typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
408
409 typedef boost::fusion::vector<const Model &, Data &> ArgsType;
410
411 template<typename JointModel>
412 12048 static void algo(
413 const JointModelBase<JointModel> & jmodel,
414 JointDataBase<typename JointModel::JointDataDerived> & jdata,
415 const Model & model,
416 Data & data)
417 {
418 typedef typename Model::JointIndex JointIndex;
419 typedef typename Data::Inertia Inertia;
420 typedef typename Data::Force Force;
421
422 12048 const JointIndex i = jmodel.id();
423 12048 const JointIndex parent = model.parents[i];
424 12048 typename Inertia::Matrix6 & Ia = data.Yaba[i];
425
426
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];
427
1/2
✓ Branch 1 taken 6024 times.
✗ Branch 2 not taken.
 12048 jmodel.calc_aba(
428 12048 jdata.derived(), jmodel.jointVelocitySelector(model.armature), Ia, parent > 0);
429
430
2/2
✓ Branch 0 taken 5644 times.
✓ Branch 1 taken 380 times.
 12048 if (parent > 0)
431 {
432 11288 Force & pa = data.f[i];
433
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() +=
434
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);
435
1/2
✓ Branch 4 taken 5644 times.
✗ Branch 5 not taken.
 11288 data.Yaba[parent] += internal::SE3actOn<Scalar>::run(data.liMi[i], Ia);
436
1/2
✓ Branch 4 taken 5644 times.
✗ Branch 5 not taken.
 11288 data.f[parent] += data.liMi[i].act(pa);
437 }
438 }
439 };
440
441 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
442 struct AbaLocalConventionForwardStep2
443 : public fusion::JointUnaryVisitorBase<
444 AbaLocalConventionForwardStep2<Scalar, Options, JointCollectionTpl>>
445 {
446 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
447 typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
448
449 typedef boost::fusion::vector<const Model &, Data &> ArgsType;
450
451 template<typename JointModel>
452 12048 static void algo(
453 const pinocchio::JointModelBase<JointModel> & jmodel,
454 pinocchio::JointDataBase<typename JointModel::JointDataDerived> & jdata,
455 const Model & model,
456 Data & data)
457 {
458 typedef typename Model::JointIndex JointIndex;
459
460 12048 const JointIndex i = jmodel.id();
461 12048 const JointIndex parent = model.parents[i];
462
463
1/2
✓ Branch 5 taken 6024 times.
✗ Branch 6 not taken.
 12048 data.a_gf[i] += data.liMi[i].actInv(data.a_gf[parent]);
464
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() =
465
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)
466
1/2
✓ Branch 5 taken 6024 times.
✗ Branch 6 not taken.
 12048 - jdata.UDinv().transpose() * data.a_gf[i].toVector();
467
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);
468
469 12048 data.a[i] = data.a_gf[i];
470
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();
471
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]);
472 }
473 };
474
475 template<
476 typename Scalar,
477 int Options,
478 template<typename, int> class JointCollectionTpl,
479 typename ConfigVectorType,
480 typename TangentVectorType1,
481 typename TangentVectorType2>
482 const typename DataTpl<Scalar, Options, JointCollectionTpl>::TangentVectorType &
483 279 abaLocalConvention(
484 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
485 DataTpl<Scalar, Options, JointCollectionTpl> & data,
486 const Eigen::MatrixBase<ConfigVectorType> & q,
487 const Eigen::MatrixBase<TangentVectorType1> & v,
488 const Eigen::MatrixBase<TangentVectorType2> & tau)
489 {
490
1/2
✓ Branch 1 taken 276 times.
✗ Branch 2 not taken.
 279 assert(model.check(data) && "data is not consistent with model.");
491
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(
492 q.size(), model.nq, "The joint configuration vector is not of right size");
493
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(
494 v.size(), model.nv, "The joint velocity vector is not of right size");
495
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(
496 tau.size(), model.nv, "The joint torque vector is not of right size");
497 ;
498
499 typedef typename ModelTpl<Scalar, Options, JointCollectionTpl>::JointIndex JointIndex;
500
501 279 data.v[0].setZero();
502
1/2
✓ Branch 3 taken 276 times.
✗ Branch 4 not taken.
 279 data.a_gf[0] = -model.gravity;
503 279 data.f[0].setZero();
504 279 data.u = tau;
505
506 typedef AbaLocalConventionForwardStep1<
507 Scalar, Options, JointCollectionTpl, ConfigVectorType, TangentVectorType1>
508 Pass1;
509
2/2
✓ Branch 0 taken 3216 times.
✓ Branch 1 taken 276 times.
 3576 for (JointIndex i = 1; i < (JointIndex)model.njoints; ++i)
510 {
511
1/2
✓ Branch 1 taken 3216 times.
✗ Branch 2 not taken.
 3297 Pass1::run(
512 3297 model.joints[i], data.joints[i],
513 6594 typename Pass1::ArgsType(model, data, q.derived(), v.derived()));
514 }
515
516 typedef AbaLocalConventionBackwardStep<Scalar, Options, JointCollectionTpl> Pass2;
517
2/2
✓ Branch 0 taken 3216 times.
✓ Branch 1 taken 276 times.
 3576 for (JointIndex i = (JointIndex)model.njoints - 1; i > 0; --i)
518 {
519
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));
520 }
521
522 typedef AbaLocalConventionForwardStep2<Scalar, Options, JointCollectionTpl> Pass3;
523
2/2
✓ Branch 0 taken 3216 times.
✓ Branch 1 taken 276 times.
 3576 for (JointIndex i = 1; i < (JointIndex)model.njoints; ++i)
524 {
525
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));
526 }
527
528
2/2
✓ Branch 0 taken 3216 times.
✓ Branch 1 taken 276 times.
 3576 for (JointIndex i = (JointIndex)model.njoints - 1; i > 0; --i)
529 {
530 3297 const JointIndex parent = model.parents[i];
531
1/2
✓ Branch 5 taken 3216 times.
✗ Branch 6 not taken.
 3297 data.f[parent] += data.liMi[i].act(data.f[i]);
532 }
533
534 279 return data.ddq;
535 }
536
537 template<
538 typename Scalar,
539 int Options,
540 template<typename, int> class JointCollectionTpl,
541 typename ConfigVectorType,
542 typename TangentVectorType1,
543 typename TangentVectorType2,
544 typename ForceDerived>
545 const typename DataTpl<Scalar, Options, JointCollectionTpl>::TangentVectorType &
546 104 abaLocalConvention(
547 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
548 DataTpl<Scalar, Options, JointCollectionTpl> & data,
549 const Eigen::MatrixBase<ConfigVectorType> & q,
550 const Eigen::MatrixBase<TangentVectorType1> & v,
551 const Eigen::MatrixBase<TangentVectorType2> & tau,
552 const container::aligned_vector<ForceDerived> & fext)
553
554 {
555
1/2
✓ Branch 1 taken 104 times.
✗ Branch 2 not taken.
 104 assert(model.check(data) && "data is not consistent with model.");
556
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(
557 q.size(), model.nq, "The joint configuration vector is not of right size");
558
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(
559 v.size(), model.nv, "The joint velocity vector is not of right size");
560
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(
561 tau.size(), model.nv, "The joint torque vector is not of right size");
562
563 typedef typename ModelTpl<Scalar, Options, JointCollectionTpl>::JointIndex JointIndex;
564
565 104 data.v[0].setZero();
566
1/2
✓ Branch 3 taken 104 times.
✗ Branch 4 not taken.
 104 data.a_gf[0] = -model.gravity;
567 104 data.u = tau;
568
569 typedef AbaLocalConventionForwardStep1<
570 Scalar, Options, JointCollectionTpl, ConfigVectorType, TangentVectorType1>
571 Pass1;
572
2/2
✓ Branch 0 taken 2808 times.
✓ Branch 1 taken 104 times.
 2912 for (JointIndex i = 1; i < (JointIndex)model.njoints; ++i)
573 {
574
1/2
✓ Branch 1 taken 2808 times.
✗ Branch 2 not taken.
 2808 Pass1::run(
575 2808 model.joints[i], data.joints[i],
576 2808 typename Pass1::ArgsType(model, data, q.derived(), v.derived()));
577 2808 data.f[i] -= fext[i];
578 }
579
580 typedef AbaLocalConventionBackwardStep<Scalar, Options, JointCollectionTpl> Pass2;
581
2/2
✓ Branch 0 taken 2808 times.
✓ Branch 1 taken 104 times.
 2912 for (JointIndex i = (JointIndex)model.njoints - 1; i > 0; --i)
582 {
583
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));
584 }
585
586 typedef AbaLocalConventionForwardStep2<Scalar, Options, JointCollectionTpl> Pass3;
587
2/2
✓ Branch 0 taken 2808 times.
✓ Branch 1 taken 104 times.
 2912 for (JointIndex i = 1; i < (JointIndex)model.njoints; ++i)
588 {
589
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));
590 }
591
592
2/2
✓ Branch 0 taken 2808 times.
✓ Branch 1 taken 104 times.
 2912 for (JointIndex i = (JointIndex)model.njoints - 1; i > 0; --i)
593 {
594 2808 const JointIndex parent = model.parents[i];
595
1/2
✓ Branch 5 taken 2808 times.
✗ Branch 6 not taken.
 2808 data.f[parent] += data.liMi[i].act(data.f[i]);
596 }
597
598 104 return data.ddq;
599 }
600
601 template<
602 typename Scalar,
603 int Options,
604 template<typename, int> class JointCollectionTpl,
605 typename ConfigVectorType>
606 struct ComputeMinverseForwardStep1
607 : public fusion::JointUnaryVisitorBase<
608 ComputeMinverseForwardStep1<Scalar, Options, JointCollectionTpl, ConfigVectorType>>
609 {
610 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
611 typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
612
613 typedef boost::fusion::vector<const Model &, Data &, const ConfigVectorType &> ArgsType;
614
615 template<typename JointModel>
616 1458 static void algo(
617 const pinocchio::JointModelBase<JointModel> & jmodel,
618 pinocchio::JointDataBase<typename JointModel::JointDataDerived> & jdata,
619 const Model & model,
620 Data & data,
621 const Eigen::MatrixBase<ConfigVectorType> & q)
622 {
623 typedef typename Model::JointIndex JointIndex;
624
625
1/2
✓ Branch 1 taken 729 times.
✗ Branch 2 not taken.
 1458 const JointIndex & i = jmodel.id();
626
1/2
✓ Branch 3 taken 729 times.
✗ Branch 4 not taken.
 1458 jmodel.calc(jdata.derived(), q.derived());
627
628 1458 const JointIndex & parent = model.parents[i];
629
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();
630
631
2/2
✓ Branch 0 taken 702 times.
✓ Branch 1 taken 27 times.
 1458 if (parent > 0)
632
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];
633 else
634
1/2
✓ Branch 3 taken 27 times.
✗ Branch 4 not taken.
 54 data.oMi[i] = data.liMi[i];
635
636 typedef
637 typename SizeDepType<JointModel::NV>::template ColsReturn<typename Data::Matrix6x>::Type
638 ColsBlock;
639
1/2
✓ Branch 1 taken 729 times.
✗ Branch 2 not taken.
 1458 ColsBlock J_cols = jmodel.jointCols(data.J);
640
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());
641
642
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]);
643
1/2
✓ Branch 2 taken 729 times.
✗ Branch 3 not taken.
 1458 data.oYaba[i] = data.oYcrb[i].matrix();
644 }
645 };
646
647 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
648 struct ComputeMinverseBackwardStep
649 : public fusion::JointUnaryVisitorBase<
650 ComputeMinverseBackwardStep<Scalar, Options, JointCollectionTpl>>
651 {
652 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
653 typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
654
655 typedef boost::fusion::vector<const Model &, Data &> ArgsType;
656
657 template<typename JointModel>
658 1458 static void algo(
659 const JointModelBase<JointModel> & jmodel,
660 JointDataBase<typename JointModel::JointDataDerived> & jdata,
661 const Model & model,
662 Data & data)
663 {
664 typedef typename Model::JointIndex JointIndex;
665 typedef typename Data::Inertia Inertia;
666
667
1/2
✓ Branch 1 taken 729 times.
✗ Branch 2 not taken.
 1458 const JointIndex i = jmodel.id();
668 1458 const JointIndex parent = model.parents[i];
669
670 1458 typename Inertia::Matrix6 & Ia = data.oYaba[i];
671 1458 typename Data::RowMatrixXs & Minv = data.Minv;
672 1458 typename Data::Matrix6x & Fcrb = data.Fcrb[0];
673 typedef
674 typename SizeDepType<JointModel::NV>::template ColsReturn<typename Data::Matrix6x>::Type
675 ColsBlock;
676
677
1/2
✓ Branch 1 taken 729 times.
✗ Branch 2 not taken.
 1458 ColsBlock J_cols = jmodel.jointCols(data.J);
678
679
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;
680
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();
681
682 // Account for the rotor inertia contribution
683
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);
684
685
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());
686
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();
687
688
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();
689
1/2
✓ Branch 2 taken 729 times.
✗ Branch 3 not taken.
 1458 const int nv_children = data.nvSubtree[i] - jmodel.nv();
690
2/2
✓ Branch 0 taken 621 times.
✓ Branch 1 taken 108 times.
 1458 if (nv_children > 0)
691 {
692
1/2
✓ Branch 1 taken 621 times.
✗ Branch 2 not taken.
 1242 ColsBlock SDinv_cols = jmodel.jointCols(data.SDinv);
693
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();
694
695
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)
696
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() =
697
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);
698
699
2/2
✓ Branch 0 taken 594 times.
✓ Branch 1 taken 27 times.
 1242 if (parent > 0)
700 {
701
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() +=
702
1/2
✓ Branch 1 taken 594 times.
✗ Branch 2 not taken.
 1188 jdata.U()
703
5/10
✓ 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.
 3564 * Minv.block(jmodel.idx_v(), jmodel.idx_v(), jmodel.nv(), data.nvSubtree[i]);
704 ;
705 }
706 }
707 else
708 {
709
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() =
710
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]);
711 }
712
713
2/2
✓ Branch 0 taken 702 times.
✓ Branch 1 taken 27 times.
 1458 if (parent > 0)
714 {
715
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();
716
1/2
✓ Branch 2 taken 702 times.
✗ Branch 3 not taken.
 1404 data.oYaba[parent] += Ia;
717 }
718 }
719 };
720
721 namespace optimized
722 {
723 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
724 struct ComputeMinverseBackwardStep
725 : public fusion::JointUnaryVisitorBase<
726 ComputeMinverseBackwardStep<Scalar, Options, JointCollectionTpl>>
727 {
728 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
729 typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
730
731 typedef boost::fusion::vector<const Model &, Data &> ArgsType;
732
733 template<typename JointModel>
734 54 static void algo(
735 const JointModelBase<JointModel> & jmodel,
736 JointDataBase<typename JointModel::JointDataDerived> & jdata,
737 const Model & model,
738 Data & data)
739 {
740 typedef typename Model::JointIndex JointIndex;
741
742
1/2
✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
 54 const JointIndex i = jmodel.id();
743 54 const JointIndex parent = model.parents[i];
744
745 54 typename Data::RowMatrixXs & Minv = data.Minv;
746 54 typename Data::Matrix6x & Fcrb = data.Fcrb[0];
747 typedef
748 typename SizeDepType<JointModel::NV>::template ColsReturn<typename Data::Matrix6x>::Type
749 ColsBlock;
750
751
1/2
✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
 54 const ColsBlock J_cols = jmodel.jointCols(data.J);
752
753
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();
754
1/2
✓ Branch 2 taken 27 times.
✗ Branch 3 not taken.
 54 const int nv_children = data.nvSubtree[i] - jmodel.nv();
755
2/2
✓ Branch 0 taken 23 times.
✓ Branch 1 taken 4 times.
 54 if (nv_children > 0)
756 {
757
1/2
✓ Branch 1 taken 23 times.
✗ Branch 2 not taken.
 46 ColsBlock SDinv_cols = jmodel.jointCols(data.SDinv);
758
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();
759
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)
760
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() =
761
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);
762
763
2/2
✓ Branch 0 taken 22 times.
✓ Branch 1 taken 1 times.
 46 if (parent > 0)
764 {
765
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() +=
766
1/2
✓ Branch 1 taken 22 times.
✗ Branch 2 not taken.
 44 jdata.U()
767
5/10
✓ 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.
 132 * Minv.block(jmodel.idx_v(), jmodel.idx_v(), jmodel.nv(), data.nvSubtree[i]);
768 ;
769 }
770 }
771 else
772 {
773
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() =
774
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 8 jdata.U()
775
5/10
✓ 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.
 24 * Minv.block(jmodel.idx_v(), jmodel.idx_v(), jmodel.nv(), data.nvSubtree[i]);
776 }
777 }
778 };
779 } // namespace optimized
780
781 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
782 struct ComputeMinverseForwardStep2
783 : public fusion::JointUnaryVisitorBase<
784 ComputeMinverseForwardStep2<Scalar, Options, JointCollectionTpl>>
785 {
786 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
787 typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
788
789 typedef boost::fusion::vector<const Model &, Data &> ArgsType;
790
791 template<typename JointModel>
792 1512 static void algo(
793 const pinocchio::JointModelBase<JointModel> & jmodel,
794 pinocchio::JointDataBase<typename JointModel::JointDataDerived> & jdata,
795 const Model & model,
796 Data & data)
797 {
798 typedef typename Model::JointIndex JointIndex;
799
800
1/2
✓ Branch 1 taken 756 times.
✗ Branch 2 not taken.
 1512 const JointIndex i = jmodel.id();
801 1512 const JointIndex parent = model.parents[i];
802 1512 typename Data::RowMatrixXs & Minv = data.Minv;
803
804 typedef
805 typename SizeDepType<JointModel::NV>::template ColsReturn<typename Data::Matrix6x>::Type
806 ColsBlock;
807
1/2
✓ Branch 1 taken 756 times.
✗ Branch 2 not taken.
 1512 ColsBlock J_cols = jmodel.jointCols(data.J);
808
809
2/2
✓ Branch 0 taken 728 times.
✓ Branch 1 taken 28 times.
 1512 if (parent > 0)
810 {
811
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())
812
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())
813
2/4
✓ Branch 1 taken 728 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 728 times.
✗ Branch 5 not taken.
 1456 .noalias() -=
814
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());
815 }
816
817
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() =
818 J_cols
819
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());
820
2/2
✓ Branch 0 taken 728 times.
✓ Branch 1 taken 28 times.
 1512 if (parent > 0)
821
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()) +=
822
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());
823 }
824 };
825
826 template<
827 typename Scalar,
828 int Options,
829 template<typename, int> class JointCollectionTpl,
830 typename ConfigVectorType>
831 27 const typename DataTpl<Scalar, Options, JointCollectionTpl>::RowMatrixXs & computeMinverse(
832 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
833 DataTpl<Scalar, Options, JointCollectionTpl> & data,
834 const Eigen::MatrixBase<ConfigVectorType> & q)
835 {
836
1/2
✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
 27 assert(model.check(data) && "data is not consistent with model.");
837
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(
838 q.size(), model.nq, "The joint configuration vector is not of right size");
839
840 typedef typename ModelTpl<Scalar, Options, JointCollectionTpl>::JointIndex JointIndex;
841
1/2
✓ Branch 2 taken 27 times.
✗ Branch 3 not taken.
 27 data.Minv.template triangularView<Eigen::Upper>().setZero();
842
843 typedef ComputeMinverseForwardStep1<Scalar, Options, JointCollectionTpl, ConfigVectorType>
844 Pass1;
845
2/2
✓ Branch 0 taken 729 times.
✓ Branch 1 taken 27 times.
 756 for (JointIndex i = 1; i < (JointIndex)model.njoints; ++i)
846 {
847
1/2
✓ Branch 1 taken 729 times.
✗ Branch 2 not taken.
 729 Pass1::run(
848 1458 model.joints[i], data.joints[i], typename Pass1::ArgsType(model, data, q.derived()));
849 }
850
851 27 data.Fcrb[0].setZero();
852 typedef ComputeMinverseBackwardStep<Scalar, Options, JointCollectionTpl> Pass2;
853
2/2
✓ Branch 0 taken 729 times.
✓ Branch 1 taken 27 times.
 756 for (JointIndex i = (JointIndex)model.njoints - 1; i > 0; --i)
854 {
855
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));
856 }
857
858 typedef ComputeMinverseForwardStep2<Scalar, Options, JointCollectionTpl> Pass3;
859
2/2
✓ Branch 0 taken 729 times.
✓ Branch 1 taken 27 times.
 756 for (JointIndex i = 1; i < (JointIndex)model.njoints; ++i)
860 {
861
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));
862 }
863
864 27 return data.Minv;
865 }
866 } // namespace impl
867
868 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
869 1 const typename DataTpl<Scalar, Options, JointCollectionTpl>::RowMatrixXs & computeMinverse(
870 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
871 DataTpl<Scalar, Options, JointCollectionTpl> & data)
872 {
873
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 assert(model.check(data) && "data is not consistent with model.");
874
875 typedef typename ModelTpl<Scalar, Options, JointCollectionTpl>::JointIndex JointIndex;
876
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 data.Minv.template triangularView<Eigen::Upper>().setZero();
877
878 1 data.Fcrb[0].setZero();
879 typedef impl::optimized::ComputeMinverseBackwardStep<Scalar, Options, JointCollectionTpl> Pass2;
880
2/2
✓ Branch 0 taken 27 times.
✓ Branch 1 taken 1 times.
 28 for (JointIndex i = (JointIndex)model.njoints - 1; i > 0; --i)
881 {
882
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));
883 }
884
885 typedef impl::ComputeMinverseForwardStep2<Scalar, Options, JointCollectionTpl> Pass3;
886
2/2
✓ Branch 0 taken 27 times.
✓ Branch 1 taken 1 times.
 28 for (JointIndex i = 1; i < (JointIndex)model.njoints; ++i)
887 {
888
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));
889 }
890
891 1 return data.Minv;
892 }
893
894 // --- CHECKER ---------------------------------------------------------------
895 // --- CHECKER ---------------------------------------------------------------
896 // --- CHECKER ---------------------------------------------------------------
897
898 // Check whether all masses are nonzero and diagonal of inertia is nonzero
899 // The second test is overconstraining.
900 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
901 inline bool
902 3 ABAChecker::checkModel_impl(const ModelTpl<Scalar, Options, JointCollectionTpl> & model) const
903 {
904 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
905 typedef typename Model::JointIndex JointIndex;
906
907
2/2
✓ Branch 0 taken 55 times.
✓ Branch 1 taken 2 times.
 57 for (JointIndex j = 1; j < (JointIndex)model.njoints; j++)
908 55 if (
909
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)
910
1/2
✓ Branch 4 taken 54 times.
✗ Branch 5 not taken.
 54 || (model.inertias[j].inertia().data()[2] < 1e-5)
911
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))
912 1 return false;
913 2 return true;
914 }
915
916 template<
917 typename Scalar,
918 int Options,
919 template<typename, int> class JointCollectionTpl,
920 typename ConfigVectorType,
921 typename TangentVectorType1,
922 typename TangentVectorType2>
923 330 const typename DataTpl<Scalar, Options, JointCollectionTpl>::TangentVectorType & aba(
924 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
925 DataTpl<Scalar, Options, JointCollectionTpl> & data,
926 const Eigen::MatrixBase<ConfigVectorType> & q,
927 const Eigen::MatrixBase<TangentVectorType1> & v,
928 const Eigen::MatrixBase<TangentVectorType2> & tau,
929 const Convention convention)
930 {
931
2/3
✓ Branch 0 taken 276 times.
✓ Branch 1 taken 35 times.
✗ Branch 2 not taken.
 330 switch (convention)
932 {
933 279 case Convention::LOCAL:
934 279 return impl::abaLocalConvention(model, data, q, v, tau);
935 51 case Convention::WORLD:
936 51 return impl::abaWorldConvention(model, data, q, v, tau);
937 }
938 }
939
940 template<
941 typename Scalar,
942 int Options,
943 template<typename, int> class JointCollectionTpl,
944 typename ConfigVectorType,
945 typename TangentVectorType1,
946 typename TangentVectorType2,
947 typename ForceDerived>
948 107 const typename DataTpl<Scalar, Options, JointCollectionTpl>::TangentVectorType & aba(
949 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
950 DataTpl<Scalar, Options, JointCollectionTpl> & data,
951 const Eigen::MatrixBase<ConfigVectorType> & q,
952 const Eigen::MatrixBase<TangentVectorType1> & v,
953 const Eigen::MatrixBase<TangentVectorType2> & tau,
954 const container::aligned_vector<ForceDerived> & fext,
955 const Convention convention)
956 {
957
2/3
✓ Branch 0 taken 104 times.
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 107 switch (convention)
958 {
959 104 case Convention::LOCAL:
960 104 return impl::abaLocalConvention(model, data, q, v, tau, fext);
961 3 case Convention::WORLD:
962 3 return impl::abaWorldConvention(model, data, q, v, tau, fext);
963 }
964 }
965
966 template<
967 typename Scalar,
968 int Options,
969 template<typename, int> class JointCollectionTpl,
970 typename ConfigVectorType>
971 27 const typename DataTpl<Scalar, Options, JointCollectionTpl>::RowMatrixXs & computeMinverse(
972 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
973 DataTpl<Scalar, Options, JointCollectionTpl> & data,
974 const Eigen::MatrixBase<ConfigVectorType> & q)
975 {
976
1/2
✓ Branch 2 taken 27 times.
✗ Branch 3 not taken.
 27 return impl::computeMinverse(model, data, make_const_ref(q));
977 }
978
979 } // namespace pinocchio
980
981 /// @endcond
982
983 #endif // ifndef __pinocchio_algorithm_aba_hxx__
984