GCC Code Coverage Report

Directory: ./
File: include/pinocchio/algorithm/aba-derivatives.hxx
Date: 2025-02-12 21:03:38
Exec Total Coverage
Lines: 347 353 98.3%
Branches: 590 1578 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/10
✓ 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.
 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
407 typedef typename ModelTpl<Scalar, Options, JointCollectionTpl>::JointIndex JointIndex;
408
409
1/2
✓ Branch 3 taken 32 times.
✗ Branch 4 not taken.
 64 data.oa_gf[0] = -model.gravity;
410 64 data.u = tau;
411
412 64 MatrixType3 & Minv_ = PINOCCHIO_EIGEN_CONST_CAST(MatrixType3, aba_partial_dtau);
413
1/2
✓ Branch 2 taken 32 times.
✗ Branch 3 not taken.
 64 Minv_.template triangularView<Eigen::Upper>().setZero();
414
415 /// First, compute Minv and a, the joint acceleration vector
416 typedef ComputeABADerivativesForwardStep1<
417 Scalar, Options, JointCollectionTpl, ConfigVectorType, TangentVectorType1>
418 Pass1;
419
2/2
✓ Branch 0 taken 843 times.
✓ Branch 1 taken 32 times.
 1750 for (JointIndex i = 1; i < (JointIndex)model.njoints; ++i)
420 {
421
1/2
✓ Branch 1 taken 843 times.
✗ Branch 2 not taken.
 1686 Pass1::run(
422 1686 model.joints[i], data.joints[i],
423 3372 typename Pass1::ArgsType(model, data, q.derived(), v.derived()));
424 }
425
426 64 data.Fcrb[0].setZero();
427 typedef ComputeABADerivativesBackwardStep1<Scalar, Options, JointCollectionTpl, MatrixType3>
428 Pass2;
429
2/2
✓ Branch 0 taken 843 times.
✓ Branch 1 taken 32 times.
 1750 for (JointIndex i = (JointIndex)(model.njoints - 1); i > 0; --i)
430 {
431
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_));
432 }
433
434 typedef ComputeABADerivativesForwardStep2<Scalar, Options, JointCollectionTpl, MatrixType3>
435 Pass3;
436
2/2
✓ Branch 0 taken 843 times.
✓ Branch 1 taken 32 times.
 1750 for (JointIndex i = 1; i < (JointIndex)model.njoints; ++i)
437 {
438
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_));
439 }
440
441 typedef ComputeABADerivativesBackwardStep2<Scalar, Options, JointCollectionTpl> Pass4;
442
2/2
✓ Branch 0 taken 843 times.
✓ Branch 1 taken 32 times.
 1750 for (JointIndex i = (JointIndex)(model.njoints - 1); i > 0; --i)
443 {
444
1/2
✓ Branch 3 taken 843 times.
✗ Branch 4 not taken.
 1686 Pass4::run(model.joints[i], typename Pass4::ArgsType(model, data));
445 }
446
447
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>() =
448
1/2
✓ Branch 2 taken 32 times.
✗ Branch 3 not taken.
 64 Minv_.transpose().template triangularView<Eigen::StrictlyLower>();
449
450
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;
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(MatrixType2, aba_partial_dv).noalias() = -Minv_ * data.dtau_dv;
452 64 }
453
454 template<
455 typename Scalar,
456 int Options,
457 template<typename, int> class JointCollectionTpl,
458 typename ConfigVectorType,
459 typename TangentVectorType1,
460 typename TangentVectorType2,
461 typename MatrixType1,
462 typename MatrixType2,
463 typename MatrixType3>
464 10 void computeABADerivatives(
465 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
466 DataTpl<Scalar, Options, JointCollectionTpl> & data,
467 const Eigen::MatrixBase<ConfigVectorType> & q,
468 const Eigen::MatrixBase<TangentVectorType1> & v,
469 const Eigen::MatrixBase<TangentVectorType2> & tau,
470 const container::aligned_vector<ForceTpl<Scalar, Options>> & fext,
471 const Eigen::MatrixBase<MatrixType1> & aba_partial_dq,
472 const Eigen::MatrixBase<MatrixType2> & aba_partial_dv,
473 const Eigen::MatrixBase<MatrixType3> & aba_partial_dtau)
474 {
475
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(
476 q.size(), model.nq, "The joint configuration vector is not of right size");
477
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(
478 v.size(), model.nv, "The joint velocity vector is not of right size");
479
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(
480 tau.size(), model.nv, "The joint torque vector is not of right size");
481
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(
482 fext.size(), (size_t)model.njoints, "The external forces vector is not of right size");
483
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);
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.rows(), 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_dv.cols(), 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.rows(), 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_dtau.cols(), 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.rows(), model.nv);
489
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(
490 isZero(model.gravity.angular()),
491 "The gravity must be a pure force vector, no angular part");
492
1/2
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
 10 assert(model.check(data) && "data is not consistent with model.");
