GCC Code Coverage Report

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