GCC Code Coverage Report

Directory: ./
File: include/pinocchio/algorithm/frames.hxx
Date: 2025-04-30 16:14:33
Exec Total Coverage
Lines: 177 182 97.3%
Branches: 152 381 39.9%
Line Branch Exec Source
1 //
2 // Copyright (c) 2015-2024 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 9007 inline void updateFramePlacements(
17 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
18 DataTpl<Scalar, Options, JointCollectionTpl> & data)
19 {
20
1/2
✓ Branch 1 taken 9007 times.
✗ Branch 2 not taken.
 9007 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 495339 times.
✓ Branch 1 taken 9007 times.
 504346 for (FrameIndex i = 1; i < (FrameIndex)model.nframes; ++i)
30 {
31 495339 const Frame & frame = model.frames[i];
32 495339 const JointIndex & parent = frame.parentJoint;
33
1/2
✓ Branch 4 taken 495339 times.
✗ Branch 5 not taken.
 495339 data.oMf[i] = data.oMi[parent] * frame.placement;
34 }
35 9007 }
36
37 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
38 569 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 569 times.
✗ Branch 2 not taken.
 569 assert(model.check(data) && "data is not consistent with model.");
44
45 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
46 569 const typename Model::Frame & frame = model.frames[frame_id];
47 569 const typename Model::JointIndex & parent = frame.parentJoint;
48
49
1/2
✓ Branch 4 taken 569 times.
✗ Branch 5 not taken.
 569 data.oMf[frame_id] = data.oMi[parent] * frame.placement;