493
494 typedef typename ModelTpl<Scalar, Options, JointCollectionTpl>::JointIndex JointIndex;
495
496
1/2
✓ Branch 3 taken 5 times.
✗ Branch 4 not taken.
 10 data.oa_gf[0] = -model.gravity;
497 10 data.u = tau;
498
499 10 MatrixType3 & Minv_ = PINOCCHIO_EIGEN_CONST_CAST(MatrixType3, aba_partial_dtau);
500
1/2
✓ Branch 2 taken 5 times.
✗ Branch 3 not taken.
 10 Minv_.template triangularView<Eigen::Upper>().setZero();
501
502 /// First, compute Minv and a, the joint acceleration vector
503 typedef ComputeABADerivativesForwardStep1<
504 Scalar, Options, JointCollectionTpl, ConfigVectorType, TangentVectorType1>
505 Pass1;
506
2/2
✓ Branch 0 taken 135 times.
✓ Branch 1 taken 5 times.
 280 for (JointIndex i = 1; i < (JointIndex)model.njoints; ++i)
507 {
508
1/2
✓ Branch 1 taken 135 times.
✗ Branch 2 not taken.
 270 Pass1::run(
509 270 model.joints[i], data.joints[i],
510 270 typename Pass1::ArgsType(model, data, q.derived(), v.derived()));
511
1/2
✓ Branch 5 taken 135 times.
✗ Branch 6 not taken.
 270 data.of[i] -= data.oMi[i].act(fext[i]);
512 }
513
514 10 data.Fcrb[0].setZero();
515 typedef ComputeABADerivativesBackwardStep1<Scalar, Options, JointCollectionTpl, MatrixType3>
516 Pass2;
517
2/2
✓ Branch 0 taken 135 times.
✓ Branch 1 taken 5 times.
 280 for (JointIndex i = (JointIndex)(model.njoints - 1); i > 0; --i)
518 {
519
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_));
520 }
521
522 typedef ComputeABADerivativesForwardStep2<Scalar, Options, JointCollectionTpl, MatrixType3>
523 Pass3;
524
2/2
✓ Branch 0 taken 135 times.
✓ Branch 1 taken 5 times.
 280 for (JointIndex i = 1; i < (JointIndex)model.njoints; ++i)
525 {
526
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_));
527
1/2
✓ Branch 5 taken 135 times.
✗ Branch 6 not taken.
 270 data.of[i] -= data.oMi[i].act(fext[i]);
528 }
529
530 typedef ComputeABADerivativesBackwardStep2<Scalar, Options, JointCollectionTpl> Pass4;
531
2/2
✓ Branch 0 taken 135 times.
✓ Branch 1 taken 5 times.
 280 for (JointIndex i = (JointIndex)(model.njoints - 1); i > 0; --i)
532 {
533
1/2
✓ Branch 3 taken 135 times.
✗ Branch 4 not taken.
 270 Pass4::run(model.joints[i], typename Pass4::ArgsType(model, data));
534 }
535
536
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>() =
537
1/2
✓ Branch 2 taken 5 times.
✗ Branch 3 not taken.
 10 Minv_.transpose().template triangularView<Eigen::StrictlyLower>();
