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 |