50
51 569 return data.oMf[frame_id];
52 }
53
54 template<
55 typename Scalar,
56 int Options,
57 template<typename, int> class JointCollectionTpl,
58 typename ConfigVectorType>
59 13 inline void framesForwardKinematics(
60 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
61 DataTpl<Scalar, Options, JointCollectionTpl> & data,
62 const Eigen::MatrixBase<ConfigVectorType> & q)
63 {
64
1/2
✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
 13 assert(model.check(data) && "data is not consistent with model.");
65
66 13 forwardKinematics(model, data, q);
67 13 updateFramePlacements(model, data);
68 13 }
69
70 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
71 1698 inline MotionTpl<Scalar, Options> getFrameVelocity(
72 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
73 const DataTpl<Scalar, Options, JointCollectionTpl> & data,
74 const JointIndex joint_id,
75 const SE3Tpl<Scalar, Options> & placement,
76 const ReferenceFrame rf)
77 {
78 PINOCCHIO_UNUSED_VARIABLE(model);
79
1/2
✓ Branch 1 taken 1698 times.
✗ Branch 2 not taken.
 1698 assert(model.check(data) && "data is not consistent with model.");
80
81 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
82 typedef typename Model::Motion Motion;
83
84 1698 const typename Model::SE3 & oMi = data.oMi[joint_id];
85 1698 const typename Model::Motion & v = data.v[joint_id];
86
3/4
✓ Branch 0 taken 666 times.
✓ Branch 1 taken 449 times.
✓ Branch 2 taken 583 times.
✗ Branch 3 not taken.
 1698 switch (rf)
87 {
88 666 case LOCAL:
89 666 return placement.actInv(v);
90 449 case WORLD:
91 449 return oMi.act(v);
92 583 case LOCAL_WORLD_ALIGNED:
93 return Motion(
94
4/8
✓ Branch 1 taken 583 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 583 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 583 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 583 times.
✗ Branch 11 not taken.
 1749 oMi.rotation() * (v.linear() + v.angular().cross(placement.translation())),
95
6/12
✓ Branch 2 taken 583 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 583 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 583 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 583 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 583 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 583 times.
✗ Branch 18 not taken.
 2332 oMi.rotation() * v.angular());
96 ✗ default:
97 ✗ throw std::invalid_argument("Bad reference frame.");
98 }
99 }
100
101 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
102 327 inline MotionTpl<Scalar, Options> getFrameAcceleration(
103 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
104 const DataTpl<Scalar, Options, JointCollectionTpl> & data,
105 const JointIndex joint_id,
106 const SE3Tpl<Scalar, Options> & placement,
107 const ReferenceFrame rf)
108 {
109 PINOCCHIO_UNUSED_VARIABLE(model);
110
1/2
✓ Branch 1 taken 327 times.
✗ Branch 2 not taken.
 327 assert(model.check(data) && "data is not consistent with model.");
111
112 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
113 typedef typename Model::Motion Motion;
114
115 327 const typename Model::SE3 & oMi = data.oMi[joint_id];
116 327 const typename Model::Motion & a = data.a[joint_id];
117
3/4
✓ Branch 0 taken 119 times.
✓ Branch 1 taken 104 times.
✓ Branch 2 taken 104 times.
✗ Branch 3 not taken.
 327 switch (rf)
118 {
119 119 case LOCAL:
120 119 return placement.actInv(a);
121 104 case WORLD:
122 104 return oMi.act(a);
123 104 case LOCAL_WORLD_ALIGNED:
124 return Motion(
125
4/8
✓ Branch 1 taken 104 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 104 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 104 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 104 times.
✗ Branch 11 not taken.
 312 oMi.rotation() * (a.linear() + a.angular().cross(placement.translation())),
126
6/12
✓ Branch 2 taken 104 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 104 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 104 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 104 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 104 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 104 times.
✗ Branch 18 not taken.
 416 oMi.rotation() * a.angular());
127 ✗ default:
128 ✗ throw std::invalid_argument("Bad reference frame.");
129 }
130 }
131
132 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
133 17 inline MotionTpl<Scalar, Options> getFrameClassicalAcceleration(
134 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
135 const DataTpl<Scalar, Options, JointCollectionTpl> & data,
136 const JointIndex joint_id,
137 const SE3Tpl<Scalar, Options> & placement,
138 const ReferenceFrame rf)
139 {
140
2/4
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 17 times.
✗ Branch 4 not taken.
 17 assert(model.check(data) && "data is not consistent with model.");
141
142 typedef MotionTpl<Scalar, Options> Motion;
143
1/2
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
 17 const Motion vel = getFrameVelocity(model, data, joint_id, placement, rf);
144
1/2
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
 17 Motion acc = getFrameAcceleration(model, data, joint_id, placement, rf);
145
146
5/10
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 17 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 17 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 17 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 17 times.
✗ Branch 14 not taken.
 17 acc.linear() += vel.angular().cross(vel.linear());
147
148 34 return acc;
149 }
150
151 template<
152 typename Scalar,
153 int Options,
154 template<typename, int> class JointCollectionTpl,
155 typename Matrix6xLike>
156 605 inline void getFrameJacobian(
157 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
158 DataTpl<Scalar, Options, JointCollectionTpl> & data,
159 const JointIndex joint_id,
160 const SE3Tpl<Scalar, Options> & placement,
161 const ReferenceFrame reference_frame,
162 const Eigen::MatrixBase<Matrix6xLike> & J)
163 {
164
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 605 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.
 605 PINOCCHIO_CHECK_ARGUMENT_SIZE(J.rows(), 6);
165
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 605 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.
 605 PINOCCHIO_CHECK_ARGUMENT_SIZE(J.cols(), model.nv);
166
1/4
✗ Branch 0 not taken.
✓ Branch 1 taken 605 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
 605 PINOCCHIO_CHECK_INPUT_ARGUMENT(
167 joint_id < (JointIndex)model.njoints, "The index of the Joint is outside the bounds.");
168
2/4
✓ Branch 1 taken 605 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 605 times.
✗ Branch 4 not taken.
 605 assert(model.check(data) && "data is not consistent with model.");
169
170 typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
171
172
1/2
✓ Branch 2 taken 605 times.
✗ Branch 3 not taken.
 605 const typename Data::SE3 oMframe = data.oMi[joint_id] * placement;
173 605 details::translateJointJacobian(
174
1/2
✓ Branch 2 taken 605 times.
✗ Branch 3 not taken.
 605 model, data, joint_id, reference_frame, oMframe, data.J, J.const_cast_derived());
175 605 }
176
177 template<
178 typename Scalar,
179 int Options,
180 template<typename, int> class JointCollectionTpl,
181 typename ConfigVectorType,
182 typename Matrix6xLike>
183 573 inline void computeFrameJacobian(
184 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
185 DataTpl<Scalar, Options, JointCollectionTpl> & data,
186 const Eigen::MatrixBase<ConfigVectorType> & q,
187 const FrameIndex frameId,
188 const ReferenceFrame reference_frame,
189 const Eigen::MatrixBase<Matrix6xLike> & J)
190 {
191
1/2
✓ Branch 1 taken 573 times.
✗ Branch 2 not taken.
 573 assert(model.check(data) && "data is not consistent with model.");
192
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 573 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.
 573 PINOCCHIO_CHECK_ARGUMENT_SIZE(
193 q.size(), model.nq, "The configuration vector is not of right size");
194
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 573 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.
 573 PINOCCHIO_CHECK_ARGUMENT_SIZE(
195 J.cols(), model.nv,
196 "The numbers of columns in the Jacobian matrix does not math the "
197 "number of Dofs in the model.");
198
199 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
200 typedef typename Model::Frame Frame;
201 typedef typename Model::JointIndex JointIndex;
202 typedef typename Model::IndexVector IndexVector;
203
204 573 const Frame & frame = model.frames[frameId];
205 573 const JointIndex & joint_id = frame.parentJoint;
206 573 const IndexVector & joint_support = model.supports[joint_id];
207
208
3/4
✓ Branch 0 taken 189 times.
✓ Branch 1 taken 189 times.
✓ Branch 2 taken 195 times.
✗ Branch 3 not taken.
 573 switch (reference_frame)
209 {
210 // In WORLD and LOCAL_WORLD_ALIGNED we first compute the WORLD jacobian.
211 // Then, in LOCAL_WORLD_ALIGNED, we will apply the translation to frameId.
212 189 case WORLD: {
213 typedef impl::JointJacobianWorldForwardStep<
214 Scalar, Options, JointCollectionTpl, ConfigVectorType, Matrix6xLike>
215 ForwardPass;
216 typedef impl::JointJacobianWorldMimicStep<Scalar, Options, JointCollectionTpl, Matrix6xLike>
217 MimicPass;
218
2/2
✓ Branch 1 taken 966 times.
✓ Branch 2 taken 189 times.
 1155 for (size_t k = 1; k < joint_support.size(); k++)
219 {
220 966 JointIndex parent = joint_support[k];
221
1/2
✓ Branch 1 taken 966 times.
✗ Branch 2 not taken.
 966 ForwardPass::run(
222 966 model.joints[parent], data.joints[parent],
223 1932 typename ForwardPass::ArgsType(model, data, q.derived(), J.const_cast_derived()));
224 }
225
226 189 const IndexVector & mimic_joint_support = model.mimic_joint_supports[joint_id];
227
2/2
✓ Branch 1 taken 38 times.
✓ Branch 2 taken 189 times.
 227 for (size_t i = 1; i < mimic_joint_support.size(); i++)
228 {
229
1/2
✓ Branch 1 taken 38 times.
✗ Branch 2 not taken.
 38 MimicPass::run(
230 38 model.joints[mimic_joint_support[i]], data.joints[mimic_joint_support[i]],
231 76 typename MimicPass::ArgsType(data, J.const_cast_derived()));
232 }
233 189 break;
234 }
235 189 case LOCAL_WORLD_ALIGNED: {
236 typedef impl::ForwardKinematicZeroStep<Scalar, Options, JointCollectionTpl, ConfigVectorType>
237 KinematicsPass;
238 typedef impl::JointJacobianLocalWorldAlignedForwardStep<
239 Scalar, Options, JointCollectionTpl, Matrix6xLike>
240 JacobianPass;
241
2/2
✓ Branch 1 taken 966 times.
✓ Branch 2 taken 189 times.
 1155 for (size_t k = 1; k < joint_support.size(); k++)
242 {
243 966 JointIndex support_idx = joint_support[k];
244
1/2
✓ Branch 1 taken 966 times.
✗ Branch 2 not taken.
 966 KinematicsPass::run(
245 966 model.joints[support_idx], data.joints[support_idx],
246 1932 typename KinematicsPass::ArgsType(model, data, q.derived()));
247 }
248 189 auto & oMframe = data.oMf[frameId];
249
1/2
✓ Branch 3 taken 189 times.
✗ Branch 4 not taken.
 189 oMframe = data.oMi[joint_id] * frame.placement;
250
2/2
✓ Branch 1 taken 966 times.
✓ Branch 2 taken 189 times.
 1155 for (size_t k = 1; k < joint_support.size(); k++)
251 {
252 966 JointIndex support_idx = joint_support[k];
253
1/2
✓ Branch 1 taken 966 times.
✗ Branch 2 not taken.
 966 JacobianPass::run(
254 966 model.joints[support_idx], data.joints[support_idx],
255 1932 typename JacobianPass::ArgsType(data, oMframe, J.const_cast_derived()));
256 }
257 189 break;
258 }
259 195 case LOCAL: {
260 195 data.iMf[joint_id] = frame.placement;
261
262 typedef impl::JointJacobianLocalBackwardStep<
263 Scalar, Options, JointCollectionTpl, ConfigVectorType, Matrix6xLike>
264 BackwardPass;
265 typedef impl::JointJacobianLocalMimicStep<Scalar, Options, JointCollectionTpl, Matrix6xLike>
266 MimicPass;
267
268
2/2
✓ Branch 0 taken 992 times.
✓ Branch 1 taken 195 times.
 1187 for (JointIndex i = joint_id; i > 0; i = model.parents[i])
269 {
270
1/2
✓ Branch 1 taken 992 times.
✗ Branch 2 not taken.
 992 BackwardPass::run(
271 992 model.joints[i], data.joints[i],
272 1984 typename BackwardPass::ArgsType(model, data, q.derived(), J.const_cast_derived()));
273 }
274
275 const typename Model::IndexVector & mimic_joint_support =
276 195 model.mimic_joint_supports[joint_id];
277
2/2
✓ Branch 1 taken 38 times.
✓ Branch 2 taken 195 times.
 233 for (size_t i = 1; i < mimic_joint_support.size(); i++)
278 {
279
1/2
✓ Branch 1 taken 38 times.
✗ Branch 2 not taken.
 38 MimicPass::run(
280 38 model.joints[mimic_joint_support[i]], data.joints[mimic_joint_support[i]],
281 76 typename MimicPass::ArgsType(data, J.const_cast_derived()));
282 }
283 195 break;
284 }
285 }
286 573 }
287
288 template<
289 typename Scalar,
290 int Options,
291 template<typename, int> class JointCollectionTpl,
292 typename Matrix6xLike>
293 8 void getFrameJacobianTimeVariation(
294 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
295 DataTpl<Scalar, Options, JointCollectionTpl> & data,
296 const FrameIndex frame_id,
297 const ReferenceFrame rf,
298 const Eigen::MatrixBase<Matrix6xLike> & dJ_)
299 {
300
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 assert(model.check(data) && "data is not consistent with model.");
301
302 8 Matrix6xLike & dJ = dJ_.const_cast_derived();
303
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 8 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(
304 dJ.cols(), model.nv,
305 "The numbers of columns in the Jacobian matrix does not math the "
306 "number of Dofs in the model.");
307
308 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
309 typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
310 typedef typename Model::Frame Frame;
311 typedef typename Data::SE3 SE3;
312 typedef typename SE3::Vector3 Vector3;
313 typedef typename Data::Motion Motion;
314
315 8 const Frame & frame = model.frames[frame_id];
316 8 const JointIndex & joint_id = frame.parentJoint;
317
318 8 typename Data::SE3 & oMframe = data.oMf[frame_id];
319
1/2
✓ Branch 3 taken 8 times.
✗ Branch 4 not taken.
 8 oMframe = data.oMi[joint_id] * frame.placement;
320
321 8 details::translateJointJacobian(
322 8 model, data, joint_id, rf, oMframe, data.dJ, dJ.const_cast_derived());
323
324 // Add contribution for LOCAL and LOCAL_WORLD_ALIGNED
325
3/3
✓ Branch 0 taken 4 times.
✓ Branch 1 taken 2 times.
✓ Branch 2 taken 2 times.
 8 switch (rf)
326 {
327 4 case LOCAL: {
328 4 const Motion & v_joint = data.v[joint_id];
329
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 4 const Motion v_frame = frame.placement.actInv(v_joint);
330
331
2/4
✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 4 times.
✗ Branch 7 not taken.
 4 const int colRef = nv(model.joints[joint_id]) + idx_v(model.joints[joint_id]) - 1;
332
2/2
✓ Branch 0 taken 40 times.
✓ Branch 1 taken 4 times.
 44 for (Eigen::DenseIndex j = colRef; j >= 0; j = data.parents_fromRow[(size_t)j])
333 {
334 typedef typename Data::Matrix6x::ColXpr ColXprIn;
335 typedef const MotionRef<ColXprIn> MotionIn;
336
337 typedef typename Matrix6xLike::ColXpr ColXprOut;
338 typedef MotionRef<ColXprOut> MotionOut;
339
2/4
✓ Branch 1 taken 40 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 40 times.
✗ Branch 5 not taken.
 40 MotionIn v_in(data.J.col(j));
340
2/4
✓ Branch 1 taken 40 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 40 times.
✗ Branch 5 not taken.
 40 MotionOut v_out(dJ.col(j));
341
342
3/6
✓ Branch 1 taken 40 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 40 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 40 times.
✗ Branch 8 not taken.
 40 v_out -= v_frame.cross(oMframe.actInv(v_in));
343 }
344 4 break;
345 }
346 2 case LOCAL_WORLD_ALIGNED: {
347 2 const Motion & ov_joint = data.ov[joint_id];
348 2 const int colRef = nv(model.joints[joint_id]) + idx_v(model.joints[joint_id]) - 1;
349
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])
350 {
351 typedef typename Data::Matrix6x::ColXpr ColXprIn;
352 typedef const MotionRef<ColXprIn> MotionIn;
353
354 typedef typename Matrix6xLike::ColXpr ColXprOut;
355 typedef MotionRef<ColXprOut> MotionOut;
356
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
 20 MotionIn v_in(data.J.col(j));
357
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
 20 MotionOut v_out(dJ.col(j));
358
359
3/6
✓ 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.
 20 v_out.linear() -=
360
6/12
✓ 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.
✓ Branch 16 taken 20 times.
✗ Branch 17 not taken.
 20 Vector3(ov_joint.linear() + ov_joint.angular().cross(oMframe.translation()))
361
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 40 .cross(v_in.angular());
362 }
363 2 break;
364 }
365
366 2 case WORLD:
367 default:
368 2 break;
369 }
370 8 }
371
372 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
373 3 InertiaTpl<Scalar, Options> computeSupportedInertiaByFrame(
374 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
375 const DataTpl<Scalar, Options, JointCollectionTpl> & data,
376 const FrameIndex frame_id,
377 bool with_subtree)
378 {
379
2/4
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 3 times.
✗ Branch 4 not taken.
 3 assert(model.check(data) && "data is not consistent with model.");
380
381 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
382 typedef InertiaTpl<Scalar, Options> Inertia;
383
384 3 const Frame & frame = model.frames[frame_id];
385 3 const JointIndex & joint_id = frame.parentJoint;
386
387 // Add all the inertia of child frames (i.e that are part of the same joint but comes after
388 // the given frame)
389
1/2
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
 3 std::vector<typename Model::JointIndex> child_frames = {frame_id};
390
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 Inertia I = frame.placement.act(frame.inertia); // Express the inertia in the parent joint frame
391
2/2
✓ Branch 0 taken 105 times.
✓ Branch 1 taken 3 times.
 108 for (FrameIndex i = frame_id + 1; i < (FrameIndex)model.nframes; ++i)
392 {
393
2/2
✓ Branch 1 taken 102 times.
✓ Branch 2 taken 3 times.
 105 if (model.frames[i].parentJoint != joint_id)
394 102 continue;
395 3 if (
396
1/2
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
 3 std::find(child_frames.begin(), child_frames.end(), model.frames[i].parentFrame)
397
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 3 times.
 6 == child_frames.end())
398 ✗ continue;
399
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 child_frames.push_back(i);
400
2/4
✓ Branch 3 taken 3 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 3 times.
✗ Branch 7 not taken.
 3 I += model.frames[i].placement.act(model.frames[i].inertia);
401 }
402
403
2/2
✓ Branch 0 taken 2 times.
✓ Branch 1 taken 1 times.
 3 if (!with_subtree)
404 {
405
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 return frame.placement.actInv(I);
406 }
407
408 // Express the inertia in the origin frame for simplicity.
409
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 I = data.oMi[joint_id].act(I);
410
411 // Add inertia of child joints
412 1 const std::vector<typename Model::JointIndex> & subtree = model.subtrees[joint_id];
413
2/2
✓ Branch 1 taken 14 times.
✓ Branch 2 taken 1 times.
 15 for (size_t k = 1; k < subtree.size();
414 ++k) // Skip the first joint as it is the one before the frame
415 {
416 14 const typename Model::JointIndex j_id = subtree[k];
417
2/4
✓ Branch 3 taken 14 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 14 times.
✗ Branch 7 not taken.
 14 I += data.oMi[j_id].act(model.inertias[j_id]);
418 }
419
420
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 const pinocchio::SE3 oMf = data.oMi[joint_id] * frame.placement;
421
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 return oMf.actInv(I);
422 3 }
423
424 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
425 1 ForceTpl<Scalar, Options> computeSupportedForceByFrame(
426 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
427 const DataTpl<Scalar, Options, JointCollectionTpl> & data,
428 const FrameIndex frame_id)
429 {
430 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
431 typedef InertiaTpl<Scalar, Options> Inertia;
432 typedef MotionTpl<Scalar, Options> Motion;
433 typedef ForceTpl<Scalar, Options> Force;
434
435 1 const Frame & frame = model.frames[frame_id];
436 1 const JointIndex & joint_id = frame.parentJoint;
437
438 // Compute 'in body' forces
439
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 const Inertia fI = computeSupportedInertiaByFrame(model, data, frame_id, false);
440
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 const pinocchio::SE3 oMf = data.oMi[joint_id] * frame.placement;
441
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 const Motion v = getFrameVelocity(model, data, frame_id, LOCAL);
442
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 const Motion a = getFrameAcceleration(model, data, frame_id, LOCAL);
443
5/10
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
 1 Force f = fI.vxiv(v) + fI * (a - oMf.actInv(model.gravity));
444
445 // Add child joints forces
446
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 f = frame.placement.act(f); // Express force in parent joint frame
447 1 const std::vector<typename Model::JointIndex> & subtree = model.subtrees[joint_id];
448
2/2
✓ Branch 1 taken 14 times.
✓ Branch 2 taken 1 times.
 15 for (size_t k = 1; k < subtree.size();
449 ++k) // Skip the first joint as it is the one before the frame
450 {
451 14 const typename Model::JointIndex j_id = subtree[k];
452
2/2
✓ Branch 1 taken 11 times.
✓ Branch 2 taken 3 times.
 14 if (model.parents[j_id] != joint_id) // Joint is not a direct child
453 {
454 11 continue;
455 }
456
2/4
✓ Branch 3 taken 3 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 3 times.
✗ Branch 7 not taken.
 3 f += data.liMi[j_id].act(data.f[j_id]);
457 }
458
459 // Transform back to local frame
460
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 return frame.placement.actInv(f);
461 }
462 } // namespace pinocchio
463
464 #endif // ifndef __pinocchio_algorithm_frames_hxx__
465