538
539
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;
540
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;
541 10 }
542
543 namespace optimized
544 {
545 template<
546 typename Scalar,
547 int Options,
548 template<typename, int> class JointCollectionTpl,
549 typename MatrixType>
550 struct ComputeABADerivativesBackwardStep1
551 : public fusion::JointUnaryVisitorBase<
552 ComputeABADerivativesBackwardStep1<Scalar, Options, JointCollectionTpl, MatrixType>>
553 {
554 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
555 typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
556
557 typedef boost::fusion::vector<const Model &, Data &, MatrixType &> ArgsType;
558
559 template<typename JointModel>
560 2268 static void algo(
561 const JointModelBase<JointModel> & jmodel,
562 JointDataBase<typename JointModel::JointDataDerived> & jdata,
563 const Model & model,
564 Data & data,
565 const Eigen::MatrixBase<MatrixType> & Minv)
566 {
567 typedef typename Model::JointIndex JointIndex;
568 typedef typename Data::Matrix6x Matrix6x;
569
570
1/2
✓ Branch 1 taken 1134 times.
✗ Branch 2 not taken.
 2268 const JointIndex i = jmodel.id();
571 2268 const JointIndex parent = model.parents[i];
572
573 2268 Matrix6x & Fcrb = data.Fcrb[0];
574
575 typedef
576 typename SizeDepType<JointModel::NV>::template ColsReturn<typename Data::Matrix6x>::Type
577 ColsBlock;
578
579 typedef
580 typename SizeDepType<JointModel::NV>::template ColsReturn<Matrix6x>::Type ColBlock;
581
1/2
✓ Branch 1 taken 1134 times.
✗ Branch 2 not taken.
 2268 const ColBlock J_cols = jmodel.jointCols(data.J);
582
583 2268 MatrixType & Minv_ = PINOCCHIO_EIGEN_CONST_CAST(MatrixType, Minv);
584
585
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();
586
1/2
✓ Branch 2 taken 1134 times.
✗ Branch 3 not taken.
 2268 const int nv_children = data.nvSubtree[i] - jmodel.nv();
587
2/2
✓ Branch 0 taken 966 times.
✓ Branch 1 taken 168 times.
 2268 if (nv_children > 0)
588 {
589
1/2
✓ Branch 1 taken 966 times.
✗ Branch 2 not taken.
 1932 ColsBlock SDinv_cols = jmodel.jointCols(data.SDinv);
590
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();
591
592
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)
593
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() =
594
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);
595
596
2/2
✓ Branch 0 taken 924 times.
✓ Branch 1 taken 42 times.
 1932 if (parent > 0)
597 {
598
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() +=
599
1/2
✓ Branch 1 taken 924 times.
✗ Branch 2 not taken.
 1848 jdata.U()
600
5/10
✓ Branch 2 taken 924 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 924 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 924 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 924 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 924 times.
✗ Branch 15 not taken.
 5544 * Minv_.block(jmodel.idx_v(), jmodel.idx_v(), jmodel.nv(), data.nvSubtree[i]);
601 ;
602 }
603 }
604 else // This a leaf of the kinematic tree
605 {
606
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() =
607
1/2
✓ Branch 1 taken 168 times.
✗ Branch 2 not taken.
 336 jdata.U()
608
5/10
✓ Branch 2 taken 168 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 168 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 168 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 168 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 168 times.
✗ Branch 15 not taken.
 1008 * Minv_.block(jmodel.idx_v(), jmodel.idx_v(), jmodel.nv(), data.nvSubtree[i]);
609 }
610 }
611 };
612
613 template<
614 typename Scalar,
615 int Options,
616 template<typename, int> class JointCollectionTpl,
617 typename MatrixType>
618 struct ComputeABADerivativesForwardStep2
619 : public fusion::JointUnaryVisitorBase<
620 ComputeABADerivativesForwardStep2<Scalar, Options, JointCollectionTpl, MatrixType>>
621 {
622 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
623 typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
624
625 typedef ::pinocchio::impl::
626 ComputeABADerivativesForwardStep2<Scalar, Options, JointCollectionTpl, MatrixType>
627 SimilarBase;
628
629 typedef boost::fusion::vector<const Model &, Data &, MatrixType &> ArgsType;
630
631 template<typename JointModel>
632 2268 static void algo(
633 const JointModelBase<JointModel> & jmodel,
634 JointDataBase<typename JointModel::JointDataDerived> & jdata,
635 const Model & model,
636 Data & data,
637 MatrixType & Minv)
638 {
639 typedef typename Model::JointIndex JointIndex;
640 typedef typename Data::Matrix6x Matrix6x;
641
642
1/2
✓ Branch 1 taken 1134 times.
✗ Branch 2 not taken.
 2268 const JointIndex i = jmodel.id();
643 2268 const JointIndex parent = model.parents[i];
644
645 2268 const typename Data::Motion & ov = data.ov[i];
646 // typename Data::Motion & oa = data.oa[i];
647 2268 typename Data::Force & of = data.of[i];
648 2268 const typename Data::Motion & oa_gf = data.oa_gf[i];
649
650 typedef
651 typename SizeDepType<JointModel::NV>::template ColsReturn<Matrix6x>::Type ColsBlock;
652
1/2
✓ Branch 1 taken 1134 times.
✗ Branch 2 not taken.
 2268 const ColsBlock J_cols = jmodel.jointCols(data.J);
653
654 // Already done in optimized::aba
655 // oa = oa_gf + model.gravity;
656
1/2
✓ Branch 3 taken 1134 times.
✗ Branch 4 not taken.
 2268 data.oYcrb[i] = data.oinertias[i];
657
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]);
658
659 2268 MatrixType & Minv_ = PINOCCHIO_EIGEN_CONST_CAST(MatrixType, Minv);
660
661
2/2
✓ Branch 0 taken 1092 times.
✓ Branch 1 taken 42 times.
 2268 if (parent > 0)
