GCC Code Coverage Report

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