GCC Code Coverage Report

Directory: ./
File: include/pinocchio/algorithm/impulse-dynamics-derivatives.hxx
Date: 2025-02-12 21:03:38
Exec Total Coverage
Lines: 153 163 93.9%
Branches: 198 472 41.9%
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
258
1/4
✗ Branch 2 not taken.
✓ Branch 3 taken 4 times.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
 4 PINOCCHIO_CHECK_INPUT_ARGUMENT(
259 contact_data.size() == contact_models.size(),
260 "contact_data and contact_models do not have the same size");
261
262
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);
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.rows() == model.nv);
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_dv.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_dv.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(impulse_partial_dq.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(impulse_partial_dq.rows() == nc);
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_dv.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_dv.rows() == nc);
273
274
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(
275 check_expression_if_real<Scalar>(
276 model.gravity.angular()[0] == Scalar(0) && model.gravity.angular()[1] == Scalar(0)
277 && model.gravity.angular()[2] == Scalar(0)),
278 "The gravity must be a pure force vector, no angular part");
279
280
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(
281 check_expression_if_real<Scalar>((r_coeff >= Scalar(0)) && (r_coeff <= Scalar(1)))
282 && "coeff of restitution lies between 0 and 1.");
283
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(
284 check_expression_if_real<Scalar>(settings.mu >= (Scalar(0))) && "mu must be positive.");
285
286
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 4 assert(model.check(data) && "data is not consistent with model.");
287
288 // TODO: User should make sure the internal quantities are reset.
289 4 data.dtau_dq.setZero();
290
291 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
292 typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
293 typedef typename Data::Force Force;
294
295 typedef RigidConstraintModelTpl<Scalar, Options> RigidConstraintModel;
296 typedef RigidConstraintDataTpl<Scalar, Options> RigidConstraintData;
297
298 typedef typename ModelTpl<Scalar, Options, JointCollectionTpl>::JointIndex JointIndex;
299
300 typedef ComputeConstraintDynamicsDerivativesForwardStep<
301 Scalar, Options, JointCollectionTpl, false>
302 Pass1;
303
2/2
✓ Branch 0 taken 108 times.
✓ Branch 1 taken 4 times.
 112 for (JointIndex i = 1; i < (JointIndex)model.njoints; ++i)
304 {
305
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));
306 }
307
308 4 internal::ContactForceContribution<Scalar>::run(contact_models, data, contact_data);
309
310 typedef ComputeContactDynamicDerivativesBackwardStep<Scalar, Options, JointCollectionTpl, false>
311 Pass2;
312
2/2
✓ Branch 0 taken 108 times.
✓ Branch 1 taken 4 times.
 112 for (JointIndex i = (JointIndex)(model.njoints - 1); i > 0; --i)
313 {
314
1/2
✓ Branch 3 taken 108 times.
✗ Branch 4 not taken.
 108 Pass2::run(model.joints[i], typename Pass2::ArgsType(model, data));
315 }
316
317 4 Eigen::DenseIndex current_row_sol_id = 0;
318 typedef typename SizeDepType<3>::template RowsReturn<typename Data::MatrixXs>::Type Rows3Block;
319 typedef typename SizeDepType<6>::template RowsReturn<typename Data::MatrixXs>::Type Rows6Block;
320
2/2
✓ Branch 1 taken 6 times.
✓ Branch 2 taken 4 times.
 10 for (size_t k = 0; k < contact_models.size(); ++k)
321 {
322 6 const RigidConstraintModel & cmodel = contact_models[k];
323
324 6 const typename Model::JointIndex joint1_id = cmodel.joint1_id;
325
326
2/3
✓ Branch 0 taken 3 times.
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 6 switch (cmodel.type)
327 {
328 3 case CONTACT_6D: {
329 typedef JointImpulseVelocityDerivativesBackwardStep6D<
330 Scalar, Options, JointCollectionTpl, Rows6Block, Rows6Block>
331 Pass3;
332
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);
333
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);
334
2/2
✓ Branch 0 taken 21 times.
✓ Branch 1 taken 3 times.
 24 for (JointIndex i = joint1_id; i > 0; i = model.parents[i])
335 {
336
1/2
✓ Branch 1 taken 21 times.
✗ Branch 2 not taken.
 21 Pass3::run(
337 21 model.joints[i],
338 42 typename Pass3::ArgsType(
339
1/2
✓ Branch 1 taken 21 times.
✗ Branch 2 not taken.
 21 model, data, joint1_id, cmodel.joint1_placement, cmodel.reference_frame, r_coeff,
340 PINOCCHIO_EIGEN_CONST_CAST(Rows6Block, contact_dvc_dq),
341 PINOCCHIO_EIGEN_CONST_CAST(Rows6Block, contact_dvc_dv)));
342 }
343
344 3 break;
345 }
346 3 case CONTACT_3D: {
347 typedef JointImpulseVelocityDerivativesBackwardStep3D<
348 Scalar, Options, JointCollectionTpl, Rows3Block, Rows3Block>
349 Pass3;
350
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);
351
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);
352
2/2
✓ Branch 0 taken 21 times.
✓ Branch 1 taken 3 times.
 24 for (JointIndex i = joint1_id; i > 0; i = model.parents[i])