662 {
663
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())
664
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())
665
2/4
✓ Branch 1 taken 1092 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1092 times.
✗ Branch 5 not taken.
 2184 .noalias() -=
666
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());
667 ;
668 }
669
670
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() =
671 J_cols
672
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());
673
2/2
✓ Branch 0 taken 1092 times.
✓ Branch 1 taken 42 times.
 2268 if (parent > 0)
674
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()) +=
675
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());
676
677
1/2
✓ Branch 1 taken 1134 times.
✗ Branch 2 not taken.
 2268 ColsBlock dJ_cols = jmodel.jointCols(data.dJ);
678
1/2
✓ Branch 1 taken 1134 times.
✗ Branch 2 not taken.
 2268 ColsBlock dVdq_cols = jmodel.jointCols(data.dVdq);
679
1/2
✓ Branch 1 taken 1134 times.
✗ Branch 2 not taken.
 2268 ColsBlock dAdq_cols = jmodel.jointCols(data.dAdq);
680
1/2
✓ Branch 1 taken 1134 times.
✗ Branch 2 not taken.
 2268 ColsBlock dAdv_cols = jmodel.jointCols(data.dAdv);
681
682
1/2
✓ Branch 1 taken 1134 times.
✗ Branch 2 not taken.
 2268 motionSet::motionAction(ov, J_cols, dJ_cols);
683
1/2
✓ Branch 2 taken 1134 times.
✗ Branch 3 not taken.
 2268 motionSet::motionAction(data.oa_gf[parent], J_cols, dAdq_cols);
684
1/2
✓ Branch 1 taken 1134 times.
✗ Branch 2 not taken.
 2268 dAdv_cols = dJ_cols;
685
2/2
✓ Branch 0 taken 1092 times.
✓ Branch 1 taken 42 times.
 2268 if (parent > 0)
686 {
687
1/2
✓ Branch 2 taken 1092 times.
✗ Branch 3 not taken.
 2184 motionSet::motionAction(data.ov[parent], J_cols, dVdq_cols);
688
1/2
✓ Branch 2 taken 1092 times.
✗ Branch 3 not taken.
 2184 motionSet::motionAction<ADDTO>(data.ov[parent], dVdq_cols, dAdq_cols);
689
1/2
✓ Branch 1 taken 1092 times.
✗ Branch 2 not taken.
 2184 dAdv_cols += dVdq_cols;
690 }
691 else
692
1/2
✓ Branch 1 taken 42 times.
✗ Branch 2 not taken.
 84 dVdq_cols.setZero();
