GCC Code Coverage Report

Directory: ./
File: include/pinocchio/algorithm/frames-derivatives.hxx
Date: 2025-02-12 21:03:38
Exec Total Coverage
Lines: 69 73 94.5%
Branches: 104 284 36.6%
Line Branch Exec Source
1 //
2 // Copyright (c) 2020 INRIA
3 //
4
5 #ifndef __pinocchio_algorithm_frames_derivatives_hxx__
6 #define __pinocchio_algorithm_frames_derivatives_hxx__
7
8 #include "pinocchio/multibody/visitor.hpp"
9 #include "pinocchio/algorithm/check.hpp"
10 #include "pinocchio/algorithm/kinematics-derivatives.hpp"
11
12 namespace pinocchio
13 {
14 template<
15 typename Scalar,
16 int Options,
17 template<typename, int> class JointCollectionTpl,
18 typename Matrix6xOut1,
19 typename Matrix6xOut2>
20 11 void getFrameVelocityDerivatives(
21 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
22 const DataTpl<Scalar, Options, JointCollectionTpl> & data,
23 const JointIndex joint_id,
24 const SE3Tpl<Scalar, Options> & placement,
25 const ReferenceFrame rf,
26 const Eigen::MatrixBase<Matrix6xOut1> & v_partial_dq,
27 const Eigen::MatrixBase<Matrix6xOut2> & v_partial_dv)
28 {
29 typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
30
31 typedef typename Data::Matrix6x Matrix6x;
32 typedef typename Data::SE3 SE3;
33 typedef typename Data::Motion Motion;
34
35 EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(Matrix6xOut1, Matrix6x);
36 EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(Matrix6xOut2, Matrix6x);
37
38 11 Matrix6xOut1 & v_partial_dq_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut1, v_partial_dq);
39 11 Matrix6xOut2 & v_partial_dv_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut2, v_partial_dv);
40
1/2
✓ Branch 1 taken 11 times.
✗ Branch 2 not taken.
 11 getJointVelocityDerivatives(model, data, joint_id, rf, v_partial_dq_, v_partial_dv_);
41
42 typedef typename SizeDepType<1>::template ColsReturn<Matrix6xOut1>::Type ColsBlockOut1;
43 typedef MotionRef<ColsBlockOut1> MotionOut1;
44 typedef typename SizeDepType<1>::template ColsReturn<Matrix6xOut2>::Type ColsBlockOut2;
45 typedef MotionRef<ColsBlockOut2> MotionOut2;
46
47
1/2
✓ Branch 1 taken 11 times.
✗ Branch 2 not taken.
 11 Motion v_tmp;
48
4/8
✓ Branch 1 taken 11 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 11 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 11 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 11 times.
✗ Branch 12 not taken.
 11 const typename SE3::Vector3 trans = data.oMi[joint_id].rotation() * placement.translation();
49
2/4
✓ Branch 2 taken 11 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 11 times.
✗ Branch 7 not taken.
 11 const int colRef = nv(model.joints[joint_id]) + idx_v(model.joints[joint_id]) - 1;
50
3/4
✓ Branch 0 taken 3 times.
✓ Branch 1 taken 3 times.
✓ Branch 2 taken 5 times.
✗ Branch 3 not taken.
 11 switch (rf)
