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 |