GCC Code Coverage Report

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