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