GCC Code Coverage Report

Directory: ./
File: include/pinocchio/algorithm/frames.hxx
Date: 2024-08-27 18:20:05
Exec Total Coverage
Lines: 101 147 68.7%
Branches: 77 327 23.5%
Line Branch Exec Source
1 //
2 // Copyright (c) 2015-2021 CNRS INRIA
3 //
4
5 #ifndef __pinocchio_algorithm_frames_hxx__
6 #define __pinocchio_algorithm_frames_hxx__
7
8 #include "pinocchio/algorithm/kinematics.hpp"
9 #include "pinocchio/algorithm/jacobian.hpp"
10 #include "pinocchio/algorithm/check.hpp"
11
12 namespace pinocchio
13 {
14
15 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
16 9005 inline void updateFramePlacements(
17 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
18 DataTpl<Scalar, Options, JointCollectionTpl> & data)
19 {
20
1/2
✓ Branch 1 taken 9005 times.
✗ Branch 2 not taken.
 9005 assert(model.check(data) && "data is not consistent with model.");
21
22 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
23 typedef typename Model::Frame Frame;
24 typedef typename Model::FrameIndex FrameIndex;
25 typedef typename Model::JointIndex JointIndex;
26
27 // The following for loop starts by index 1 because the first frame is fixed
28 // and corresponds to the universe.
29
2/2
✓ Branch 0 taken 495231 times.
✓ Branch 1 taken 9005 times.
 504236 for (FrameIndex i = 1; i < (FrameIndex)model.nframes; ++i)
30 {
31 495231 const Frame & frame = model.frames[i];
32 495231 const JointIndex & parent = frame.parentJoint;
33
1/2
✓ Branch 4 taken 495231 times.
✗ Branch 5 not taken.
 495231 data.oMf[i] = data.oMi[parent] * frame.placement;
34 }
35 9005 }
36
37 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
38 4 inline const typename DataTpl<Scalar, Options, JointCollectionTpl>::SE3 & updateFramePlacement(
39 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
40 DataTpl<Scalar, Options, JointCollectionTpl> & data,
41 const FrameIndex frame_id)
42 {
43
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 4 assert(model.check(data) && "data is not consistent with model.");
44
45 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
46 4 const typename Model::Frame & frame = model.frames[frame_id];
47 4 const typename Model::JointIndex & parent = frame.parentJoint;
48
49
1/2
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
 4 data.oMf[frame_id] = data.oMi[parent] * frame.placement;
50
51 4 return data.oMf[frame_id];
52 }
53
54 template<
55 typename Scalar,
56 int Options,
57 template<typename, int>
58 class JointCollectionTpl,
59 typename ConfigVectorType>
60 13 inline void framesForwardKinematics(
61 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
62 DataTpl<Scalar, Options, JointCollectionTpl> & data,
63 const Eigen::MatrixBase<ConfigVectorType> & q)
64 {
65
1/2
✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
 13 assert(model.check(data) && "data is not consistent with model.");
66
67 13 forwardKinematics(model, data, q);
68 13 updateFramePlacements(model, data);
69 13 }
70
71 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
72 556 inline MotionTpl<Scalar, Options> getFrameVelocity(
73 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
74 const DataTpl<Scalar, Options, JointCollectionTpl> & data,
75 const JointIndex joint_id,
76 const SE3Tpl<Scalar, Options> & placement,
77 const ReferenceFrame rf)
78 {
79 PINOCCHIO_UNUSED_VARIABLE(model);
80
1/2
✓ Branch 1 taken 556 times.
✗ Branch 2 not taken.
 556 assert(model.check(data) && "data is not consistent with model.");
81
82 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
83 typedef typename Model::Motion Motion;
84
85 556 const typename Model::SE3 & oMi = data.oMi[joint_id];
86 556 const typename Model::Motion & v = data.v[joint_id];
87
3/4
✓ Branch 0 taken 284 times.
✓ Branch 1 taken 69 times.
✓ Branch 2 taken 203 times.
✗ Branch 3 not taken.
 556 switch (rf)
88 {
89 284 case LOCAL:
90 284 return placement.actInv(v);
91 69 case WORLD:
92 69 return oMi.act(v);
93 203 case LOCAL_WORLD_ALIGNED:
94 return Motion(
95
4/8
✓ Branch 1 taken 203 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 203 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 203 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 203 times.
✗ Branch 11 not taken.
 609 oMi.rotation() * (v.linear() + v.angular().cross(placement.translation())),
96
6/12
✓ Branch 2 taken 203 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 203 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 203 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 203 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 203 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 203 times.
✗ Branch 18 not taken.
 812 oMi.rotation() * v.angular());
97 ✗ default:
98 ✗ throw std::invalid_argument("Bad reference frame.");
99 }
100 }
101
102 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
103 319 inline MotionTpl<Scalar, Options> getFrameAcceleration(
104 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
105 const DataTpl<Scalar, Options, JointCollectionTpl> & data,
106 const JointIndex joint_id,
107 const SE3Tpl<Scalar, Options> & placement,
108 const ReferenceFrame rf)
109 {
110 PINOCCHIO_UNUSED_VARIABLE(model);
111
1/2
✓ Branch 1 taken 319 times.
✗ Branch 2 not taken.
 319 assert(model.check(data) && "data is not consistent with model.");
112
113 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
114 typedef typename Model::Motion Motion;
115
116 319 const typename Model::SE3 & oMi = data.oMi[joint_id];
117 319 const typename Model::Motion & a = data.a[joint_id];
118
3/4
✓ Branch 0 taken 115 times.
✓ Branch 1 taken 102 times.
✓ Branch 2 taken 102 times.
✗ Branch 3 not taken.
 319 switch (rf)
119 {
120 115 case LOCAL:
121 115 return placement.actInv(a);
122 102 case WORLD:
123 102 return oMi.act(a);
124 102 case LOCAL_WORLD_ALIGNED:
125 return Motion(
126
4/8
✓ Branch 1 taken 102 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 102 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 102 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 102 times.
✗ Branch 11 not taken.
 306 oMi.rotation() * (a.linear() + a.angular().cross(placement.translation())),
127
6/12
✓ Branch 2 taken 102 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 102 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 102 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 102 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 102 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 102 times.
✗ Branch 18 not taken.
 408 oMi.rotation() * a.angular());
128 ✗ default:
129 ✗ throw std::invalid_argument("Bad reference frame.");
130 }
131 }
132
133 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
134 13 inline MotionTpl<Scalar, Options> getFrameClassicalAcceleration(
135 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
136 const DataTpl<Scalar, Options, JointCollectionTpl> & data,
137 const JointIndex joint_id,
138 const SE3Tpl<Scalar, Options> & placement,
139 const ReferenceFrame rf)
140 {
141
2/4
✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 13 times.
✗ Branch 4 not taken.
 13 assert(model.check(data) && "data is not consistent with model.");
142
143 typedef MotionTpl<Scalar, Options> Motion;
144
1/2
✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
 13 const Motion vel = getFrameVelocity(model, data, joint_id, placement, rf);
145
1/2
✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
 13 Motion acc = getFrameAcceleration(model, data, joint_id, placement, rf);
146
147
5/10
✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 13 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 13 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 13 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 13 times.
✗ Branch 14 not taken.
 13 acc.linear() += vel.angular().cross(vel.linear());
148
149 26 return acc;
150 }
151
152 template<
153 typename Scalar,
154 int Options,
155 template<typename, int>
156 class JointCollectionTpl,
157 typename Matrix6xLike>
158 38 inline void getFrameJacobian(
159 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
160 DataTpl<Scalar, Options, JointCollectionTpl> & data,
161 const JointIndex joint_id,
162 const SE3Tpl<Scalar, Options> & placement,
163 const ReferenceFrame reference_frame,
164 const Eigen::MatrixBase<Matrix6xLike> & J)
165 {
166
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 38 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.
 38 PINOCCHIO_CHECK_ARGUMENT_SIZE(J.rows(), 6);
167
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 38 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.
 38 PINOCCHIO_CHECK_ARGUMENT_SIZE(J.cols(), model.nv);
168
1/4
✗ Branch 0 not taken.
✓ Branch 1 taken 38 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
 38 PINOCCHIO_CHECK_INPUT_ARGUMENT(
169 joint_id < (JointIndex)model.njoints, "The index of the Joint is outside the bounds.");
170
2/4
✓ Branch 1 taken 38 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 38 times.
✗ Branch 4 not taken.
 38 assert(model.check(data) && "data is not consistent with model.");
171
172 typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
173
174
1/2
✓ Branch 2 taken 38 times.
✗ Branch 3 not taken.
 38 const typename Data::SE3 oMframe = data.oMi[joint_id] * placement;
175 38 details::translateJointJacobian(
176
1/2
✓ Branch 1 taken 38 times.
✗ Branch 2 not taken.
 38 model, data, joint_id, reference_frame, oMframe, data.J,
177 38 PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLike, J));
178 38 }
179
180 template<
181 typename Scalar,
182 int Options,
183 template<typename, int>
184 class JointCollectionTpl,
185 typename ConfigVectorType,
186 typename Matrix6xLike>
187 9 inline void computeFrameJacobian(
188 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
189 DataTpl<Scalar, Options, JointCollectionTpl> & data,
190 const Eigen::MatrixBase<ConfigVectorType> & q,
191 const FrameIndex frameId,
192 const ReferenceFrame reference_frame,
193 const Eigen::MatrixBase<Matrix6xLike> & J)
194 {
195
1/2
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
 9 assert(model.check(data) && "data is not consistent with model.");
196
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 9 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.
 9 PINOCCHIO_CHECK_ARGUMENT_SIZE(
197 q.size(), model.nq, "The configuration vector is not of right size");
198
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 9 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.
 9 PINOCCHIO_CHECK_ARGUMENT_SIZE(
199 J.cols(), model.nv,
200 "The numbers of columns in the Jacobian matrix does not math the "
201 "number of Dofs in the model.");
202
203 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
204 typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
205 typedef typename Model::Frame Frame;
206 typedef typename Model::JointIndex JointIndex;
207 typedef typename Model::IndexVector IndexVector;
208
209 9 const Frame & frame = model.frames[frameId];
210 9 const JointIndex & joint_id = frame.parentJoint;
211
212 9 const IndexVector & joint_support = model.supports[joint_id];
213
214
2/3
✓ Branch 0 taken 3 times.
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
 9 switch (reference_frame)
215 {
216 3 case WORLD:
217 case LOCAL_WORLD_ALIGNED: {
218 typedef impl::JointJacobiansForwardStep<
219 Scalar, Options, JointCollectionTpl, ConfigVectorType, Matrix6xLike>
220 Pass;
221
2/2
✓ Branch 1 taken 15 times.
✓ Branch 2 taken 3 times.
 18 for (size_t k = 1; k < joint_support.size(); k++)
222 {
223 15 JointIndex parent = joint_support[k];
224
1/2
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
 15 Pass::run(
225 15 model.joints[parent], data.joints[parent],
226 30 typename Pass::ArgsType(
227 15 model, data, q.derived(), PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLike, J)));
228 }
229
230
2/2
✓ Branch 0 taken 2 times.
✓ Branch 1 taken 1 times.
 3 if (reference_frame == LOCAL_WORLD_ALIGNED)
231 {
232 2 typename Data::SE3 & oMframe = data.oMf[frameId];
233
1/2
✓ Branch 3 taken 2 times.
✗ Branch 4 not taken.
 2 oMframe = data.oMi[joint_id] * frame.placement;
234
235 2 Matrix6xLike & J_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLike, J);
236
237 2 const int colRef = nv(model.joints[joint_id]) + idx_v(model.joints[joint_id]) - 1;
238
2/2
✓ Branch 0 taken 20 times.
✓ Branch 1 taken 2 times.
 22 for (Eigen::DenseIndex j = colRef; j >= 0; j = data.parents_fromRow[(size_t)j])
239 {
240 typedef typename Matrix6xLike::ColXpr ColXprOut;
241
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
 20 MotionRef<ColXprOut> J_col(J_.col(j));
242
243
5/10
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 20 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 20 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 20 times.
✗ Branch 14 not taken.
 20 J_col.linear() -= oMframe.translation().cross(J_col.angular());
244 }
245 }
246 3 break;
247 }
248 6 case LOCAL: {
249 6 data.iMf[joint_id] = frame.placement;
250
251 typedef impl::JointJacobianForwardStep<
252 Scalar, Options, JointCollectionTpl, ConfigVectorType, Matrix6xLike>
253 Pass;
254
2/2
✓ Branch 0 taken 26 times.
✓ Branch 1 taken 6 times.
 32 for (JointIndex i = joint_id; i > 0; i = model.parents[i])
255 {
256
1/2
✓ Branch 1 taken 26 times.
✗ Branch 2 not taken.
 26 Pass::run(
257 26 model.joints[i], data.joints[i],
258 52 typename Pass::ArgsType(
259 26 model, data, q.derived(), PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLike, J)));
260 }
261 6 break;
262 }
263 ✗ default: {
264 ✗ assert(false && "must never happened");
265 }
266 }
267 9 }
268
269 template<
270 typename Scalar,
271 int Options,
272 template<typename, int>
273 class JointCollectionTpl,
274 typename Matrix6xLike>
275 6 void getFrameJacobianTimeVariation(
276 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
277 DataTpl<Scalar, Options, JointCollectionTpl> & data,
278 const FrameIndex frame_id,
279 const ReferenceFrame rf,
280 const Eigen::MatrixBase<Matrix6xLike> & dJ)
281 {
282
1/2
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
 6 assert(model.check(data) && "data is not consistent with model.");
283
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 6 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.
 6 PINOCCHIO_CHECK_ARGUMENT_SIZE(
284 dJ.cols(), model.nv,
285 "The numbers of columns in the Jacobian matrix does not math the "
286 "number of Dofs in the model.");
287
288 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
289 typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
290 typedef typename Model::Frame Frame;
291
292 6 const Frame & frame = model.frames[frame_id];
293 6 const JointIndex & joint_id = frame.parentJoint;
294
295 6 typename Data::SE3 & oMframe = data.oMf[frame_id];
296
1/2
✓ Branch 3 taken 6 times.
✗ Branch 4 not taken.
 6 oMframe = data.oMi[joint_id] * frame.placement;
297
298 6 details::translateJointJacobian(
299 6 model, data, joint_id, rf, oMframe, data.dJ, PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLike, dJ));
300 6 }
301
302 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
303 ✗ InertiaTpl<Scalar, Options> computeSupportedInertiaByFrame(
304 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
305 const DataTpl<Scalar, Options, JointCollectionTpl> & data,
306 const FrameIndex frame_id,
307 bool with_subtree)
308 {
309 ✗ assert(model.check(data) && "data is not consistent with model.");
310
311 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
312 typedef InertiaTpl<Scalar, Options> Inertia;
313
314 ✗ const Frame & frame = model.frames[frame_id];
315 ✗ const JointIndex & joint_id = frame.parentJoint;
316
317 // Add all the inertia of child frames (i.e that are part of the same joint but comes after the
318 // given frame)
319 ✗ std::vector<typename Model::JointIndex> child_frames = {frame_id};
320 ✗ Inertia I = frame.placement.act(frame.inertia); // Express the inertia in the parent joint frame
321 ✗ for (FrameIndex i = frame_id + 1; i < (FrameIndex)model.nframes; ++i)
322 {
323 ✗ if (model.frames[i].parentJoint != joint_id)
324 ✗ continue;
325 ✗ if (
326 ✗ std::find(child_frames.begin(), child_frames.end(), model.frames[i].parentFrame)
327 ✗ == child_frames.end())
328 ✗ continue;
329 ✗ child_frames.push_back(i);
330 ✗ I += model.frames[i].placement.act(model.frames[i].inertia);
331 }
332
333 ✗ if (!with_subtree)
334 {
335 ✗ return frame.placement.actInv(I);
336 }
337
338 // Express the inertia in the origin frame for simplicity.
339 ✗ I = data.oMi[joint_id].act(I);
340
341 // Add inertia of child joints
342 ✗ const std::vector<typename Model::JointIndex> & subtree = model.subtrees[joint_id];
343 ✗ for (size_t k = 1; k < subtree.size();
344 ++k) // Skip the first joint as it is the one before the frame
345 {
346 ✗ const typename Model::JointIndex j_id = subtree[k];
347 ✗ I += data.oMi[j_id].act(model.inertias[j_id]);
348 }
349
350 ✗ const pinocchio::SE3 oMf = data.oMi[joint_id] * frame.placement;
351 ✗ return oMf.actInv(I);
352 }
353
354 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
355 ✗ ForceTpl<Scalar, Options> computeSupportedForceByFrame(
356 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
357 const DataTpl<Scalar, Options, JointCollectionTpl> & data,
358 const FrameIndex frame_id)
359 {
360 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
361 typedef InertiaTpl<Scalar, Options> Inertia;
362 typedef MotionTpl<Scalar, Options> Motion;
363 typedef ForceTpl<Scalar, Options> Force;
364
365 ✗ const Frame & frame = model.frames[frame_id];
366 ✗ const JointIndex & joint_id = frame.parentJoint;
367
368 // Compute 'in body' forces
369 ✗ const Inertia fI = computeSupportedInertiaByFrame(model, data, frame_id, false);
370 ✗ const pinocchio::SE3 oMf = data.oMi[joint_id] * frame.placement;
371 ✗ const Motion v = getFrameVelocity(model, data, frame_id, LOCAL);
372 ✗ const Motion a = getFrameAcceleration(model, data, frame_id, LOCAL);
373 ✗ Force f = fI.vxiv(v) + fI * (a - oMf.actInv(model.gravity));
374
375 // Add child joints forces
376 ✗ f = frame.placement.act(f); // Express force in parent joint frame
377 ✗ const std::vector<typename Model::JointIndex> & subtree = model.subtrees[joint_id];
378 ✗ for (size_t k = 1; k < subtree.size();
379 ++k) // Skip the first joint as it is the one before the frame
380 {
381 ✗ const typename Model::JointIndex j_id = subtree[k];
382 ✗ if (model.parents[j_id] != joint_id) // Joint is not a direct child
383 {
384 ✗ continue;
385 }
386 ✗ f += data.liMi[j_id].act(data.f[j_id]);
387 }
388
389 // Transform back to local frame
390 ✗ return frame.placement.actInv(f);
391 }
392 } // namespace pinocchio
393
394 #endif // ifndef __pinocchio_algorithm_frames_hxx__
395