GCC Code Coverage Report

Directory: ./
File: include/pinocchio/algorithm/regressor.hxx
Date: 2025-04-30 16:14:33
Exec Total Coverage
Lines: 191 191 100.0%
Branches: 244 894 27.3%
Line Branch Exec Source
1 //
2 // Copyright (c) 2018-2020 CNRS INRIA
3 //
4
5 #ifndef __pinocchio_algorithm_regressor_hxx__
6 #define __pinocchio_algorithm_regressor_hxx__
7
8 #include "pinocchio/algorithm/check.hpp"
9 #include "pinocchio/algorithm/kinematics.hpp"
10 #include "pinocchio/spatial/skew.hpp"
11 #include "pinocchio/spatial/symmetric3.hpp"
12
13 namespace pinocchio
14 {
15
16 namespace internal
17 {
18 template<
19 typename Scalar,
20 int Options,
21 template<typename, int> class JointCollectionTpl,
22 typename Matrix6xReturnType>
23 576 void computeJointKinematicRegressorGeneric(
24 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
25 const DataTpl<Scalar, Options, JointCollectionTpl> & data,
26 const JointIndex joint_id,
27 const ReferenceFrame rf,
28 const SE3Tpl<Scalar, Options> & global_frame_placement,
29 const Eigen::MatrixBase<Matrix6xReturnType> & kinematic_regressor)
30 {
31
2/4
✓ Branch 1 taken 576 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 576 times.
✗ Branch 4 not taken.
 576 assert(model.check(data) && "data is not consistent with model.");
32
2/4
✓ Branch 1 taken 576 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 576 times.
✗ Branch 4 not taken.
 576 assert(model.check(MimicChecker()) && "Function does not support mimic joints");
33
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 576 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.
 576 PINOCCHIO_CHECK_ARGUMENT_SIZE(kinematic_regressor.rows(), 6);
34
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 576 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.
 576 PINOCCHIO_CHECK_ARGUMENT_SIZE(kinematic_regressor.cols(), 6 * (model.njoints - 1));
35
36 typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
37 typedef typename Data::SE3 SE3;
38
39 576 Matrix6xReturnType & kinematic_regressor_ = kinematic_regressor.const_cast_derived();
40
1/2
✓ Branch 1 taken 576 times.
✗ Branch 2 not taken.
 576 kinematic_regressor_.setZero();
41
42 576 const SE3Tpl<Scalar, Options> & oMi = global_frame_placement;
43
1/2
✓ Branch 1 taken 576 times.
✗ Branch 2 not taken.
 576 SE3 oMp; // placement of the frame following the jointPlacement transform
44
1/2
✓ Branch 1 taken 576 times.
✗ Branch 2 not taken.
 576 SE3 iMp; // relative placement between the joint frame and the jointPlacement
45
2/2
✓ Branch 1 taken 2973 times.
✓ Branch 2 taken 576 times.
 3549 for (JointIndex i = joint_id; i > 0; i = model.parents[i])
46 {
47 2973 const JointIndex parent_id = model.parents[i];
48
2/4
✓ Branch 3 taken 2973 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 2973 times.
✗ Branch 7 not taken.
 2973 oMp = data.oMi[parent_id] * model.jointPlacements[i];
49
3/4
✓ Branch 0 taken 1009 times.
✓ Branch 1 taken 982 times.
✓ Branch 2 taken 982 times.
✗ Branch 3 not taken.
 2973 switch (rf)
50 {
51 1009 case LOCAL:
52
2/4
✓ Branch 1 taken 1009 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1009 times.
✗ Branch 5 not taken.
 1009 iMp = oMi.actInv(oMp);
53
3/6
✓ Branch 1 taken 1009 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1009 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1009 times.
✗ Branch 8 not taken.
 1009 kinematic_regressor_.template middleCols<6>((Eigen::DenseIndex)(6 * (i - 1))) =
54 iMp.toActionMatrix(); // TODO: we can avoid a copy
55 1009 break;
56 982 case LOCAL_WORLD_ALIGNED:
57
3/6
✓ Branch 1 taken 982 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 982 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 982 times.
✗ Branch 8 not taken.
 982 iMp.rotation() = oMp.rotation();
58
5/10
✓ Branch 1 taken 982 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 982 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 982 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 982 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 982 times.
✗ Branch 14 not taken.
 982 iMp.translation() = oMp.translation() - oMi.translation();
59
3/6
✓ Branch 1 taken 982 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 982 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 982 times.
✗ Branch 8 not taken.
 982 kinematic_regressor_.template middleCols<6>((Eigen::DenseIndex)(6 * (i - 1))) =
60 iMp.toActionMatrix(); // TODO: we can avoid a copy
61 982 break;
62 982 case WORLD:
63
3/6
✓ Branch 1 taken 982 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 982 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 982 times.
✗ Branch 8 not taken.
 982 kinematic_regressor_.template middleCols<6>((Eigen::DenseIndex)(6 * (i - 1))) =
64 oMp.toActionMatrix(); // TODO: we can avoid a copy
65 982 break;
66 }
67 }
68 576 }
69 } // namespace internal
70
71 template<
72 typename Scalar,
73 int Options,
74 template<typename, int> class JointCollectionTpl,
75 typename Matrix6xReturnType>
76 163 void computeJointKinematicRegressor(
77 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
78 const DataTpl<Scalar, Options, JointCollectionTpl> & data,
79 const JointIndex joint_id,
80 const ReferenceFrame rf,
81 const Eigen::MatrixBase<Matrix6xReturnType> & kinematic_regressor)
82 {
83
2/6
✓ Branch 0 taken 163 times.
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✓ Branch 3 taken 163 times.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
 163 PINOCCHIO_CHECK_INPUT_ARGUMENT(joint_id > 0 && (Eigen::DenseIndex)joint_id < model.njoints);
84 163 internal::computeJointKinematicRegressorGeneric(
85 163 model, data, joint_id, rf, data.oMi[joint_id], kinematic_regressor.const_cast_derived());
86 163 }
87
88 template<
89 typename Scalar,
90 int Options,
91 template<typename, int> class JointCollectionTpl,
92 typename Matrix6xReturnType>
93 247 void computeJointKinematicRegressor(
94 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
95 const DataTpl<Scalar, Options, JointCollectionTpl> & data,
96 const JointIndex joint_id,
97 const ReferenceFrame rf,
98 const SE3Tpl<Scalar, Options> & placement,
99 const Eigen::MatrixBase<Matrix6xReturnType> & kinematic_regressor)
100 {
101
2/6
✓ Branch 0 taken 247 times.
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✓ Branch 3 taken 247 times.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
 247 PINOCCHIO_CHECK_INPUT_ARGUMENT(joint_id > 0 && (Eigen::DenseIndex)joint_id < model.njoints);
102
103 typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
104 typedef typename Data::SE3 SE3;
105
106
1/2
✓ Branch 2 taken 247 times.
✗ Branch 3 not taken.
 247 const SE3 global_placement = data.oMi[joint_id] * placement;
107
108
1/2
✓ Branch 1 taken 247 times.
✗ Branch 2 not taken.
 247 internal::computeJointKinematicRegressorGeneric(
109 247 model, data, joint_id, rf, global_placement, kinematic_regressor.const_cast_derived());
110 247 }
111
112 template<
113 typename Scalar,
114 int Options,
115 template<typename, int> class JointCollectionTpl,
116 typename Matrix6xReturnType>
117 166 void computeFrameKinematicRegressor(
118 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
119 DataTpl<Scalar, Options, JointCollectionTpl> & data,
120 const FrameIndex frame_id,
121 const ReferenceFrame rf,
122 const Eigen::MatrixBase<Matrix6xReturnType> & kinematic_regressor)
123 {
124
2/6
✓ Branch 0 taken 166 times.
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✓ Branch 3 taken 166 times.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
 166 PINOCCHIO_CHECK_INPUT_ARGUMENT(frame_id > 0 && (Eigen::DenseIndex)frame_id < model.nframes);
125
126 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
127 typedef typename Model::Frame Frame;
128
129 166 const Frame & frame = model.frames[frame_id];
130
1/2
✓ Branch 4 taken 166 times.
✗ Branch 5 not taken.
 166 data.oMf[frame_id] = data.oMi[frame.parentJoint] * frame.placement;
131
132 166 internal::computeJointKinematicRegressorGeneric(
133 166 model, data, frame.parentJoint, rf, data.oMf[frame_id],
134 166 kinematic_regressor.const_cast_derived());
135 166 }
136
137 template<
138 typename Scalar,
139 int Options,
140 template<typename, int> class JointCollectionTpl,
141 typename ConfigVectorType>
142 2 inline typename DataTpl<Scalar, Options, JointCollectionTpl>::Matrix3x & computeStaticRegressor(
143 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
144 DataTpl<Scalar, Options, JointCollectionTpl> & data,
145 const Eigen::MatrixBase<ConfigVectorType> & q)
146 {
147
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 2 times.
✗ Branch 4 not taken.
 2 assert(model.check(data) && "data is not consistent with model.");
148
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 2 times.
✗ Branch 4 not taken.
 2 assert(model.check(MimicChecker()) && "Function does not support mimic joints");
149
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 2 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.
 2 PINOCCHIO_CHECK_ARGUMENT_SIZE(q.size(), model.nq);
150
151 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
152 typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
153 typedef typename Model::JointIndex JointIndex;
154 typedef typename Data::SE3 SE3;
155
156 typedef typename Data::Matrix3x Matrix3x;
157 typedef typename SizeDepType<4>::ColsReturn<Matrix3x>::Type ColsBlock;
158
159
1/2
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
 2 forwardKinematics(model, data, q.derived());
160
161 // Computes the total mass of the system
162
0/2
✗ Branch 1 not taken.
✗ Branch 2 not taken.
 2 Scalar mass = Scalar(0);
163
2/2
✓ Branch 0 taken 54 times.
✓ Branch 1 taken 2 times.
 56 for (JointIndex i = 1; i < (JointIndex)model.njoints; ++i)
164
0/4
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
 54 mass += model.inertias[(JointIndex)i].mass();
165
166
0/4
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
 2 const Scalar mass_inv = Scalar(1) / mass;
167
2/2
✓ Branch 0 taken 54 times.
✓ Branch 1 taken 2 times.
 56 for (JointIndex i = 1; i < (JointIndex)model.njoints; ++i)
168 {
169 54 const SE3 & oMi = data.oMi[i];
170 ColsBlock sr_cols =
171
1/2
✓ Branch 1 taken 54 times.
✗ Branch 2 not taken.
 54 data.staticRegressor.template middleCols<4>((Eigen::DenseIndex)(i - 1) * 4);
172
3/6
✓ 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.
 54 sr_cols.col(0) = oMi.translation();
173
3/6
✓ 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.
 54 sr_cols.template rightCols<3>() = oMi.rotation();
174
1/2
✓ Branch 1 taken 54 times.
✗ Branch 2 not taken.
 54 sr_cols *= mass_inv;
175 }
176
177 2 return data.staticRegressor;
178 }
179
180 namespace details
181 {
182 // auxiliary function for bodyRegressor: bigL(omega)*I.toDynamicParameters().tail<6>() =
183 // I.inertia() * omega
184 /*
185 template<typename Vector3Like>
186 inline Eigen::Matrix<typename
187 Vector3Like::Scalar,3,6,PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like)::Options> bigL(const
188 Eigen::MatrixBase<Vector3Like> & omega)
189 {
190 typedef typename Vector3Like::Scalar Scalar;
191 enum { Options = PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like)::Options };
192 typedef Eigen::Matrix<Scalar,3,6,Options> ReturnType;
193
194 ReturnType L;
195 L << omega[0], omega[1], Scalar(0), omega[2], Scalar(0), Scalar(0),
196 Scalar(0), omega[0], omega[1], Scalar(0), omega[2], Scalar(0),
197 Scalar(0), Scalar(0), Scalar(0), omega[0], omega[1], omega[2];
198 return L;
199 }
200 */
201
202 // auxiliary function for bodyRegressor: res += bigL(omega)
203 template<typename Vector3Like, typename OutputType>
204 inline void
205 60 addBigL(const Eigen::MatrixBase<Vector3Like> & omega, const Eigen::MatrixBase<OutputType> & out)
206 {
207 60 OutputType & res = PINOCCHIO_EIGEN_CONST_CAST(OutputType, out);
208
0/4
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
 60 res(0, 0) += omega[0];
209
0/4
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
 60 res(0, 1) += omega[1];
210
0/4
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
 60 res(0, 3) += omega[2];
211
0/4
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
 60 res(1, 1) += omega[0];
212
0/4
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
 60 res(1, 2) += omega[1];
213
0/4
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
 60 res(1, 4) += omega[2];
214
0/4
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
 60 res(2, 3) += omega[0];
215
0/4
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
 60 res(2, 4) += omega[1];
216
0/4
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
 60 res(2, 5) += omega[2];
217 60 }
218
219 // auxiliary function for bodyRegressor: res = cross(omega,bigL(omega))
220 template<typename Vector3Like, typename OutputType>
221 inline void
222 60 crossBigL(const Eigen::MatrixBase<Vector3Like> & v, const Eigen::MatrixBase<OutputType> & out)
223 {
224 typedef typename Vector3Like::Scalar Scalar;
225 60 OutputType & res = PINOCCHIO_EIGEN_CONST_CAST(OutputType, out);
226
227
15/69
✓ Branch 1 taken 60 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 60 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✓ Branch 7 taken 60 times.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
✓ Branch 10 taken 60 times.
✗ Branch 11 not taken.
✗ Branch 12 not taken.
✓ Branch 13 taken 60 times.
✗ Branch 14 not taken.
✗ Branch 15 not taken.
✓ Branch 16 taken 60 times.
✗ Branch 17 not taken.
✗ Branch 18 not taken.
✓ Branch 19 taken 60 times.
✗ Branch 20 not taken.
✗ Branch 21 not taken.
✓ Branch 22 taken 60 times.
✗ Branch 23 not taken.
✗ Branch 24 not taken.
✓ Branch 25 taken 60 times.
✗ Branch 26 not taken.
✗ Branch 27 not taken.
✓ Branch 28 taken 60 times.
✗ Branch 29 not taken.
✗ Branch 30 not taken.
✓ Branch 31 taken 60 times.
✗ Branch 32 not taken.
✗ Branch 33 not taken.
✓ Branch 34 taken 60 times.
✗ Branch 35 not taken.
✗ Branch 36 not taken.
✓ Branch 37 taken 60 times.
✗ Branch 38 not taken.
✗ Branch 39 not taken.
✓ Branch 40 taken 60 times.
✗ Branch 41 not taken.
✗ Branch 42 not taken.
✓ Branch 43 taken 60 times.
✗ Branch 44 not taken.
✗ Branch 45 not taken.
✗ Branch 47 not taken.
✗ Branch 48 not taken.
✗ Branch 50 not taken.
✗ Branch 51 not taken.
✗ Branch 53 not taken.
✗ Branch 54 not taken.
✗ Branch 56 not taken.
✗ Branch 57 not taken.
✗ Branch 59 not taken.
✗ Branch 60 not taken.
✗ Branch 62 not taken.
✗ Branch 63 not taken.
✗ Branch 65 not taken.
✗ Branch 66 not taken.
✗ Branch 68 not taken.
✗ Branch 69 not taken.
✗ Branch 71 not taken.
✗ Branch 72 not taken.
✗ Branch 74 not taken.
✗ Branch 75 not taken.
✗ Branch 77 not taken.
✗ Branch 78 not taken.
✗ Branch 80 not taken.
✗ Branch 81 not taken.
 60 res << Scalar(0), -v[2] * v[0], -v[2] * v[1], v[1] * v[0], v[1] * v[1] - v[2] * v[2],
228
18/56
✓ Branch 1 taken 60 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 60 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 60 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 60 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 60 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 60 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 60 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 60 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 60 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 60 times.
✗ Branch 29 not taken.
✓ Branch 31 taken 60 times.
✗ Branch 32 not taken.
✓ Branch 34 taken 60 times.
✗ Branch 35 not taken.
✓ Branch 37 taken 60 times.
✗ Branch 38 not taken.
✓ Branch 40 taken 60 times.
✗ Branch 41 not taken.
✓ Branch 43 taken 60 times.
✗ Branch 44 not taken.
✓ Branch 46 taken 60 times.
✗ Branch 47 not taken.
✓ Branch 49 taken 60 times.
✗ Branch 50 not taken.
✓ Branch 52 taken 60 times.
✗ Branch 53 not taken.
✗ Branch 55 not taken.
✗ Branch 56 not taken.
✗ Branch 58 not taken.
✗ Branch 59 not taken.
✗ Branch 61 not taken.
✗ Branch 62 not taken.
✗ Branch 64 not taken.
✗ Branch 65 not taken.
✗ Branch 67 not taken.
✗ Branch 68 not taken.
✗ Branch 70 not taken.
✗ Branch 71 not taken.
✗ Branch 73 not taken.
✗ Branch 74 not taken.
✗ Branch 76 not taken.
✗ Branch 77 not taken.
✗ Branch 79 not taken.
✗ Branch 80 not taken.
✗ Branch 82 not taken.
✗ Branch 83 not taken.
 60 v[2] * v[1], v[2] * v[0], v[2] * v[1], Scalar(0), v[2] * v[2] - v[0] * v[0], -v[1] * v[0],
229
17/52
✓ Branch 1 taken 60 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 60 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 60 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 60 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 60 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 60 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 60 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 60 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 60 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 60 times.
✗ Branch 29 not taken.
✓ Branch 31 taken 60 times.
✗ Branch 32 not taken.
✓ Branch 34 taken 60 times.
✗ Branch 35 not taken.
✓ Branch 37 taken 60 times.
✗ Branch 38 not taken.
✓ Branch 40 taken 60 times.
✗ Branch 41 not taken.
✓ Branch 43 taken 60 times.
✗ Branch 44 not taken.
✓ Branch 46 taken 60 times.
✗ Branch 47 not taken.
✓ Branch 49 taken 60 times.
✗ Branch 50 not taken.
✗ Branch 52 not taken.
✗ Branch 53 not taken.
✗ Branch 55 not taken.
✗ Branch 56 not taken.
✗ Branch 58 not taken.
✗ Branch 59 not taken.
✗ Branch 61 not taken.
✗ Branch 62 not taken.
✗ Branch 64 not taken.
✗ Branch 65 not taken.
✗ Branch 67 not taken.
✗ Branch 68 not taken.
✗ Branch 70 not taken.
✗ Branch 71 not taken.
✗ Branch 73 not taken.
✗ Branch 74 not taken.
✗ Branch 76 not taken.
✗ Branch 77 not taken.
 60 -v[2] * v[0], -v[1] * v[0], v[0] * v[0] - v[1] * v[1], v[1] * v[0], -v[2] * v[1],
230
4/8
✓ Branch 1 taken 60 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 60 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 60 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 60 times.
✗ Branch 11 not taken.
 60 v[2] * v[0], Scalar(0);
231 60 }
232 } // namespace details
233
234 template<typename MotionVelocity, typename MotionAcceleration, typename OutputType>
235 60 inline void bodyRegressor(
236 const MotionDense<MotionVelocity> & v,
237 const MotionDense<MotionAcceleration> & a,
238 const Eigen::MatrixBase<OutputType> & regressor)
239 {
240
2/4
✓ Branch 1 taken 60 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 60 times.
✗ Branch 5 not taken.
 60 PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE(OutputType, regressor, 6, 10);
241
242 typedef typename MotionVelocity::Scalar Scalar;
243 enum
244 {
245 Options = PINOCCHIO_EIGEN_PLAIN_TYPE(typename MotionVelocity::Vector3)::Options
246 };
247
248 typedef Symmetric3Tpl<Scalar, Options> Symmetric3;
249 typedef typename Symmetric3::SkewSquare SkewSquare;
250 using ::pinocchio::details::addBigL;
251 using ::pinocchio::details::crossBigL;
252
253 60 OutputType & res = PINOCCHIO_EIGEN_CONST_CAST(OutputType, regressor);
254
255
4/8
✓ Branch 1 taken 60 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 60 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 60 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 60 times.
✗ Branch 11 not taken.
 120 res.template block<3, 1>(MotionVelocity::LINEAR, 0) =
256
3/6
✓ Branch 1 taken 60 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 60 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 60 times.
✗ Branch 8 not taken.
 120 a.linear() + v.angular().cross(v.linear());
257 60 const Eigen::Block<OutputType, 3, 1> & acc =
258
1/2
✓ Branch 1 taken 60 times.
✗ Branch 2 not taken.
 60 res.template block<3, 1>(MotionVelocity::LINEAR, 0);
259
3/6
✓ Branch 1 taken 60 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 60 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 60 times.
✗ Branch 8 not taken.
 60 res.template block<3, 3>(MotionVelocity::LINEAR, 1) =
260
3/6
✓ Branch 1 taken 60 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 60 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 60 times.
✗ Branch 9 not taken.
 60 Symmetric3(SkewSquare(v.angular())).matrix();
261
3/6
✓ Branch 1 taken 60 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 60 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 60 times.
✗ Branch 8 not taken.
 60 addSkew(a.angular(), res.template block<3, 3>(MotionVelocity::LINEAR, 1));
262
263
2/4
✓ Branch 1 taken 60 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 60 times.
✗ Branch 5 not taken.
 60 res.template block<3, 6>(MotionVelocity::LINEAR, 4).setZero();
264
265
2/4
✓ Branch 1 taken 60 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 60 times.
✗ Branch 5 not taken.
 60 res.template block<3, 1>(MotionVelocity::ANGULAR, 0).setZero();
266
3/6
✓ Branch 1 taken 60 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 60 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 60 times.
✗ Branch 8 not taken.
 60 skew(-acc, res.template block<3, 3>(MotionVelocity::ANGULAR, 1));
267 // res.template block<3,6>(MotionVelocity::ANGULAR,4) = bigL(a.angular()) + cross(v.angular(),
268 // bigL(v.angular()));
269
3/6
✓ Branch 1 taken 60 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 60 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 60 times.
✗ Branch 8 not taken.
 60 crossBigL(v.angular(), res.template block<3, 6>(MotionVelocity::ANGULAR, 4));
270
3/6
✓ Branch 1 taken 60 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 60 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 60 times.
✗ Branch 8 not taken.
 60 addBigL(a.angular(), res.template block<3, 6>(MotionVelocity::ANGULAR, 4));
271 60 }
272
273 template<typename MotionVelocity, typename MotionAcceleration>
274 inline Eigen::Matrix<
275 typename MotionVelocity::Scalar,
276 6,
277 10,
278 PINOCCHIO_EIGEN_PLAIN_TYPE(typename MotionVelocity::Vector3)::Options>
279 2 bodyRegressor(const MotionDense<MotionVelocity> & v, const MotionDense<MotionAcceleration> & a)
280 {
281 typedef typename MotionVelocity::Scalar Scalar;
282 enum
283 {
284 Options = PINOCCHIO_EIGEN_PLAIN_TYPE(typename MotionVelocity::Vector3)::Options
285 };
286 typedef Eigen::Matrix<Scalar, 6, 10, Options> ReturnType;
287
288 2 ReturnType res;
289
0/2
✗ Branch 1 not taken.
✗ Branch 2 not taken.
 2 bodyRegressor(v, a, res);
290 2 return res;
291 }
292
293 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
294 inline typename DataTpl<Scalar, Options, JointCollectionTpl>::BodyRegressorType &
295 56 jointBodyRegressor(
296 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
297 DataTpl<Scalar, Options, JointCollectionTpl> & data,
298 JointIndex joint_id)
299 {
300
1/2
✓ Branch 1 taken 56 times.
✗ Branch 2 not taken.
 56 assert(model.check(data) && "data is not consistent with model.");
301
2/4
✓ Branch 1 taken 56 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 56 times.
✗ Branch 4 not taken.
 56 assert(model.check(MimicChecker()) && "Function does not support mimic joints");
302
303 PINOCCHIO_UNUSED_VARIABLE(model);
304
305 56 bodyRegressor(data.v[joint_id], data.a_gf[joint_id], data.bodyRegressor);
306 56 return data.bodyRegressor;
307 }
308
309 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
310 inline typename DataTpl<Scalar, Options, JointCollectionTpl>::BodyRegressorType &
311 2 frameBodyRegressor(
312 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
313 DataTpl<Scalar, Options, JointCollectionTpl> & data,
314 FrameIndex frame_id)
315 {
316
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 assert(model.check(data) && "data is not consistent with model.");
317
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 2 times.
✗ Branch 4 not taken.
 2 assert(model.check(MimicChecker()) && "Function does not support mimic joints");
318
319 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
320 typedef typename Model::Frame Frame;
321 typedef typename Model::JointIndex JointIndex;
322 typedef typename Model::SE3 SE3;
323
324 2 const Frame & frame = model.frames[frame_id];
325 2 const JointIndex & parent = frame.parentJoint;
326 2 const SE3 & placement = frame.placement;
327
328
2/4
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
 4 bodyRegressor(
329 4 placement.actInv(data.v[parent]), placement.actInv(data.a_gf[parent]), data.bodyRegressor);
330 2 return data.bodyRegressor;
331 }
332
333 template<
334 typename Scalar,
335 int Options,
336 template<typename, int> class JointCollectionTpl,
337 typename ConfigVectorType,
338 typename TangentVectorType1,
339 typename TangentVectorType2>
340 struct JointTorqueRegressorForwardStep
341 : public fusion::JointUnaryVisitorBase<JointTorqueRegressorForwardStep<
342 Scalar,
343 Options,
344 JointCollectionTpl,
345 ConfigVectorType,
346 TangentVectorType1,
347 TangentVectorType2>>
348 {
349 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
350 typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
351
352 typedef boost::fusion::vector<
353 const Model &,
354 Data &,
355 const ConfigVectorType &,
356 const TangentVectorType1 &,
357 const TangentVectorType2 &>
358 ArgsType;
359
360 template<typename JointModel>
361 108 static void algo(
362 const JointModelBase<JointModel> & jmodel,
363 JointDataBase<typename JointModel::JointDataDerived> & jdata,
364 const Model & model,
365 Data & data,
366 const Eigen::MatrixBase<ConfigVectorType> & q,
367 const Eigen::MatrixBase<TangentVectorType1> & v,
368 const Eigen::MatrixBase<TangentVectorType2> & a)
369 {
370 typedef typename Model::JointIndex JointIndex;
371
372 108 const JointIndex i = jmodel.id();
373 108 const JointIndex parent = model.parents[i];
374
375 108 jmodel.calc(jdata.derived(), q.derived(), v.derived());
376
377
3/7
✗ Branch 3 not taken.
✓ Branch 4 taken 52 times.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✓ Branch 8 taken 52 times.
✗ Branch 9 not taken.
 108 data.liMi[i] = model.jointPlacements[i] * jdata.M();
378
379
0/2
✗ Branch 3 not taken.
✗ Branch 4 not taken.
 108 data.v[i] = jdata.v();
380
2/2
✓ Branch 0 taken 52 times.
✓ Branch 1 taken 2 times.
 108 if (parent > 0)
381
1/2
✓ Branch 5 taken 52 times.
✗ Branch 6 not taken.
 104 data.v[i] += data.liMi[i].actInv(data.v[parent]);
382
383
2/12
✗ Branch 3 not taken.
✓ Branch 4 taken 54 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✓ Branch 9 taken 54 times.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 12 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
 108 data.a_gf[i] = jdata.c() + (data.v[i] ^ jdata.v());
384
3/6
✓ Branch 2 taken 54 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 54 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 54 times.
✗ Branch 10 not taken.
 108 data.a_gf[i] += jdata.S() * jmodel.jointVelocitySelector(a);
385
1/2
✓ Branch 5 taken 54 times.
✗ Branch 6 not taken.
 108 data.a_gf[i] += data.liMi[i].actInv(data.a_gf[parent]);
386 }
387 };
388
389 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
390 struct JointTorqueRegressorBackwardStep
391 : public fusion::JointUnaryVisitorBase<
392 JointTorqueRegressorBackwardStep<Scalar, Options, JointCollectionTpl>>
393 {
394 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
395 typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
396 typedef
397 typename DataTpl<Scalar, Options, JointCollectionTpl>::BodyRegressorType BodyRegressorType;
398
399 typedef boost::fusion::vector<const Model &, Data &, const JointIndex &> ArgsType;
400
401 template<typename JointModel>
402 552 static void algo(
403 const JointModelBase<JointModel> & jmodel,
404 JointDataBase<typename JointModel::JointDataDerived> & jdata,
405 const Model & model,
406 Data & data,
407 const JointIndex & col_idx)
408 {
409 typedef typename Model::JointIndex JointIndex;
410
411 552 const JointIndex i = jmodel.id();
412 552 const JointIndex parent = model.parents[i];
413
414
1/2
✓ Branch 1 taken 276 times.
✗ Branch 2 not taken.
 552 data.jointTorqueRegressor.block(
415
3/8
✓ Branch 1 taken 276 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 276 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 276 times.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
 552 jmodel.idx_v(), 10 * (Eigen::DenseIndex(col_idx) - 1), jmodel.nv(), 10) =
416
1/2
✓ Branch 3 taken 222 times.
✗ Branch 4 not taken.
 552 jdata.S().transpose() * data.bodyRegressor;
417
418
2/2
✓ Branch 0 taken 222 times.
✓ Branch 1 taken 54 times.
 552 if (parent > 0)
419 444 forceSet::se3Action(data.liMi[i], data.bodyRegressor, data.bodyRegressor);
420 }
421 };
422
423 template<
424 typename Scalar,
425 int Options,
426 template<typename, int> class JointCollectionTpl,
427 typename ConfigVectorType,
428 typename TangentVectorType1,
429 typename TangentVectorType2>
430 inline typename DataTpl<Scalar, Options, JointCollectionTpl>::MatrixXs &
431 2 computeJointTorqueRegressor(
432 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
433 DataTpl<Scalar, Options, JointCollectionTpl> & data,
434 const Eigen::MatrixBase<ConfigVectorType> & q,
435 const Eigen::MatrixBase<TangentVectorType1> & v,
436 const Eigen::MatrixBase<TangentVectorType2> & a)
437 {
438
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 2 times.
✗ Branch 4 not taken.
 2 assert(model.check(data) && "data is not consistent with model.");
439
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 2 times.
✗ Branch 4 not taken.
 2 assert(model.check(MimicChecker()) && "Function does not support mimic joints");
440
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 2 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.
 2 PINOCCHIO_CHECK_ARGUMENT_SIZE(q.size(), model.nq);
441
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 2 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.
 2 PINOCCHIO_CHECK_ARGUMENT_SIZE(v.size(), model.nv);
442
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 2 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.
 2 PINOCCHIO_CHECK_ARGUMENT_SIZE(a.size(), model.nv);
443
444
1/2
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
 2 data.v[0].setZero();
445
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
 2 data.a_gf[0] = -model.gravity;
446
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 data.jointTorqueRegressor.setZero();
447
448 typedef JointTorqueRegressorForwardStep<
449 Scalar, Options, JointCollectionTpl, ConfigVectorType, TangentVectorType1, TangentVectorType2>
450 Pass1;
451
1/2
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 2 typename Pass1::ArgsType arg1(model, data, q.derived(), v.derived(), a.derived());
452
2/2
✓ Branch 0 taken 54 times.
✓ Branch 1 taken 2 times.
 56 for (JointIndex i = 1; i < (JointIndex)model.njoints; ++i)
453 {
454
2/4
✓ Branch 1 taken 54 times.
✗ Branch 2 not taken.
✓ Branch 6 taken 54 times.
✗ Branch 7 not taken.
 54 Pass1::run(model.joints[i], data.joints[i], arg1);
455 }
456
457 typedef JointTorqueRegressorBackwardStep<Scalar, Options, JointCollectionTpl> Pass2;
458
2/2
✓ Branch 0 taken 54 times.
✓ Branch 1 taken 2 times.
 56 for (JointIndex i = (JointIndex)model.njoints - 1; i > 0; --i)
459 {
460
1/2
✓ Branch 1 taken 54 times.
✗ Branch 2 not taken.
 54 jointBodyRegressor(model, data, i);
461
462
1/2
✓ Branch 1 taken 54 times.
✗ Branch 2 not taken.
 54 typename Pass2::ArgsType arg2(model, data, i);
463
2/2
✓ Branch 0 taken 276 times.
✓ Branch 1 taken 54 times.
 330 for (JointIndex j = i; j > 0; j = model.parents[j])
464 {
465
2/4
✓ Branch 1 taken 276 times.
✗ Branch 2 not taken.
✓ Branch 6 taken 276 times.
✗ Branch 7 not taken.
 276 Pass2::run(model.joints[j], data.joints[j], arg2);
466 }
467 }
468
469 2 return data.jointTorqueRegressor;
470 }
471
472 template<
473 typename Scalar,
474 int Options,
475 template<typename, int> class JointCollectionTpl,
476 typename ConfigVectorType,
477 typename TangentVectorType>
478 const typename DataTpl<Scalar, Options, JointCollectionTpl>::RowVectorXs &
479 1 computeKineticEnergyRegressor(
480 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
481 DataTpl<Scalar, Options, JointCollectionTpl> & data,
482 const Eigen::MatrixBase<ConfigVectorType> & q,
483 const Eigen::MatrixBase<TangentVectorType> & v)
484 {
485
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 assert(model.check(data) && "data is not consistent with model.");
486
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 1 assert(model.check(MimicChecker()) && "Function does not support mimic joints");
487
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 1 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.
 1 PINOCCHIO_CHECK_ARGUMENT_SIZE(q.size(), model.nq);
488
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 1 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.
 1 PINOCCHIO_CHECK_ARGUMENT_SIZE(v.size(), model.nv);
489
490 1 forwardKinematics(model, data, q.derived(), v.derived());
491
492 1 data.kineticEnergyRegressor.setZero();
493 // iterate over each joint and compute the kinetic energy regressor
494
2/4
✓ Branch 0 taken 27 times.
✓ Branch 1 taken 1 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
 28 for (JointIndex joint_id = 1; joint_id < (JointIndex)model.njoints; ++joint_id)
495 {
496 // linear and angular velocities
497
1/2
✓ Branch 2 taken 27 times.
✗ Branch 3 not taken.
 27 const auto linear_vel = data.v[joint_id].linear();
498
1/2
✓ Branch 2 taken 27 times.
✗ Branch 3 not taken.
 27 const auto angular_vel = data.v[joint_id].angular();
499
500
3/12
✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 27 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 27 times.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 16 not taken.
✗ Branch 17 not taken.
 27 const Scalar v_x = linear_vel[0], v_y = linear_vel[1], v_z = linear_vel[2],
501
3/12
✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 27 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 27 times.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 16 not taken.
✗ Branch 17 not taken.
 27 w_x = angular_vel[0], w_y = angular_vel[1], w_z = angular_vel[2];
502
503 auto joint_regressor =
504
1/2
✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
 27 data.kineticEnergyRegressor.template segment<10>(10 * Eigen::DenseIndex(joint_id - 1));
505
506
2/10
✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 27 times.
✗ 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.
 27 joint_regressor[0] = 0.5 * linear_vel.dot(linear_vel);
507
1/12
✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
✗ 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 16 not taken.
✗ Branch 17 not taken.
 27 joint_regressor[1] = -w_y * v_z + w_z * v_y;
508
1/10
✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
✗ 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.
 27 joint_regressor[2] = w_x * v_z - w_z * v_x;
509
1/12
✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
✗ 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 16 not taken.
✗ Branch 17 not taken.
 27 joint_regressor[3] = -w_x * v_y + w_y * v_x;
510
1/10
✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
✗ 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.
 27 joint_regressor[4] = 0.5 * w_x * w_x;
511
1/6
✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
 27 joint_regressor[5] = w_x * w_y;
512
1/10
✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
✗ 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.
 27 joint_regressor[6] = 0.5 * w_y * w_y;
513
1/6
✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
 27 joint_regressor[7] = w_x * w_z;
514
1/6
✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
 27 joint_regressor[8] = w_y * w_z;
515
1/10
✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
✗ 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.
 27 joint_regressor[9] = 0.5 * w_z * w_z;
516 }
517
518 1 return data.kineticEnergyRegressor;
519 }
520
521 template<
522 typename Scalar,
523 int Options,
524 template<typename, int> class JointCollectionTpl,
525 typename ConfigVectorType>
526 const typename DataTpl<Scalar, Options, JointCollectionTpl>::RowVectorXs &
527 1 computePotentialEnergyRegressor(
528 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
529 DataTpl<Scalar, Options, JointCollectionTpl> & data,
530 const Eigen::MatrixBase<ConfigVectorType> & q)
531 {
532
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 assert(model.check(data) && "data is not consistent with model.");
533
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 1 assert(model.check(MimicChecker()) && "Function does not support mimic joints");
534
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 1 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.
 1 PINOCCHIO_CHECK_ARGUMENT_SIZE(q.size(), model.nq);
535 typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
536
537 1 forwardKinematics(model, data, q.derived());
538
539 1 data.potentialEnergyRegressor.setZero();
540
541 // iterate over each joint and compute the kinetic energy regressor
542
2/2
✓ Branch 0 taken 27 times.
✓ Branch 1 taken 1 times.
 28 for (JointIndex joint_id = 1; joint_id < (JointIndex)model.njoints; ++joint_id)
543 {
544
1/2
✓ Branch 2 taken 27 times.
✗ Branch 3 not taken.
 27 const auto & t = data.oMi[joint_id].translation();
545
1/2
✓ Branch 2 taken 27 times.
✗ Branch 3 not taken.
 27 const auto & R = data.oMi[joint_id].rotation();
546
2/4
✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 27 times.
✗ Branch 5 not taken.
 27 const auto g = -model.gravity.linear();
547
548 auto joint_regressor =
549
1/2
✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
 27 data.potentialEnergyRegressor.template segment<10>(10 * Eigen::DenseIndex(joint_id - 1));
550
551
3/6
✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 27 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 27 times.
✗ Branch 8 not taken.
 27 const typename Data::Vector3 gravity_local = R.transpose() * g;
552
2/6
✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 27 times.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
 27 joint_regressor[0] = g.dot(t);
553
2/4
✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 27 times.
✗ Branch 5 not taken.
 27 joint_regressor.template segment<3>(1) = gravity_local;
554 }
555
556 1 return data.potentialEnergyRegressor;
557 }
558 } // namespace pinocchio
559
560 #endif // ifndef __pinocchio_algorithm_regressor_hxx__
561