51 {
52 3 case WORLD:
53 // Do nothing
54 3 break;
55
56 3 case LOCAL_WORLD_ALIGNED:
57
2/2
✓ Branch 0 taken 30 times.
✓ Branch 1 taken 3 times.
 33 for (Eigen::DenseIndex col_id = colRef; col_id >= 0;
58 30 col_id = data.parents_fromRow[(size_t)col_id])
59 {
60
2/4
✓ Branch 1 taken 30 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 30 times.
✗ Branch 5 not taken.
 30 MotionOut1 m1(v_partial_dq_.col(col_id));
61
4/8
✓ Branch 1 taken 30 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 30 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 30 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 30 times.
✗ Branch 11 not taken.
 30 m1.linear() -= trans.cross(m1.angular());
62
2/4
✓ Branch 1 taken 30 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 30 times.
✗ Branch 5 not taken.
 30 MotionOut2 m2(v_partial_dv_.col(col_id));
63
4/8
✓ Branch 1 taken 30 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 30 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 30 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 30 times.
✗ Branch 11 not taken.
 30 m2.linear() -= trans.cross(m2.angular());
64 }
65 3 break;
66
67 5 case LOCAL:
68
2/2
✓ Branch 0 taken 54 times.
✓ Branch 1 taken 5 times.
 59 for (Eigen::DenseIndex col_id = colRef; col_id >= 0;
69 54 col_id = data.parents_fromRow[(size_t)col_id])
70 {
71
2/4
✓ Branch 1 taken 54 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 54 times.
✗ Branch 5 not taken.
 54 v_tmp = v_partial_dq_.col(col_id);
72
4/8
✓ Branch 1 taken 54 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 54 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 54 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 54 times.
✗ Branch 11 not taken.
 54 MotionOut1(v_partial_dq_.col(col_id)) = placement.actInv(v_tmp);
73
2/4
✓ Branch 1 taken 54 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 54 times.
✗ Branch 5 not taken.
 54 v_tmp = v_partial_dv_.col(col_id);
74
4/8
✓ Branch 1 taken 54 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 54 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 54 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 54 times.
✗ Branch 11 not taken.
 54 MotionOut2(v_partial_dv_.col(col_id)) = placement.actInv(v_tmp);
75 }
76 5 break;
77
78 ✗ default:
79 ✗ break;
80 }
81 11 }
82
83 template<
84 typename Scalar,
85 int Options,
86 template<typename, int> class JointCollectionTpl,
87 typename Matrix6xOut1,
88 typename Matrix6xOut2,
89 typename Matrix6xOut3,
90 typename Matrix6xOut4>
91 30 void getFrameAccelerationDerivatives(
92 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
93 DataTpl<Scalar, Options, JointCollectionTpl> & data,
94 const JointIndex joint_id,
95 const SE3Tpl<Scalar, Options> & placement,
96 const ReferenceFrame rf,
97 const Eigen::MatrixBase<Matrix6xOut1> & v_partial_dq,
98 const Eigen::MatrixBase<Matrix6xOut2> & a_partial_dq,
99 const Eigen::MatrixBase<Matrix6xOut3> & a_partial_dv,
100 const Eigen::MatrixBase<Matrix6xOut4> & a_partial_da)
101 {
102 typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
103
104 typedef typename Data::Matrix6x Matrix6x;
105 typedef typename Data::SE3 SE3;
106 typedef typename Data::Motion Motion;
107
108 EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(Matrix6xOut1, Matrix6x);
109 EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(Matrix6xOut2, Matrix6x);
110 EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(Matrix6xOut3, Matrix6x);
111 EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(Matrix6xOut4, Matrix6x);
112
113
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 30 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.
 30 PINOCCHIO_CHECK_ARGUMENT_SIZE(v_partial_dq.cols(), model.nv);
114
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 30 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.
 30 PINOCCHIO_CHECK_ARGUMENT_SIZE(a_partial_dq.cols(), model.nv);
115
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 30 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.
 30 PINOCCHIO_CHECK_ARGUMENT_SIZE(a_partial_dv.cols(), model.nv);
116
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 30 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.
 30 PINOCCHIO_CHECK_ARGUMENT_SIZE(a_partial_da.cols(), model.nv);
117
1/2
✓ Branch 1 taken 30 times.
✗ Branch 2 not taken.
 30 assert(model.check(data) && "data is not consistent with model.");
118
119 30 Matrix6xOut1 & v_partial_dq_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut1, v_partial_dq);
120 30 Matrix6xOut2 & a_partial_dq_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut2, a_partial_dq);
121 30 Matrix6xOut3 & a_partial_dv_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut3, a_partial_dv);
122 30 Matrix6xOut4 & a_partial_da_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut4, a_partial_da);
123
124 30 getJointAccelerationDerivatives(
125 model, data, joint_id, rf, v_partial_dq_, a_partial_dq_, a_partial_dv_, a_partial_da_);
126
127 typedef typename SizeDepType<1>::template ColsReturn<Matrix6xOut1>::Type ColsBlockOut1;
128 typedef MotionRef<ColsBlockOut1> MotionOut1;
129 typedef typename SizeDepType<1>::template ColsReturn<Matrix6xOut2>::Type ColsBlockOut2;
130 typedef MotionRef<ColsBlockOut2> MotionOut2;
131 typedef typename SizeDepType<1>::template ColsReturn<Matrix6xOut3>::Type ColsBlockOut3;
132 typedef MotionRef<ColsBlockOut3> MotionOut3;
133 typedef typename SizeDepType<1>::template ColsReturn<Matrix6xOut4>::Type ColsBlockOut4;
134 typedef MotionRef<ColsBlockOut4> MotionOut4;
135
136 30 const int colRef = nv(model.joints[joint_id]) + idx_v(model.joints[joint_id]) - 1;
137
3/4
✓ Branch 0 taken 4 times.
✓ Branch 1 taken 8 times.
✓ Branch 2 taken 18 times.
✗ Branch 3 not taken.
 30 switch (rf)
