GCC Code Coverage Report

Directory: ./
File: include/pinocchio/algorithm/frames.hxx
Date: 2025-02-12 21:03:38
Exec Total Coverage
Lines: 161 168 95.8%
Branches: 150 382 39.3%
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 5 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 5 times.
✗ Branch 2 not taken.
 5 assert(model.check(data) && "data is not consistent with model.");
44
45 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
46 5 const typename Model::Frame & frame = model.frames[frame_id];
47 5 const typename Model::JointIndex & parent = frame.parentJoint;
48
49
1/2
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
 5 data.oMf[frame_id] = data.oMi[parent] * frame.placement;
50
51 5 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 564 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 564 times.
✗ Branch 2 not taken.
 564 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 564 const typename Model::SE3 & oMi = data.oMi[joint_id];
85 564 const typename Model::Motion & v = data.v[joint_id];
86
3/4
✓ Branch 0 taken 288 times.
✓ Branch 1 taken 71 times.
✓ Branch 2 taken 205 times.
✗ Branch 3 not taken.
 564 switch (rf)
87 {
88 288 case LOCAL:
89 288 return placement.actInv(v);
90 71 case WORLD:
91 71 return oMi.act(v);
92 205 case LOCAL_WORLD_ALIGNED:
93 return Motion(
94
4/8
✓ Branch 1 taken 205 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 205 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 205 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 205 times.
✗ Branch 11 not taken.
 615 oMi.rotation() * (v.linear() + v.angular().cross(placement.translation())),
95
6/12
✓ Branch 2 taken 205 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 205 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 205 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 205 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 205 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 205 times.
✗ Branch 18 not taken.
 820 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 39 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 39 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.
 39 PINOCCHIO_CHECK_ARGUMENT_SIZE(J.rows(), 6);
165
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 39 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.
 39 PINOCCHIO_CHECK_ARGUMENT_SIZE(J.cols(), model.nv);
166
1/4
✗ Branch 0 not taken.
✓ Branch 1 taken 39 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
 39 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 39 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 39 times.
✗ Branch 4 not taken.
 39 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 39 times.
✗ Branch 3 not taken.
 39 const typename Data::SE3 oMframe = data.oMi[joint_id] * placement;
173 39 details::translateJointJacobian(
174
1/2
✓ Branch 2 taken 39 times.
✗ Branch 3 not taken.
 39 model, data, joint_id, reference_frame, oMframe, data.J, J.const_cast_derived());
175 39 }
176
177 template<
178 typename Scalar,
179 int Options,
180 template<typename, int> class JointCollectionTpl,
181 typename ConfigVectorType,
182 typename Matrix6xLike>
183 11 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 11 times.
✗ Branch 2 not taken.
 11 assert(model.check(data) && "data is not consistent with model.");
192
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 11 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.
 11 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 11 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.
 11 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 DataTpl<Scalar, Options, JointCollectionTpl> Data;
201 typedef typename Model::Frame Frame;
202 typedef typename Model::JointIndex JointIndex;
203 typedef typename Model::IndexVector IndexVector;
204
205 11 const Frame & frame = model.frames[frameId];
206 11 const JointIndex & joint_id = frame.parentJoint;
207
208 11 const IndexVector & joint_support = model.supports[joint_id];
209
210
2/3
✓ Branch 0 taken 3 times.
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 11 switch (reference_frame)
211 {
212 3 case WORLD:
213 case LOCAL_WORLD_ALIGNED: {
214 typedef impl::JointJacobiansForwardStep<
215 Scalar, Options, JointCollectionTpl, ConfigVectorType, Matrix6xLike>
216 Pass;
217
2/2
✓ Branch 1 taken 15 times.
✓ Branch 2 taken 3 times.
 18 for (size_t k = 1; k < joint_support.size(); k++)
218 {
219 15 JointIndex parent = joint_support[k];
220
1/2
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
 15 Pass::run(
221 15 model.joints[parent], data.joints[parent],
222 30 typename Pass::ArgsType(model, data, q.derived(), J.const_cast_derived()));
223 }
224
225
2/2
✓ Branch 0 taken 2 times.
✓ Branch 1 taken 1 times.
 3 if (reference_frame == LOCAL_WORLD_ALIGNED)
226 {
227 2 typename Data::SE3 & oMframe = data.oMf[frameId];
228
1/2
✓ Branch 3 taken 2 times.
✗ Branch 4 not taken.
 2 oMframe = data.oMi[joint_id] * frame.placement;
229
230 2 Matrix6xLike & J_ = J.const_cast_derived();
231
232 2 const int colRef = nv(model.joints[joint_id]) + idx_v(model.joints[joint_id]) - 1;
233
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])
234 {
235 typedef typename Matrix6xLike::ColXpr ColXprOut;
236
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));
237
238
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());
239 }
240 }
241 3 break;
242 }
243 8 case LOCAL: {
244 8 data.iMf[joint_id] = frame.placement;
245
246 typedef impl::JointJacobianForwardStep<
247 Scalar, Options, JointCollectionTpl, ConfigVectorType, Matrix6xLike>
248 Pass;
249
2/2
✓ Branch 0 taken 36 times.
✓ Branch 1 taken 8 times.
 44 for (JointIndex i = joint_id; i > 0; i = model.parents[i])
250 {
251
1/2
✓ Branch 1 taken 36 times.
✗ Branch 2 not taken.
 36 Pass::run(
252 36 model.joints[i], data.joints[i],
253 72 typename Pass::ArgsType(model, data, q.derived(), J.const_cast_derived()));
254 }
255 8 break;
256 }
257 ✗ default: {
258 ✗ assert(false && "must never happened");
259 }
260 }
261 11 }
262
263 template<
264 typename Scalar,
265 int Options,
266 template<typename, int> class JointCollectionTpl,
267 typename Matrix6xLike>
268 8 void getFrameJacobianTimeVariation(
269 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
270 DataTpl<Scalar, Options, JointCollectionTpl> & data,
271 const FrameIndex frame_id,
272 const ReferenceFrame rf,
273 const Eigen::MatrixBase<Matrix6xLike> & dJ_)
274 {
275
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 assert(model.check(data) && "data is not consistent with model.");
276
277 8 Matrix6xLike & dJ = dJ_.const_cast_derived();
278
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(
279 dJ.cols(), model.nv,
280 "The numbers of columns in the Jacobian matrix does not math the "
281 "number of Dofs in the model.");
282
283 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
284 typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
285 typedef typename Model::Frame Frame;
286 typedef typename Data::SE3 SE3;
287 typedef typename SE3::Vector3 Vector3;
288 typedef typename Data::Motion Motion;
289
290 8 const Frame & frame = model.frames[frame_id];
291 8 const JointIndex & joint_id = frame.parentJoint;
292
293 8 typename Data::SE3 & oMframe = data.oMf[frame_id];
294
1/2
✓ Branch 3 taken 8 times.
✗ Branch 4 not taken.
 8 oMframe = data.oMi[joint_id] * frame.placement;
295
296 8 details::translateJointJacobian(
297 8 model, data, joint_id, rf, oMframe, data.dJ, dJ.const_cast_derived());
298
299 // Add contribution for LOCAL and LOCAL_WORLD_ALIGNED
300
3/3
✓ Branch 0 taken 4 times.
✓ Branch 1 taken 2 times.
✓ Branch 2 taken 2 times.
 8 switch (rf)
301 {
302 4 case LOCAL: {
303 4 const Motion & v_joint = data.v[joint_id];
304
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 4 const Motion v_frame = frame.placement.actInv(v_joint);
305
306
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;
307
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])
308 {
309 typedef typename Data::Matrix6x::ColXpr ColXprIn;
310 typedef const MotionRef<ColXprIn> MotionIn;
311
312 typedef typename Matrix6xLike::ColXpr ColXprOut;
313 typedef MotionRef<ColXprOut> MotionOut;
314
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));
315
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));
316
317
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));
318 }
319 4 break;
320 }
321 2 case LOCAL_WORLD_ALIGNED: {
322 2 const Motion & ov_joint = data.ov[joint_id];
323 2 const int colRef = nv(model.joints[joint_id]) + idx_v(model.joints[joint_id]) - 1;
324
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])
325 {
326 typedef typename Data::Matrix6x::ColXpr ColXprIn;
327 typedef const MotionRef<ColXprIn> MotionIn;
328
329 typedef typename Matrix6xLike::ColXpr ColXprOut;
330 typedef MotionRef<ColXprOut> MotionOut;
331
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));
332
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));
333
334
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() -=
335
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()))
336
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 40 .cross(v_in.angular());
337 }
338 2 break;
339 }
340
341 2 case WORLD:
342 default:
343 2 break;
344 }
345 8 }
346
347 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
348 3 InertiaTpl<Scalar, Options> computeSupportedInertiaByFrame(
349 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
350 const DataTpl<Scalar, Options, JointCollectionTpl> & data,
351 const FrameIndex frame_id,
352 bool with_subtree)
353 {
354
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.");
355
356 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
357 typedef InertiaTpl<Scalar, Options> Inertia;
358
359 3 const Frame & frame = model.frames[frame_id];
360 3 const JointIndex & joint_id = frame.parentJoint;
361
362 // Add all the inertia of child frames (i.e that are part of the same joint but comes after the
363 // given frame)
364
1/2
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
 3 std::vector<typename Model::JointIndex> child_frames = {frame_id};
365
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
366
2/2
✓ Branch 0 taken 105 times.
✓ Branch 1 taken 3 times.
 108 for (FrameIndex i = frame_id + 1; i < (FrameIndex)model.nframes; ++i)
367 {
368
2/2
✓ Branch 1 taken 102 times.
✓ Branch 2 taken 3 times.
 105 if (model.frames[i].parentJoint != joint_id)
369 102 continue;
370 3 if (
371
1/2
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
 3 std::find(child_frames.begin(), child_frames.end(), model.frames[i].parentFrame)
372
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 3 times.
 6 == child_frames.end())
373 ✗ continue;
374
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 child_frames.push_back(i);
375
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);
376 }
377
378
2/2
✓ Branch 0 taken 2 times.
✓ Branch 1 taken 1 times.
 3 if (!with_subtree)
