Directory: | ./ |
---|---|
File: | include/pinocchio/algorithm/center-of-mass.hxx |
Date: | 2024-08-27 18:20:05 |
Exec | Total | Coverage | |
---|---|---|---|
Lines: | 197 | 209 | 94.3% |
Branches: | 173 | 406 | 42.6% |
Line | Branch | Exec | Source |
---|---|---|---|
1 | // | ||
2 | // Copyright (c) 2015-2020 CNRS INRIA | ||
3 | // | ||
4 | |||
5 | #ifndef __pinocchio_algorithm_center_of_mass_hxx__ | ||
6 | #define __pinocchio_algorithm_center_of_mass_hxx__ | ||
7 | |||
8 | #include "pinocchio/algorithm/check.hpp" | ||
9 | #include "pinocchio/multibody/visitor.hpp" | ||
10 | #include "pinocchio/algorithm/kinematics.hpp" | ||
11 | #include "pinocchio/algorithm/jacobian.hpp" | ||
12 | |||
13 | /// @cond DEV | ||
14 | |||
15 | namespace pinocchio | ||
16 | { | ||
17 | template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl> | ||
18 | 5 | Scalar computeTotalMass(const ModelTpl<Scalar, Options, JointCollectionTpl> & model) | |
19 | { | ||
20 | 5 | Scalar m = Scalar(0); | |
21 |
2/2✓ Branch 0 taken 124 times.
✓ Branch 1 taken 5 times.
|
129 | for (JointIndex i = 1; i < (JointIndex)(model.njoints); ++i) |
22 | { | ||
23 | 124 | m += model.inertias[i].mass(); | |
24 | } | ||
25 | 5 | return m; | |
26 | } | ||
27 | |||
28 | template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl> | ||
29 | 1 | Scalar computeTotalMass( | |
30 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
31 | DataTpl<Scalar, Options, JointCollectionTpl> & data) | ||
32 | { | ||
33 | 1 | data.mass[0] = computeTotalMass(model); | |
34 | 1 | return data.mass[0]; | |
35 | } | ||
36 | |||
37 | template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl> | ||
38 | 1 | void computeSubtreeMasses( | |
39 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
40 | DataTpl<Scalar, Options, JointCollectionTpl> & data) | ||
41 | { | ||
42 | 1 | data.mass[0] = Scalar(0); | |
43 | |||
44 | // Forward Step | ||
45 |
2/2✓ Branch 0 taken 27 times.
✓ Branch 1 taken 1 times.
|
28 | for (JointIndex i = 1; i < (JointIndex)(model.njoints); ++i) |
46 | { | ||
47 | 27 | data.mass[i] = model.inertias[i].mass(); | |
48 | } | ||
49 | |||
50 | // Backward Step | ||
51 |
2/2✓ Branch 0 taken 27 times.
✓ Branch 1 taken 1 times.
|
28 | for (JointIndex i = (JointIndex)(model.njoints - 1); i > 0; --i) |
52 | { | ||
53 | 27 | const JointIndex & parent = model.parents[i]; | |
54 | 27 | data.mass[parent] += data.mass[i]; | |
55 | } | ||
56 | 1 | } | |
57 | namespace impl | ||
58 | { | ||
59 | template< | ||
60 | typename Scalar, | ||
61 | int Options, | ||
62 | template<typename, int> | ||
63 | class JointCollectionTpl, | ||
64 | typename ConfigVectorType> | ||
65 | 899 | const typename DataTpl<Scalar, Options, JointCollectionTpl>::Vector3 & centerOfMass( | |
66 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
67 | DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
68 | const Eigen::MatrixBase<ConfigVectorType> & q, | ||
69 | const bool computeSubtreeComs) | ||
70 | { | ||
71 | 899 | ::pinocchio::impl::forwardKinematics(model, data, q.derived()); | |
72 | |||
73 | 899 | centerOfMass(model, data, POSITION, computeSubtreeComs); | |
74 | 899 | return data.com[0]; | |
75 | } | ||
76 | |||
77 | template< | ||
78 | typename Scalar, | ||
79 | int Options, | ||
80 | template<typename, int> | ||
81 | class JointCollectionTpl, | ||
82 | typename ConfigVectorType, | ||
83 | typename TangentVectorType> | ||
84 | 44 | const typename DataTpl<Scalar, Options, JointCollectionTpl>::Vector3 & centerOfMass( | |
85 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
86 | DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
87 | const Eigen::MatrixBase<ConfigVectorType> & q, | ||
88 | const Eigen::MatrixBase<TangentVectorType> & v, | ||
89 | const bool computeSubtreeComs) | ||
90 | { | ||
91 | 44 | ::pinocchio::impl::forwardKinematics(model, data, q.derived(), v.derived()); | |
92 | |||
93 | 44 | centerOfMass(model, data, VELOCITY, computeSubtreeComs); | |
94 | 44 | return data.com[0]; | |
95 | } | ||
96 | |||
97 | template< | ||
98 | typename Scalar, | ||
99 | int Options, | ||
100 | template<typename, int> | ||
101 | class JointCollectionTpl, | ||
102 | typename ConfigVectorType, | ||
103 | typename TangentVectorType1, | ||
104 | typename TangentVectorType2> | ||
105 | 3 | const typename DataTpl<Scalar, Options, JointCollectionTpl>::Vector3 & centerOfMass( | |
106 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
107 | DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
108 | const Eigen::MatrixBase<ConfigVectorType> & q, | ||
109 | const Eigen::MatrixBase<TangentVectorType1> & v, | ||
110 | const Eigen::MatrixBase<TangentVectorType2> & a, | ||
111 | const bool computeSubtreeComs) | ||
112 | { | ||
113 | 3 | ::pinocchio::impl::forwardKinematics(model, data, q, v, a); | |
114 | |||
115 | 3 | centerOfMass(model, data, ACCELERATION, computeSubtreeComs); | |
116 | 3 | return data.com[0]; | |
117 | } | ||
118 | } // namespace impl | ||
119 | template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl> | ||
120 | 946 | const typename DataTpl<Scalar, Options, JointCollectionTpl>::Vector3 & centerOfMass( | |
121 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
122 | DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
123 | KinematicLevel kinematic_level, | ||
124 | const bool computeSubtreeComs) | ||
125 | { | ||
126 |
1/2✓ Branch 1 taken 946 times.
✗ Branch 2 not taken.
|
946 | assert(model.check(data) && "data is not consistent with model."); |
127 |
2/6✓ Branch 0 taken 946 times.
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✓ Branch 3 taken 946 times.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
|
946 | PINOCCHIO_CHECK_INPUT_ARGUMENT(kinematic_level >= 0 && kinematic_level <= 2); |
128 | |||
129 | typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model; | ||
130 | typedef DataTpl<Scalar, Options, JointCollectionTpl> Data; | ||
131 | |||
132 | typedef typename Model::JointIndex JointIndex; | ||
133 | |||
134 | typedef typename Data::SE3 SE3; | ||
135 | typedef typename Data::Motion Motion; | ||
136 | typedef typename Data::Inertia Inertia; | ||
137 | |||
138 | 946 | const bool do_position = (kinematic_level >= POSITION); | |
139 | 946 | const bool do_velocity = (kinematic_level >= VELOCITY); | |
140 | 946 | const bool do_acceleration = (kinematic_level >= ACCELERATION); | |
141 | |||
142 | 946 | data.mass[0] = 0; | |
143 |
1/2✓ Branch 0 taken 946 times.
✗ Branch 1 not taken.
|
946 | if (do_position) |
144 | 946 | data.com[0].setZero(); | |
145 |
2/2✓ Branch 0 taken 47 times.
✓ Branch 1 taken 899 times.
|
946 | if (do_velocity) |
146 | 47 | data.vcom[0].setZero(); | |
147 |
2/2✓ Branch 0 taken 3 times.
✓ Branch 1 taken 943 times.
|
946 | if (do_acceleration) |
148 | 3 | data.acom[0].setZero(); | |
149 | |||
150 | // Forward Step | ||
151 |
2/2✓ Branch 0 taken 25628 times.
✓ Branch 1 taken 946 times.
|
26574 | for (JointIndex i = 1; i < (JointIndex)(model.njoints); ++i) |
152 | { | ||
153 | 25628 | const typename Inertia::Scalar & mass = model.inertias[i].mass(); | |
154 | 25628 | const typename SE3::Vector3 & lever = model.inertias[i].lever(); | |
155 | |||
156 | 25628 | const Motion & v = data.v[i]; | |
157 | 25628 | const Motion & a = data.a[i]; | |
158 | |||
159 | 25628 | data.mass[i] = mass; | |
160 | |||
161 |
1/2✓ Branch 0 taken 25628 times.
✗ Branch 1 not taken.
|
25628 | if (do_position) |
162 |
3/6✓ Branch 1 taken 25628 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 25628 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 25628 times.
✗ Branch 9 not taken.
|
25628 | data.com[i].noalias() = mass * lever; |
163 | |||
164 |
2/2✓ Branch 0 taken 1343 times.
✓ Branch 1 taken 24285 times.
|
25628 | if (do_velocity) |
165 |
7/14✓ Branch 1 taken 1343 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1343 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1343 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1343 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1343 times.
✗ Branch 14 not taken.
✓ Branch 17 taken 1343 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1343 times.
✗ Branch 21 not taken.
|
1343 | data.vcom[i].noalias() = mass * (v.angular().cross(lever) + v.linear()); |
166 | |||
167 |
2/2✓ Branch 0 taken 82 times.
✓ Branch 1 taken 25546 times.
|
25628 | if (do_acceleration) |
168 |
7/14✓ Branch 1 taken 82 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 82 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 82 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 82 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 82 times.
✗ Branch 14 not taken.
✓ Branch 17 taken 82 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 82 times.
✗ Branch 21 not taken.
|
164 | data.acom[i].noalias() = |
169 |
2/4✓ Branch 1 taken 82 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 82 times.
✗ Branch 5 not taken.
|
82 | mass * (a.angular().cross(lever) + a.linear()) |
170 |
1/2✓ Branch 1 taken 82 times.
✗ Branch 2 not taken.
|
164 | + v.angular().cross( |
171 | 82 | data.vcom[i]); // take into accound the coriolis part of the acceleration | |
172 | } | ||
173 | |||
174 | // Backward Step | ||
175 |
2/2✓ Branch 0 taken 25628 times.
✓ Branch 1 taken 946 times.
|
26574 | for (JointIndex i = (JointIndex)(model.njoints - 1); i > 0; --i) |
176 | { | ||
177 | 25628 | const JointIndex & parent = model.parents[i]; | |
178 | 25628 | const SE3 & liMi = data.liMi[i]; | |
179 | |||
180 | 25628 | data.mass[parent] += data.mass[i]; | |
181 | |||
182 |
1/2✓ Branch 0 taken 25628 times.
✗ Branch 1 not taken.
|
25628 | if (do_position) |
183 |
4/8✓ Branch 5 taken 25628 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 25628 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 25628 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 25628 times.
✗ Branch 16 not taken.
|
25628 | data.com[parent] += (liMi.rotation() * data.com[i] + data.mass[i] * liMi.translation()); |
184 | |||
185 |
2/2✓ Branch 0 taken 1343 times.
✓ Branch 1 taken 24285 times.
|
25628 | if (do_velocity) |
186 |
1/2✓ Branch 5 taken 1343 times.
✗ Branch 6 not taken.
|
1343 | data.vcom[parent] += liMi.rotation() * data.vcom[i]; |
187 | |||
188 |
2/2✓ Branch 0 taken 82 times.
✓ Branch 1 taken 25546 times.
|
25628 | if (do_acceleration) |
189 |
1/2✓ Branch 5 taken 82 times.
✗ Branch 6 not taken.
|
82 | data.acom[parent] += liMi.rotation() * data.acom[i]; |
190 | |||
191 |
2/2✓ Branch 0 taken 25600 times.
✓ Branch 1 taken 28 times.
|
25628 | if (computeSubtreeComs) |
192 | { | ||
193 |
1/2✓ Branch 0 taken 25600 times.
✗ Branch 1 not taken.
|
25600 | if (do_position) |
194 | 25600 | data.com[i] /= data.mass[i]; | |
195 |
2/2✓ Branch 0 taken 1315 times.
✓ Branch 1 taken 24285 times.
|
25600 | if (do_velocity) |
196 | 1315 | data.vcom[i] /= data.mass[i]; | |
197 |
2/2✓ Branch 0 taken 82 times.
✓ Branch 1 taken 25518 times.
|
25600 | if (do_acceleration) |
198 | 82 | data.acom[i] /= data.mass[i]; | |
199 | } | ||
200 | } | ||
201 | |||
202 |
1/2✓ Branch 0 taken 946 times.
✗ Branch 1 not taken.
|
946 | if (do_position) |
203 | 946 | data.com[0] /= data.mass[0]; | |
204 |
2/2✓ Branch 0 taken 47 times.
✓ Branch 1 taken 899 times.
|
946 | if (do_velocity) |
205 | 47 | data.vcom[0] /= data.mass[0]; | |
206 |
2/2✓ Branch 0 taken 3 times.
✓ Branch 1 taken 943 times.
|
946 | if (do_acceleration) |
207 | 3 | data.acom[0] /= data.mass[0]; | |
208 | |||
209 | 946 | return data.com[0]; | |
210 | } | ||
211 | |||
212 | template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl> | ||
213 | 1 | const typename DataTpl<Scalar, Options, JointCollectionTpl>::Vector3 & getComFromCrba( | |
214 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
215 | DataTpl<Scalar, Options, JointCollectionTpl> & data) | ||
216 | { | ||
217 | PINOCCHIO_UNUSED_VARIABLE(model); | ||
218 | |||
219 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | assert(model.check(data) && "data is not consistent with model."); |
220 | 1 | return data.com[0] = data.liMi[1].act(data.oYcrb[1].lever()); | |
221 | } | ||
222 | |||
223 | /* --- JACOBIAN ---------------------------------------------------------- */ | ||
224 | /* --- JACOBIAN ---------------------------------------------------------- */ | ||
225 | /* --- JACOBIAN ---------------------------------------------------------- */ | ||
226 | |||
227 | template< | ||
228 | typename Scalar, | ||
229 | int Options, | ||
230 | template<typename, int> | ||
231 | class JointCollectionTpl, | ||
232 | typename Matrix3x> | ||
233 | struct JacobianCenterOfMassBackwardStep | ||
234 | : public fusion::JointUnaryVisitorBase< | ||
235 | JacobianCenterOfMassBackwardStep<Scalar, Options, JointCollectionTpl, Matrix3x>> | ||
236 | { | ||
237 | typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model; | ||
238 | typedef DataTpl<Scalar, Options, JointCollectionTpl> Data; | ||
239 | |||
240 | typedef boost::fusion:: | ||
241 | vector<const Model &, Data &, const Eigen::MatrixBase<Matrix3x> &, const bool &> | ||
242 | ArgsType; | ||
243 | |||
244 | template<typename JointModel> | ||
245 | 654 | static void algo( | |
246 | const JointModelBase<JointModel> & jmodel, | ||
247 | JointDataBase<typename JointModel::JointDataDerived> & jdata, | ||
248 | const Model & model, | ||
249 | Data & data, | ||
250 | const Eigen::MatrixBase<Matrix3x> & Jcom, | ||
251 | const bool & computeSubtreeComs) | ||
252 | { | ||
253 |
1/2✓ Branch 1 taken 327 times.
✗ Branch 2 not taken.
|
654 | const JointIndex & i = (JointIndex)jmodel.id(); |
254 | 654 | const JointIndex & parent = model.parents[i]; | |
255 | |||
256 |
1/2✓ Branch 3 taken 327 times.
✗ Branch 4 not taken.
|
654 | data.com[parent] += data.com[i]; |
257 | 654 | data.mass[parent] += data.mass[i]; | |
258 | |||
259 | typedef typename Data::Matrix6x Matrix6x; | ||
260 | typedef typename SizeDepType<JointModel::NV>::template ColsReturn<Matrix6x>::Type ColBlock; | ||
261 | |||
262 | 654 | Matrix3x & Jcom_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix3x, Jcom); | |
263 | |||
264 |
1/2✓ Branch 1 taken 327 times.
✗ Branch 2 not taken.
|
654 | ColBlock Jcols = jmodel.jointCols(data.J); |
265 |
3/6✓ Branch 2 taken 327 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 327 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 327 times.
✗ Branch 9 not taken.
|
654 | Jcols = data.oMi[i].act(jdata.S()); |
266 | |||
267 |
3/4✓ Branch 1 taken 694 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 367 times.
✓ Branch 4 taken 327 times.
|
1388 | for (Eigen::DenseIndex col_id = 0; col_id < jmodel.nv(); ++col_id) |
268 | { | ||
269 |
6/12✓ Branch 1 taken 367 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 367 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 367 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 367 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 367 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 367 times.
✗ Branch 17 not taken.
|
1468 | jmodel.jointCols(Jcom_).col(col_id) = |
270 |
2/4✓ Branch 1 taken 367 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 367 times.
✗ Branch 5 not taken.
|
734 | data.mass[i] * Jcols.col(col_id).template segment<3>(Motion::LINEAR) |
271 |
2/4✓ Branch 2 taken 367 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 367 times.
✗ Branch 6 not taken.
|
1468 | - data.com[i].cross(Jcols.col(col_id).template segment<3>(Motion::ANGULAR)); |
272 | } | ||
273 | |||
274 |
1/2✓ Branch 0 taken 327 times.
✗ Branch 1 not taken.
|
654 | if (computeSubtreeComs) |
275 |
1/2✓ Branch 3 taken 327 times.
✗ Branch 4 not taken.
|
654 | data.com[i] /= data.mass[i]; |
276 | } | ||
277 | }; | ||
278 | namespace impl | ||
279 | { | ||
280 | template< | ||
281 | typename Scalar, | ||
282 | int Options, | ||
283 | template<typename, int> | ||
284 | class JointCollectionTpl, | ||
285 | typename ConfigVectorType> | ||
286 | 5 | const typename DataTpl<Scalar, Options, JointCollectionTpl>::Matrix3x & jacobianCenterOfMass( | |
287 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
288 | DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
289 | const Eigen::MatrixBase<ConfigVectorType> & q, | ||
290 | const bool computeSubtreeComs) | ||
291 | { | ||
292 | 5 | ::pinocchio::impl::forwardKinematics(model, data, q); | |
293 | 5 | return jacobianCenterOfMass(model, data, computeSubtreeComs); | |
294 | } | ||
295 | } // namespace impl | ||
296 | template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl> | ||
297 | 5 | const typename DataTpl<Scalar, Options, JointCollectionTpl>::Matrix3x & jacobianCenterOfMass( | |
298 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
299 | DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
300 | const bool computeSubtreeComs) | ||
301 | { | ||
302 |
1/2✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
|
5 | assert(model.check(data) && "data is not consistent with model."); |
303 | |||
304 | typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model; | ||
305 | typedef DataTpl<Scalar, Options, JointCollectionTpl> Data; | ||
306 | |||
307 | typedef typename Model::JointIndex JointIndex; | ||
308 | |||
309 | typedef typename Data::Matrix3x Matrix3x; | ||
310 | typedef typename Data::SE3 SE3; | ||
311 | typedef typename Data::Inertia Inertia; | ||
312 | |||
313 | 5 | data.com[0].setZero(); | |
314 | 5 | data.mass[0] = Scalar(0); | |
315 | |||
316 |
2/2✓ Branch 0 taken 135 times.
✓ Branch 1 taken 5 times.
|
140 | for (JointIndex i = 1; i < (JointIndex)(model.njoints); ++i) |
317 | { | ||
318 | 135 | const typename Inertia::Scalar & mass = model.inertias[i].mass(); | |
319 | 135 | const typename SE3::Vector3 & lever = model.inertias[i].lever(); | |
320 | |||
321 | 135 | data.mass[i] = mass; | |
322 |
4/8✓ Branch 2 taken 135 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 135 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 135 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 135 times.
✗ Branch 13 not taken.
|
135 | data.com[i].noalias() = mass * data.oMi[i].act(lever); |
323 | } | ||
324 | |||
325 | // Backward step | ||
326 | typedef JacobianCenterOfMassBackwardStep<Scalar, Options, JointCollectionTpl, Matrix3x> Pass2; | ||
327 |
2/2✓ Branch 0 taken 135 times.
✓ Branch 1 taken 5 times.
|
140 | for (JointIndex i = (JointIndex)(model.njoints - 1); i > 0; --i) |
328 | { | ||
329 |
1/2✓ Branch 1 taken 135 times.
✗ Branch 2 not taken.
|
135 | Pass2::run( |
330 | 135 | model.joints[i], data.joints[i], | |
331 | 270 | typename Pass2::ArgsType(model, data, data.Jcom, computeSubtreeComs)); | |
332 | } | ||
333 | |||
334 | 5 | data.com[0] /= data.mass[0]; | |
335 | 5 | data.Jcom /= data.mass[0]; | |
336 | |||
337 | 5 | return data.Jcom; | |
338 | } | ||
339 | |||
340 | template< | ||
341 | typename Scalar, | ||
342 | int Options, | ||
343 | template<typename, int> | ||
344 | class JointCollectionTpl, | ||
345 | typename Matrix3x> | ||
346 | struct JacobianSubtreeCenterOfMassBackwardStep | ||
347 | : public fusion::JointUnaryVisitorBase< | ||
348 | JacobianSubtreeCenterOfMassBackwardStep<Scalar, Options, JointCollectionTpl, Matrix3x>> | ||
349 | { | ||
350 | typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model; | ||
351 | typedef DataTpl<Scalar, Options, JointCollectionTpl> Data; | ||
352 | |||
353 | typedef boost::fusion:: | ||
354 | vector<const Model &, Data &, const JointIndex &, const Eigen::MatrixBase<Matrix3x> &> | ||
355 | ArgsType; | ||
356 | |||
357 | template<typename JointModel> | ||
358 | 222 | static void algo( | |
359 | const JointModelBase<JointModel> & jmodel, | ||
360 | JointDataBase<typename JointModel::JointDataDerived> & jdata, | ||
361 | const Model & model, | ||
362 | Data & data, | ||
363 | const JointIndex & subtree_root_id, | ||
364 | const Eigen::MatrixBase<Matrix3x> & Jcom) | ||
365 | { | ||
366 | PINOCCHIO_UNUSED_VARIABLE(model); | ||
367 | |||
368 |
1/2✓ Branch 1 taken 111 times.
✗ Branch 2 not taken.
|
222 | const JointIndex & i = (JointIndex)jmodel.id(); |
369 | |||
370 | typedef typename Data::Matrix6x Matrix6x; | ||
371 | typedef typename SizeDepType<JointModel::NV>::template ColsReturn<Matrix6x>::Type ColBlock; | ||
372 | |||
373 | 222 | Matrix3x & Jcom_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix3x, Jcom); | |
374 | |||
375 |
1/2✓ Branch 1 taken 111 times.
✗ Branch 2 not taken.
|
222 | ColBlock Jcols = jmodel.jointCols(data.J); |
376 |
3/6✓ Branch 2 taken 111 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 111 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 111 times.
✗ Branch 9 not taken.
|
222 | Jcols = data.oMi[i].act(jdata.S()); |
377 | |||
378 |
3/4✓ Branch 1 taken 352 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 241 times.
✓ Branch 4 taken 111 times.
|
704 | for (Eigen::DenseIndex col_id = 0; col_id < jmodel.nv(); ++col_id) |
379 | { | ||
380 |
5/10✓ Branch 1 taken 241 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 241 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 241 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 241 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 241 times.
✗ Branch 14 not taken.
|
964 | jmodel.jointCols(Jcom_).col(col_id) = |
381 |
2/4✓ Branch 1 taken 241 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 241 times.
✗ Branch 5 not taken.
|
482 | Jcols.col(col_id).template segment<3>(Motion::LINEAR) |
382 |
2/4✓ Branch 2 taken 241 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 241 times.
✗ Branch 6 not taken.
|
964 | - data.com[subtree_root_id].cross(Jcols.col(col_id).template segment<3>(Motion::ANGULAR)); |
383 | } | ||
384 | } | ||
385 | }; | ||
386 | namespace impl | ||
387 | { | ||
388 | template< | ||
389 | typename Scalar, | ||
390 | int Options, | ||
391 | template<typename, int> | ||
392 | class JointCollectionTpl, | ||
393 | typename ConfigVectorType, | ||
394 | typename Matrix3xLike> | ||
395 | 1 | void jacobianSubtreeCenterOfMass( | |
396 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
397 | DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
398 | const Eigen::MatrixBase<ConfigVectorType> & q, | ||
399 | const JointIndex & rootSubtreeId, | ||
400 | const Eigen::MatrixBase<Matrix3xLike> & res) | ||
401 | { | ||
402 | 1 | ::pinocchio::impl::forwardKinematics(model, data, q); | |
403 | 1 | ::pinocchio::jacobianSubtreeCenterOfMass(model, data, rootSubtreeId, res); | |
404 | 1 | } | |
405 | |||
406 | template< | ||
407 | typename Scalar, | ||
408 | int Options, | ||
409 | template<typename, int> | ||
410 | class JointCollectionTpl, | ||
411 | typename Matrix3xLike> | ||
412 | 29 | void jacobianSubtreeCenterOfMass( | |
413 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
414 | DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
415 | const JointIndex & rootSubtreeId, | ||
416 | const Eigen::MatrixBase<Matrix3xLike> & res) | ||
417 | { | ||
418 | typedef DataTpl<Scalar, Options, JointCollectionTpl> Data; | ||
419 | typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model; | ||
420 | |||
421 |
2/4✓ Branch 1 taken 29 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 29 times.
✗ Branch 4 not taken.
|
29 | assert(model.check(data) && "data is not consistent with model."); |
422 |
1/4✗ Branch 0 not taken.
✓ Branch 1 taken 29 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
|
29 | PINOCCHIO_CHECK_INPUT_ARGUMENT((int)rootSubtreeId < model.njoints, "Invalid joint id."); |
423 |
1/24✗ Branch 1 not taken.
✓ Branch 2 taken 29 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.
|
29 | PINOCCHIO_CHECK_ARGUMENT_SIZE( |
424 | res.rows(), 3, "the resulting matrix does not have the right size."); | ||
425 |
1/24✗ Branch 1 not taken.
✓ Branch 2 taken 29 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.
|
29 | PINOCCHIO_CHECK_ARGUMENT_SIZE( |
426 | res.cols(), model.nv, "the resulting matrix does not have the right size."); | ||
427 | |||
428 | 29 | Matrix3xLike & Jcom_subtree = PINOCCHIO_EIGEN_CONST_CAST(Matrix3xLike, res); | |
429 | |||
430 | typedef typename Data::SE3 SE3; | ||
431 | typedef typename Data::Inertia Inertia; | ||
432 | |||
433 | typedef typename Model::IndexVector IndexVector; | ||
434 | 29 | const IndexVector & subtree = model.subtrees[rootSubtreeId]; | |
435 | |||
436 | 29 | const bool computeSubtreeComs = true; | |
437 | |||
438 |
2/2✓ Branch 0 taken 2 times.
✓ Branch 1 taken 27 times.
|
29 | if (rootSubtreeId == 0) |
439 | { | ||
440 | 2 | data.mass[0] = 0; | |
441 |
1/2✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
|
2 | data.com[0].setZero(); |
442 | } | ||
443 | |||
444 |
2/2✓ Branch 1 taken 192 times.
✓ Branch 2 taken 29 times.
|
221 | for (size_t k = 0; k < subtree.size(); ++k) |
445 | { | ||
446 | 192 | const JointIndex joint_id = subtree[k]; | |
447 | 192 | const typename Inertia::Scalar & mass = model.inertias[joint_id].mass(); | |
448 | 192 | const typename SE3::Vector3 & lever = model.inertias[joint_id].lever(); | |
449 | |||
450 | 192 | data.mass[joint_id] = mass; | |
451 |
3/6✓ Branch 2 taken 192 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 192 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 192 times.
✗ Branch 10 not taken.
|
192 | data.com[joint_id] = mass * data.oMi[joint_id].act(lever); |
452 | } | ||
453 | |||
454 | // Backward step | ||
455 | typedef JacobianCenterOfMassBackwardStep<Scalar, Options, JointCollectionTpl, Matrix3xLike> | ||
456 | Pass2; | ||
457 |
2/2✓ Branch 1 taken 192 times.
✓ Branch 2 taken 29 times.
|
221 | for (Eigen::DenseIndex k = (Eigen::DenseIndex)subtree.size() - 1; k >= 0; --k) |
458 | { | ||
459 | 192 | const JointIndex joint_id = subtree[(size_t)k]; | |
460 |
1/2✓ Branch 1 taken 192 times.
✗ Branch 2 not taken.
|
192 | Pass2::run( |
461 | 192 | model.joints[joint_id], data.joints[joint_id], | |
462 |
1/2✓ Branch 1 taken 192 times.
✗ Branch 2 not taken.
|
384 | typename Pass2::ArgsType(model, data, Jcom_subtree, computeSubtreeComs)); |
463 | } | ||
464 | |||
465 |
2/6✓ Branch 2 taken 29 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 29 times.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
|
29 | PINOCCHIO_CHECK_INPUT_ARGUMENT( |
466 | check_expression_if_real<Scalar>(data.mass[rootSubtreeId] > 0.), | ||
467 | "The mass of the subtree is not positive."); | ||
468 | 29 | const Scalar mass_inv_subtree = Scalar(1) / data.mass[rootSubtreeId]; | |
469 | 29 | typename Data::Vector3 & com_subtree = data.com[rootSubtreeId]; | |
470 | if (!computeSubtreeComs) | ||
471 | com_subtree *= mass_inv_subtree; | ||
472 | |||
473 |
2/2✓ Branch 0 taken 2 times.
✓ Branch 1 taken 27 times.
|
29 | if (rootSubtreeId == 0) |
474 | { | ||
475 |
1/2✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
|
2 | Jcom_subtree *= mass_inv_subtree; |
476 | 2 | return; // skip the rest | |
477 | } | ||
478 | |||
479 |
1/2✓ Branch 2 taken 27 times.
✗ Branch 3 not taken.
|
27 | const int idx_v = model.joints[rootSubtreeId].idx_v(); |
480 | 27 | const int nv_subtree = data.nvSubtree[rootSubtreeId]; | |
481 | |||
482 |
2/4✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 27 times.
✗ Branch 5 not taken.
|
27 | Jcom_subtree.middleCols(idx_v, nv_subtree) *= mass_inv_subtree; |
483 | |||
484 | // Second backward step | ||
485 | typedef JacobianSubtreeCenterOfMassBackwardStep< | ||
486 | Scalar, Options, JointCollectionTpl, Matrix3xLike> | ||
487 | Pass3; | ||
488 |
2/2✓ Branch 1 taken 111 times.
✓ Branch 2 taken 27 times.
|
138 | for (JointIndex parent = model.parents[rootSubtreeId]; parent > 0; |
489 | 111 | parent = model.parents[parent]) | |
490 | { | ||
491 |
1/2✓ Branch 1 taken 111 times.
✗ Branch 2 not taken.
|
111 | Pass3::run( |
492 | 111 | model.joints[parent], data.joints[parent], | |
493 |
1/2✓ Branch 1 taken 111 times.
✗ Branch 2 not taken.
|
222 | typename Pass3::ArgsType(model, data, rootSubtreeId, Jcom_subtree)); |
494 | } | ||
495 | } | ||
496 | |||
497 | template< | ||
498 | typename Scalar, | ||
499 | int Options, | ||
500 | template<typename, int> | ||
501 | class JointCollectionTpl, | ||
502 | typename Matrix3xLike> | ||
503 | 28 | void getJacobianSubtreeCenterOfMass( | |
504 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
505 | const DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
506 | const JointIndex & rootSubtreeId, | ||
507 | const Eigen::MatrixBase<Matrix3xLike> & res) | ||
508 | { | ||
509 | typedef DataTpl<Scalar, Options, JointCollectionTpl> Data; | ||
510 | |||
511 |
2/4✓ Branch 1 taken 28 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 28 times.
✗ Branch 4 not taken.
|
28 | assert(model.check(data) && "data is not consistent with model."); |
512 |
1/4✗ Branch 0 not taken.
✓ Branch 1 taken 28 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
|
28 | PINOCCHIO_CHECK_INPUT_ARGUMENT(((int)rootSubtreeId < model.njoints), "Invalid joint id."); |
513 |
1/24✗ Branch 1 not taken.
✓ Branch 2 taken 28 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.
|
28 | PINOCCHIO_CHECK_ARGUMENT_SIZE( |
514 | res.rows(), 3, "the resulting matrix does not have the right size."); | ||
515 |
1/24✗ Branch 1 not taken.
✓ Branch 2 taken 28 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.
|
28 | PINOCCHIO_CHECK_ARGUMENT_SIZE( |
516 | res.cols(), model.nv, "the resulting matrix does not have the right size."); | ||
517 | |||
518 | 28 | Matrix3xLike & Jcom_subtree = PINOCCHIO_EIGEN_CONST_CAST(Matrix3xLike, res); | |
519 | |||
520 |
2/2✓ Branch 0 taken 1 times.
✓ Branch 1 taken 27 times.
|
28 | if (rootSubtreeId == 0) |
521 | { | ||
522 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | Jcom_subtree = data.Jcom; |
523 | 1 | return; | |
524 | } | ||
525 | |||
526 |
1/2✓ Branch 2 taken 27 times.
✗ Branch 3 not taken.
|
27 | const int idx_v = model.joints[rootSubtreeId].idx_v(); |
527 | 27 | const int nv_subtree = data.nvSubtree[rootSubtreeId]; | |
528 | |||
529 | 27 | const Scalar mass_ratio = data.mass[0] / data.mass[rootSubtreeId]; | |
530 |
3/6✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 27 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 27 times.
✗ Branch 8 not taken.
|
27 | Jcom_subtree.middleCols(idx_v, nv_subtree) = |
531 |
1/2✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
|
27 | mass_ratio * data.Jcom.middleCols(idx_v, nv_subtree); |
532 | |||
533 | 27 | const typename Data::Vector3 & com_subtree = data.com[rootSubtreeId]; | |
534 | |||
535 |
2/2✓ Branch 1 taken 241 times.
✓ Branch 2 taken 27 times.
|
268 | for (int parent = data.parents_fromRow[(size_t)idx_v]; parent >= 0; |
536 | 241 | parent = data.parents_fromRow[(size_t)parent]) | |
537 | { | ||
538 |
1/2✓ Branch 1 taken 241 times.
✗ Branch 2 not taken.
|
241 | typename Data::Matrix6x::ConstColXpr Jcol = data.J.col(parent); |
539 |
5/10✓ Branch 1 taken 241 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 241 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 241 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 241 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 241 times.
✗ Branch 14 not taken.
|
482 | Jcom_subtree.col(parent).noalias() = |
540 |
1/2✓ Branch 1 taken 241 times.
✗ Branch 2 not taken.
|
241 | Jcol.template segment<3>(Motion::LINEAR) |
541 |
1/2✓ Branch 1 taken 241 times.
✗ Branch 2 not taken.
|
482 | - com_subtree.cross(Jcol.template segment<3>(Motion::ANGULAR)); |
542 | } | ||
543 | } | ||
544 | } // namespace impl | ||
545 | template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl> | ||
546 | 7 | const typename DataTpl<Scalar, Options, JointCollectionTpl>::Matrix3x & getJacobianComFromCrba( | |
547 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
548 | DataTpl<Scalar, Options, JointCollectionTpl> & data) | ||
549 | { | ||
550 | PINOCCHIO_UNUSED_VARIABLE(model); | ||
551 |
1/2✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
|
7 | assert(model.check(data) && "data is not consistent with model."); |
552 | |||
553 | // Extract the total mass of the system. | ||
554 | 7 | data.mass[0] = data.oYcrb[0].mass(); | |
555 | 7 | data.com[0] = data.oYcrb[0].lever(); | |
556 | |||
557 |
3/6✓ Branch 3 taken 7 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 7 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 7 times.
✗ Branch 10 not taken.
|
7 | data.Jcom.noalias() = data.Ag.template middleRows<3>(Force::LINEAR) / data.mass[0]; |
558 | |||
559 | 7 | return data.Jcom; | |
560 | } | ||
561 | |||
562 | template< | ||
563 | typename Scalar, | ||
564 | int Options, | ||
565 | template<typename, int> | ||
566 | class JointCollectionTpl, | ||
567 | typename ConfigVectorType> | ||
568 | 2 | const typename DataTpl<Scalar, Options, JointCollectionTpl>::Vector3 & centerOfMass( | |
569 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
570 | DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
571 | const Eigen::MatrixBase<ConfigVectorType> & q, | ||
572 | const bool computeSubtreeComs) | ||
573 | { | ||
574 |
1/2✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
|
2 | return impl::centerOfMass(model, data, make_const_ref(q), computeSubtreeComs); |
575 | } | ||
576 | |||
577 | template< | ||
578 | typename Scalar, | ||
579 | int Options, | ||
580 | template<typename, int> | ||
581 | class JointCollectionTpl, | ||
582 | typename ConfigVectorType, | ||
583 | typename TangentVectorType> | ||
584 | ✗ | const typename DataTpl<Scalar, Options, JointCollectionTpl>::Vector3 & centerOfMass( | |
585 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
586 | DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
587 | const Eigen::MatrixBase<ConfigVectorType> & q, | ||
588 | const Eigen::MatrixBase<TangentVectorType> & v, | ||
589 | const bool computeSubtreeComs) | ||
590 | { | ||
591 | ✗ | return impl::centerOfMass( | |
592 | ✗ | model, data, make_const_ref(q), make_const_ref(v), computeSubtreeComs); | |
593 | } | ||
594 | |||
595 | template< | ||
596 | typename Scalar, | ||
597 | int Options, | ||
598 | template<typename, int> | ||
599 | class JointCollectionTpl, | ||
600 | typename ConfigVectorType, | ||
601 | typename TangentVectorType1, | ||
602 | typename TangentVectorType2> | ||
603 | ✗ | const typename DataTpl<Scalar, Options, JointCollectionTpl>::Vector3 & centerOfMass( | |
604 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
605 | DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
606 | const Eigen::MatrixBase<ConfigVectorType> & q, | ||
607 | const Eigen::MatrixBase<TangentVectorType1> & v, | ||
608 | const Eigen::MatrixBase<TangentVectorType2> & a, | ||
609 | const bool computeSubtreeComs) | ||
610 | { | ||
611 | ✗ | return impl::centerOfMass( | |
612 | ✗ | model, data, make_const_ref(q), make_const_ref(v), make_const_ref(a), computeSubtreeComs); | |
613 | } | ||
614 | |||
615 | template< | ||
616 | typename Scalar, | ||
617 | int Options, | ||
618 | template<typename, int> | ||
619 | class JointCollectionTpl, | ||
620 | typename ConfigVectorType> | ||
621 | ✗ | const typename DataTpl<Scalar, Options, JointCollectionTpl>::Matrix3x & jacobianCenterOfMass( | |
622 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
623 | DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
624 | const Eigen::MatrixBase<ConfigVectorType> & q, | ||
625 | const bool computeSubtreeComs) | ||
626 | { | ||
627 | ✗ | return impl::jacobianCenterOfMass(model, data, make_const_ref(q), computeSubtreeComs); | |
628 | } | ||
629 | |||
630 | template< | ||
631 | typename Scalar, | ||
632 | int Options, | ||
633 | template<typename, int> | ||
634 | class JointCollectionTpl, | ||
635 | typename ConfigVectorType, | ||
636 | typename Matrix3xLike> | ||
637 | ✗ | void jacobianSubtreeCenterOfMass( | |
638 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
639 | DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
640 | const Eigen::MatrixBase<ConfigVectorType> & q, | ||
641 | const JointIndex & rootSubtreeId, | ||
642 | const Eigen::MatrixBase<Matrix3xLike> & res) | ||
643 | { | ||
644 | ✗ | impl::jacobianSubtreeCenterOfMass(model, data, make_const_ref(q), rootSubtreeId, make_ref(res)); | |
645 | } | ||
646 | |||
647 | template< | ||
648 | typename Scalar, | ||
649 | int Options, | ||
650 | template<typename, int> | ||
651 | class JointCollectionTpl, | ||
652 | typename Matrix3xLike> | ||
653 | 1 | void jacobianSubtreeCenterOfMass( | |
654 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
655 | DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
656 | const JointIndex & rootSubtreeId, | ||
657 | const Eigen::MatrixBase<Matrix3xLike> & res) | ||
658 | { | ||
659 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
1 | impl::jacobianSubtreeCenterOfMass(model, data, rootSubtreeId, make_ref(res)); |
660 | 1 | } | |
661 | |||
662 | template< | ||
663 | typename Scalar, | ||
664 | int Options, | ||
665 | template<typename, int> | ||
666 | class JointCollectionTpl, | ||
667 | typename Matrix3xLike> | ||
668 | ✗ | void getJacobianSubtreeCenterOfMass( | |
669 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
670 | const DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
671 | const JointIndex & rootSubtreeId, | ||
672 | const Eigen::MatrixBase<Matrix3xLike> & res) | ||
673 | { | ||
674 | ✗ | impl::getJacobianSubtreeCenterOfMass(model, data, rootSubtreeId, make_ref(res)); | |
675 | } | ||
676 | |||
677 | } // namespace pinocchio | ||
678 | |||
679 | /// @endcond | ||
680 | |||
681 | #endif // ifndef __pinocchio_algorithm_center_of_mass_hxx__ | ||
682 |