Directory: | ./ |
---|---|
File: | include/pinocchio/algorithm/crba.hxx |
Date: | 2024-08-27 18:20:05 |
Exec | Total | Coverage | |
---|---|---|---|
Lines: | 40 | 83 | 48.2% |
Branches: | 59 | 253 | 23.3% |
Line | Branch | Exec | Source |
---|---|---|---|
1 | // | ||
2 | // Copyright (c) 2015-2021 CNRS INRIA | ||
3 | // | ||
4 | |||
5 | #ifndef __pinocchio_crba_hxx__ | ||
6 | #define __pinocchio_crba_hxx__ | ||
7 | |||
8 | #include "pinocchio/multibody/visitor.hpp" | ||
9 | #include "pinocchio/spatial/act-on-set.hpp" | ||
10 | #include "pinocchio/algorithm/kinematics.hpp" | ||
11 | #include "pinocchio/algorithm/check.hpp" | ||
12 | |||
13 | /// @cond DEV | ||
14 | |||
15 | namespace pinocchio | ||
16 | { | ||
17 | namespace impl | ||
18 | { | ||
19 | template< | ||
20 | typename Scalar, | ||
21 | int Options, | ||
22 | template<typename, int> | ||
23 | class JointCollectionTpl, | ||
24 | typename ConfigVectorType> | ||
25 | struct CrbaWorldConventionForwardStep | ||
26 | : public fusion::JointUnaryVisitorBase< | ||
27 | CrbaWorldConventionForwardStep<Scalar, Options, JointCollectionTpl, ConfigVectorType>> | ||
28 | { | ||
29 | typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model; | ||
30 | typedef DataTpl<Scalar, Options, JointCollectionTpl> Data; | ||
31 | |||
32 | typedef boost::fusion::vector<const Model &, Data &, const ConfigVectorType &> ArgsType; | ||
33 | |||
34 | template<typename JointModel> | ||
35 | 216 | static void algo( | |
36 | const JointModelBase<JointModel> & jmodel, | ||
37 | JointDataBase<typename JointModel::JointDataDerived> & jdata, | ||
38 | const Model & model, | ||
39 | Data & data, | ||
40 | const Eigen::MatrixBase<ConfigVectorType> & q) | ||
41 | { | ||
42 | typedef typename Model::JointIndex JointIndex; | ||
43 | |||
44 |
1/2✓ Branch 1 taken 108 times.
✗ Branch 2 not taken.
|
216 | const JointIndex & i = jmodel.id(); |
45 |
1/2✓ Branch 3 taken 108 times.
✗ Branch 4 not taken.
|
216 | jmodel.calc(jdata.derived(), q.derived()); |
46 | |||
47 |
6/10✓ Branch 1 taken 108 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 104 times.
✓ Branch 5 taken 4 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 104 times.
✓ Branch 9 taken 4 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 104 times.
✗ Branch 13 not taken.
|
216 | data.liMi[i] = model.jointPlacements[i] * jdata.M(); |
48 | |||
49 | 216 | const JointIndex & parent = model.parents[i]; | |
50 |
2/2✓ Branch 0 taken 104 times.
✓ Branch 1 taken 4 times.
|
216 | if (parent > 0) |
51 |
2/4✓ Branch 3 taken 104 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 104 times.
✗ Branch 8 not taken.
|
208 | data.oMi[i] = data.oMi[parent] * data.liMi[i]; |
52 | else | ||
53 |
1/2✓ Branch 3 taken 4 times.
✗ Branch 4 not taken.
|
8 | data.oMi[i] = data.liMi[i]; |
54 | |||
55 |
4/8✓ Branch 2 taken 108 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 108 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 108 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 108 times.
✗ Branch 12 not taken.
|
216 | jmodel.jointCols(data.J) = data.oMi[i].act(jdata.S()); |
56 | |||
57 |
2/4✓ Branch 3 taken 108 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 108 times.
✗ Branch 8 not taken.
|
216 | data.oYcrb[i] = data.oMi[i].act(model.inertias[i]); |
58 | } | ||
59 | }; | ||
60 | |||
61 | template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl> | ||
62 | struct CrbaWorldConventionBackwardStep | ||
63 | : public fusion::JointUnaryVisitorBase< | ||
64 | CrbaWorldConventionBackwardStep<Scalar, Options, JointCollectionTpl>> | ||
65 | { | ||
66 | typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model; | ||
67 | typedef DataTpl<Scalar, Options, JointCollectionTpl> Data; | ||
68 | |||
69 | typedef boost::fusion::vector<const Model &, Data &> ArgsType; | ||
70 | |||
71 | template<typename JointModel> | ||
72 | 216 | static void algo(const JointModelBase<JointModel> & jmodel, const Model & model, Data & data) | |
73 | { | ||
74 | typedef typename Model::JointIndex JointIndex; | ||
75 | typedef | ||
76 | typename SizeDepType<JointModel::NV>::template ColsReturn<typename Data::Matrix6x>::Type | ||
77 | ColsBlock; | ||
78 |
1/2✓ Branch 1 taken 108 times.
✗ Branch 2 not taken.
|
216 | const JointIndex & i = jmodel.id(); |
79 | |||
80 | // Centroidal momentum map | ||
81 |
1/2✓ Branch 1 taken 108 times.
✗ Branch 2 not taken.
|
216 | ColsBlock Ag_cols = jmodel.jointCols(data.Ag); |
82 |
1/2✓ Branch 1 taken 108 times.
✗ Branch 2 not taken.
|
216 | ColsBlock J_cols = jmodel.jointCols(data.J); |
83 |
1/2✓ Branch 2 taken 108 times.
✗ Branch 3 not taken.
|
216 | motionSet::inertiaAction(data.oYcrb[i], J_cols, Ag_cols); |
84 | |||
85 | // Joint Space Inertia Matrix | ||
86 |
6/12✓ Branch 2 taken 108 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 108 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 108 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 108 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 108 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 108 times.
✗ Branch 18 not taken.
|
216 | data.M.block(jmodel.idx_v(), jmodel.idx_v(), jmodel.nv(), data.nvSubtree[i]).noalias() = |
87 |
4/8✓ Branch 2 taken 108 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 108 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 108 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 108 times.
✗ Branch 12 not taken.
|
216 | J_cols.transpose() * data.Ag.middleCols(jmodel.idx_v(), data.nvSubtree[i]); |
88 | |||
89 | 216 | const JointIndex & parent = model.parents[i]; | |
90 |
1/2✓ Branch 3 taken 108 times.
✗ Branch 4 not taken.
|
216 | data.oYcrb[parent] += data.oYcrb[i]; |
91 | } | ||
92 | }; | ||
93 | |||
94 | template< | ||
95 | typename Scalar, | ||
96 | int Options, | ||
97 | template<typename, int> | ||
98 | class JointCollectionTpl, | ||
99 | typename ConfigVectorType> | ||
100 | struct CrbaLocalConventionForwardStep | ||
101 | : public fusion::JointUnaryVisitorBase< | ||
102 | CrbaLocalConventionForwardStep<Scalar, Options, JointCollectionTpl, ConfigVectorType>> | ||
103 | { | ||
104 | typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model; | ||
105 | typedef DataTpl<Scalar, Options, JointCollectionTpl> Data; | ||
106 | |||
107 | typedef boost::fusion::vector<const Model &, Data &, const ConfigVectorType &> ArgsType; | ||
108 | |||
109 | template<typename JointModel> | ||
110 | ✗ | static void algo( | |
111 | const JointModelBase<JointModel> & jmodel, | ||
112 | JointDataBase<typename JointModel::JointDataDerived> & jdata, | ||
113 | const Model & model, | ||
114 | Data & data, | ||
115 | const Eigen::MatrixBase<ConfigVectorType> & q) | ||
116 | { | ||
117 | typedef typename Model::JointIndex JointIndex; | ||
118 | |||
119 | ✗ | const JointIndex & i = jmodel.id(); | |
120 | ✗ | jmodel.calc(jdata.derived(), q.derived()); | |
121 | |||
122 | ✗ | data.liMi[i] = model.jointPlacements[i] * jdata.M(); | |
123 | ✗ | data.Ycrb[i] = model.inertias[i]; | |
124 | } | ||
125 | }; | ||
126 | |||
127 | template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl> | ||
128 | struct CrbaLocalConventionBackwardStep | ||
129 | : public fusion::JointUnaryVisitorBase< | ||
130 | CrbaLocalConventionBackwardStep<Scalar, Options, JointCollectionTpl>> | ||
131 | { | ||
132 | typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model; | ||
133 | typedef DataTpl<Scalar, Options, JointCollectionTpl> Data; | ||
134 | |||
135 | typedef boost::fusion::vector<const Model &, Data &> ArgsType; | ||
136 | |||
137 | template<typename JointModel> | ||
138 | ✗ | static void algo( | |
139 | const JointModelBase<JointModel> & jmodel, | ||
140 | JointDataBase<typename JointModel::JointDataDerived> & jdata, | ||
141 | const Model & model, | ||
142 | Data & data) | ||
143 | { | ||
144 | /* | ||
145 | * F[1:6,i] = Y*S | ||
146 | * M[i,SUBTREE] = S'*F[1:6,SUBTREE] | ||
147 | * if li>0 | ||
148 | * Yli += liXi Yi | ||
149 | * F[1:6,SUBTREE] = liXi F[1:6,SUBTREE] | ||
150 | */ | ||
151 | |||
152 | typedef typename Model::JointIndex JointIndex; | ||
153 | typedef typename Data::Matrix6x::ColsBlockXpr Block; | ||
154 | ✗ | const JointIndex & i = jmodel.id(); | |
155 | |||
156 | /* F[1:6,i] = Y*S */ | ||
157 | // data.Fcrb[i].block<6,JointModel::NV>(0,jmodel.idx_v()) = data.Ycrb[i] * jdata.S(); | ||
158 | ✗ | jmodel.jointCols(data.Fcrb[i]) = data.Ycrb[i] * jdata.S(); | |
159 | |||
160 | /* M[i,SUBTREE] = S'*F[1:6,SUBTREE] */ | ||
161 | ✗ | data.M.block(jmodel.idx_v(), jmodel.idx_v(), jmodel.nv(), data.nvSubtree[i]).noalias() = | |
162 | ✗ | jdata.S().transpose() * data.Fcrb[i].middleCols(jmodel.idx_v(), data.nvSubtree[i]); | |
163 | |||
164 | ✗ | const JointIndex & parent = model.parents[i]; | |
165 | ✗ | if (parent > 0) | |
166 | { | ||
167 | /* Yli += liXi Yi */ | ||
168 | ✗ | data.Ycrb[parent] += data.liMi[i].act(data.Ycrb[i]); | |
169 | |||
170 | /* F[1:6,SUBTREE] = liXi F[1:6,SUBTREE] */ | ||
171 | ✗ | Block jF = data.Fcrb[parent].middleCols(jmodel.idx_v(), data.nvSubtree[i]); | |
172 | ✗ | Block iF = data.Fcrb[i].middleCols(jmodel.idx_v(), data.nvSubtree[i]); | |
173 | ✗ | forceSet::se3Action(data.liMi[i], iF, jF); | |
174 | } | ||
175 | } | ||
176 | }; | ||
177 | |||
178 | template< | ||
179 | typename Scalar, | ||
180 | int Options, | ||
181 | template<typename, int> | ||
182 | class JointCollectionTpl, | ||
183 | typename ConfigVectorType> | ||
184 | ✗ | const typename DataTpl<Scalar, Options, JointCollectionTpl>::MatrixXs & crbaLocalConvention( | |
185 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
186 | DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
187 | const Eigen::MatrixBase<ConfigVectorType> & q) | ||
188 | { | ||
189 | ✗ | assert(model.check(data) && "data is not consistent with model."); | |
190 | ✗ | PINOCCHIO_CHECK_ARGUMENT_SIZE( | |
191 | q.size(), model.nq, "The configuration vector is not of right size"); | ||
192 | |||
193 | typedef typename ModelTpl<Scalar, Options, JointCollectionTpl>::JointIndex JointIndex; | ||
194 | |||
195 | typedef CrbaLocalConventionForwardStep<Scalar, Options, JointCollectionTpl, ConfigVectorType> | ||
196 | Pass1; | ||
197 | ✗ | for (JointIndex i = 1; i < (JointIndex)(model.njoints); ++i) | |
198 | { | ||
199 | ✗ | Pass1::run( | |
200 | ✗ | model.joints[i], data.joints[i], typename Pass1::ArgsType(model, data, q.derived())); | |
201 | } | ||
202 | |||
203 | typedef CrbaLocalConventionBackwardStep<Scalar, Options, JointCollectionTpl> Pass2; | ||
204 | ✗ | for (JointIndex i = (JointIndex)(model.njoints - 1); i > 0; --i) | |
205 | { | ||
206 | ✗ | Pass2::run(model.joints[i], data.joints[i], typename Pass2::ArgsType(model, data)); | |
207 | } | ||
208 | |||
209 | // Add the armature contribution | ||
210 | ✗ | data.M.diagonal() += model.armature; | |
211 | |||
212 | ✗ | return data.M; | |
213 | } | ||
214 | |||
215 | template< | ||
216 | typename Scalar, | ||
217 | int Options, | ||
218 | template<typename, int> | ||
219 | class JointCollectionTpl, | ||
220 | typename ConfigVectorType> | ||
221 | 4 | const typename DataTpl<Scalar, Options, JointCollectionTpl>::MatrixXs & crbaWorldConvention( | |
222 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
223 | DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
224 | const Eigen::MatrixBase<ConfigVectorType> & q) | ||
225 | { | ||
226 |
2/4✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 4 times.
✗ Branch 4 not taken.
|
4 | assert(model.check(data) && "data is not consistent with model."); |
227 |
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.
|
4 | PINOCCHIO_CHECK_ARGUMENT_SIZE( |
228 | q.size(), model.nq, "The configuration vector is not of right size"); | ||
229 | |||
230 | typedef typename ModelTpl<Scalar, Options, JointCollectionTpl>::JointIndex JointIndex; | ||
231 | |||
232 |
1/2✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
|
4 | data.oYcrb[0].setZero(); |
233 | typedef CrbaWorldConventionForwardStep<Scalar, Options, JointCollectionTpl, ConfigVectorType> | ||
234 | Pass1; | ||
235 |
2/2✓ Branch 0 taken 108 times.
✓ Branch 1 taken 4 times.
|
112 | for (JointIndex i = 1; i < (JointIndex)(model.njoints); ++i) |
236 | { | ||
237 |
1/2✓ Branch 1 taken 108 times.
✗ Branch 2 not taken.
|
108 | Pass1::run( |
238 |
1/2✓ Branch 2 taken 108 times.
✗ Branch 3 not taken.
|
216 | model.joints[i], data.joints[i], typename Pass1::ArgsType(model, data, q.derived())); |
239 | } | ||
240 | |||
241 | typedef CrbaWorldConventionBackwardStep<Scalar, Options, JointCollectionTpl> Pass2; | ||
242 |
2/2✓ Branch 0 taken 108 times.
✓ Branch 1 taken 4 times.
|
112 | for (JointIndex i = (JointIndex)(model.njoints - 1); i > 0; --i) |
243 | { | ||
244 |
2/4✓ Branch 1 taken 108 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 108 times.
✗ Branch 6 not taken.
|
108 | Pass2::run(model.joints[i], typename Pass2::ArgsType(model, data)); |
245 | } | ||
246 | |||
247 | // Add the armature contribution | ||
248 |
2/4✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
|
4 | data.M.diagonal() += model.armature; |
249 | |||
250 | // Retrieve the Centroidal Momemtum map | ||
251 | typedef DataTpl<Scalar, Options, JointCollectionTpl> Data; | ||
252 | typedef typename Data::Force Force; | ||
253 | typedef Eigen::Block<typename Data::Matrix6x, 3, -1> Block3x; | ||
254 | |||
255 | 4 | data.mass[0] = data.oYcrb[0].mass(); | |
256 |
1/2✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
|
4 | data.com[0] = data.oYcrb[0].lever(); |
257 | |||
258 |
1/2✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
|
4 | const Block3x Ag_lin = data.Ag.template middleRows<3>(Force::LINEAR); |
259 |
1/2✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
|
4 | Block3x Ag_ang = data.Ag.template middleRows<3>(Force::ANGULAR); |
260 |
2/2✓ Branch 0 taken 128 times.
✓ Branch 1 taken 4 times.
|
132 | for (long i = 0; i < model.nv; ++i) |
261 |
4/8✓ Branch 1 taken 128 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 128 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 128 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 128 times.
✗ Branch 12 not taken.
|
128 | Ag_ang.col(i) += Ag_lin.col(i).cross(data.com[0]); |
262 | |||
263 | 4 | return data.M; | |
264 | } | ||
265 | } // namespace impl | ||
266 | // --- CHECKER --------------------------------------------------------------- | ||
267 | // --- CHECKER --------------------------------------------------------------- | ||
268 | // --- CHECKER --------------------------------------------------------------- | ||
269 | |||
270 | namespace internal | ||
271 | { | ||
272 | template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl> | ||
273 | ✗ | inline bool isDescendant( | |
274 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
275 | const typename ModelTpl<Scalar, Options, JointCollectionTpl>::JointIndex j, | ||
276 | const typename ModelTpl<Scalar, Options, JointCollectionTpl>::JointIndex root) | ||
277 | { | ||
278 | typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model; | ||
279 | typedef typename Model::JointIndex JointIndex; | ||
280 | |||
281 | ✗ | if (j >= (JointIndex)model.njoints) | |
282 | ✗ | return false; | |
283 | ✗ | if (j == 0) | |
284 | ✗ | return root == 0; | |
285 | ✗ | return (j == root) || isDescendant(model, model.parents[j], root); | |
286 | } | ||
287 | } // namespace internal | ||
288 | |||
289 | template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl> | ||
290 | inline bool | ||
291 | ✗ | CRBAChecker::checkModel_impl(const ModelTpl<Scalar, Options, JointCollectionTpl> & model) const | |
292 | { | ||
293 | typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model; | ||
294 | typedef typename Model::JointIndex JointIndex; | ||
295 | |||
296 | // For CRBA, the tree must be "compact", i.e. all descendants of a node i are stored | ||
297 | // immediately after i in the "parents" map, i.e. forall joint i, the interval i+1..n-1 | ||
298 | // can be separated in two intervals [i+1..k] and [k+1..n-1], where any [i+1..k] is a descendant | ||
299 | // of i and none of [k+1..n-1] is a descendant of i. | ||
300 | ✗ | for (JointIndex i = 1; i < (JointIndex)(model.njoints - 1); | |
301 | ++i) // no need to check joints 0 and N-1 | ||
302 | { | ||
303 | ✗ | JointIndex k = i + 1; | |
304 | ✗ | while (internal::isDescendant(model, k, i)) | |
305 | ✗ | ++k; | |
306 | ✗ | for (; int(k) < model.njoints; ++k) | |
307 | ✗ | if (internal::isDescendant(model, k, i)) | |
308 | ✗ | return false; | |
309 | } | ||
310 | ✗ | return true; | |
311 | } | ||
312 | |||
313 | template< | ||
314 | typename Scalar, | ||
315 | int Options, | ||
316 | template<typename, int> | ||
317 | class JointCollectionTpl, | ||
318 | typename ConfigVectorType> | ||
319 | 4 | const typename DataTpl<Scalar, Options, JointCollectionTpl>::MatrixXs & crba( | |
320 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
321 | DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
322 | const Eigen::MatrixBase<ConfigVectorType> & q, | ||
323 | const Convention convention) | ||
324 | { | ||
325 |
1/3✗ Branch 0 not taken.
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
|
4 | switch (convention) |
326 | { | ||
327 | ✗ | case Convention::LOCAL: | |
328 | ✗ | return ::pinocchio::impl::crbaLocalConvention(model, data, make_const_ref(q)); | |
329 | 4 | case Convention::WORLD: | |
330 |
1/2✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
|
4 | return ::pinocchio::impl::crbaWorldConvention(model, data, make_const_ref(q)); |
331 | } | ||
332 | } | ||
333 | |||
334 | } // namespace pinocchio | ||
335 | |||
336 | /// @endcond | ||
337 | |||
338 | #endif // ifndef __pinocchio_crba_hxx__ | ||
339 |