Directory: | ./ |
---|---|
File: | include/pinocchio/algorithm/impulse-dynamics-derivatives.hxx |
Date: | 2024-08-27 18:20:05 |
Exec | Total | Coverage | |
---|---|---|---|
Lines: | 153 | 163 | 93.9% |
Branches: | 198 | 385 | 51.4% |
Line | Branch | Exec | Source |
---|---|---|---|
1 | // | ||
2 | // Copyright (c) 2020 CNRS INRIA | ||
3 | // | ||
4 | |||
5 | #ifndef __pinocchio_algorithm_impulse_dynamics_derivatives_hxx__ | ||
6 | #define __pinocchio_algorithm_impulse_dynamics_derivatives_hxx__ | ||
7 | |||
8 | #include "pinocchio/algorithm/check.hpp" | ||
9 | #include "pinocchio/algorithm/rnea-derivatives.hpp" | ||
10 | #include "pinocchio/algorithm/kinematics-derivatives.hpp" | ||
11 | #include "pinocchio/algorithm/contact-cholesky.hpp" | ||
12 | #include "pinocchio/algorithm/constrained-dynamics-derivatives.hxx" | ||
13 | |||
14 | namespace pinocchio | ||
15 | { | ||
16 | |||
17 | template< | ||
18 | typename Scalar, | ||
19 | int Options, | ||
20 | template<typename, int> | ||
21 | class JointCollectionTpl, | ||
22 | typename Matrix3xOut1, | ||
23 | typename Matrix3xOut2> | ||
24 | struct JointImpulseVelocityDerivativesBackwardStep3D | ||
25 | : public fusion::JointUnaryVisitorBase<JointImpulseVelocityDerivativesBackwardStep3D< | ||
26 | Scalar, | ||
27 | Options, | ||
28 | JointCollectionTpl, | ||
29 | Matrix3xOut1, | ||
30 | Matrix3xOut2>> | ||
31 | { | ||
32 | typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model; | ||
33 | typedef DataTpl<Scalar, Options, JointCollectionTpl> Data; | ||
34 | |||
35 | typedef boost::fusion::vector< | ||
36 | const Model &, | ||
37 | Data &, | ||
38 | const typename Model::JointIndex &, | ||
39 | const SE3Tpl<Scalar> &, | ||
40 | const ReferenceFrame &, | ||
41 | const Scalar &, | ||
42 | Matrix3xOut1 &, | ||
43 | Matrix3xOut2 &> | ||
44 | ArgsType; | ||
45 | |||
46 | template<typename JointModel> | ||
47 | 42 | static void algo( | |
48 | const JointModelBase<JointModel> & jmodel, | ||
49 | const Model & model, | ||
50 | Data & data, | ||
51 | const typename Model::JointIndex & joint_id, | ||
52 | const SE3Tpl<Scalar> & placement, | ||
53 | const ReferenceFrame & rf, | ||
54 | const Scalar & r_coeff, | ||
55 | const Eigen::MatrixBase<Matrix3xOut1> & v_partial_dq, | ||
56 | const Eigen::MatrixBase<Matrix3xOut2> & v_partial_dv) | ||
57 | { | ||
58 | typedef typename Model::JointIndex JointIndex; | ||
59 | typedef typename Data::SE3 SE3; | ||
60 | typedef typename Data::Motion Motion; | ||
61 | |||
62 |
1/2✓ Branch 1 taken 21 times.
✗ Branch 2 not taken.
|
42 | const JointIndex i = jmodel.id(); |
63 | 42 | const JointIndex parent = model.parents[i]; | |
64 | |||
65 |
1/2✓ Branch 2 taken 21 times.
✗ Branch 3 not taken.
|
42 | const SE3 oMlast = data.oMi[joint_id] * placement; |
66 | |||
67 | typedef | ||
68 | typename SizeDepType<JointModel::NV>::template ColsReturn<typename Data::Matrix6x>::Type | ||
69 | ColsBlock; | ||
70 |
1/2✓ Branch 1 taken 21 times.
✗ Branch 2 not taken.
|
42 | ColsBlock Jcols = jmodel.jointCols(data.J); |
71 | |||
72 | typedef | ||
73 | typename SizeDepType<JointModel::NV>::template ColsReturn<Matrix3xOut1>::Type ColsBlockOut1; | ||
74 | 42 | Matrix3xOut1 & v_partial_dq_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix3xOut1, v_partial_dq); | |
75 | typedef | ||
76 | typename SizeDepType<JointModel::NV>::template ColsReturn<Matrix3xOut2>::Type ColsBlockOut2; | ||
77 | 42 | Matrix3xOut2 & v_partial_dv_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix3xOut2, v_partial_dv); | |
78 | |||
79 | // dvec/dv | ||
80 |
1/2✓ Branch 1 taken 21 times.
✗ Branch 2 not taken.
|
42 | const int nv = jmodel.nv(); |
81 |
1/2✓ Branch 1 taken 21 times.
✗ Branch 2 not taken.
|
42 | Eigen::Matrix<Scalar, 6, JointModel::NV, Options> v_spatial_partial_dv_cols(6, nv); |
82 |
1/2✓ Branch 1 taken 21 times.
✗ Branch 2 not taken.
|
42 | ColsBlockOut2 v_partial_dv_cols = jmodel.jointCols(v_partial_dv_); |
83 | |||
84 |
1/2✓ Branch 1 taken 21 times.
✗ Branch 2 not taken.
|
42 | motionSet::se3ActionInverse(oMlast, Jcols, v_spatial_partial_dv_cols); |
85 |
2/4✓ Branch 1 taken 21 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 21 times.
✗ Branch 5 not taken.
|
42 | v_partial_dv_cols = v_spatial_partial_dv_cols.template middleRows<3>(Motion::LINEAR); |
86 | |||
87 | // dvec/dq | ||
88 |
1/2✓ Branch 1 taken 21 times.
✗ Branch 2 not taken.
|
42 | ColsBlockOut1 v_partial_dq_cols = jmodel.jointCols(v_partial_dq_); |
89 | |||
90 | #define FOR_NV() for (Eigen::DenseIndex j = 0; j < nv; ++j) | ||
91 | #define GET_LINEAR(vec6) vec6.template segment<3>(Motion::LINEAR) | ||
92 | #define GET_ANGULAR(vec6) vec6.template segment<3>(Motion::ANGULAR) | ||
93 | |||
94 | 42 | const Scalar factor = Scalar(1) + r_coeff; | |
95 | |||
96 |
2/2✓ Branch 0 taken 18 times.
✓ Branch 1 taken 3 times.
|
42 | if (parent > 0) |
97 | { | ||
98 |
3/6✓ Branch 3 taken 18 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 18 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 18 times.
✗ Branch 10 not taken.
|
36 | const Motion vtmp = oMlast.actInv(factor * data.ov[parent] + data.oa[parent]); |
99 |
2/2✓ Branch 0 taken 18 times.
✓ Branch 1 taken 18 times.
|
72 | FOR_NV() |
100 |
6/12✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 18 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 18 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 18 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 18 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 18 times.
✗ Branch 17 not taken.
|
72 | v_partial_dq_cols.col(j).noalias() = |
101 |
3/6✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 18 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 18 times.
✗ Branch 8 not taken.
|
36 | vtmp.angular().cross(GET_LINEAR(v_spatial_partial_dv_cols.col(j))) |
102 |
3/6✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 18 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 18 times.
✗ Branch 8 not taken.
|
72 | + vtmp.linear().cross(GET_ANGULAR(v_spatial_partial_dv_cols.col(j))); |
103 | } | ||
104 | else | ||
105 |
1/2✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
|
6 | v_partial_dq_cols.setZero(); |
106 | |||
107 |
2/2✓ Branch 0 taken 7 times.
✓ Branch 1 taken 14 times.
|
42 | if (rf == LOCAL_WORLD_ALIGNED) |
108 | { | ||
109 |
3/6✓ Branch 3 taken 7 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 7 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 7 times.
✗ Branch 10 not taken.
|
14 | const Motion vtmp = oMlast.actInv(factor * data.ov[joint_id] + data.oa[joint_id]); |
110 |
2/2✓ Branch 0 taken 12 times.
✓ Branch 1 taken 7 times.
|
38 | FOR_NV() |
111 |
4/8✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 12 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 12 times.
✗ Branch 11 not taken.
|
72 | v_partial_dq_cols.col(j) = |
112 |
2/4✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12 times.
✗ Branch 5 not taken.
|
24 | oMlast.rotation() |
113 |
4/8✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 12 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 12 times.
✗ Branch 11 not taken.
|
72 | * (v_partial_dq_cols.col(j) + GET_ANGULAR(v_spatial_partial_dv_cols.col(j)).cross(vtmp.linear())); |
114 |
2/2✓ Branch 0 taken 12 times.
✓ Branch 1 taken 7 times.
|
38 | FOR_NV() |
115 |
5/10✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 12 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 12 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 12 times.
✗ Branch 14 not taken.
|
24 | v_partial_dv_cols.col(j) = oMlast.rotation() * v_partial_dv_cols.col(j); |
116 | } | ||
117 | |||
118 | #undef FOR_NV | ||
119 | #undef GET_LINEAR | ||
120 | #undef GET_ANGULAR | ||
121 | } | ||
122 | }; | ||
123 | |||
124 | template< | ||
125 | typename Scalar, | ||
126 | int Options, | ||
127 | template<typename, int> | ||
128 | class JointCollectionTpl, | ||
129 | typename Matrix6xOut1, | ||
130 | typename Matrix6xOut2> | ||
131 | struct JointImpulseVelocityDerivativesBackwardStep6D | ||
132 | : public fusion::JointUnaryVisitorBase<JointImpulseVelocityDerivativesBackwardStep6D< | ||
133 | Scalar, | ||
134 | Options, | ||
135 | JointCollectionTpl, | ||
136 | Matrix6xOut1, | ||
137 | Matrix6xOut2>> | ||
138 | { | ||
139 | typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model; | ||
140 | typedef DataTpl<Scalar, Options, JointCollectionTpl> Data; | ||
141 | |||
142 | typedef boost::fusion::vector< | ||
143 | const Model &, | ||
144 | Data &, | ||
145 | const typename Model::JointIndex &, | ||
146 | const SE3Tpl<Scalar> &, | ||
147 | const ReferenceFrame &, | ||
148 | const Scalar &, | ||
149 | Matrix6xOut1 &, | ||
150 | Matrix6xOut2 &> | ||
151 | ArgsType; | ||
152 | |||
153 | template<typename JointModel> | ||
154 | 42 | static void algo( | |
155 | const JointModelBase<JointModel> & jmodel, | ||
156 | const Model & model, | ||
157 | Data & data, | ||
158 | const typename Model::JointIndex & joint_id, | ||
159 | const SE3Tpl<Scalar> & placement, | ||
160 | const ReferenceFrame & rf, | ||
161 | const Scalar & r_coeff, | ||
162 | const Eigen::MatrixBase<Matrix6xOut1> & v_partial_dq, | ||
163 | const Eigen::MatrixBase<Matrix6xOut2> & v_partial_dv) | ||
164 | { | ||
165 | typedef typename Model::JointIndex JointIndex; | ||
166 | typedef typename Data::SE3 SE3; | ||
167 | typedef typename Data::Motion Motion; | ||
168 | |||
169 |
1/2✓ Branch 1 taken 21 times.
✗ Branch 2 not taken.
|
42 | const JointIndex i = jmodel.id(); |
170 | 42 | const JointIndex parent = model.parents[i]; | |
171 |
1/2✓ Branch 1 taken 21 times.
✗ Branch 2 not taken.
|
42 | Motion vtmp; // Temporary variables |
172 | |||
173 |
1/2✓ Branch 2 taken 21 times.
✗ Branch 3 not taken.
|
42 | const SE3 oMlast = data.oMi[joint_id] * placement; |
174 | 42 | const Motion & v_last = data.ov[joint_id]; | |
175 | 42 | const Motion & dv_last = data.oa[joint_id]; | |
176 | |||
177 | typedef | ||
178 | typename SizeDepType<JointModel::NV>::template ColsReturn<typename Data::Matrix6x>::Type | ||
179 | ColsBlock; | ||
180 |
1/2✓ Branch 1 taken 21 times.
✗ Branch 2 not taken.
|
42 | ColsBlock Jcols = jmodel.jointCols(data.J); |
181 | |||
182 | typedef | ||
183 | typename SizeDepType<JointModel::NV>::template ColsReturn<Matrix6xOut1>::Type ColsBlockOut1; | ||
184 | 42 | Matrix6xOut1 & v_partial_dq_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut1, v_partial_dq); | |
185 | typedef | ||
186 | typename SizeDepType<JointModel::NV>::template ColsReturn<Matrix6xOut2>::Type ColsBlockOut2; | ||
187 | 42 | Matrix6xOut2 & v_partial_dv_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut2, v_partial_dv); | |
188 | |||
189 | // dvec/dv | ||
190 |
1/2✓ Branch 1 taken 21 times.
✗ Branch 2 not taken.
|
42 | ColsBlockOut2 v_partial_dv_cols = jmodel.jointCols(v_partial_dv_); |
191 | |||
192 |
2/3✓ Branch 0 taken 7 times.
✓ Branch 1 taken 14 times.
✗ Branch 2 not taken.
|
42 | switch (rf) |
193 | { | ||
194 | 14 | case LOCAL_WORLD_ALIGNED: | |
195 |
1/2✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
|
14 | details::translateJointJacobian(oMlast, Jcols, v_partial_dv_cols); |
196 | 14 | break; | |
197 | 28 | case LOCAL: | |
198 |
1/2✓ Branch 1 taken 14 times.
✗ Branch 2 not taken.
|
28 | motionSet::se3ActionInverse(oMlast, Jcols, v_partial_dv_cols); |
199 | 28 | break; | |
200 | ✗ | default: | |
201 | ✗ | assert(false && "This must never happened"); | |
202 | } | ||
203 | |||
204 | // dvec/dq | ||
205 |
1/2✓ Branch 1 taken 21 times.
✗ Branch 2 not taken.
|
42 | ColsBlockOut1 v_partial_dq_cols = jmodel.jointCols(v_partial_dq_); |
206 | 42 | const Scalar factor = Scalar(1) + r_coeff; | |
207 |
2/3✓ Branch 0 taken 7 times.
✓ Branch 1 taken 14 times.
✗ Branch 2 not taken.
|
42 | switch (rf) |
208 | { | ||
209 | 14 | case LOCAL_WORLD_ALIGNED: | |
210 |
2/2✓ Branch 0 taken 6 times.
✓ Branch 1 taken 1 times.
|
14 | if (parent > 0) |
211 | { | ||
212 |
3/6✓ Branch 2 taken 6 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 6 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 6 times.
✗ Branch 9 not taken.
|
12 | vtmp = factor * (data.ov[parent] - v_last); |
213 |
2/4✓ Branch 2 taken 6 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 6 times.
✗ Branch 6 not taken.
|
12 | vtmp += data.oa[parent] - dv_last; |
214 | } | ||
215 | else | ||
216 | { | ||
217 |
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.
|
2 | vtmp = -(factor * v_last + dv_last); |
218 | } | ||
219 |
5/10✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 7 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 7 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 7 times.
✗ Branch 14 not taken.
|
14 | vtmp.linear() += vtmp.angular().cross(oMlast.translation()); |
220 |
1/2✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
|
14 | motionSet::motionAction(vtmp, v_partial_dv_cols, v_partial_dq_cols); |
221 | 14 | break; | |
222 | 28 | case LOCAL: | |
223 |
2/2✓ Branch 0 taken 12 times.
✓ Branch 1 taken 2 times.
|
28 | if (parent > 0) |
224 | { | ||
225 |
4/8✓ Branch 3 taken 12 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 12 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 12 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 12 times.
✗ Branch 13 not taken.
|
24 | vtmp = oMlast.actInv(factor * data.ov[parent] + data.oa[parent]); |
226 |
1/2✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
|
24 | motionSet::motionAction(vtmp, v_partial_dv_cols, v_partial_dq_cols); |
227 | } | ||
228 | 28 | break; | |
229 | ✗ | default: | |
230 | ✗ | assert(false && "This must never happened"); | |
231 | } | ||
232 | } | ||
233 | }; | ||
234 | |||
235 | template< | ||
236 | typename Scalar, | ||
237 | int Options, | ||
238 | template<typename, int> | ||
239 | class JointCollectionTpl, | ||
240 | class ConstraintModelAllocator, | ||
241 | class ConstraintDataAllocator, | ||
242 | typename MatrixType1, | ||
243 | typename MatrixType2, | ||
244 | typename MatrixType3, | ||
245 | typename MatrixType4> | ||
246 | 4 | inline void computeImpulseDynamicsDerivatives( | |
247 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
248 | DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
249 | const std::vector<RigidConstraintModelTpl<Scalar, Options>, ConstraintModelAllocator> & | ||
250 | contact_models, | ||
251 | std::vector<RigidConstraintDataTpl<Scalar, Options>, ConstraintDataAllocator> & contact_data, | ||
252 | const Scalar r_coeff, | ||
253 | const ProximalSettingsTpl<Scalar> & settings, | ||
254 | const Eigen::MatrixBase<MatrixType1> & dvimpulse_partial_dq, | ||
255 | const Eigen::MatrixBase<MatrixType2> & dvimpulse_partial_dv, | ||
256 | const Eigen::MatrixBase<MatrixType3> & impulse_partial_dq, | ||
257 | const Eigen::MatrixBase<MatrixType4> & impulse_partial_dv) | ||
258 | { | ||
259 | 4 | const Eigen::DenseIndex nc = data.contact_chol.constraintDim(); | |
260 | |||
261 |
1/4✗ Branch 2 not taken.
✓ Branch 3 taken 4 times.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
|
4 | PINOCCHIO_CHECK_INPUT_ARGUMENT( |
262 | contact_data.size() == contact_models.size(), | ||
263 | "contact_data and contact_models do not have the same size"); | ||
264 | |||
265 |
1/4✗ Branch 1 not taken.
✓ Branch 2 taken 4 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
|
4 | PINOCCHIO_CHECK_INPUT_ARGUMENT(dvimpulse_partial_dq.cols() == model.nv); |
266 |
1/4✗ Branch 1 not taken.
✓ Branch 2 taken 4 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
|
4 | PINOCCHIO_CHECK_INPUT_ARGUMENT(dvimpulse_partial_dq.rows() == model.nv); |
267 | |||
268 |
1/4✗ Branch 1 not taken.
✓ Branch 2 taken 4 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
|
4 | PINOCCHIO_CHECK_INPUT_ARGUMENT(dvimpulse_partial_dv.cols() == model.nv); |
269 |
1/4✗ Branch 1 not taken.
✓ Branch 2 taken 4 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
|
4 | PINOCCHIO_CHECK_INPUT_ARGUMENT(dvimpulse_partial_dv.rows() == model.nv); |
270 | |||
271 |
1/4✗ Branch 1 not taken.
✓ Branch 2 taken 4 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
|
4 | PINOCCHIO_CHECK_INPUT_ARGUMENT(impulse_partial_dq.cols() == model.nv); |
272 |
1/4✗ Branch 1 not taken.
✓ Branch 2 taken 4 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
|
4 | PINOCCHIO_CHECK_INPUT_ARGUMENT(impulse_partial_dq.rows() == nc); |
273 | |||
274 |
1/4✗ Branch 1 not taken.
✓ Branch 2 taken 4 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
|
4 | PINOCCHIO_CHECK_INPUT_ARGUMENT(impulse_partial_dv.cols() == model.nv); |
275 |
1/4✗ Branch 1 not taken.
✓ Branch 2 taken 4 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
|
4 | PINOCCHIO_CHECK_INPUT_ARGUMENT(impulse_partial_dv.rows() == nc); |
276 | |||
277 |
11/24✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 4 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 4 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 4 times.
✗ Branch 13 not taken.
✓ Branch 14 taken 4 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 4 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 4 times.
✗ Branch 21 not taken.
✓ Branch 22 taken 4 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 4 times.
✗ Branch 26 not taken.
✗ Branch 27 not taken.
✓ Branch 28 taken 4 times.
✗ Branch 31 not taken.
✗ Branch 32 not taken.
|
4 | PINOCCHIO_CHECK_INPUT_ARGUMENT( |
278 | check_expression_if_real<Scalar>( | ||
279 | model.gravity.angular()[0] == Scalar(0) && model.gravity.angular()[1] == Scalar(0) | ||
280 | && model.gravity.angular()[2] == Scalar(0)), | ||
281 | "The gravity must be a pure force vector, no angular part"); | ||
282 | |||
283 |
5/12✓ Branch 0 taken 4 times.
✗ Branch 1 not taken.
✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 4 times.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✓ Branch 8 taken 4 times.
✗ Branch 9 not taken.
✓ Branch 10 taken 4 times.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
|
4 | PINOCCHIO_CHECK_INPUT_ARGUMENT( |
284 | check_expression_if_real<Scalar>((r_coeff >= Scalar(0)) && (r_coeff <= Scalar(1))) | ||
285 | && "coeff of restitution lies between 0 and 1."); | ||
286 |
3/8✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 4 times.
✗ Branch 9 not taken.
✗ Branch 10 not taken.
|
4 | PINOCCHIO_CHECK_INPUT_ARGUMENT( |
287 | check_expression_if_real<Scalar>(settings.mu >= (Scalar(0))) && "mu must be positive."); | ||
288 | |||
289 |
1/2✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
|
4 | assert(model.check(data) && "data is not consistent with model."); |
290 | |||
291 | // TODO: User should make sure the internal quantities are reset. | ||
292 | 4 | data.dtau_dq.setZero(); | |
293 | |||
294 | typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model; | ||
295 | typedef DataTpl<Scalar, Options, JointCollectionTpl> Data; | ||
296 | typedef typename Data::Force Force; | ||
297 | |||
298 | typedef RigidConstraintModelTpl<Scalar, Options> RigidConstraintModel; | ||
299 | typedef RigidConstraintDataTpl<Scalar, Options> RigidConstraintData; | ||
300 | |||
301 | typedef typename ModelTpl<Scalar, Options, JointCollectionTpl>::JointIndex JointIndex; | ||
302 | |||
303 | typedef ComputeConstraintDynamicsDerivativesForwardStep< | ||
304 | Scalar, Options, JointCollectionTpl, false> | ||
305 | Pass1; | ||
306 |
2/2✓ Branch 0 taken 108 times.
✓ Branch 1 taken 4 times.
|
112 | for (JointIndex i = 1; i < (JointIndex)model.njoints; ++i) |
307 | { | ||
308 |
1/2✓ Branch 4 taken 108 times.
✗ Branch 5 not taken.
|
108 | Pass1::run(model.joints[i], data.joints[i], typename Pass1::ArgsType(model, data)); |
309 | } | ||
310 | |||
311 | 4 | internal::ContactForceContribution<Scalar>::run(contact_models, data, contact_data); | |
312 | |||
313 | typedef ComputeContactDynamicDerivativesBackwardStep<Scalar, Options, JointCollectionTpl, false> | ||
314 | Pass2; | ||
315 |
2/2✓ Branch 0 taken 108 times.
✓ Branch 1 taken 4 times.
|
112 | for (JointIndex i = (JointIndex)(model.njoints - 1); i > 0; --i) |
316 | { | ||
317 |
1/2✓ Branch 3 taken 108 times.
✗ Branch 4 not taken.
|
108 | Pass2::run(model.joints[i], typename Pass2::ArgsType(model, data)); |
318 | } | ||
319 | |||
320 | 4 | Eigen::DenseIndex current_row_sol_id = 0; | |
321 | typedef typename SizeDepType<3>::template RowsReturn<typename Data::MatrixXs>::Type Rows3Block; | ||
322 | typedef typename SizeDepType<6>::template RowsReturn<typename Data::MatrixXs>::Type Rows6Block; | ||
323 |
2/2✓ Branch 1 taken 6 times.
✓ Branch 2 taken 4 times.
|
10 | for (size_t k = 0; k < contact_models.size(); ++k) |
324 | { | ||
325 | 6 | const RigidConstraintModel & cmodel = contact_models[k]; | |
326 | |||
327 | 6 | const typename Model::JointIndex joint1_id = cmodel.joint1_id; | |
328 | |||
329 |
2/3✓ Branch 0 taken 3 times.
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
|
6 | switch (cmodel.type) |
330 | { | ||
331 | 3 | case CONTACT_6D: { | |
332 | typedef JointImpulseVelocityDerivativesBackwardStep6D< | ||
333 | Scalar, Options, JointCollectionTpl, Rows6Block, Rows6Block> | ||
334 | Pass3; | ||
335 |
1/2✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
|
3 | Rows6Block contact_dvc_dq = SizeDepType<6>::middleRows(data.dvc_dq, current_row_sol_id); |
336 |
1/2✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
|
3 | Rows6Block contact_dvc_dv = SizeDepType<6>::middleRows(data.dac_da, current_row_sol_id); |
337 |
2/2✓ Branch 0 taken 21 times.
✓ Branch 1 taken 3 times.
|
24 | for (JointIndex i = joint1_id; i > 0; i = model.parents[i]) |
338 | { | ||
339 |
1/2✓ Branch 1 taken 21 times.
✗ Branch 2 not taken.
|
21 | Pass3::run( |
340 | 21 | model.joints[i], | |
341 | 42 | typename Pass3::ArgsType( | |
342 |
1/2✓ Branch 1 taken 21 times.
✗ Branch 2 not taken.
|
21 | model, data, joint1_id, cmodel.joint1_placement, cmodel.reference_frame, r_coeff, |
343 | PINOCCHIO_EIGEN_CONST_CAST(Rows6Block, contact_dvc_dq), | ||
344 | PINOCCHIO_EIGEN_CONST_CAST(Rows6Block, contact_dvc_dv))); | ||
345 | } | ||
346 | |||
347 | 3 | break; | |
348 | } | ||
349 | 3 | case CONTACT_3D: { | |
350 | typedef JointImpulseVelocityDerivativesBackwardStep3D< | ||
351 | Scalar, Options, JointCollectionTpl, Rows3Block, Rows3Block> | ||
352 | Pass3; | ||
353 |
1/2✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
|
3 | Rows3Block contact_dvc_dq = SizeDepType<3>::middleRows(data.dvc_dq, current_row_sol_id); |
354 |
1/2✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
|
3 | Rows3Block contact_dvc_dv = SizeDepType<3>::middleRows(data.dac_da, current_row_sol_id); |
355 |
2/2✓ Branch 0 taken 21 times.
✓ Branch 1 taken 3 times.
|
24 | for (JointIndex i = joint1_id; i > 0; i = model.parents[i]) |
356 | { | ||
357 |
1/2✓ Branch 1 taken 21 times.
✗ Branch 2 not taken.
|
21 | Pass3::run( |
358 | 42 | model.joints[i], typename Pass3::ArgsType( | |
359 | 21 | model, data, joint1_id, cmodel.joint1_placement, | |
360 |
1/2✓ Branch 1 taken 21 times.
✗ Branch 2 not taken.
|
21 | cmodel.reference_frame, r_coeff, contact_dvc_dq, contact_dvc_dv)); |
361 | } | ||
362 | 3 | break; | |
363 | } | ||
364 | ✗ | default: | |
365 | ✗ | assert(false && "must never happen"); | |
366 | break; | ||
367 | } | ||
368 | 6 | current_row_sol_id += cmodel.size(); | |
369 | } | ||
370 | |||
371 | 4 | data.contact_chol.getOperationalSpaceInertiaMatrix(data.osim); | |
372 | 4 | data.contact_chol.getInverseMassMatrix(data.Minv); | |
373 | // Temporary: dlambda_dtau stores J*Minv | ||
374 | 4 | typename Data::MatrixXs & JMinv = data.dlambda_dtau; | |
375 | 4 | typename Data::MatrixXs & Jc = data.dac_da; | |
376 | |||
377 |
2/4✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 4 times.
✗ Branch 6 not taken.
|
4 | JMinv.noalias() = Jc * data.Minv; |
378 |
2/4✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 4 times.
✗ Branch 6 not taken.
|
4 | data.dvc_dq.noalias() -= JMinv * data.dtau_dq; |
379 | |||
380 | 4 | MatrixType3 & dic_dq = PINOCCHIO_EIGEN_CONST_CAST(MatrixType3, impulse_partial_dq); | |
381 |
3/6✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 4 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 4 times.
✗ Branch 9 not taken.
|
4 | dic_dq.noalias() = -data.osim * data.dvc_dq; // OUTPUT |
382 | |||
383 | // TODO: Implem sparse. | ||
384 |
3/6✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 4 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 4 times.
✗ Branch 9 not taken.
|
4 | data.dtau_dq.noalias() -= Jc.transpose() * impulse_partial_dq; |
385 | |||
386 |
3/6✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 4 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 4 times.
✗ Branch 9 not taken.
|
4 | PINOCCHIO_EIGEN_CONST_CAST(MatrixType4, impulse_partial_dv).noalias() = |
387 |
1/2✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
|
4 | -(Scalar(1) + r_coeff) * data.osim * Jc; |
388 | ; // OUTPUT | ||
389 | |||
390 |
3/6✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 4 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 4 times.
✗ Branch 9 not taken.
|
4 | PINOCCHIO_EIGEN_CONST_CAST(MatrixType1, dvimpulse_partial_dq).noalias() = |
391 | 4 | -data.Minv * data.dtau_dq; // OUTPUT | |
392 |
2/4✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 4 times.
✗ Branch 6 not taken.
|
4 | PINOCCHIO_EIGEN_CONST_CAST(MatrixType2, dvimpulse_partial_dv).noalias() = |
393 |
1/2✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
|
4 | JMinv.transpose() * impulse_partial_dv; // OUTPUT |
394 | |||
395 | // update in world frame a(df/dt) = d(af)/dt + av X af | ||
396 | |||
397 | 4 | current_row_sol_id = 0; | |
398 |
2/2✓ Branch 1 taken 6 times.
✓ Branch 2 taken 4 times.
|
10 | for (size_t k = 0; k < contact_models.size(); ++k) |
399 | { | ||
400 | 6 | const RigidConstraintModel & cmodel = contact_models[k]; | |
401 | 6 | const RigidConstraintData & cdata = contact_data[k]; | |
402 | 6 | const typename Model::JointIndex joint1_id = cmodel.joint1_id; | |
403 | 6 | const int colRef = nv(model.joints[joint1_id]) + idx_v(model.joints[joint1_id]) - 1; | |
404 |
2/3✓ Branch 0 taken 4 times.
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
|
6 | switch (cmodel.reference_frame) |
405 | { | ||
406 | 4 | case LOCAL: | |
407 | 4 | break; | |
408 | 2 | case LOCAL_WORLD_ALIGNED: { | |
409 | 2 | const Force & of = cdata.contact_force; | |
410 |
2/3✓ Branch 0 taken 1 times.
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | switch (cmodel.type) |
411 | { | ||
412 | 1 | case CONTACT_6D: { | |
413 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | Rows6Block contact_dvc_dv = SizeDepType<6>::middleRows(data.dac_da, current_row_sol_id); |
414 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | Rows6Block contact_dic_dq = SizeDepType<6>::middleRows(dic_dq, current_row_sol_id); |
415 |
2/2✓ Branch 0 taken 12 times.
✓ Branch 1 taken 1 times.
|
13 | for (Eigen::DenseIndex j = colRef; j >= 0; j = data.parents_fromRow[(size_t)j]) |
416 | { | ||
417 | typedef typename Rows6Block::ColXpr ColType; | ||
418 | typedef typename Rows6Block::ColXpr ColTypeOut; | ||
419 |
2/4✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12 times.
✗ Branch 5 not taken.
|
12 | MotionRef<ColType> min(contact_dvc_dv.col(j)); |
420 |
2/4✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12 times.
✗ Branch 5 not taken.
|
12 | ForceRef<ColTypeOut> fout(contact_dic_dq.col(j)); |
421 |
6/12✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 12 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 12 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 12 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 12 times.
✗ Branch 17 not taken.
|
12 | fout.linear().noalias() += min.angular().cross(of.linear()); |
422 |
6/12✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 12 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 12 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 12 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 12 times.
✗ Branch 17 not taken.
|
12 | fout.angular().noalias() += min.angular().cross(of.angular()); |
423 | } | ||
424 | 1 | break; | |
425 | } | ||
426 | 1 | case CONTACT_3D: { | |
427 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | Rows3Block contact_dic_dq = SizeDepType<3>::middleRows(dic_dq, current_row_sol_id); |
428 |
2/2✓ Branch 0 taken 12 times.
✓ Branch 1 taken 1 times.
|
13 | for (Eigen::DenseIndex j = colRef; j >= 0; j = data.parents_fromRow[(size_t)j]) |
429 | { | ||
430 | typedef typename Data::Matrix6x::ColXpr ColType; | ||
431 |
2/4✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12 times.
✗ Branch 5 not taken.
|
12 | MotionRef<ColType> min(data.J.col(j)); |
432 |
6/12✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 12 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 12 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 12 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 12 times.
✗ Branch 17 not taken.
|
12 | contact_dic_dq.col(j).noalias() += min.angular().cross(of.linear()); |
433 | } | ||
434 | 1 | break; | |
435 | } | ||
436 | ✗ | default: | |
437 | ✗ | assert(false && "must never happen"); | |
438 | break; | ||
439 | } | ||
440 | 2 | break; | |
441 | } | ||
442 | ✗ | default: | |
443 | ✗ | assert(false && "must never happen"); | |
444 | break; | ||
445 | } | ||
446 | 6 | current_row_sol_id += cmodel.size(); | |
447 | } | ||
448 | 4 | } | |
449 | } // namespace pinocchio | ||
450 | |||
451 | #endif // ifndef __pinocchio_algorithm_impulse_dynamics_derivatives_hxx__ | ||
452 |