379 {
380
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 return frame.placement.actInv(I);
381 }
382
383 // Express the inertia in the origin frame for simplicity.
384
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);
385
386 // Add inertia of child joints
387 1 const std::vector<typename Model::JointIndex> & subtree = model.subtrees[joint_id];
388
2/2
✓ Branch 1 taken 14 times.
✓ Branch 2 taken 1 times.
 15 for (size_t k = 1; k < subtree.size();
389 ++k) // Skip the first joint as it is the one before the frame
390 {
391 14 const typename Model::JointIndex j_id = subtree[k];
392
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]);
393 }
394
395
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 const pinocchio::SE3 oMf = data.oMi[joint_id] * frame.placement;
396
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 return oMf.actInv(I);
397 3 }
398
399 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
400 1 ForceTpl<Scalar, Options> computeSupportedForceByFrame(
401 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
402 const DataTpl<Scalar, Options, JointCollectionTpl> & data,
403 const FrameIndex frame_id)
404 {
405 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
406 typedef InertiaTpl<Scalar, Options> Inertia;
407 typedef MotionTpl<Scalar, Options> Motion;
408 typedef ForceTpl<Scalar, Options> Force;
409
410 1 const Frame & frame = model.frames[frame_id];
411 1 const JointIndex & joint_id = frame.parentJoint;
412
413 // Compute 'in body' forces
414
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 const Inertia fI = computeSupportedInertiaByFrame(model, data, frame_id, false);
415
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 const pinocchio::SE3 oMf = data.oMi[joint_id] * frame.placement;
416
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 const Motion v = getFrameVelocity(model, data, frame_id, LOCAL);
417
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 const Motion a = getFrameAcceleration(model, data, frame_id, LOCAL);
418
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));
419
420 // Add child joints forces
421
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
422 1 const std::vector<typename Model::JointIndex> & subtree = model.subtrees[joint_id];
423
2/2
✓ Branch 1 taken 14 times.
✓ Branch 2 taken 1 times.
 15 for (size_t k = 1; k < subtree.size();
424 ++k) // Skip the first joint as it is the one before the frame
425 {
426 14 const typename Model::JointIndex j_id = subtree[k];
427
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
428 {
429 11 continue;
430 }
431
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]);
432 }
433
434 // Transform back to local frame
435
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 return frame.placement.actInv(f);
436 }
437 } // namespace pinocchio
438
439 #endif // ifndef __pinocchio_algorithm_frames_hxx__
440