693
694 // computes variation of inertias
695
1/2
✓ Branch 2 taken 1134 times.
✗ Branch 3 not taken.
 2268 data.doYcrb[i] = data.oinertias[i].variation(ov);
696
1/2
✓ Branch 3 taken 1134 times.
✗ Branch 4 not taken.
 2268 SimilarBase::addForceCrossMatrix(data.oh[i], data.doYcrb[i]);
697 }
698 };
699
700 } // namespace optimized
701
702 template<
703 typename Scalar,
704 int Options,
705 template<typename, int> class JointCollectionTpl,
706 typename MatrixType1,
707 typename MatrixType2,
708 typename MatrixType3>
709 typename std::enable_if<
710 !(MatrixType1::IsVectorAtCompileTime || MatrixType2::IsVectorAtCompileTime
711 || MatrixType3::IsVectorAtCompileTime),
712 void>::type
713 42 computeABADerivatives(
714 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
715 DataTpl<Scalar, Options, JointCollectionTpl> & data,
716 const Eigen::MatrixBase<MatrixType1> & aba_partial_dq,
717 const Eigen::MatrixBase<MatrixType2> & aba_partial_dv,
718 const Eigen::MatrixBase<MatrixType3> & aba_partial_dtau)
719 {
720
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);
721
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);
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_dv.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_dv.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_dtau.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_dtau.rows() == model.nv);
726
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(
727 isZero(model.gravity.angular()),
728 "The gravity must be a pure force vector, no angular part");
729
1/2
✓ Branch 1 taken 21 times.
✗ Branch 2 not taken.
 42 assert(model.check(data) && "data is not consistent with model.");
730
731 typedef typename ModelTpl<Scalar, Options, JointCollectionTpl>::JointIndex JointIndex;
732
733 42 MatrixType3 & Minv_ = PINOCCHIO_EIGEN_CONST_CAST(MatrixType3, aba_partial_dtau);
734
1/2
✓ Branch 2 taken 21 times.
✗ Branch 3 not taken.
 42 Minv_.template triangularView<Eigen::Upper>().setZero();
735
736 42 data.Fcrb[0].setZero();
737 typedef optimized::ComputeABADerivativesBackwardStep1<
738 Scalar, Options, JointCollectionTpl, MatrixType3>
739 Pass2;
740
2/2
✓ Branch 0 taken 567 times.
✓ Branch 1 taken 21 times.
 1176 for (JointIndex i = (JointIndex)(model.njoints - 1); i > 0; --i)
741 {
742
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_));
743 }
744
745 typedef optimized::ComputeABADerivativesForwardStep2<
746 Scalar, Options, JointCollectionTpl, MatrixType3>
747 Pass3;
748
2/2
✓ Branch 0 taken 567 times.
✓ Branch 1 taken 21 times.
 1176 for (JointIndex i = 1; i < (JointIndex)model.njoints; ++i)
749 {
750
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_));
751 }
752
753 typedef ComputeABADerivativesBackwardStep2<Scalar, Options, JointCollectionTpl> Pass4;
754
2/2
✓ Branch 0 taken 567 times.
✓ Branch 1 taken 21 times.
 1176 for (JointIndex i = (JointIndex)(model.njoints - 1); i > 0; --i)
755 {
756
1/2
✓ Branch 3 taken 567 times.
✗ Branch 4 not taken.
 1134 Pass4::run(model.joints[i], typename Pass4::ArgsType(model, data));
757 }
758
759
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>() =
760
1/2
✓ Branch 2 taken 21 times.
✗ Branch 3 not taken.
 42 Minv_.transpose().template triangularView<Eigen::StrictlyLower>();