353 {
354
1/2
✓ Branch 1 taken 21 times.
✗ Branch 2 not taken.
 21 Pass3::run(
355 42 model.joints[i], typename Pass3::ArgsType(
356 21 model, data, joint1_id, cmodel.joint1_placement,
357
1/2
✓ Branch 1 taken 21 times.
✗ Branch 2 not taken.
 21 cmodel.reference_frame, r_coeff, contact_dvc_dq, contact_dvc_dv));
358 }
359 3 break;
360 }
361 ✗ default:
362 ✗ assert(false && "must never happen");
363 break;
364 }
365 6 current_row_sol_id += cmodel.size();
366 }
367
368 4 data.contact_chol.getOperationalSpaceInertiaMatrix(data.osim);
369 4 data.contact_chol.getInverseMassMatrix(data.Minv);
370 // Temporary: dlambda_dtau stores J*Minv
371 4 typename Data::MatrixXs & JMinv = data.dlambda_dtau;
372 4 typename Data::MatrixXs & Jc = data.dac_da;
373
374
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;
375
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;
376
377 4 MatrixType3 & dic_dq = PINOCCHIO_EIGEN_CONST_CAST(MatrixType3, impulse_partial_dq);
378
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
379
380 // TODO: Implem sparse.
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 data.dtau_dq.noalias() -= Jc.transpose() * impulse_partial_dq;
382
383
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() =
384
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 4 -(Scalar(1) + r_coeff) * data.osim * Jc;
385 ; // OUTPUT
386
387
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() =
388 4 -data.Minv * data.dtau_dq; // OUTPUT
389
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() =
390
1/2
✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
 4 JMinv.transpose() * impulse_partial_dv; // OUTPUT
391
392 // update in world frame a(df/dt) = d(af)/dt + av X af
393
394 4 current_row_sol_id = 0;
395
2/2
✓ Branch 1 taken 6 times.
✓ Branch 2 taken 4 times.
 10 for (size_t k = 0; k < contact_models.size(); ++k)
396 {
397 6 const RigidConstraintModel & cmodel = contact_models[k];
398 6 const RigidConstraintData & cdata = contact_data[k];
399 6 const typename Model::JointIndex joint1_id = cmodel.joint1_id;
400 6 const int colRef = nv(model.joints[joint1_id]) + idx_v(model.joints[joint1_id]) - 1;
401
2/3
✓ Branch 0 taken 4 times.
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 6 switch (cmodel.reference_frame)
402 {
403 4 case LOCAL:
404 4 break;
405 2 case LOCAL_WORLD_ALIGNED: {
406 2 const Force & of = cdata.contact_force;
407
2/3
✓ Branch 0 taken 1 times.
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 switch (cmodel.type)
408 {
409 1 case CONTACT_6D: {
410
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);
411
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);
412
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])
413 {
414 typedef typename Rows6Block::ColXpr ColType;
415 typedef typename Rows6Block::ColXpr ColTypeOut;
416
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));
417
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));
418
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());
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.angular().noalias() += min.angular().cross(of.angular());
420 }
421 1 break;
422 }
423 1 case CONTACT_3D: {
424
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);
425
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])
426 {
427 typedef typename Data::Matrix6x::ColXpr ColType;
428
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));
429
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());
430 }
431 1 break;
432 }
433 ✗ default:
434 ✗ assert(false && "must never happen");
435 break;
436 }
437 2 break;
438 }
439 ✗ default:
440 ✗ assert(false && "must never happen");
441 break;
442 }
443 6 current_row_sol_id += cmodel.size();
444 }
445 4 }
446 } // namespace pinocchio
447
448 #endif // ifndef __pinocchio_algorithm_impulse_dynamics_derivatives_hxx__
449