138 {
139 4 case WORLD:
140 // Do nothing
141 4 break;
142
143 8 case LOCAL_WORLD_ALIGNED: {
144
4/8
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 8 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 8 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 8 times.
✗ Branch 12 not taken.
 8 const typename SE3::Vector3 trans = data.oMi[joint_id].rotation() * placement.translation();
145
2/2
✓ Branch 0 taken 90 times.
✓ Branch 1 taken 8 times.
 98 for (Eigen::DenseIndex col_id = colRef; col_id >= 0;
146 90 col_id = data.parents_fromRow[(size_t)col_id])
147 {
148
2/4
✓ Branch 1 taken 90 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 90 times.
✗ Branch 5 not taken.
 90 MotionOut1 m1(v_partial_dq_.col(col_id));
149
4/8
✓ Branch 1 taken 90 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 90 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 90 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 90 times.
✗ Branch 11 not taken.
 90 m1.linear() -= trans.cross(m1.angular());
150
2/4
✓ Branch 1 taken 90 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 90 times.
✗ Branch 5 not taken.
 90 MotionOut2 m2(a_partial_dq_.col(col_id));
151
4/8
✓ Branch 1 taken 90 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 90 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 90 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 90 times.
✗ Branch 11 not taken.
 90 m2.linear() -= trans.cross(m2.angular());
152
2/4
✓ Branch 1 taken 90 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 90 times.
✗ Branch 5 not taken.
 90 MotionOut3 m3(a_partial_dv_.col(col_id));
153
4/8
✓ Branch 1 taken 90 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 90 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 90 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 90 times.
✗ Branch 11 not taken.
 90 m3.linear() -= trans.cross(m3.angular());
154
2/4
✓ Branch 1 taken 90 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 90 times.
✗ Branch 5 not taken.
 90 MotionOut4 m4(a_partial_da_.col(col_id));
155
4/8
✓ Branch 1 taken 90 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 90 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 90 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 90 times.
✗ Branch 11 not taken.
 90 m4.linear() -= trans.cross(m4.angular());
156 }
157 8 break;
158 }
159 18 case LOCAL: {
160
1/2
✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
 18 Motion v_tmp;
161
2/2
✓ Branch 0 taken 215 times.
✓ Branch 1 taken 18 times.
 233 for (Eigen::DenseIndex col_id = colRef; col_id >= 0;
162 215 col_id = data.parents_fromRow[(size_t)col_id])
163 {
164
2/4
✓ Branch 1 taken 215 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 215 times.
✗ Branch 5 not taken.
 215 v_tmp = v_partial_dq_.col(col_id);
165
4/8
✓ Branch 1 taken 215 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 215 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 215 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 215 times.
✗ Branch 11 not taken.
 215 MotionOut1(v_partial_dq_.col(col_id)) = placement.actInv(v_tmp);
166
2/4
✓ Branch 1 taken 215 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 215 times.
✗ Branch 5 not taken.
 215 v_tmp = a_partial_dq_.col(col_id);
167
4/8
✓ Branch 1 taken 215 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 215 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 215 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 215 times.
✗ Branch 11 not taken.
 215 MotionOut2(a_partial_dq_.col(col_id)) = placement.actInv(v_tmp);
168
2/4
✓ Branch 1 taken 215 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 215 times.
✗ Branch 5 not taken.
 215 v_tmp = a_partial_dv_.col(col_id);
169
4/8
✓ Branch 1 taken 215 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 215 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 215 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 215 times.
✗ Branch 11 not taken.
 215 MotionOut3(a_partial_dv_.col(col_id)) = placement.actInv(v_tmp);
170
2/4
✓ Branch 1 taken 215 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 215 times.
✗ Branch 5 not taken.
 215 v_tmp = a_partial_da_.col(col_id);
171
4/8
✓ Branch 1 taken 215 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 215 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 215 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 215 times.
✗ Branch 11 not taken.
 215 MotionOut4(a_partial_da_.col(col_id)) = placement.actInv(v_tmp);
172 }
173 18 break;
174 }
175 ✗ default:
176 ✗ break;
177 }
178 30 }
179 } // namespace pinocchio
180
181 #endif // ifndef __pinocchio_algorithm_frames_derivatives_hxx__
182