761
762
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;
763
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;
764 }
765
766 template<
767 typename Scalar,
768 int Options,
769 template<typename, int> class JointCollectionTpl,
770 typename MatrixType1,
771 typename MatrixType2,
772 typename MatrixType3>
773 42 void computeABADerivatives(
774 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
775 DataTpl<Scalar, Options, JointCollectionTpl> & data,
776 const container::aligned_vector<ForceTpl<Scalar, Options>> & fext,
777 const Eigen::MatrixBase<MatrixType1> & aba_partial_dq,
778 const Eigen::MatrixBase<MatrixType2> & aba_partial_dv,
779 const Eigen::MatrixBase<MatrixType3> & aba_partial_dtau)
780 {
781
1/4
✗ Branch 1 not taken.
✓ Branch 2 taken 21 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
 42 PINOCCHIO_CHECK_INPUT_ARGUMENT(
782 fext.size() == (size_t)model.njoints,
783 "The size of the external forces is not of right size");
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(aba_partial_dq.cols() == model.nv);
785
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);
786
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);
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_dv.rows() == 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_dtau.cols() == 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_dtau.rows() == model.nv);
790
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(
791 isZero(model.gravity.angular()),
792 "The gravity must be a pure force vector, no angular part");
793
1/2
✓ Branch 1 taken 21 times.
✗ Branch 2 not taken.
 42 assert(model.check(data) && "data is not consistent with model.");
794
795 typedef typename ModelTpl<Scalar, Options, JointCollectionTpl>::JointIndex JointIndex;
796
797 42 MatrixType3 & Minv_ = PINOCCHIO_EIGEN_CONST_CAST(MatrixType3, aba_partial_dtau);
798
1/2
✓ Branch 2 taken 21 times.
✗ Branch 3 not taken.
 42 Minv_.template triangularView<Eigen::Upper>().setZero();
799
800 /// First, compute Minv and a, the joint acceleration vector
801 // for(JointIndex i=1; i<(JointIndex) model.njoints; ++i)
802 // data.of[i] -= data.oMi[i].act(fext[i]);
803
804 42 data.Fcrb[0].setZero();
805 typedef optimized::ComputeABADerivativesBackwardStep1<
806 Scalar, Options, JointCollectionTpl, MatrixType3>
807 Pass2;
808
2/2
✓ Branch 0 taken 567 times.
✓ Branch 1 taken 21 times.
 1176 for (JointIndex i = (JointIndex)(model.njoints - 1); i > 0; --i)
809 {
810
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_));
811 }
812
813 typedef optimized::ComputeABADerivativesForwardStep2<
814 Scalar, Options, JointCollectionTpl, MatrixType3>
815 Pass3;
816
2/2
✓ Branch 0 taken 567 times.
✓ Branch 1 taken 21 times.
 1176 for (JointIndex i = 1; i < (JointIndex)model.njoints; ++i)
817 {
818
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_));
819
1/2
✓ Branch 5 taken 567 times.
✗ Branch 6 not taken.
 1134 data.of[i] -= data.oMi[i].act(fext[i]);
820 }
821
822 typedef ComputeABADerivativesBackwardStep2<Scalar, Options, JointCollectionTpl> Pass4;
823
2/2
✓ Branch 0 taken 567 times.
✓ Branch 1 taken 21 times.
 1176 for (JointIndex i = (JointIndex)(model.njoints - 1); i > 0; --i)
824 {
825
1/2
✓ Branch 3 taken 567 times.
✗ Branch 4 not taken.
 1134 Pass4::run(model.joints[i], typename Pass4::ArgsType(model, data));
826 }
827
828
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>() =
829
1/2
✓ Branch 2 taken 21 times.
✗ Branch 3 not taken.
 42 Minv_.transpose().template triangularView<Eigen::StrictlyLower>();
