GCC Code Coverage Report

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