Directory: | ./ |
---|---|
File: | include/pinocchio/algorithm/rnea-derivatives.hxx |
Date: | 2024-08-27 18:20:05 |
Exec | Total | Coverage | |
---|---|---|---|
Lines: | 233 | 248 | 94.0% |
Branches: | 328 | 1221 | 26.9% |
Line | Branch | Exec | Source |
---|---|---|---|
1 | // | ||
2 | // Copyright (c) 2017-2021 CNRS INRIA | ||
3 | // | ||
4 | |||
5 | #ifndef __pinocchio_algorithm_rnea_derivatives_hxx__ | ||
6 | #define __pinocchio_algorithm_rnea_derivatives_hxx__ | ||
7 | |||
8 | #include "pinocchio/multibody/visitor.hpp" | ||
9 | #include "pinocchio/algorithm/check.hpp" | ||
10 | |||
11 | namespace pinocchio | ||
12 | { | ||
13 | namespace impl | ||
14 | { | ||
15 | template< | ||
16 | typename Scalar, | ||
17 | int Options, | ||
18 | template<typename, int> | ||
19 | class JointCollectionTpl, | ||
20 | typename ConfigVectorType> | ||
21 | struct ComputeGeneralizedGravityDerivativeForwardStep | ||
22 | : public fusion::JointUnaryVisitorBase<ComputeGeneralizedGravityDerivativeForwardStep< | ||
23 | Scalar, | ||
24 | Options, | ||
25 | JointCollectionTpl, | ||
26 | ConfigVectorType>> | ||
27 | { | ||
28 | typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model; | ||
29 | typedef DataTpl<Scalar, Options, JointCollectionTpl> Data; | ||
30 | |||
31 | typedef boost::fusion::vector<const Model &, Data &, const ConfigVectorType &> ArgsType; | ||
32 | |||
33 | template<typename JointModel> | ||
34 | 162 | static void algo( | |
35 | const JointModelBase<JointModel> & jmodel, | ||
36 | JointDataBase<typename JointModel::JointDataDerived> & jdata, | ||
37 | const Model & model, | ||
38 | Data & data, | ||
39 | const Eigen::MatrixBase<ConfigVectorType> & q) | ||
40 | { | ||
41 | typedef typename Model::JointIndex JointIndex; | ||
42 | typedef typename Data::Motion Motion; | ||
43 | |||
44 |
1/2✓ Branch 1 taken 81 times.
✗ Branch 2 not taken.
|
162 | const JointIndex i = jmodel.id(); |
45 | 162 | const JointIndex parent = model.parents[i]; | |
46 | 162 | const Motion & minus_gravity = data.oa_gf[0]; | |
47 | |||
48 |
1/2✓ Branch 3 taken 81 times.
✗ Branch 4 not taken.
|
162 | jmodel.calc(jdata.derived(), q.derived()); |
49 | |||
50 |
6/10✓ Branch 1 taken 81 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 78 times.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 78 times.
✓ Branch 9 taken 3 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 78 times.
✗ Branch 13 not taken.
|
162 | data.liMi[i] = model.jointPlacements[i] * jdata.M(); |
51 | |||
52 |
2/2✓ Branch 0 taken 78 times.
✓ Branch 1 taken 3 times.
|
162 | if (parent > 0) |
53 |
2/4✓ Branch 3 taken 78 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 78 times.
✗ Branch 8 not taken.
|
156 | data.oMi[i] = data.oMi[parent] * data.liMi[i]; |
54 | else | ||
55 |
1/2✓ Branch 3 taken 3 times.
✗ Branch 4 not taken.
|
6 | data.oMi[i] = data.liMi[i]; |
56 | |||
57 |
3/6✓ Branch 3 taken 81 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 81 times.
✗ Branch 8 not taken.
✓ Branch 11 taken 81 times.
✗ Branch 12 not taken.
|
162 | data.oYcrb[i] = data.oinertias[i] = data.oMi[i].act(model.inertias[i]); |
58 |
2/4✓ Branch 2 taken 81 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 81 times.
✗ Branch 7 not taken.
|
162 | data.of[i] = data.oYcrb[i] * minus_gravity; |
59 | |||
60 | typedef | ||
61 | typename SizeDepType<JointModel::NV>::template ColsReturn<typename Data::Matrix6x>::Type | ||
62 | ColsBlock; | ||
63 |
1/2✓ Branch 1 taken 81 times.
✗ Branch 2 not taken.
|
162 | ColsBlock J_cols = jmodel.jointCols(data.J); |
64 |
1/2✓ Branch 1 taken 81 times.
✗ Branch 2 not taken.
|
162 | ColsBlock dAdq_cols = jmodel.jointCols(data.dAdq); |
65 |
3/6✓ Branch 2 taken 81 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 81 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 81 times.
✗ Branch 9 not taken.
|
162 | J_cols = data.oMi[i].act(jdata.S()); |
66 |
1/2✓ Branch 1 taken 81 times.
✗ Branch 2 not taken.
|
162 | motionSet::motionAction(minus_gravity, J_cols, dAdq_cols); |
67 | } | ||
68 | }; | ||
69 | |||
70 | template< | ||
71 | typename Scalar, | ||
72 | int Options, | ||
73 | template<typename, int> | ||
74 | class JointCollectionTpl, | ||
75 | typename ReturnMatrixType> | ||
76 | struct ComputeGeneralizedGravityDerivativeBackwardStep | ||
77 | : public fusion::JointUnaryVisitorBase<ComputeGeneralizedGravityDerivativeBackwardStep< | ||
78 | Scalar, | ||
79 | Options, | ||
80 | JointCollectionTpl, | ||
81 | ReturnMatrixType>> | ||
82 | { | ||
83 | typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model; | ||
84 | typedef DataTpl<Scalar, Options, JointCollectionTpl> Data; | ||
85 | |||
86 | typedef boost::fusion:: | ||
87 | vector<const Model &, Data &, typename Data::VectorXs &, ReturnMatrixType &> | ||
88 | ArgsType; | ||
89 | |||
90 | template<typename JointModel> | ||
91 | 162 | static void algo( | |
92 | const JointModelBase<JointModel> & jmodel, | ||
93 | const Model & model, | ||
94 | Data & data, | ||
95 | typename Data::VectorXs & g, | ||
96 | const Eigen::MatrixBase<ReturnMatrixType> & gravity_partial_dq) | ||
97 | { | ||
98 | typedef typename Model::JointIndex JointIndex; | ||
99 | typedef Eigen::Matrix< | ||
100 | Scalar, JointModel::NV, 6, Options, JointModel::NV == Eigen::Dynamic ? 6 : JointModel::NV, | ||
101 | 6> | ||
102 | MatrixNV6; | ||
103 | |||
104 |
1/2✓ Branch 1 taken 81 times.
✗ Branch 2 not taken.
|
162 | const JointIndex i = jmodel.id(); |
105 | 162 | const JointIndex parent = model.parents[i]; | |
106 | |||
107 |
2/4✓ Branch 1 taken 81 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 81 times.
✗ Branch 5 not taken.
|
162 | typename PINOCCHIO_EIGEN_PLAIN_ROW_MAJOR_TYPE(MatrixNV6) YS(jmodel.nv(), 6); |
108 | |||
109 | typedef | ||
110 | typename SizeDepType<JointModel::NV>::template ColsReturn<typename Data::Matrix6x>::Type | ||
111 | ColsBlock; | ||
112 | |||
113 |
1/2✓ Branch 1 taken 81 times.
✗ Branch 2 not taken.
|
162 | ColsBlock J_cols = jmodel.jointCols(data.J); |
114 |
1/2✓ Branch 1 taken 81 times.
✗ Branch 2 not taken.
|
162 | ColsBlock dAdq_cols = jmodel.jointCols(data.dAdq); |
115 |
1/2✓ Branch 1 taken 81 times.
✗ Branch 2 not taken.
|
162 | ColsBlock dFdq_cols = jmodel.jointCols(data.dFdq); |
116 |
1/2✓ Branch 1 taken 81 times.
✗ Branch 2 not taken.
|
162 | ColsBlock Ag_cols = jmodel.jointCols(data.Ag); |
117 | |||
118 |
1/2✓ Branch 2 taken 81 times.
✗ Branch 3 not taken.
|
162 | motionSet::inertiaAction(data.oYcrb[i], dAdq_cols, dFdq_cols); |
119 | |||
120 | ReturnMatrixType & gravity_partial_dq_ = | ||
121 | 162 | PINOCCHIO_EIGEN_CONST_CAST(ReturnMatrixType, gravity_partial_dq); | |
122 |
4/8✓ Branch 2 taken 81 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 81 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 81 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 81 times.
✗ Branch 12 not taken.
|
162 | gravity_partial_dq_.block(jmodel.idx_v(), jmodel.idx_v(), jmodel.nv(), data.nvSubtree[i]) |
123 |
6/12✓ Branch 2 taken 81 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 81 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 81 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 81 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 81 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 81 times.
✗ Branch 18 not taken.
|
324 | .noalias() = J_cols.transpose() * data.dFdq.middleCols(jmodel.idx_v(), data.nvSubtree[i]); |
124 | |||
125 |
1/2✓ Branch 2 taken 81 times.
✗ Branch 3 not taken.
|
162 | motionSet::act<ADDTO>(J_cols, data.of[i], dFdq_cols); |
126 | |||
127 |
1/2✓ Branch 2 taken 81 times.
✗ Branch 3 not taken.
|
162 | motionSet::inertiaAction(data.oYcrb[i], J_cols, Ag_cols); |
128 |
3/4✓ Branch 1 taken 81 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 723 times.
✓ Branch 5 taken 81 times.
|
1608 | for (int j = data.parents_fromRow[(typename Model::Index)jmodel.idx_v()]; j >= 0; |
129 | 1446 | j = data.parents_fromRow[(typename Model::Index)j]) | |
130 |
6/12✓ Branch 1 taken 723 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 723 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 723 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 723 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 723 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 723 times.
✗ Branch 17 not taken.
|
1446 | gravity_partial_dq_.middleRows(jmodel.idx_v(), jmodel.nv()).col(j).noalias() = |
131 |
3/6✓ Branch 1 taken 723 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 723 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 723 times.
✗ Branch 8 not taken.
|
2892 | Ag_cols.transpose() * data.dAdq.col(j); |
132 | |||
133 |
6/12✓ Branch 2 taken 81 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 81 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 81 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 81 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 81 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 81 times.
✗ Branch 18 not taken.
|
162 | jmodel.jointVelocitySelector(g).noalias() = J_cols.transpose() * data.of[i].toVector(); |
134 |
2/2✓ Branch 0 taken 78 times.
✓ Branch 1 taken 3 times.
|
162 | if (parent > 0) |
135 | { | ||
136 |
1/2✓ Branch 3 taken 78 times.
✗ Branch 4 not taken.
|
156 | data.oYcrb[parent] += data.oYcrb[i]; |
137 |
1/2✓ Branch 3 taken 78 times.
✗ Branch 4 not taken.
|
156 | data.of[parent] += data.of[i]; |
138 | } | ||
139 | } | ||
140 | }; | ||
141 | |||
142 | template< | ||
143 | typename Scalar, | ||
144 | int Options, | ||
145 | template<typename, int> | ||
146 | class JointCollectionTpl, | ||
147 | typename ConfigVectorType, | ||
148 | typename ReturnMatrixType> | ||
149 | 2 | void computeGeneralizedGravityDerivatives( | |
150 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
151 | DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
152 | const Eigen::MatrixBase<ConfigVectorType> & q, | ||
153 | const Eigen::MatrixBase<ReturnMatrixType> & gravity_partial_dq) | ||
154 | { | ||
155 |
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( |
156 | q.size(), model.nq, "The configuration vector is not of right size"); | ||
157 |
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(gravity_partial_dq.cols(), model.nv); |
158 |
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(gravity_partial_dq.rows(), model.nv); |
159 |
1/2✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
|
2 | assert(model.check(data) && "data is not consistent with model."); |
160 | |||
161 | typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model; | ||
162 | typedef typename Model::JointIndex JointIndex; | ||
163 | |||
164 |
1/2✓ Branch 3 taken 2 times.
✗ Branch 4 not taken.
|
2 | data.oa_gf[0] = -model.gravity; // minus_gravity used in the two Passes |
165 | |||
166 | typedef ComputeGeneralizedGravityDerivativeForwardStep< | ||
167 | Scalar, Options, JointCollectionTpl, ConfigVectorType> | ||
168 | Pass1; | ||
169 |
2/2✓ Branch 0 taken 54 times.
✓ Branch 1 taken 2 times.
|
56 | for (JointIndex i = 1; i < (JointIndex)model.njoints; ++i) |
170 | { | ||
171 |
1/2✓ Branch 1 taken 54 times.
✗ Branch 2 not taken.
|
54 | Pass1::run( |
172 | 108 | model.joints[i], data.joints[i], typename Pass1::ArgsType(model, data, q.derived())); | |
173 | } | ||
174 | |||
175 | typedef ComputeGeneralizedGravityDerivativeBackwardStep< | ||
176 | Scalar, Options, JointCollectionTpl, ReturnMatrixType> | ||
177 | Pass2; | ||
178 | ReturnMatrixType & gravity_partial_dq_ = | ||
179 | 2 | PINOCCHIO_EIGEN_CONST_CAST(ReturnMatrixType, gravity_partial_dq); | |
180 |
2/2✓ Branch 0 taken 54 times.
✓ Branch 1 taken 2 times.
|
56 | for (JointIndex i = (JointIndex)(model.njoints - 1); i > 0; --i) |
181 | { | ||
182 |
1/2✓ Branch 1 taken 54 times.
✗ Branch 2 not taken.
|
54 | Pass2::run( |
183 | 108 | model.joints[i], typename Pass2::ArgsType(model, data, data.g, gravity_partial_dq_)); | |
184 | } | ||
185 | 2 | } | |
186 | |||
187 | template< | ||
188 | typename Scalar, | ||
189 | int Options, | ||
190 | template<typename, int> | ||
191 | class JointCollectionTpl, | ||
192 | typename ConfigVectorType, | ||
193 | typename ReturnMatrixType> | ||
194 | 1 | void computeStaticTorqueDerivatives( | |
195 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
196 | DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
197 | const Eigen::MatrixBase<ConfigVectorType> & q, | ||
198 | const container::aligned_vector<ForceTpl<Scalar, Options>> & fext, | ||
199 | const Eigen::MatrixBase<ReturnMatrixType> & static_torque_partial_dq) | ||
200 | { | ||
201 |
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( |
202 | q.size(), model.nq, "The configuration vector is not of right size"); | ||
203 |
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(static_torque_partial_dq.cols(), model.nv); |
204 |
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(static_torque_partial_dq.rows(), model.nv); |
205 |
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( |
206 | fext.size(), (size_t)model.njoints, "The size of the external forces is not of right size"); | ||
207 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | assert(model.check(data) && "data is not consistent with model."); |
208 | |||
209 | typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model; | ||
210 | typedef typename Model::JointIndex JointIndex; | ||
211 | |||
212 |
1/2✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
|
1 | data.oa_gf[0] = -model.gravity; // minus_gravity used in the two Passes |
213 | |||
214 | typedef ComputeGeneralizedGravityDerivativeForwardStep< | ||
215 | Scalar, Options, JointCollectionTpl, ConfigVectorType> | ||
216 | Pass1; | ||
217 |
2/2✓ Branch 0 taken 27 times.
✓ Branch 1 taken 1 times.
|
28 | for (JointIndex i = 1; i < (JointIndex)model.njoints; ++i) |
218 | { | ||
219 |
1/2✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
|
27 | Pass1::run( |
220 | 27 | model.joints[i], data.joints[i], typename Pass1::ArgsType(model, data, q.derived())); | |
221 |
1/2✓ Branch 5 taken 27 times.
✗ Branch 6 not taken.
|
27 | data.of[i] -= data.oMi[i].act(fext[i]); |
222 | } | ||
223 | |||
224 | typedef ComputeGeneralizedGravityDerivativeBackwardStep< | ||
225 | Scalar, Options, JointCollectionTpl, ReturnMatrixType> | ||
226 | Pass2; | ||
227 | ReturnMatrixType & static_torque_partial_dq_ = | ||
228 | 1 | PINOCCHIO_EIGEN_CONST_CAST(ReturnMatrixType, static_torque_partial_dq); | |
229 |
2/2✓ Branch 0 taken 27 times.
✓ Branch 1 taken 1 times.
|
28 | for (JointIndex i = (JointIndex)(model.njoints - 1); i > 0; --i) |
230 | { | ||
231 |
1/2✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
|
27 | Pass2::run( |
232 | 27 | model.joints[i], | |
233 | 54 | typename Pass2::ArgsType(model, data, data.tau, static_torque_partial_dq_)); | |
234 | } | ||
235 | 1 | } | |
236 | |||
237 | template< | ||
238 | typename Scalar, | ||
239 | int Options, | ||
240 | template<typename, int> | ||
241 | class JointCollectionTpl, | ||
242 | typename ConfigVectorType, | ||
243 | typename TangentVectorType1, | ||
244 | typename TangentVectorType2> | ||
245 | struct ComputeRNEADerivativesForwardStep | ||
246 | : public fusion::JointUnaryVisitorBase<ComputeRNEADerivativesForwardStep< | ||
247 | Scalar, | ||
248 | Options, | ||
249 | JointCollectionTpl, | ||
250 | ConfigVectorType, | ||
251 | TangentVectorType1, | ||
252 | TangentVectorType2>> | ||
253 | { | ||
254 | typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model; | ||
255 | typedef DataTpl<Scalar, Options, JointCollectionTpl> Data; | ||
256 | |||
257 | typedef boost::fusion::vector< | ||
258 | const Model &, | ||
259 | Data &, | ||
260 | const ConfigVectorType &, | ||
261 | const TangentVectorType1 &, | ||
262 | const TangentVectorType2 &> | ||
263 | ArgsType; | ||
264 | |||
265 | template<typename JointModel> | ||
266 | 9356 | static void algo( | |
267 | const JointModelBase<JointModel> & jmodel, | ||
268 | JointDataBase<typename JointModel::JointDataDerived> & jdata, | ||
269 | const Model & model, | ||
270 | Data & data, | ||
271 | const Eigen::MatrixBase<ConfigVectorType> & q, | ||
272 | const Eigen::MatrixBase<TangentVectorType1> & v, | ||
273 | const Eigen::MatrixBase<TangentVectorType2> & a) | ||
274 | { | ||
275 | typedef typename Model::JointIndex JointIndex; | ||
276 | typedef typename Data::Motion Motion; | ||
277 | |||
278 |
1/2✓ Branch 1 taken 4678 times.
✗ Branch 2 not taken.
|
9356 | const JointIndex i = jmodel.id(); |
279 | 9356 | const JointIndex parent = model.parents[i]; | |
280 | 9356 | Motion & ov = data.ov[i]; | |
281 | 9356 | Motion & oa = data.oa[i]; | |
282 | 9356 | Motion & oa_gf = data.oa_gf[i]; | |
283 | |||
284 |
1/2✓ Branch 4 taken 4678 times.
✗ Branch 5 not taken.
|
9356 | jmodel.calc(jdata.derived(), q.derived(), v.derived()); |
285 | |||
286 |
6/10✓ Branch 1 taken 4678 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4504 times.
✓ Branch 5 taken 174 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 4504 times.
✓ Branch 9 taken 174 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 4504 times.
✗ Branch 13 not taken.
|
9356 | data.liMi[i] = model.jointPlacements[i] * jdata.M(); |
287 | |||
288 |
2/4✓ Branch 1 taken 4678 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 4678 times.
✗ Branch 6 not taken.
|
9356 | data.v[i] = jdata.v(); |
289 | |||
290 |
2/2✓ Branch 0 taken 4504 times.
✓ Branch 1 taken 174 times.
|
9356 | if (parent > 0) |
291 | { | ||
292 |
2/4✓ Branch 3 taken 4504 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 4504 times.
✗ Branch 8 not taken.
|
9008 | data.oMi[i] = data.oMi[parent] * data.liMi[i]; |
293 |
2/4✓ Branch 3 taken 4504 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 4504 times.
✗ Branch 8 not taken.
|
9008 | data.v[i] += data.liMi[i].actInv(data.v[parent]); |
294 | } | ||
295 | else | ||
296 |
1/2✓ Branch 3 taken 174 times.
✗ Branch 4 not taken.
|
348 | data.oMi[i] = data.liMi[i]; |
297 | |||
298 |
5/9✓ Branch 1 taken 4678 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4505 times.
✓ Branch 5 taken 173 times.
✗ Branch 6 not taken.
✓ Branch 7 taken 4505 times.
✗ Branch 8 not taken.
✓ Branch 11 taken 4505 times.
✗ Branch 12 not taken.
|
18712 | data.a[i] = |
299 |
7/14✓ Branch 1 taken 4678 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 4678 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 4678 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 4678 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 4678 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 173 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 173 times.
✗ Branch 21 not taken.
|
18712 | jdata.S() * jmodel.jointVelocitySelector(a) + jdata.c() + (data.v[i] ^ jdata.v()); |
300 |
2/2✓ Branch 0 taken 4504 times.
✓ Branch 1 taken 174 times.
|
9356 | if (parent > 0) |
301 | { | ||
302 |
2/4✓ Branch 3 taken 4504 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 4504 times.
✗ Branch 8 not taken.
|
9008 | data.a[i] += data.liMi[i].actInv(data.a[parent]); |
303 | } | ||
304 | |||
305 |
3/6✓ Branch 3 taken 4678 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 4678 times.
✗ Branch 8 not taken.
✓ Branch 11 taken 4678 times.
✗ Branch 12 not taken.
|
9356 | data.oYcrb[i] = data.oinertias[i] = data.oMi[i].act(model.inertias[i]); |
306 |
2/4✓ Branch 3 taken 4678 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 4678 times.
✗ Branch 7 not taken.
|
9356 | ov = data.oMi[i].act(data.v[i]); |
307 |
2/4✓ Branch 3 taken 4678 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 4678 times.
✗ Branch 7 not taken.
|
9356 | oa = data.oMi[i].act(data.a[i]); |
308 |
2/4✓ Branch 1 taken 4678 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4678 times.
✗ Branch 5 not taken.
|
9356 | oa_gf = oa - model.gravity; // add gravity contribution |
309 | |||
310 |
2/4✓ Branch 2 taken 4678 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 4678 times.
✗ Branch 7 not taken.
|
9356 | data.oh[i] = data.oYcrb[i] * ov; |
311 |
4/8✓ Branch 2 taken 4678 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 4678 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 4678 times.
✗ Branch 10 not taken.
✓ Branch 13 taken 4678 times.
✗ Branch 14 not taken.
|
9356 | data.of[i] = data.oYcrb[i] * oa_gf + ov.cross(data.oh[i]); |
312 | |||
313 | typedef | ||
314 | typename SizeDepType<JointModel::NV>::template ColsReturn<typename Data::Matrix6x>::Type | ||
315 | ColsBlock; | ||
316 |
1/2✓ Branch 1 taken 4678 times.
✗ Branch 2 not taken.
|
9356 | ColsBlock J_cols = jmodel.jointCols(data.J); |
317 |
1/2✓ Branch 1 taken 4678 times.
✗ Branch 2 not taken.
|
9356 | ColsBlock dJ_cols = jmodel.jointCols(data.dJ); |
318 |
1/2✓ Branch 1 taken 4678 times.
✗ Branch 2 not taken.
|
9356 | ColsBlock dVdq_cols = jmodel.jointCols(data.dVdq); |
319 |
1/2✓ Branch 1 taken 4678 times.
✗ Branch 2 not taken.
|
9356 | ColsBlock dAdq_cols = jmodel.jointCols(data.dAdq); |
320 |
1/2✓ Branch 1 taken 4678 times.
✗ Branch 2 not taken.
|
9356 | ColsBlock dAdv_cols = jmodel.jointCols(data.dAdv); |
321 | |||
322 |
3/6✓ Branch 2 taken 4678 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 4678 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 4678 times.
✗ Branch 9 not taken.
|
9356 | J_cols = data.oMi[i].act(jdata.S()); |
323 |
1/2✓ Branch 1 taken 4678 times.
✗ Branch 2 not taken.
|
9356 | motionSet::motionAction(ov, J_cols, dJ_cols); |
324 |
1/2✓ Branch 2 taken 4678 times.
✗ Branch 3 not taken.
|
9356 | motionSet::motionAction(data.oa_gf[parent], J_cols, dAdq_cols); |
325 |
1/2✓ Branch 1 taken 4678 times.
✗ Branch 2 not taken.
|
9356 | dAdv_cols = dJ_cols; |
326 |
2/2✓ Branch 0 taken 4504 times.
✓ Branch 1 taken 174 times.
|
9356 | if (parent > 0) |
327 | { | ||
328 |
1/2✓ Branch 2 taken 4504 times.
✗ Branch 3 not taken.
|
9008 | motionSet::motionAction(data.ov[parent], J_cols, dVdq_cols); |
329 |
1/2✓ Branch 2 taken 4504 times.
✗ Branch 3 not taken.
|
9008 | motionSet::motionAction<ADDTO>(data.ov[parent], dVdq_cols, dAdq_cols); |
330 |
2/4✓ Branch 1 taken 4504 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4504 times.
✗ Branch 5 not taken.
|
9008 | dAdv_cols.noalias() += dVdq_cols; |
331 | } | ||
332 | else | ||
333 | { | ||
334 |
1/2✓ Branch 1 taken 174 times.
✗ Branch 2 not taken.
|
348 | dVdq_cols.setZero(); |
335 | } | ||
336 | |||
337 | // computes variation of inertias | ||
338 |
1/2✓ Branch 2 taken 4678 times.
✗ Branch 3 not taken.
|
9356 | data.doYcrb[i] = data.oYcrb[i].variation(ov); |
339 | |||
340 |
1/2✓ Branch 3 taken 4678 times.
✗ Branch 4 not taken.
|
9356 | addForceCrossMatrix(data.oh[i], data.doYcrb[i]); |
341 | } | ||
342 | |||
343 | template<typename ForceDerived, typename M6> | ||
344 | static void | ||
345 | 5272 | addForceCrossMatrix(const ForceDense<ForceDerived> & f, const Eigen::MatrixBase<M6> & mout) | |
346 | { | ||
347 | 5272 | M6 & mout_ = PINOCCHIO_EIGEN_CONST_CAST(M6, mout); | |
348 |
2/4✓ Branch 1 taken 5272 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5272 times.
✗ Branch 5 not taken.
|
5272 | addSkew( |
349 |
1/2✓ Branch 2 taken 5272 times.
✗ Branch 3 not taken.
|
5272 | -f.linear(), mout_.template block<3, 3>(ForceDerived::LINEAR, ForceDerived::ANGULAR)); |
350 |
2/4✓ Branch 1 taken 5272 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5272 times.
✗ Branch 5 not taken.
|
5272 | addSkew( |
351 |
1/2✓ Branch 2 taken 5272 times.
✗ Branch 3 not taken.
|
5272 | -f.linear(), mout_.template block<3, 3>(ForceDerived::ANGULAR, ForceDerived::LINEAR)); |
352 |
2/4✓ Branch 1 taken 5272 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5272 times.
✗ Branch 5 not taken.
|
5272 | addSkew( |
353 |
1/2✓ Branch 2 taken 5272 times.
✗ Branch 3 not taken.
|
5272 | -f.angular(), mout_.template block<3, 3>(ForceDerived::ANGULAR, ForceDerived::ANGULAR)); |
354 | 5272 | } | |
355 | }; | ||
356 | |||
357 | template< | ||
358 | typename Scalar, | ||
359 | int Options, | ||
360 | template<typename, int> | ||
361 | class JointCollectionTpl, | ||
362 | typename MatrixType1, | ||
363 | typename MatrixType2, | ||
364 | typename MatrixType3> | ||
365 | struct ComputeRNEADerivativesBackwardStep | ||
366 | : public fusion::JointUnaryVisitorBase<ComputeRNEADerivativesBackwardStep< | ||
367 | Scalar, | ||
368 | Options, | ||
369 | JointCollectionTpl, | ||
370 | MatrixType1, | ||
371 | MatrixType2, | ||
372 | MatrixType3>> | ||
373 | { | ||
374 | typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model; | ||
375 | typedef DataTpl<Scalar, Options, JointCollectionTpl> Data; | ||
376 | |||
377 | typedef boost::fusion:: | ||
378 | vector<const Model &, Data &, const MatrixType1 &, const MatrixType2 &, const MatrixType3 &> | ||
379 | ArgsType; | ||
380 | |||
381 | template<typename JointModel> | ||
382 | 9356 | static void algo( | |
383 | const JointModelBase<JointModel> & jmodel, | ||
384 | const Model & model, | ||
385 | Data & data, | ||
386 | const Eigen::MatrixBase<MatrixType1> & rnea_partial_dq, | ||
387 | const Eigen::MatrixBase<MatrixType2> & rnea_partial_dv, | ||
388 | const Eigen::MatrixBase<MatrixType3> & rnea_partial_da) | ||
389 | { | ||
390 | typedef typename Model::JointIndex JointIndex; | ||
391 | typedef typename Data::Matrix6x Matrix6x; | ||
392 | |||
393 |
1/2✓ Branch 1 taken 4678 times.
✗ Branch 2 not taken.
|
9356 | const JointIndex i = jmodel.id(); |
394 | 9356 | const JointIndex parent = model.parents[i]; | |
395 | |||
396 | typedef typename SizeDepType<JointModel::NV>::template ColsReturn<Matrix6x>::Type ColsBlock; | ||
397 | |||
398 | 9356 | Matrix6x & dYtJ = data.Fcrb[0]; | |
399 |
1/2✓ Branch 1 taken 4678 times.
✗ Branch 2 not taken.
|
9356 | ColsBlock J_cols = jmodel.jointCols(data.J); |
400 |
1/2✓ Branch 1 taken 4678 times.
✗ Branch 2 not taken.
|
9356 | ColsBlock dVdq_cols = jmodel.jointCols(data.dVdq); |
401 |
1/2✓ Branch 1 taken 4678 times.
✗ Branch 2 not taken.
|
9356 | ColsBlock dAdq_cols = jmodel.jointCols(data.dAdq); |
402 |
1/2✓ Branch 1 taken 4678 times.
✗ Branch 2 not taken.
|
9356 | ColsBlock dAdv_cols = jmodel.jointCols(data.dAdv); |
403 |
1/2✓ Branch 1 taken 4678 times.
✗ Branch 2 not taken.
|
9356 | ColsBlock dFdq_cols = jmodel.jointCols(data.dFdq); |
404 |
1/2✓ Branch 1 taken 4678 times.
✗ Branch 2 not taken.
|
9356 | ColsBlock dFdv_cols = jmodel.jointCols(data.dFdv); |
405 |
1/2✓ Branch 1 taken 4678 times.
✗ Branch 2 not taken.
|
9356 | ColsBlock dFda_cols = jmodel.jointCols(data.dFda); // Also equals to Ag_cols |
406 |
1/2✓ Branch 1 taken 4678 times.
✗ Branch 2 not taken.
|
9356 | ColsBlock dYtJ_cols = jmodel.jointCols(dYtJ); |
407 | |||
408 | 9356 | MatrixType1 & rnea_partial_dq_ = PINOCCHIO_EIGEN_CONST_CAST(MatrixType1, rnea_partial_dq); | |
409 | 9356 | MatrixType2 & rnea_partial_dv_ = PINOCCHIO_EIGEN_CONST_CAST(MatrixType2, rnea_partial_dv); | |
410 | 9356 | MatrixType3 & rnea_partial_da_ = PINOCCHIO_EIGEN_CONST_CAST(MatrixType3, rnea_partial_da); | |
411 | |||
412 | // tau | ||
413 |
3/6✓ Branch 1 taken 4678 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4678 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4678 times.
✗ Branch 8 not taken.
|
9356 | jmodel.jointVelocitySelector(data.tau).noalias() = |
414 |
3/6✓ Branch 2 taken 4678 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 4678 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 4678 times.
✗ Branch 9 not taken.
|
9356 | J_cols.transpose() * data.of[i].toVector(); |
415 | |||
416 | 9356 | const Eigen::DenseIndex nv_subtree = data.nvSubtree[i]; | |
417 |
1/2✓ Branch 1 taken 4678 times.
✗ Branch 2 not taken.
|
9356 | const Eigen::DenseIndex nv = jmodel.nv(); |
418 |
1/2✓ Branch 1 taken 4678 times.
✗ Branch 2 not taken.
|
9356 | const Eigen::DenseIndex idx_v = jmodel.idx_v(); |
419 | 9356 | const Eigen::DenseIndex idx_v_plus = idx_v + nv; | |
420 | 9356 | const Eigen::DenseIndex nv_subtree_plus = nv_subtree - nv; | |
421 | |||
422 | // dtau/da similar to data.M | ||
423 |
1/2✓ Branch 2 taken 4678 times.
✗ Branch 3 not taken.
|
9356 | motionSet::inertiaAction(data.oYcrb[i], J_cols, dFda_cols); |
424 |
3/6✓ Branch 1 taken 4678 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4678 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4678 times.
✗ Branch 8 not taken.
|
9356 | rnea_partial_da_.block(idx_v, idx_v, nv, nv_subtree).noalias() = |
425 |
3/6✓ Branch 1 taken 4678 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4678 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4678 times.
✗ Branch 8 not taken.
|
9356 | J_cols.transpose() * data.dFda.middleCols(idx_v, nv_subtree); |
426 | |||
427 | // dtau/dq | ||
428 |
2/2✓ Branch 0 taken 4504 times.
✓ Branch 1 taken 174 times.
|
9356 | if (parent > 0) |
429 | { | ||
430 |
3/6✓ Branch 2 taken 4504 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 4504 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 4504 times.
✗ Branch 9 not taken.
|
9008 | dFdq_cols.noalias() = data.doYcrb[i] * dVdq_cols; |
431 |
1/2✓ Branch 2 taken 4504 times.
✗ Branch 3 not taken.
|
9008 | motionSet::inertiaAction<ADDTO>(data.oYcrb[i], dAdq_cols, dFdq_cols); |
432 | } | ||
433 | else | ||
434 |
1/2✓ Branch 2 taken 174 times.
✗ Branch 3 not taken.
|
348 | motionSet::inertiaAction(data.oYcrb[i], dAdq_cols, dFdq_cols); |
435 | |||
436 |
5/10✓ Branch 2 taken 4678 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 4678 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 4678 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 4678 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 4678 times.
✗ Branch 15 not taken.
|
9356 | dYtJ_cols.transpose().noalias() = J_cols.transpose() * data.doYcrb[i]; |
437 |
4/8✓ Branch 1 taken 4678 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4678 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4678 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 4678 times.
✗ Branch 11 not taken.
|
9356 | rnea_partial_dq_.block(idx_v_plus, idx_v, nv_subtree_plus, nv).noalias() = |
438 |
3/6✓ Branch 1 taken 4678 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4678 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4678 times.
✗ Branch 8 not taken.
|
9356 | data.dFda.middleCols(idx_v_plus, nv_subtree_plus).transpose() * dAdq_cols |
439 |
3/6✓ Branch 1 taken 4678 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4678 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4678 times.
✗ Branch 8 not taken.
|
9356 | + dYtJ.middleCols(idx_v_plus, nv_subtree_plus).transpose() * dVdq_cols; |
440 | |||
441 |
3/6✓ Branch 1 taken 4678 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4678 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4678 times.
✗ Branch 8 not taken.
|
9356 | rnea_partial_dq_.block(idx_v, idx_v, nv, nv_subtree).noalias() = |
442 |
3/6✓ Branch 1 taken 4678 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4678 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4678 times.
✗ Branch 8 not taken.
|
9356 | J_cols.transpose() * data.dFdq.middleCols(idx_v, nv_subtree); |
443 | |||
444 |
1/2✓ Branch 2 taken 4678 times.
✗ Branch 3 not taken.
|
9356 | motionSet::act<ADDTO>(J_cols, data.of[i], dFdq_cols); |
445 | |||
446 | // dtau/dv | ||
447 |
3/6✓ Branch 2 taken 4678 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 4678 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 4678 times.
✗ Branch 9 not taken.
|
9356 | dFdv_cols.noalias() = data.doYcrb[i] * J_cols; |
448 |
1/2✓ Branch 2 taken 4678 times.
✗ Branch 3 not taken.
|
9356 | motionSet::inertiaAction<ADDTO>(data.oYcrb[i], dAdv_cols, dFdv_cols); |
449 | |||
450 |
4/8✓ Branch 1 taken 4678 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4678 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4678 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 4678 times.
✗ Branch 11 not taken.
|
9356 | rnea_partial_dv_.block(idx_v_plus, idx_v, nv_subtree_plus, nv).noalias() = |
451 |
3/6✓ Branch 1 taken 4678 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4678 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4678 times.
✗ Branch 8 not taken.
|
9356 | data.dFda.middleCols(idx_v_plus, nv_subtree_plus).transpose() * dAdv_cols |
452 |
3/6✓ Branch 1 taken 4678 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4678 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4678 times.
✗ Branch 8 not taken.
|
9356 | + dYtJ.middleCols(idx_v_plus, nv_subtree_plus).transpose() * J_cols; |
453 | |||
454 |
3/6✓ Branch 1 taken 4678 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4678 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4678 times.
✗ Branch 8 not taken.
|
9356 | rnea_partial_dv_.block(idx_v, idx_v, nv, nv_subtree).noalias() = |
455 |
3/6✓ Branch 1 taken 4678 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4678 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4678 times.
✗ Branch 8 not taken.
|
9356 | J_cols.transpose() * data.dFdv.middleCols(idx_v, nv_subtree); |
456 | |||
457 |
2/2✓ Branch 0 taken 4504 times.
✓ Branch 1 taken 174 times.
|
9356 | if (parent > 0) |
458 | { | ||
459 |
1/2✓ Branch 3 taken 4504 times.
✗ Branch 4 not taken.
|
9008 | data.oYcrb[parent] += data.oYcrb[i]; |
460 |
1/2✓ Branch 3 taken 4504 times.
✗ Branch 4 not taken.
|
9008 | data.doYcrb[parent] += data.doYcrb[i]; |
461 |
1/2✓ Branch 3 taken 4504 times.
✗ Branch 4 not taken.
|
9008 | data.of[parent] += data.of[i]; |
462 | } | ||
463 | } | ||
464 | }; | ||
465 | |||
466 | template< | ||
467 | typename Scalar, | ||
468 | int Options, | ||
469 | template<typename, int> | ||
470 | class JointCollectionTpl, | ||
471 | typename ConfigVectorType, | ||
472 | typename TangentVectorType1, | ||
473 | typename TangentVectorType2, | ||
474 | typename MatrixType1, | ||
475 | typename MatrixType2, | ||
476 | typename MatrixType3> | ||
477 | 338 | void computeRNEADerivatives( | |
478 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
479 | DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
480 | const Eigen::MatrixBase<ConfigVectorType> & q, | ||
481 | const Eigen::MatrixBase<TangentVectorType1> & v, | ||
482 | const Eigen::MatrixBase<TangentVectorType2> & a, | ||
483 | const Eigen::MatrixBase<MatrixType1> & rnea_partial_dq, | ||
484 | const Eigen::MatrixBase<MatrixType2> & rnea_partial_dv, | ||
485 | const Eigen::MatrixBase<MatrixType3> & rnea_partial_da) | ||
486 | { | ||
487 |
1/24✗ Branch 1 not taken.
✓ Branch 2 taken 169 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.
|
338 | PINOCCHIO_CHECK_ARGUMENT_SIZE( |
488 | q.size(), model.nq, "The joint configuration vector is not of right size"); | ||
489 |
1/24✗ Branch 1 not taken.
✓ Branch 2 taken 169 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.
|
338 | PINOCCHIO_CHECK_ARGUMENT_SIZE( |
490 | v.size(), model.nv, "The joint velocity vector is not of right size"); | ||
491 |
1/24✗ Branch 1 not taken.
✓ Branch 2 taken 169 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.
|
338 | PINOCCHIO_CHECK_ARGUMENT_SIZE( |
492 | a.size(), model.nv, "The joint acceleration vector is not of right size"); | ||
493 |
1/24✗ Branch 1 not taken.
✓ Branch 2 taken 169 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.
|
338 | PINOCCHIO_CHECK_ARGUMENT_SIZE(rnea_partial_dq.cols(), model.nv); |
494 |
1/24✗ Branch 1 not taken.
✓ Branch 2 taken 169 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.
|
338 | PINOCCHIO_CHECK_ARGUMENT_SIZE(rnea_partial_dq.rows(), model.nv); |
495 |
1/24✗ Branch 1 not taken.
✓ Branch 2 taken 169 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.
|
338 | PINOCCHIO_CHECK_ARGUMENT_SIZE(rnea_partial_dv.cols(), model.nv); |
496 |
1/24✗ Branch 1 not taken.
✓ Branch 2 taken 169 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.
|
338 | PINOCCHIO_CHECK_ARGUMENT_SIZE(rnea_partial_dv.rows(), model.nv); |
497 |
1/24✗ Branch 1 not taken.
✓ Branch 2 taken 169 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.
|
338 | PINOCCHIO_CHECK_ARGUMENT_SIZE(rnea_partial_da.cols(), model.nv); |
498 |
1/24✗ Branch 1 not taken.
✓ Branch 2 taken 169 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.
|
338 | PINOCCHIO_CHECK_ARGUMENT_SIZE(rnea_partial_da.rows(), model.nv); |
499 |
3/8✓ Branch 2 taken 169 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 169 times.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✓ Branch 8 taken 169 times.
✗ Branch 11 not taken.
✗ Branch 12 not taken.
|
338 | PINOCCHIO_CHECK_INPUT_ARGUMENT( |
500 | isZero(model.gravity.angular()), | ||
501 | "The gravity must be a pure force vector, no angular part"); | ||
502 |
1/2✓ Branch 1 taken 169 times.
✗ Branch 2 not taken.
|
338 | assert(model.check(data) && "data is not consistent with model."); |
503 | |||
504 | typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model; | ||
505 | typedef DataTpl<Scalar, Options, JointCollectionTpl> Data; | ||
506 | typedef typename Model::JointIndex JointIndex; | ||
507 | |||
508 |
1/2✓ Branch 3 taken 169 times.
✗ Branch 4 not taken.
|
338 | data.oa_gf[0] = -model.gravity; |
509 | |||
510 | typedef ComputeRNEADerivativesForwardStep< | ||
511 | Scalar, Options, JointCollectionTpl, ConfigVectorType, TangentVectorType1, | ||
512 | TangentVectorType2> | ||
513 | Pass1; | ||
514 |
2/2✓ Branch 0 taken 4543 times.
✓ Branch 1 taken 169 times.
|
9424 | for (JointIndex i = 1; i < (JointIndex)model.njoints; ++i) |
515 | { | ||
516 |
1/2✓ Branch 1 taken 4543 times.
✗ Branch 2 not taken.
|
9086 | Pass1::run( |
517 | 9086 | model.joints[i], data.joints[i], | |
518 | 18172 | typename Pass1::ArgsType(model, data, q.derived(), v.derived(), a.derived())); | |
519 | } | ||
520 | |||
521 | typedef ComputeRNEADerivativesBackwardStep< | ||
522 | Scalar, Options, JointCollectionTpl, MatrixType1, MatrixType2, MatrixType3> | ||
523 | Pass2; | ||
524 |
2/2✓ Branch 0 taken 4543 times.
✓ Branch 1 taken 169 times.
|
9424 | for (JointIndex i = (JointIndex)(model.njoints - 1); i > 0; --i) |
525 | { | ||
526 |
1/2✓ Branch 1 taken 4543 times.
✗ Branch 2 not taken.
|
9086 | Pass2::run( |
527 | 18172 | model.joints[i], typename Pass2::ArgsType( | |
528 | 9086 | model, data, PINOCCHIO_EIGEN_CONST_CAST(MatrixType1, rnea_partial_dq), | |
529 | 9086 | PINOCCHIO_EIGEN_CONST_CAST(MatrixType2, rnea_partial_dv), | |
530 | 9086 | PINOCCHIO_EIGEN_CONST_CAST(MatrixType3, rnea_partial_da))); | |
531 | } | ||
532 | |||
533 | // Restore the status of dAdq_cols (remove gravity) | ||
534 |
2/2✓ Branch 0 taken 5385 times.
✓ Branch 1 taken 169 times.
|
11108 | for (Eigen::DenseIndex k = 0; k < model.nv; ++k) |
535 | { | ||
536 |
2/4✓ Branch 1 taken 5385 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5385 times.
✗ Branch 5 not taken.
|
10770 | MotionRef<typename Data::Matrix6x::ColXpr> m_in(data.J.col(k)); |
537 |
2/4✓ Branch 1 taken 5385 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5385 times.
✗ Branch 5 not taken.
|
10770 | MotionRef<typename Data::Matrix6x::ColXpr> m_out(data.dAdq.col(k)); |
538 |
5/10✓ Branch 1 taken 5385 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5385 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 5385 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 5385 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 5385 times.
✗ Branch 14 not taken.
|
10770 | m_out.linear() += model.gravity.linear().cross(m_in.angular()); |
539 | } | ||
540 | |||
541 | // Add armature contribution | ||
542 |
3/6✓ Branch 1 taken 169 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 169 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 169 times.
✗ Branch 8 not taken.
|
338 | data.tau.array() += |
543 |
1/2✓ Branch 2 taken 169 times.
✗ Branch 3 not taken.
|
338 | model.armature.array() * a.array(); // TODO: check if there is memory allocation |
544 |
1/2✓ Branch 3 taken 169 times.
✗ Branch 4 not taken.
|
338 | PINOCCHIO_EIGEN_CONST_CAST(MatrixType3, rnea_partial_da).diagonal() += model.armature; |
545 | } | ||
546 | |||
547 | template< | ||
548 | typename Scalar, | ||
549 | int Options, | ||
550 | template<typename, int> | ||
551 | class JointCollectionTpl, | ||
552 | typename ConfigVectorType, | ||
553 | typename TangentVectorType1, | ||
554 | typename TangentVectorType2, | ||
555 | typename MatrixType1, | ||
556 | typename MatrixType2, | ||
557 | typename MatrixType3> | ||
558 | 10 | void computeRNEADerivatives( | |
559 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
560 | DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
561 | const Eigen::MatrixBase<ConfigVectorType> & q, | ||
562 | const Eigen::MatrixBase<TangentVectorType1> & v, | ||
563 | const Eigen::MatrixBase<TangentVectorType2> & a, | ||
564 | const container::aligned_vector<ForceTpl<Scalar, Options>> & fext, | ||
565 | const Eigen::MatrixBase<MatrixType1> & rnea_partial_dq, | ||
566 | const Eigen::MatrixBase<MatrixType2> & rnea_partial_dv, | ||
567 | const Eigen::MatrixBase<MatrixType3> & rnea_partial_da) | ||
568 | { | ||
569 |
1/24✗ Branch 1 not taken.
✓ Branch 2 taken 5 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.
|
10 | PINOCCHIO_CHECK_ARGUMENT_SIZE( |
570 | q.size(), model.nq, "The joint configuration vector is not of right size"); | ||
571 |
1/24✗ Branch 1 not taken.
✓ Branch 2 taken 5 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.
|
10 | PINOCCHIO_CHECK_ARGUMENT_SIZE( |
572 | v.size(), model.nv, "The joint velocity vector is not of right size"); | ||
573 |
1/24✗ Branch 1 not taken.
✓ Branch 2 taken 5 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.
|
10 | PINOCCHIO_CHECK_ARGUMENT_SIZE( |
574 | a.size(), model.nv, "The joint acceleration vector is not of right size"); | ||
575 |
1/24✗ Branch 1 not taken.
✓ Branch 2 taken 5 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.
|
10 | PINOCCHIO_CHECK_ARGUMENT_SIZE( |
576 | fext.size(), (size_t)model.njoints, "The size of the external forces is not of right size"); | ||
577 |
1/24✗ Branch 1 not taken.
✓ Branch 2 taken 5 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.
|
10 | PINOCCHIO_CHECK_ARGUMENT_SIZE(rnea_partial_dq.cols(), model.nv); |
578 |
1/24✗ Branch 1 not taken.
✓ Branch 2 taken 5 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.
|
10 | PINOCCHIO_CHECK_ARGUMENT_SIZE(rnea_partial_dq.rows(), model.nv); |
579 |
1/24✗ Branch 1 not taken.
✓ Branch 2 taken 5 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.
|
10 | PINOCCHIO_CHECK_ARGUMENT_SIZE(rnea_partial_dv.cols(), model.nv); |
580 |
1/24✗ Branch 1 not taken.
✓ Branch 2 taken 5 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.
|
10 | PINOCCHIO_CHECK_ARGUMENT_SIZE(rnea_partial_dv.rows(), model.nv); |
581 |
1/24✗ Branch 1 not taken.
✓ Branch 2 taken 5 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.
|
10 | PINOCCHIO_CHECK_ARGUMENT_SIZE(rnea_partial_da.cols(), model.nv); |
582 |
1/24✗ Branch 1 not taken.
✓ Branch 2 taken 5 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.
|
10 | PINOCCHIO_CHECK_ARGUMENT_SIZE(rnea_partial_da.rows(), model.nv); |
583 |
1/2✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
|
10 | assert(model.check(data) && "data is not consistent with model."); |
584 | |||
585 | typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model; | ||
586 | typedef DataTpl<Scalar, Options, JointCollectionTpl> Data; | ||
587 | typedef typename Model::JointIndex JointIndex; | ||
588 | |||
589 |
1/2✓ Branch 3 taken 5 times.
✗ Branch 4 not taken.
|
10 | data.oa_gf[0] = -model.gravity; |
590 | |||
591 | typedef ComputeRNEADerivativesForwardStep< | ||
592 | Scalar, Options, JointCollectionTpl, ConfigVectorType, TangentVectorType1, | ||
593 | TangentVectorType2> | ||
594 | Pass1; | ||
595 |
2/2✓ Branch 0 taken 135 times.
✓ Branch 1 taken 5 times.
|
280 | for (JointIndex i = 1; i < (JointIndex)model.njoints; ++i) |
596 | { | ||
597 |
1/2✓ Branch 1 taken 135 times.
✗ Branch 2 not taken.
|
270 | Pass1::run( |
598 | 270 | model.joints[i], data.joints[i], | |
599 | 270 | typename Pass1::ArgsType(model, data, q.derived(), v.derived(), a.derived())); | |
600 |
1/2✓ Branch 5 taken 135 times.
✗ Branch 6 not taken.
|
270 | data.of[i] -= data.oMi[i].act(fext[i]); |
601 | } | ||
602 | |||
603 | typedef ComputeRNEADerivativesBackwardStep< | ||
604 | Scalar, Options, JointCollectionTpl, MatrixType1, MatrixType2, MatrixType3> | ||
605 | Pass2; | ||
606 |
2/2✓ Branch 0 taken 135 times.
✓ Branch 1 taken 5 times.
|
280 | for (JointIndex i = (JointIndex)(model.njoints - 1); i > 0; --i) |
607 | { | ||
608 |
1/2✓ Branch 1 taken 135 times.
✗ Branch 2 not taken.
|
270 | Pass2::run( |
609 | 540 | model.joints[i], typename Pass2::ArgsType( | |
610 | 270 | model, data, PINOCCHIO_EIGEN_CONST_CAST(MatrixType1, rnea_partial_dq), | |
611 | 270 | PINOCCHIO_EIGEN_CONST_CAST(MatrixType2, rnea_partial_dv), | |
612 | 270 | PINOCCHIO_EIGEN_CONST_CAST(MatrixType3, rnea_partial_da))); | |
613 | } | ||
614 | |||
615 | // Restore the status of dAdq_cols (remove gravity) | ||
616 |
2/2✓ Branch 0 taken 160 times.
✓ Branch 1 taken 5 times.
|
330 | for (Eigen::DenseIndex k = 0; k < model.nv; ++k) |
617 | { | ||
618 |
2/4✓ Branch 1 taken 160 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 160 times.
✗ Branch 5 not taken.
|
320 | MotionRef<typename Data::Matrix6x::ColXpr> m_in(data.J.col(k)); |
619 |
2/4✓ Branch 1 taken 160 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 160 times.
✗ Branch 5 not taken.
|
320 | MotionRef<typename Data::Matrix6x::ColXpr> m_out(data.dAdq.col(k)); |
620 |
5/10✓ Branch 1 taken 160 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 160 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 160 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 160 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 160 times.
✗ Branch 14 not taken.
|
320 | m_out.linear() += model.gravity.linear().cross(m_in.angular()); |
621 | } | ||
622 | |||
623 | // Add armature contribution | ||
624 |
3/6✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 5 times.
✗ Branch 8 not taken.
|
10 | data.tau.array() += |
625 |
1/2✓ Branch 2 taken 5 times.
✗ Branch 3 not taken.
|
10 | model.armature.array() * a.array(); // TODO: check if there is memory allocation |
626 |
1/2✓ Branch 2 taken 5 times.
✗ Branch 3 not taken.
|
10 | data.M.diagonal() += model.armature; |
627 | } | ||
628 | |||
629 | template< | ||
630 | typename Scalar, | ||
631 | int Options, | ||
632 | template<typename, int> | ||
633 | class JointCollectionTpl, | ||
634 | typename ConfigVectorType, | ||
635 | typename TangentVectorType1, | ||
636 | typename TangentVectorType2> | ||
637 | ✗ | void computeRNEADerivatives( | |
638 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
639 | DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
640 | const Eigen::MatrixBase<ConfigVectorType> & q, | ||
641 | const Eigen::MatrixBase<TangentVectorType1> & v, | ||
642 | const Eigen::MatrixBase<TangentVectorType2> & a) | ||
643 | { | ||
644 | ✗ | impl::computeRNEADerivatives( | |
645 | ✗ | model, data, q.derived(), v.derived(), a.derived(), data.dtau_dq, data.dtau_dv, data.M); | |
646 | } | ||
647 | |||
648 | template< | ||
649 | typename Scalar, | ||
650 | int Options, | ||
651 | template<typename, int> | ||
652 | class JointCollectionTpl, | ||
653 | typename ConfigVectorType, | ||
654 | typename TangentVectorType1, | ||
655 | typename TangentVectorType2> | ||
656 | ✗ | void computeRNEADerivatives( | |
657 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
658 | DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
659 | const Eigen::MatrixBase<ConfigVectorType> & q, | ||
660 | const Eigen::MatrixBase<TangentVectorType1> & v, | ||
661 | const Eigen::MatrixBase<TangentVectorType2> & a, | ||
662 | const container::aligned_vector<ForceTpl<Scalar, Options>> & fext) | ||
663 | { | ||
664 | ✗ | impl::computeRNEADerivatives( | |
665 | ✗ | model, data, q.derived(), v.derived(), a.derived(), fext, data.dtau_dq, data.dtau_dv, | |
666 | ✗ | data.M); | |
667 | } | ||
668 | |||
669 | } // namespace impl | ||
670 | |||
671 | template< | ||
672 | typename Scalar, | ||
673 | int Options, | ||
674 | template<typename, int> | ||
675 | class JointCollectionTpl, | ||
676 | typename ConfigVectorType, | ||
677 | typename ReturnMatrixType> | ||
678 | ✗ | void computeGeneralizedGravityDerivatives( | |
679 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
680 | DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
681 | const Eigen::MatrixBase<ConfigVectorType> & q, | ||
682 | const Eigen::MatrixBase<ReturnMatrixType> & gravity_partial_dq) | ||
683 | { | ||
684 | ✗ | impl::computeGeneralizedGravityDerivatives( | |
685 | model, data, make_const_ref(q), make_ref(gravity_partial_dq)); | ||
686 | } | ||
687 | |||
688 | template< | ||
689 | typename Scalar, | ||
690 | int Options, | ||
691 | template<typename, int> | ||
692 | class JointCollectionTpl, | ||
693 | typename ConfigVectorType, | ||
694 | typename ReturnMatrixType> | ||
695 | ✗ | void computeStaticTorqueDerivatives( | |
696 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
697 | DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
698 | const Eigen::MatrixBase<ConfigVectorType> & q, | ||
699 | const container::aligned_vector<ForceTpl<Scalar, Options>> & fext, | ||
700 | const Eigen::MatrixBase<ReturnMatrixType> & static_torque_partial_dq) | ||
701 | { | ||
702 | ✗ | impl::computeStaticTorqueDerivatives( | |
703 | model, data, make_const_ref(q), fext, make_ref(static_torque_partial_dq)); | ||
704 | } | ||
705 | |||
706 | template< | ||
707 | typename Scalar, | ||
708 | int Options, | ||
709 | template<typename, int> | ||
710 | class JointCollectionTpl, | ||
711 | typename ConfigVectorType, | ||
712 | typename TangentVectorType1, | ||
713 | typename TangentVectorType2, | ||
714 | typename MatrixType1, | ||
715 | typename MatrixType2, | ||
716 | typename MatrixType3> | ||
717 | ✗ | void computeRNEADerivatives( | |
718 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
719 | DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
720 | const Eigen::MatrixBase<ConfigVectorType> & q, | ||
721 | const Eigen::MatrixBase<TangentVectorType1> & v, | ||
722 | const Eigen::MatrixBase<TangentVectorType2> & a, | ||
723 | const Eigen::MatrixBase<MatrixType1> & rnea_partial_dq, | ||
724 | const Eigen::MatrixBase<MatrixType2> & rnea_partial_dv, | ||
725 | const Eigen::MatrixBase<MatrixType3> & rnea_partial_da) | ||
726 | { | ||
727 | ✗ | impl::computeRNEADerivatives( | |
728 | model, data, make_const_ref(q), make_const_ref(v), make_const_ref(a), | ||
729 | make_ref(rnea_partial_dq), make_ref(rnea_partial_dv), make_ref(rnea_partial_da)); | ||
730 | } | ||
731 | |||
732 | template< | ||
733 | typename Scalar, | ||
734 | int Options, | ||
735 | template<typename, int> | ||
736 | class JointCollectionTpl, | ||
737 | typename ConfigVectorType, | ||
738 | typename TangentVectorType1, | ||
739 | typename TangentVectorType2, | ||
740 | typename MatrixType1, | ||
741 | typename MatrixType2, | ||
742 | typename MatrixType3> | ||
743 | ✗ | void computeRNEADerivatives( | |
744 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
745 | DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
746 | const Eigen::MatrixBase<ConfigVectorType> & q, | ||
747 | const Eigen::MatrixBase<TangentVectorType1> & v, | ||
748 | const Eigen::MatrixBase<TangentVectorType2> & a, | ||
749 | const container::aligned_vector<ForceTpl<Scalar, Options>> & fext, | ||
750 | const Eigen::MatrixBase<MatrixType1> & rnea_partial_dq, | ||
751 | const Eigen::MatrixBase<MatrixType2> & rnea_partial_dv, | ||
752 | const Eigen::MatrixBase<MatrixType3> & rnea_partial_da) | ||
753 | { | ||
754 | ✗ | impl::computeRNEADerivatives( | |
755 | model, data, make_const_ref(q), make_const_ref(v), make_const_ref(a), fext, | ||
756 | make_ref(rnea_partial_dq), make_ref(rnea_partial_dv), make_ref(rnea_partial_da)); | ||
757 | } | ||
758 | |||
759 | template< | ||
760 | typename Scalar, | ||
761 | int Options, | ||
762 | template<typename, int> | ||
763 | class JointCollectionTpl, | ||
764 | typename ConfigVectorType, | ||
765 | typename TangentVectorType1, | ||
766 | typename TangentVectorType2> | ||
767 | 2 | void computeRNEADerivatives( | |
768 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
769 | DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
770 | const Eigen::MatrixBase<ConfigVectorType> & q, | ||
771 | const Eigen::MatrixBase<TangentVectorType1> & v, | ||
772 | const Eigen::MatrixBase<TangentVectorType2> & a) | ||
773 | { | ||
774 |
4/8✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 2 times.
✗ Branch 11 not taken.
|
2 | impl::computeRNEADerivatives( |
775 |
1/2✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
|
2 | model, data, make_const_ref(q), make_const_ref(v), make_const_ref(a), make_ref(data.dtau_dq), |
776 |
1/2✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
|
2 | make_ref(data.dtau_dv), make_ref(data.M)); |
777 | 2 | } | |
778 | |||
779 | template< | ||
780 | typename Scalar, | ||
781 | int Options, | ||
782 | template<typename, int> | ||
783 | class JointCollectionTpl, | ||
784 | typename ConfigVectorType, | ||
785 | typename TangentVectorType1, | ||
786 | typename TangentVectorType2> | ||
787 | 1 | void computeRNEADerivatives( | |
788 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
789 | DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
790 | const Eigen::MatrixBase<ConfigVectorType> & q, | ||
791 | const Eigen::MatrixBase<TangentVectorType1> & v, | ||
792 | const Eigen::MatrixBase<TangentVectorType2> & a, | ||
793 | const container::aligned_vector<ForceTpl<Scalar, Options>> & fext) | ||
794 | { | ||
795 |
4/8✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
|
1 | impl::computeRNEADerivatives( |
796 | model, data, make_const_ref(q), make_const_ref(v), make_const_ref(a), fext, | ||
797 |
2/4✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
|
1 | make_ref(data.dtau_dq), make_ref(data.dtau_dv), make_ref(data.M)); |
798 | 1 | } | |
799 | |||
800 | } // namespace pinocchio | ||
801 | |||
802 | #endif // ifndef __pinocchio_algorithm_rnea_derivatives_hxx__ | ||
803 |