GCC Code Coverage Report

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