Directory: | ./ |
---|---|
File: | include/pinocchio/algorithm/centroidal-derivatives.hxx |
Date: | 2024-08-27 18:20:05 |
Exec | Total | Coverage | |
---|---|---|---|
Lines: | 192 | 196 | 98.0% |
Branches: | 238 | 863 | 27.6% |
Line | Branch | Exec | Source |
---|---|---|---|
1 | // | ||
2 | // Copyright (c) 2018-2021 INRIA | ||
3 | // | ||
4 | |||
5 | #ifndef __pinocchio_algorithm_centroidal_derivatives_hxx__ | ||
6 | #define __pinocchio_algorithm_centroidal_derivatives_hxx__ | ||
7 | |||
8 | #include "pinocchio/multibody/visitor.hpp" | ||
9 | #include "pinocchio/spatial/act-on-set.hpp" | ||
10 | #include "pinocchio/algorithm/kinematics.hpp" | ||
11 | #include "pinocchio/algorithm/check.hpp" | ||
12 | |||
13 | /// @cond DEV | ||
14 | |||
15 | namespace pinocchio | ||
16 | { | ||
17 | namespace impl | ||
18 | { | ||
19 | template< | ||
20 | typename Scalar, | ||
21 | int Options, | ||
22 | template<typename, int> | ||
23 | class JointCollectionTpl, | ||
24 | typename ConfigVectorType, | ||
25 | typename TangentVectorType1, | ||
26 | typename TangentVectorType2> | ||
27 | struct CentroidalDynDerivativesForwardStep | ||
28 | : public fusion::JointUnaryVisitorBase<CentroidalDynDerivativesForwardStep< | ||
29 | Scalar, | ||
30 | Options, | ||
31 | JointCollectionTpl, | ||
32 | ConfigVectorType, | ||
33 | TangentVectorType1, | ||
34 | TangentVectorType2>> | ||
35 | { | ||
36 | typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model; | ||
37 | typedef DataTpl<Scalar, Options, JointCollectionTpl> Data; | ||
38 | |||
39 | typedef typename Model::JointIndex JointIndex; | ||
40 | |||
41 | typedef boost::fusion::vector< | ||
42 | const Model &, | ||
43 | Data &, | ||
44 | const ConfigVectorType &, | ||
45 | const TangentVectorType1 &, | ||
46 | const TangentVectorType2 &> | ||
47 | ArgsType; | ||
48 | |||
49 | template<typename JointModel> | ||
50 | 164 | static void algo( | |
51 | const JointModelBase<JointModel> & jmodel, | ||
52 | JointDataBase<typename JointModel::JointDataDerived> & jdata, | ||
53 | const Model & model, | ||
54 | Data & data, | ||
55 | const Eigen::MatrixBase<ConfigVectorType> & q, | ||
56 | const Eigen::MatrixBase<TangentVectorType1> & v, | ||
57 | const Eigen::MatrixBase<TangentVectorType2> & a) | ||
58 | { | ||
59 | typedef typename Model::JointIndex JointIndex; | ||
60 | typedef typename Data::Motion Motion; | ||
61 | |||
62 |
1/2✓ Branch 1 taken 82 times.
✗ Branch 2 not taken.
|
164 | const JointIndex & i = jmodel.id(); |
63 | 164 | const JointIndex & parent = model.parents[i]; | |
64 | 164 | Motion & ov = data.ov[i]; | |
65 | 164 | Motion & oa = data.oa[i]; | |
66 | |||
67 |
1/2✓ Branch 4 taken 82 times.
✗ Branch 5 not taken.
|
164 | jmodel.calc(jdata.derived(), q.derived(), v.derived()); |
68 | |||
69 |
6/10✓ Branch 1 taken 82 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 78 times.
✓ Branch 5 taken 4 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 78 times.
✓ Branch 9 taken 4 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 78 times.
✗ Branch 13 not taken.
|
164 | data.liMi[i] = model.jointPlacements[i] * jdata.M(); |
70 | |||
71 |
2/4✓ Branch 1 taken 82 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 82 times.
✗ Branch 6 not taken.
|
164 | data.v[i] = jdata.v(); |
72 | |||
73 |
2/2✓ Branch 0 taken 79 times.
✓ Branch 1 taken 3 times.
|
164 | if (parent > 0) |
74 | { | ||
75 |
2/4✓ Branch 3 taken 79 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 79 times.
✗ Branch 8 not taken.
|
158 | data.oMi[i] = data.oMi[parent] * data.liMi[i]; |
76 |
2/4✓ Branch 3 taken 79 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 79 times.
✗ Branch 8 not taken.
|
158 | data.v[i] += data.liMi[i].actInv(data.v[parent]); |
77 | } | ||
78 | else | ||
79 |
1/2✓ Branch 3 taken 3 times.
✗ Branch 4 not taken.
|
6 | data.oMi[i] = data.liMi[i]; |
80 | |||
81 |
5/9✓ Branch 1 taken 82 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 79 times.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
✓ Branch 7 taken 79 times.
✗ Branch 8 not taken.
✓ Branch 11 taken 79 times.
✗ Branch 12 not taken.
|
328 | data.a[i] = |
82 |
7/14✓ Branch 1 taken 82 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 82 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 82 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 82 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 82 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 3 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 3 times.
✗ Branch 21 not taken.
|
328 | jdata.S() * jmodel.jointVelocitySelector(a) + jdata.c() + (data.v[i] ^ jdata.v()); |
83 |
2/2✓ Branch 0 taken 79 times.
✓ Branch 1 taken 3 times.
|
164 | if (parent > 0) |
84 | { | ||
85 |
2/4✓ Branch 3 taken 79 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 79 times.
✗ Branch 8 not taken.
|
158 | data.a[i] += data.liMi[i].actInv(data.a[parent]); |
86 | } | ||
87 | |||
88 |
2/4✓ Branch 3 taken 82 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 82 times.
✗ Branch 8 not taken.
|
164 | data.oYcrb[i] = data.oMi[i].act(model.inertias[i]); |
89 |
2/4✓ Branch 3 taken 82 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 82 times.
✗ Branch 7 not taken.
|
164 | ov = data.oMi[i].act(data.v[i]); |
90 |
2/4✓ Branch 3 taken 82 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 82 times.
✗ Branch 7 not taken.
|
164 | oa = data.oMi[i].act(data.a[i]); |
91 | |||
92 |
2/4✓ Branch 2 taken 82 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 82 times.
✗ Branch 7 not taken.
|
164 | data.oh[i] = data.oYcrb[i] * ov; |
93 |
4/8✓ Branch 2 taken 82 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 82 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 82 times.
✗ Branch 10 not taken.
✓ Branch 13 taken 82 times.
✗ Branch 14 not taken.
|
164 | data.of[i] = data.oYcrb[i] * oa + ov.cross(data.oh[i]); |
94 | |||
95 | typedef | ||
96 | typename SizeDepType<JointModel::NV>::template ColsReturn<typename Data::Matrix6x>::Type | ||
97 | ColsBlock; | ||
98 |
1/2✓ Branch 1 taken 82 times.
✗ Branch 2 not taken.
|
164 | ColsBlock J_cols = jmodel.jointCols(data.J); |
99 |
1/2✓ Branch 1 taken 82 times.
✗ Branch 2 not taken.
|
164 | ColsBlock dJ_cols = jmodel.jointCols(data.dJ); |
100 |
1/2✓ Branch 1 taken 82 times.
✗ Branch 2 not taken.
|
164 | ColsBlock dVdq_cols = jmodel.jointCols(data.dVdq); |
101 |
1/2✓ Branch 1 taken 82 times.
✗ Branch 2 not taken.
|
164 | ColsBlock dAdq_cols = jmodel.jointCols(data.dAdq); |
102 |
1/2✓ Branch 1 taken 82 times.
✗ Branch 2 not taken.
|
164 | ColsBlock dAdv_cols = jmodel.jointCols(data.dAdv); |
103 | |||
104 |
3/6✓ Branch 2 taken 82 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 82 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 82 times.
✗ Branch 9 not taken.
|
164 | J_cols = data.oMi[i].act(jdata.S()); |
105 |
1/2✓ Branch 1 taken 82 times.
✗ Branch 2 not taken.
|
164 | motionSet::motionAction(ov, J_cols, dJ_cols); |
106 |
1/2✓ Branch 2 taken 82 times.
✗ Branch 3 not taken.
|
164 | motionSet::motionAction(data.oa[parent], J_cols, dAdq_cols); |
107 |
1/2✓ Branch 1 taken 82 times.
✗ Branch 2 not taken.
|
164 | dAdv_cols = dJ_cols; |
108 |
2/2✓ Branch 0 taken 79 times.
✓ Branch 1 taken 3 times.
|
164 | if (parent > 0) |
109 | { | ||
110 |
1/2✓ Branch 2 taken 79 times.
✗ Branch 3 not taken.
|
158 | motionSet::motionAction(data.ov[parent], J_cols, dVdq_cols); |
111 |
1/2✓ Branch 2 taken 79 times.
✗ Branch 3 not taken.
|
158 | motionSet::motionAction<ADDTO>(data.ov[parent], dVdq_cols, dAdq_cols); |
112 |
2/4✓ Branch 1 taken 79 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 79 times.
✗ Branch 5 not taken.
|
158 | dAdv_cols.noalias() += dVdq_cols; |
113 | } | ||
114 | else | ||
115 | { | ||
116 |
1/2✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
|
6 | dVdq_cols.setZero(); |
117 | } | ||
118 | |||
119 | // computes variation of inertias | ||
120 |
1/2✓ Branch 2 taken 82 times.
✗ Branch 3 not taken.
|
164 | data.doYcrb[i] = data.oYcrb[i].variation(ov); |
121 | |||
122 |
1/2✓ Branch 3 taken 82 times.
✗ Branch 4 not taken.
|
164 | addForceCrossMatrix(data.oh[i], data.doYcrb[i]); |
123 | } | ||
124 | |||
125 | template<typename ForceDerived, typename M6> | ||
126 | static void | ||
127 | 82 | addForceCrossMatrix(const ForceDense<ForceDerived> & f, const Eigen::MatrixBase<M6> & mout) | |
128 | { | ||
129 | 82 | M6 & mout_ = PINOCCHIO_EIGEN_CONST_CAST(M6, mout); | |
130 |
2/4✓ Branch 1 taken 82 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 82 times.
✗ Branch 5 not taken.
|
82 | addSkew( |
131 |
1/2✓ Branch 2 taken 82 times.
✗ Branch 3 not taken.
|
82 | -f.linear(), mout_.template block<3, 3>(ForceDerived::LINEAR, ForceDerived::ANGULAR)); |
132 |
2/4✓ Branch 1 taken 82 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 82 times.
✗ Branch 5 not taken.
|
82 | addSkew( |
133 |
1/2✓ Branch 2 taken 82 times.
✗ Branch 3 not taken.
|
82 | -f.linear(), mout_.template block<3, 3>(ForceDerived::ANGULAR, ForceDerived::LINEAR)); |
134 |
2/4✓ Branch 1 taken 82 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 82 times.
✗ Branch 5 not taken.
|
82 | addSkew( |
135 |
1/2✓ Branch 2 taken 82 times.
✗ Branch 3 not taken.
|
82 | -f.angular(), mout_.template block<3, 3>(ForceDerived::ANGULAR, ForceDerived::ANGULAR)); |
136 | 82 | } | |
137 | |||
138 | }; // struct CentroidalDynDerivativesForwardStep | ||
139 | |||
140 | template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl> | ||
141 | struct CentroidalDynDerivativesBackwardStep | ||
142 | : public fusion::JointUnaryVisitorBase< | ||
143 | CentroidalDynDerivativesBackwardStep<Scalar, Options, JointCollectionTpl>> | ||
144 | { | ||
145 | typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model; | ||
146 | typedef DataTpl<Scalar, Options, JointCollectionTpl> Data; | ||
147 | |||
148 | typedef boost::fusion::vector<const Model &, Data &> ArgsType; | ||
149 | |||
150 | template<typename JointModel> | ||
151 | 164 | static void algo(const JointModelBase<JointModel> & jmodel, const Model & model, Data & data) | |
152 | { | ||
153 | typedef typename Model::JointIndex JointIndex; | ||
154 | |||
155 |
1/2✓ Branch 1 taken 82 times.
✗ Branch 2 not taken.
|
164 | const JointIndex & i = jmodel.id(); |
156 | 164 | const JointIndex & parent = model.parents[i]; | |
157 | |||
158 | typedef | ||
159 | typename SizeDepType<JointModel::NV>::template ColsReturn<typename Data::Matrix6x>::Type | ||
160 | ColsBlock; | ||
161 | |||
162 |
1/2✓ Branch 1 taken 82 times.
✗ Branch 2 not taken.
|
164 | ColsBlock J_cols = jmodel.jointCols(data.J); |
163 |
1/2✓ Branch 1 taken 82 times.
✗ Branch 2 not taken.
|
164 | ColsBlock dVdq_cols = jmodel.jointCols(data.dVdq); |
164 |
1/2✓ Branch 1 taken 82 times.
✗ Branch 2 not taken.
|
164 | ColsBlock dAdq_cols = jmodel.jointCols(data.dAdq); |
165 |
1/2✓ Branch 1 taken 82 times.
✗ Branch 2 not taken.
|
164 | ColsBlock dAdv_cols = jmodel.jointCols(data.dAdv); |
166 |
1/2✓ Branch 1 taken 82 times.
✗ Branch 2 not taken.
|
164 | ColsBlock dHdq_cols = jmodel.jointCols(data.dHdq); |
167 |
1/2✓ Branch 1 taken 82 times.
✗ Branch 2 not taken.
|
164 | ColsBlock dFdq_cols = jmodel.jointCols(data.dFdq); |
168 |
1/2✓ Branch 1 taken 82 times.
✗ Branch 2 not taken.
|
164 | ColsBlock dFdv_cols = jmodel.jointCols(data.dFdv); |
169 |
1/2✓ Branch 1 taken 82 times.
✗ Branch 2 not taken.
|
164 | ColsBlock dFda_cols = jmodel.jointCols(data.dFda); |
170 | |||
171 | // tau | ||
172 |
3/6✓ Branch 1 taken 82 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 82 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 82 times.
✗ Branch 8 not taken.
|
164 | jmodel.jointVelocitySelector(data.tau).noalias() = |
173 |
3/6✓ Branch 2 taken 82 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 82 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 82 times.
✗ Branch 9 not taken.
|
164 | J_cols.transpose() * data.of[i].toVector(); |
174 | |||
175 | // dtau/da similar to data.M | ||
176 |
1/2✓ Branch 2 taken 82 times.
✗ Branch 3 not taken.
|
164 | motionSet::inertiaAction(data.oYcrb[i], J_cols, dFda_cols); |
177 | |||
178 | // dtau/dv | ||
179 |
3/6✓ Branch 2 taken 82 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 82 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 82 times.
✗ Branch 9 not taken.
|
164 | dFdv_cols.noalias() = data.doYcrb[i] * J_cols; |
180 |
1/2✓ Branch 2 taken 82 times.
✗ Branch 3 not taken.
|
164 | motionSet::inertiaAction<ADDTO>(data.oYcrb[i], dAdv_cols, dFdv_cols); |
181 | |||
182 | // dtau/dq | ||
183 |
2/2✓ Branch 0 taken 79 times.
✓ Branch 1 taken 3 times.
|
164 | if (parent > 0) |
184 | { | ||
185 |
3/6✓ Branch 2 taken 79 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 79 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 79 times.
✗ Branch 9 not taken.
|
158 | dFdq_cols.noalias() = data.doYcrb[i] * dVdq_cols; |
186 |
1/2✓ Branch 2 taken 79 times.
✗ Branch 3 not taken.
|
158 | motionSet::inertiaAction<ADDTO>(data.oYcrb[i], dAdq_cols, dFdq_cols); |
187 | } | ||
188 | else | ||
189 |
1/2✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
|
6 | motionSet::inertiaAction(data.oYcrb[i], dAdq_cols, dFdq_cols); |
190 | |||
191 |
1/2✓ Branch 2 taken 82 times.
✗ Branch 3 not taken.
|
164 | motionSet::act<ADDTO>(J_cols, data.of[i], dFdq_cols); |
192 | |||
193 |
1/2✓ Branch 3 taken 82 times.
✗ Branch 4 not taken.
|
164 | data.oYcrb[parent] += data.oYcrb[i]; |
194 |
1/2✓ Branch 3 taken 82 times.
✗ Branch 4 not taken.
|
164 | data.doYcrb[parent] += data.doYcrb[i]; |
195 |
1/2✓ Branch 3 taken 82 times.
✗ Branch 4 not taken.
|
164 | data.oh[parent] += data.oh[i]; |
196 |
1/2✓ Branch 3 taken 82 times.
✗ Branch 4 not taken.
|
164 | data.of[parent] += data.of[i]; |
197 | |||
198 |
1/2✓ Branch 2 taken 82 times.
✗ Branch 3 not taken.
|
164 | motionSet::act(J_cols, data.oh[i], dHdq_cols); |
199 |
1/2✓ Branch 2 taken 82 times.
✗ Branch 3 not taken.
|
164 | motionSet::inertiaAction<ADDTO>(data.oYcrb[i], dVdq_cols, dHdq_cols); |
200 | } | ||
201 | }; // struct CentroidalDynDerivativesBackwardStep | ||
202 | } // namespace impl | ||
203 | namespace | ||
204 | { | ||
205 | // TODO: should be moved to ForceSet | ||
206 | template<typename Matrix6xLikeIn, typename Vector3Like, typename Matrix6xLikeOut> | ||
207 | 40 | void translateForceSet( | |
208 | const Eigen::MatrixBase<Matrix6xLikeIn> & Fin, | ||
209 | const Eigen::MatrixBase<Vector3Like> & v3, | ||
210 | const Eigen::MatrixBase<Matrix6xLikeOut> & Fout) | ||
211 | { | ||
212 | EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix6xLikeIn, 6, Eigen::Dynamic) | ||
213 | EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3Like, 3) | ||
214 | EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix6xLikeOut, 6, Eigen::Dynamic) | ||
215 | |||
216 |
1/24✗ Branch 2 not taken.
✓ Branch 3 taken 20 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
✗ Branch 12 not taken.
✗ Branch 13 not taken.
✗ Branch 15 not taken.
✗ Branch 16 not taken.
✗ Branch 19 not taken.
✗ Branch 20 not taken.
✗ Branch 22 not taken.
✗ Branch 23 not taken.
✗ Branch 25 not taken.
✗ Branch 26 not taken.
✗ Branch 28 not taken.
✗ Branch 29 not taken.
✗ Branch 31 not taken.
✗ Branch 32 not taken.
✗ Branch 35 not taken.
✗ Branch 36 not taken.
✗ Branch 38 not taken.
✗ Branch 39 not taken.
|
40 | PINOCCHIO_CHECK_ARGUMENT_SIZE( |
217 | Fin.cols(), Fout.cols(), "Fin and Fout do not have the same number of columns"); | ||
218 | |||
219 |
2/2✓ Branch 1 taken 652 times.
✓ Branch 2 taken 20 times.
|
1344 | for (Eigen::DenseIndex k = 0; k < Fin.cols(); ++k) |
220 | { | ||
221 | typedef ForceRef<typename Matrix6xLikeIn::ColXpr> ForceTypeIn; | ||
222 | typedef ForceRef<typename Matrix6xLikeOut::ColXpr> ForceTypeOut; | ||
223 |
2/4✓ Branch 2 taken 652 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 652 times.
✗ Branch 6 not taken.
|
1304 | ForceTypeOut fout(PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLikeOut, Fout).col(k)); |
224 |
2/4✓ Branch 2 taken 652 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 652 times.
✗ Branch 6 not taken.
|
1304 | const ForceTypeIn fin(PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLikeIn, Fin).col(k)); |
225 |
3/6✓ Branch 1 taken 652 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 652 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 652 times.
✗ Branch 8 not taken.
|
1304 | fout.linear() = fin.linear(); |
226 |
7/14✓ Branch 1 taken 652 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 652 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 652 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 652 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 652 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 652 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 652 times.
✗ Branch 20 not taken.
|
1304 | fout.angular().noalias() = fin.angular() - v3.cross(fin.linear()); |
227 | } | ||
228 | 40 | } | |
229 | |||
230 | template<typename Matrix6xLike, typename Vector3Like, typename Matrix6xLikeOut> | ||
231 | void translateForceSet( | ||
232 | const Eigen::MatrixBase<Matrix6xLike> & F, const Eigen::MatrixBase<Vector3Like> & v3) | ||
233 | { | ||
234 | EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix6xLike, 6, Eigen::Dynamic) | ||
235 | EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3Like, 3) | ||
236 | |||
237 | for (Eigen::DenseIndex k = 0; k < F.cols(); ++k) | ||
238 | { | ||
239 | typedef ForceRef<typename Matrix6xLike::ColXpr> ForceType; | ||
240 | ForceType f(PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLike, F).col(k)); | ||
241 | f.angular() -= v3.cross(f.linear()); | ||
242 | } | ||
243 | } | ||
244 | } // namespace | ||
245 | namespace impl | ||
246 | { | ||
247 | template< | ||
248 | typename Scalar, | ||
249 | int Options, | ||
250 | template<typename, int> | ||
251 | class JointCollectionTpl, | ||
252 | typename ConfigVectorType, | ||
253 | typename TangentVectorType1, | ||
254 | typename TangentVectorType2, | ||
255 | typename Matrix6xLike0, | ||
256 | typename Matrix6xLike1, | ||
257 | typename Matrix6xLike2, | ||
258 | typename Matrix6xLike3> | ||
259 | 3 | void computeCentroidalDynamicsDerivatives( | |
260 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
261 | DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
262 | const Eigen::MatrixBase<ConfigVectorType> & q, | ||
263 | const Eigen::MatrixBase<TangentVectorType1> & v, | ||
264 | const Eigen::MatrixBase<TangentVectorType2> & a, | ||
265 | const Eigen::MatrixBase<Matrix6xLike0> & dh_dq, | ||
266 | const Eigen::MatrixBase<Matrix6xLike1> & dhdot_dq, | ||
267 | const Eigen::MatrixBase<Matrix6xLike2> & dhdot_dv, | ||
268 | const Eigen::MatrixBase<Matrix6xLike3> & dhdot_da) | ||
269 | { | ||
270 |
1/24✗ Branch 1 not taken.
✓ Branch 2 taken 3 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.
|
3 | PINOCCHIO_CHECK_ARGUMENT_SIZE( |
271 | q.size(), model.nq, "The joint configuration vector is not of right size"); | ||
272 |
1/24✗ Branch 1 not taken.
✓ Branch 2 taken 3 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.
|
3 | PINOCCHIO_CHECK_ARGUMENT_SIZE( |
273 | v.size(), model.nv, "The joint velocity vector is not of right size"); | ||
274 |
1/24✗ Branch 1 not taken.
✓ Branch 2 taken 3 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.
|
3 | PINOCCHIO_CHECK_ARGUMENT_SIZE( |
275 | a.size(), model.nv, "The joint acceleration vector is not of right size"); | ||
276 |
1/24✗ Branch 1 not taken.
✓ Branch 2 taken 3 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.
|
3 | PINOCCHIO_CHECK_ARGUMENT_SIZE(dh_dq.cols(), model.nv); |
277 |
1/24✗ Branch 1 not taken.
✓ Branch 2 taken 3 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.
|
3 | PINOCCHIO_CHECK_ARGUMENT_SIZE(dh_dq.rows(), 6); |
278 |
1/24✗ Branch 1 not taken.
✓ Branch 2 taken 3 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.
|
3 | PINOCCHIO_CHECK_ARGUMENT_SIZE(dhdot_dq.cols(), model.nv); |
279 |
1/24✗ Branch 1 not taken.
✓ Branch 2 taken 3 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.
|
3 | PINOCCHIO_CHECK_ARGUMENT_SIZE(dhdot_dq.rows(), 6); |
280 |
1/24✗ Branch 1 not taken.
✓ Branch 2 taken 3 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.
|
3 | PINOCCHIO_CHECK_ARGUMENT_SIZE(dhdot_dv.cols(), model.nv); |
281 |
1/24✗ Branch 1 not taken.
✓ Branch 2 taken 3 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.
|
3 | PINOCCHIO_CHECK_ARGUMENT_SIZE(dhdot_dv.rows(), 6); |
282 |
1/24✗ Branch 1 not taken.
✓ Branch 2 taken 3 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.
|
3 | PINOCCHIO_CHECK_ARGUMENT_SIZE(dhdot_da.cols(), model.nv); |
283 |
1/24✗ Branch 1 not taken.
✓ Branch 2 taken 3 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.
|
3 | PINOCCHIO_CHECK_ARGUMENT_SIZE(dhdot_da.rows(), 6); |
284 |
1/2✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
|
3 | assert(model.check(data) && "data is not consistent with model."); |
285 | |||
286 | typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model; | ||
287 | typedef DataTpl<Scalar, Options, JointCollectionTpl> Data; | ||
288 | typedef typename Model::JointIndex JointIndex; | ||
289 | typedef typename Data::Force Force; | ||
290 | |||
291 | typedef CentroidalDynDerivativesForwardStep< | ||
292 | Scalar, Options, JointCollectionTpl, ConfigVectorType, TangentVectorType1, | ||
293 | TangentVectorType2> | ||
294 | Pass1; | ||
295 |
2/2✓ Branch 0 taken 82 times.
✓ Branch 1 taken 3 times.
|
85 | for (JointIndex i = 1; i < (JointIndex)model.njoints; ++i) |
296 | { | ||
297 |
1/2✓ Branch 1 taken 82 times.
✗ Branch 2 not taken.
|
82 | Pass1::run( |
298 | 82 | model.joints[i], data.joints[i], | |
299 | 164 | typename Pass1::ArgsType(model, data, q.derived(), v.derived(), a.derived())); | |
300 | } | ||
301 | |||
302 | 3 | data.oYcrb[0].setZero(); | |
303 | 3 | data.oh[0].setZero(); | |
304 | 3 | data.of[0].setZero(); | |
305 | typedef CentroidalDynDerivativesBackwardStep<Scalar, Options, JointCollectionTpl> Pass2; | ||
306 |
2/2✓ Branch 0 taken 82 times.
✓ Branch 1 taken 3 times.
|
85 | for (JointIndex i = (JointIndex)(model.njoints - 1); i > 0; --i) |
307 | { | ||
308 |
1/2✓ Branch 3 taken 82 times.
✗ Branch 4 not taken.
|
82 | Pass2::run(model.joints[i], typename Pass2::ArgsType(model, data)); |
309 | } | ||
310 | |||
311 | // expressed all the quantities around the center of mass | ||
312 | typedef typename Data::Inertia Inertia; | ||
313 | |||
314 | 3 | const Inertia & Ytot = data.oYcrb[0]; | |
315 | 3 | const typename Inertia::Vector3 & com = Ytot.lever(); | |
316 | |||
317 | // Mass of the system | ||
318 | 3 | data.mass[0] = Ytot.mass(); | |
319 | |||
320 | // Center of mass of the system | ||
321 | 3 | data.com[0] = Ytot.lever(); | |
322 | |||
323 | // Compute the centroidal quantities | ||
324 | 3 | data.hg = data.oh[0]; | |
325 |
3/6✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 3 times.
✗ Branch 9 not taken.
|
3 | data.hg.angular() += data.hg.linear().cross(com); |
326 | |||
327 | 3 | data.dhg = data.of[0]; | |
328 |
3/6✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 3 times.
✗ Branch 9 not taken.
|
3 | data.dhg.angular() += data.dhg.linear().cross(com); |
329 | |||
330 | // Compute centroidal inertia | ||
331 | 3 | data.Ig.mass() = Ytot.mass(); | |
332 | 3 | data.Ig.lever().setZero(); | |
333 | 3 | data.Ig.inertia() = Ytot.inertia(); | |
334 | |||
335 | // Compute the partial derivatives | ||
336 | 3 | translateForceSet(data.dHdq, com, dh_dq.const_cast_derived()); | |
337 | 3 | Matrix6xLike0 & dh_dq_ = dh_dq.const_cast_derived(); | |
338 |
2/2✓ Branch 0 taken 99 times.
✓ Branch 1 taken 3 times.
|
102 | for (Eigen::DenseIndex k = 0; k < model.nv; ++k) |
339 |
5/10✓ Branch 1 taken 99 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 99 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 99 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 99 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 99 times.
✗ Branch 14 not taken.
|
99 | dh_dq_.col(k).template segment<3>(Force::ANGULAR) += |
340 |
3/6✓ Branch 2 taken 99 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 99 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 99 times.
✗ Branch 9 not taken.
|
198 | data.hg.linear().cross(data.dFda.col(k).template segment<3>(Force::LINEAR)) / Ytot.mass(); |
341 | |||
342 | 3 | translateForceSet(data.dFdq, com, dhdot_dq.const_cast_derived()); | |
343 | 3 | Matrix6xLike1 & dhdot_dq_ = dhdot_dq.const_cast_derived(); | |
344 |
2/2✓ Branch 0 taken 99 times.
✓ Branch 1 taken 3 times.
|
102 | for (Eigen::DenseIndex k = 0; k < model.nv; ++k) |
345 |
5/10✓ Branch 1 taken 99 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 99 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 99 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 99 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 99 times.
✗ Branch 14 not taken.
|
99 | dhdot_dq_.col(k).template segment<3>(Force::ANGULAR) += |
346 |
2/4✓ Branch 1 taken 99 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 99 times.
✗ Branch 5 not taken.
|
99 | data.dhg.linear().cross(data.dFda.col(k).template segment<3>(Force::LINEAR)) |
347 |
1/2✓ Branch 2 taken 99 times.
✗ Branch 3 not taken.
|
198 | / Ytot.mass(); |
348 | |||
349 | 3 | translateForceSet(data.dFdv, com, dhdot_dv.const_cast_derived()); | |
350 | |||
351 | 3 | translateForceSet(data.dFda, com, data.Ag); | |
352 | 3 | dhdot_da.const_cast_derived() = data.Ag; | |
353 | 3 | } | |
354 | |||
355 | template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl> | ||
356 | struct GetCentroidalDynDerivativesBackwardStep | ||
357 | : public fusion::JointUnaryVisitorBase< | ||
358 | GetCentroidalDynDerivativesBackwardStep<Scalar, Options, JointCollectionTpl>> | ||
359 | { | ||
360 | typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model; | ||
361 | typedef DataTpl<Scalar, Options, JointCollectionTpl> Data; | ||
362 | |||
363 | typedef boost::fusion::vector<const Model &, Data &> ArgsType; | ||
364 | |||
365 | template<typename JointModel> | ||
366 | 108 | static void algo(const JointModelBase<JointModel> & jmodel, const Model & model, Data & data) | |
367 | { | ||
368 | typedef typename Model::JointIndex JointIndex; | ||
369 | typedef typename Data::Vector3 Vector3; | ||
370 | typedef | ||
371 | typename SizeDepType<JointModel::NV>::template ColsReturn<typename Data::Matrix6x>::Type | ||
372 | ColsBlock; | ||
373 | |||
374 |
1/2✓ Branch 1 taken 54 times.
✗ Branch 2 not taken.
|
108 | const JointIndex & i = jmodel.id(); |
375 | 108 | const JointIndex & parent = model.parents[i]; | |
376 | |||
377 | 108 | typename Data::Motion & vtmp = data.v[0]; | |
378 | 108 | typename Data::Matrix6x & Ftmp = data.Fcrb[0]; | |
379 | |||
380 |
1/2✓ Branch 1 taken 54 times.
✗ Branch 2 not taken.
|
108 | ColsBlock J_cols = jmodel.jointCols(data.J); |
381 |
1/2✓ Branch 1 taken 54 times.
✗ Branch 2 not taken.
|
108 | ColsBlock dVdq_cols = jmodel.jointCols(data.dVdq); |
382 |
1/2✓ Branch 1 taken 54 times.
✗ Branch 2 not taken.
|
108 | ColsBlock dHdq_cols = jmodel.jointCols(data.dHdq); |
383 |
1/2✓ Branch 1 taken 54 times.
✗ Branch 2 not taken.
|
108 | ColsBlock Ftmp_cols = jmodel.jointCols(Ftmp); |
384 | |||
385 |
3/6✓ Branch 1 taken 54 times.
✗ Branch 2 not taken.
✓ Branch 6 taken 54 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 54 times.
✗ Branch 10 not taken.
|
108 | const Vector3 mg = data.oYcrb[i].mass() * model.gravity.linear(); |
386 |
3/4✓ Branch 1 taken 118 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 64 times.
✓ Branch 4 taken 54 times.
|
236 | for (Eigen::DenseIndex k = 0; k < jmodel.nv(); ++k) |
387 | { | ||
388 |
2/4✓ Branch 1 taken 64 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 64 times.
✗ Branch 5 not taken.
|
128 | MotionRef<typename ColsBlock::ColXpr> mref(J_cols.col(k)); |
389 |
6/12✓ Branch 1 taken 64 times.
✗ Branch 2 not taken.
✓ Branch 6 taken 64 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 64 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 64 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 64 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 64 times.
✗ Branch 19 not taken.
|
128 | vtmp.linear() = mref.linear() + mref.angular().cross(data.oYcrb[i].lever()); |
390 | |||
391 |
2/4✓ Branch 1 taken 64 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 64 times.
✗ Branch 5 not taken.
|
128 | ForceRef<typename ColsBlock::ColXpr> fout(Ftmp_cols.col(k)); |
392 |
4/8✓ Branch 1 taken 64 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 64 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 64 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 64 times.
✗ Branch 11 not taken.
|
128 | fout.angular() += vtmp.linear().cross(mg); |
393 | } | ||
394 | |||
395 |
1/2✓ Branch 3 taken 54 times.
✗ Branch 4 not taken.
|
108 | data.oh[parent] += data.oh[i]; |
396 |
2/2✓ Branch 0 taken 2 times.
✓ Branch 1 taken 52 times.
|
108 | if (parent == 0) |
397 | { | ||
398 |
1/2✓ Branch 3 taken 2 times.
✗ Branch 4 not taken.
|
4 | data.of[0] += data.of[i]; |
399 |
1/2✓ Branch 3 taken 2 times.
✗ Branch 4 not taken.
|
4 | data.oYcrb[0] += data.oYcrb[i]; |
400 | } | ||
401 | |||
402 |
1/2✓ Branch 2 taken 54 times.
✗ Branch 3 not taken.
|
108 | motionSet::act(J_cols, data.oh[i], dHdq_cols); |
403 |
1/2✓ Branch 2 taken 54 times.
✗ Branch 3 not taken.
|
108 | motionSet::inertiaAction<ADDTO>(data.oYcrb[i], dVdq_cols, dHdq_cols); |
404 | } | ||
405 | }; // struct GetCentroidalDynDerivativesBackwardStep | ||
406 | |||
407 | template< | ||
408 | typename Scalar, | ||
409 | int Options, | ||
410 | template<typename, int> | ||
411 | class JointCollectionTpl, | ||
412 | typename Matrix6xLike0, | ||
413 | typename Matrix6xLike1, | ||
414 | typename Matrix6xLike2, | ||
415 | typename Matrix6xLike3> | ||
416 | 2 | void getCentroidalDynamicsDerivatives( | |
417 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
418 | DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
419 | const Eigen::MatrixBase<Matrix6xLike0> & dh_dq, | ||
420 | const Eigen::MatrixBase<Matrix6xLike1> & dhdot_dq, | ||
421 | const Eigen::MatrixBase<Matrix6xLike2> & dhdot_dv, | ||
422 | const Eigen::MatrixBase<Matrix6xLike3> & dhdot_da) | ||
423 | { | ||
424 |
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(dhdot_dq.cols(), model.nv); |
425 |
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(dhdot_dq.rows(), 6); |
426 |
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(dhdot_dv.cols(), model.nv); |
427 |
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(dhdot_dv.rows(), 6); |
428 |
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(dhdot_da.cols(), model.nv); |
429 |
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(dhdot_da.rows(), 6); |
430 |
1/2✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
|
2 | assert(model.check(data) && "data is not consistent with model."); |
431 | |||
432 | typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model; | ||
433 | typedef DataTpl<Scalar, Options, JointCollectionTpl> Data; | ||
434 | typedef typename Model::JointIndex JointIndex; | ||
435 | typedef typename Model::Force Force; | ||
436 | |||
437 | // compute first data.oh[0] and data.of[0] | ||
438 | 2 | data.oh[0].setZero(); | |
439 | 2 | data.of[0].setZero(); | |
440 | 2 | data.oYcrb[0].setZero(); | |
441 | |||
442 | 2 | typename Data::Matrix6x & Ftmp = data.Fcrb[0]; | |
443 | 2 | Ftmp = data.dFdq; | |
444 | |||
445 | typedef GetCentroidalDynDerivativesBackwardStep<Scalar, Options, JointCollectionTpl> Pass2; | ||
446 |
2/2✓ Branch 0 taken 54 times.
✓ Branch 1 taken 2 times.
|
56 | for (JointIndex i = (JointIndex)(model.njoints - 1); i > 0; --i) |
447 | { | ||
448 |
1/2✓ Branch 3 taken 54 times.
✗ Branch 4 not taken.
|
54 | Pass2::run(model.joints[i], typename Pass2::ArgsType(model, data)); |
449 | } | ||
450 | |||
451 | typedef typename Data::Inertia Inertia; | ||
452 | |||
453 | 2 | const Inertia & Ytot = data.oYcrb[0]; | |
454 | 2 | const typename Inertia::Vector3 & com = Ytot.lever(); | |
455 | |||
456 | // Center of mass of the system | ||
457 | 2 | data.com[0] = com; | |
458 | 2 | data.mass[0] = Ytot.mass(); | |
459 | |||
460 | // Remove the gravity contribution | ||
461 |
1/2✓ Branch 3 taken 2 times.
✗ Branch 4 not taken.
|
2 | data.of[0] += Ytot * model.gravity; |
462 | |||
463 | // Compute the centroidal quantities | ||
464 | 2 | data.hg = data.oh[0]; | |
465 |
3/6✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 2 times.
✗ Branch 9 not taken.
|
2 | data.hg.angular() += data.hg.linear().cross(com); |
466 | |||
467 | 2 | data.dhg = data.of[0]; | |
468 |
3/6✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 2 times.
✗ Branch 9 not taken.
|
2 | data.dhg.angular() += data.dhg.linear().cross(com); |
469 | |||
470 | // Compute centroidal inertia | ||
471 | 2 | data.Ig.mass() = Ytot.mass(); | |
472 | 2 | data.Ig.lever().setZero(); | |
473 | 2 | data.Ig.inertia() = Ytot.inertia(); | |
474 | |||
475 | // Retrieve the partial derivatives from RNEA derivatives | ||
476 | 2 | translateForceSet(data.dHdq, com, dh_dq.const_cast_derived()); | |
477 | 2 | Matrix6xLike0 & dh_dq_ = dh_dq.const_cast_derived(); | |
478 |
2/2✓ Branch 0 taken 64 times.
✓ Branch 1 taken 2 times.
|
66 | for (Eigen::DenseIndex k = 0; k < model.nv; ++k) |
479 |
5/10✓ Branch 1 taken 64 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 64 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 64 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 64 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 64 times.
✗ Branch 14 not taken.
|
64 | dh_dq_.col(k).template segment<3>(Force::ANGULAR) += |
480 |
3/6✓ Branch 2 taken 64 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 64 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 64 times.
✗ Branch 9 not taken.
|
128 | data.hg.linear().cross(data.dFda.col(k).template segment<3>(Force::LINEAR)) / Ytot.mass(); |
481 | |||
482 | 2 | translateForceSet(Ftmp, com, dhdot_dq.const_cast_derived()); | |
483 | 2 | Matrix6xLike1 & dhdot_dq_ = dhdot_dq.const_cast_derived(); | |
484 |
2/2✓ Branch 0 taken 64 times.
✓ Branch 1 taken 2 times.
|
66 | for (Eigen::DenseIndex k = 0; k < model.nv; ++k) |
485 |
5/10✓ Branch 1 taken 64 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 64 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 64 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 64 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 64 times.
✗ Branch 14 not taken.
|
64 | dhdot_dq_.col(k).template segment<3>(Force::ANGULAR) += |
486 |
2/4✓ Branch 1 taken 64 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 64 times.
✗ Branch 5 not taken.
|
64 | data.dhg.linear().cross(data.dFda.col(k).template segment<3>(Force::LINEAR)) |
487 |
1/2✓ Branch 2 taken 64 times.
✗ Branch 3 not taken.
|
128 | / Ytot.mass(); |
488 | |||
489 | 2 | translateForceSet(data.dFdv, com, dhdot_dv.const_cast_derived()); | |
490 | |||
491 | 2 | translateForceSet(data.dFda, com, data.Ag); | |
492 | 2 | dhdot_da.const_cast_derived() = data.Ag; | |
493 | 2 | } | |
494 | } // namespace impl | ||
495 | |||
496 | template< | ||
497 | typename Scalar, | ||
498 | int Options, | ||
499 | template<typename, int> | ||
500 | class JointCollectionTpl, | ||
501 | typename ConfigVectorType, | ||
502 | typename TangentVectorType1, | ||
503 | typename TangentVectorType2, | ||
504 | typename Matrix6xLike0, | ||
505 | typename Matrix6xLike1, | ||
506 | typename Matrix6xLike2, | ||
507 | typename Matrix6xLike3> | ||
508 | ✗ | void computeCentroidalDynamicsDerivatives( | |
509 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
510 | DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
511 | const Eigen::MatrixBase<ConfigVectorType> & q, | ||
512 | const Eigen::MatrixBase<TangentVectorType1> & v, | ||
513 | const Eigen::MatrixBase<TangentVectorType2> & a, | ||
514 | const Eigen::MatrixBase<Matrix6xLike0> & dh_dq, | ||
515 | const Eigen::MatrixBase<Matrix6xLike1> & dhdot_dq, | ||
516 | const Eigen::MatrixBase<Matrix6xLike2> & dhdot_dv, | ||
517 | const Eigen::MatrixBase<Matrix6xLike3> & dhdot_da) | ||
518 | { | ||
519 | ✗ | impl::computeCentroidalDynamicsDerivatives( | |
520 | model, data, make_const_ref(q), make_const_ref(v), make_const_ref(a), make_ref(dh_dq), | ||
521 | make_ref(dhdot_dq), make_ref(dhdot_dv), make_ref(dhdot_da)); | ||
522 | } | ||
523 | |||
524 | template< | ||
525 | typename Scalar, | ||
526 | int Options, | ||
527 | template<typename, int> | ||
528 | class JointCollectionTpl, | ||
529 | typename Matrix6xLike0, | ||
530 | typename Matrix6xLike1, | ||
531 | typename Matrix6xLike2, | ||
532 | typename Matrix6xLike3> | ||
533 | ✗ | void getCentroidalDynamicsDerivatives( | |
534 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
535 | DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
536 | const Eigen::MatrixBase<Matrix6xLike0> & dh_dq, | ||
537 | const Eigen::MatrixBase<Matrix6xLike1> & dhdot_dq, | ||
538 | const Eigen::MatrixBase<Matrix6xLike2> & dhdot_dv, | ||
539 | const Eigen::MatrixBase<Matrix6xLike3> & dhdot_da) | ||
540 | { | ||
541 | ✗ | impl::getCentroidalDynamicsDerivatives( | |
542 | model, data, make_ref(dh_dq), make_ref(dhdot_dq), make_ref(dhdot_dv), make_ref(dhdot_da)); | ||
543 | } | ||
544 | |||
545 | } // namespace pinocchio | ||
546 | |||
547 | /// @endcond | ||
548 | |||
549 | #endif // ifndef __pinocchio_algorithm_centroidal_derivatives_hxx__ | ||
550 |