830
831
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;
832
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;
833 }
834
835 template<
836 typename Scalar,
837 int Options,
838 template<typename, int> class JointCollectionTpl,
839 typename ConfigVectorType,
840 typename TangentVectorType1,
841 typename TangentVectorType2>
842 typename std::enable_if<
843 ConfigVectorType::IsVectorAtCompileTime || TangentVectorType1::IsVectorAtCompileTime
844 || TangentVectorType2::IsVectorAtCompileTime,
845 void>::type
846 27 computeABADerivatives(
847 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
848 DataTpl<Scalar, Options, JointCollectionTpl> & data,
849 const Eigen::MatrixBase<ConfigVectorType> & q,
850 const Eigen::MatrixBase<TangentVectorType1> & v,
851 const Eigen::MatrixBase<TangentVectorType2> & tau)
852 {
853 27 ::pinocchio::impl::computeABADerivatives(
854 27 model, data, q, v, tau, data.ddq_dq, data.ddq_dv, data.Minv);
855 27 }
856
857 template<
858 typename Scalar,
859 int Options,
860 template<typename, int> class JointCollectionTpl,
861 typename ConfigVectorType,
862 typename TangentVectorType1,
863 typename TangentVectorType2>
864 3 void computeABADerivatives(
865 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
866 DataTpl<Scalar, Options, JointCollectionTpl> & data,
867 const Eigen::MatrixBase<ConfigVectorType> & q,
868 const Eigen::MatrixBase<TangentVectorType1> & v,
869 const Eigen::MatrixBase<TangentVectorType2> & tau,
870 const container::aligned_vector<ForceTpl<Scalar, Options>> & fext)
871 {
872 3 ::pinocchio::impl::computeABADerivatives(
873 3 model, data, q, v, tau, fext, data.ddq_dq, data.ddq_dv, data.Minv);
874 3 }
875
876 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
877 void computeABADerivatives(
878 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
879 DataTpl<Scalar, Options, JointCollectionTpl> & data)
880 {
881 ::pinocchio::impl::computeABADerivatives(model, data, data.ddq_dq, data.ddq_dv, data.Minv);
882 }
883
884 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
885 void computeABADerivatives(
886 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
887 DataTpl<Scalar, Options, JointCollectionTpl> & data,
888 const container::aligned_vector<ForceTpl<Scalar, Options>> & fext)
889 {
890 ::pinocchio::impl::computeABADerivatives(
891 model, data, fext, data.ddq_dq, data.ddq_dv, data.Minv);
892 }
893
894 } // namespace impl
895
896 template<
897 typename Scalar,
898 int Options,
899 template<typename, int> class JointCollectionTpl,
900 typename ConfigVectorType,
901 typename TangentVectorType1,
902 typename TangentVectorType2,
903 typename MatrixType1,
904 typename MatrixType2,
905 typename MatrixType3>
906 9 void computeABADerivatives(
907 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
908 DataTpl<Scalar, Options, JointCollectionTpl> & data,
909 const Eigen::MatrixBase<ConfigVectorType> & q,
910 const Eigen::MatrixBase<TangentVectorType1> & v,
911 const Eigen::MatrixBase<TangentVectorType2> & tau,
912 const Eigen::MatrixBase<MatrixType1> & aba_partial_dq,
913 const Eigen::MatrixBase<MatrixType2> & aba_partial_dv,
914 const Eigen::MatrixBase<MatrixType3> & aba_partial_dtau)
915 {
916
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(
917 model, data, make_const_ref(q), make_const_ref(v), make_const_ref(tau),
918 make_ref(aba_partial_dq), make_ref(aba_partial_dv), make_ref(aba_partial_dtau));
919 9 }
920
921 template<
922 typename Scalar,
923 int Options,
924 template<typename, int> class JointCollectionTpl,
925 typename ConfigVectorType,
926 typename TangentVectorType1,
927 typename TangentVectorType2,
928 typename MatrixType1,
929 typename MatrixType2,
930 typename MatrixType3>
931 4 void computeABADerivatives(
932 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
933 DataTpl<Scalar, Options, JointCollectionTpl> & data,
934 const Eigen::MatrixBase<ConfigVectorType> & q,
935 const Eigen::MatrixBase<TangentVectorType1> & v,
936 const Eigen::MatrixBase<TangentVectorType2> & tau,
937 const container::aligned_vector<ForceTpl<Scalar, Options>> & fext,
938 const Eigen::MatrixBase<MatrixType1> & aba_partial_dq,
939 const Eigen::MatrixBase<MatrixType2> & aba_partial_dv,
940 const Eigen::MatrixBase<MatrixType3> & aba_partial_dtau)
941 {
942
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(
943 model, data, make_const_ref(q), make_const_ref(v), make_const_ref(tau), fext,
944 make_ref(aba_partial_dq), make_ref(aba_partial_dv), make_ref(aba_partial_dtau));
945 4 }
946
947 template<
948 typename Scalar,
949 int Options,
950 template<typename, int> class JointCollectionTpl,
951 typename ConfigVectorType,
952 typename TangentVectorType1,
953 typename TangentVectorType2>
954 typename std::enable_if<
955 ConfigVectorType::IsVectorAtCompileTime || TangentVectorType1::IsVectorAtCompileTime
956 || TangentVectorType2::IsVectorAtCompileTime,
957 void>::type
958 27 computeABADerivatives(
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 {
965
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(
966 model, data, make_const_ref(q), make_const_ref(v), make_const_ref(tau));
967 27 }
968
969 template<
970 typename Scalar,
971 int Options,
972 template<typename, int> class JointCollectionTpl,
973 typename ConfigVectorType,
974 typename TangentVectorType1,
975 typename TangentVectorType2>
976 3 void computeABADerivatives(
977 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
978 DataTpl<Scalar, Options, JointCollectionTpl> & data,
979 const Eigen::MatrixBase<ConfigVectorType> & q,
980 const Eigen::MatrixBase<TangentVectorType1> & v,
981 const Eigen::MatrixBase<TangentVectorType2> & tau,
982 const container::aligned_vector<ForceTpl<Scalar, Options>> & fext)
983 {
984
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(
985 model, data, make_const_ref(q), make_const_ref(v), make_const_ref(tau), fext);
986 3 }
987
988 template<
989 typename Scalar,
990 int Options,
991 template<typename, int> class JointCollectionTpl,
992 typename MatrixType1,
993 typename MatrixType2,
994 typename MatrixType3>
995 typename std::enable_if<
996 !(MatrixType1::IsVectorAtCompileTime || MatrixType2::IsVectorAtCompileTime
997 || MatrixType3::IsVectorAtCompileTime),
998 void>::type
999 21 computeABADerivatives(
1000 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
1001 DataTpl<Scalar, Options, JointCollectionTpl> & data,
1002 const Eigen::MatrixBase<MatrixType1> & aba_partial_dq,
1003 const Eigen::MatrixBase<MatrixType2> & aba_partial_dv,
1004 const Eigen::MatrixBase<MatrixType3> & aba_partial_dtau)
1005 {
1006
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(
1007 model, data, make_ref(aba_partial_dq), make_ref(aba_partial_dv), make_ref(aba_partial_dtau));
1008 21 }
1009
1010 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
1011 ✗ void computeABADerivatives(
1012 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
1013 DataTpl<Scalar, Options, JointCollectionTpl> & data)
1014 {
1015 ✗ impl::computeABADerivatives(
1016 ✗ model, data, make_ref(data.ddq_dq), make_ref(data.ddq_dv), make_ref(data.Minv));
1017 }
1018
1019 template<
1020 typename Scalar,
1021 int Options,
1022 template<typename, int> class JointCollectionTpl,
1023 typename MatrixType1,
1024 typename MatrixType2,
1025 typename MatrixType3>
1026 21 void computeABADerivatives(
1027 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
1028 DataTpl<Scalar, Options, JointCollectionTpl> & data,
1029 const container::aligned_vector<ForceTpl<Scalar, Options>> & fext,
1030 const Eigen::MatrixBase<MatrixType1> & aba_partial_dq,
1031 const Eigen::MatrixBase<MatrixType2> & aba_partial_dv,
1032 const Eigen::MatrixBase<MatrixType3> & aba_partial_dtau)
1033 {
1034
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(
1035 model, data, fext, make_ref(aba_partial_dq), make_ref(aba_partial_dv),
1036 make_ref(aba_partial_dtau));
1037 21 }
1038
1039 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
1040 ✗ void computeABADerivatives(
1041 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
1042 DataTpl<Scalar, Options, JointCollectionTpl> & data,
1043 const container::aligned_vector<ForceTpl<Scalar, Options>> & fext)
1044 {
1045 ✗ impl::computeABADerivatives(
1046 ✗ model, data, fext, make_ref(data.ddq_dq), make_ref(data.ddq_dv), make_ref(data.Minv));
1047 }
1048
1049 } // namespace pinocchio
1050
1051 #endif // ifndef __pinocchio_algorithm_aba_derivatives_hxx__
1052