GCC Code Coverage Report

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