GCC Code Coverage Report

Directory: ./
File: include/pinocchio/algorithm/constrained-dynamics-derivatives.hxx
Date: 2025-02-12 21:03:38
Exec Total Coverage
Lines: 472 496 95.2%
Branches: 781 1512 51.7%
Line Branch Exec Source
1 //
2 // Copyright (c) 2020-2022 CNRS INRIA
3 //
4
5 #ifndef __pinocchio_algorithm_constraint_dynamics_derivatives_hxx__
6 #define __pinocchio_algorithm_constraint_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/kinematics.hpp"
12 #include "pinocchio/algorithm/frames.hpp"
13 #include "pinocchio/algorithm/frames-derivatives.hpp"
14 #include "pinocchio/algorithm/contact-cholesky.hpp"
15 #include "pinocchio/algorithm/utils/motion.hpp"
16
17 namespace pinocchio
18 {
19
20 template<
21 typename Scalar,
22 int Options,
23 template<typename, int> class JointCollectionTpl,
24 bool ContactMode>
25 struct ComputeConstraintDynamicsDerivativesForwardStep
26 : public fusion::JointUnaryVisitorBase<ComputeConstraintDynamicsDerivativesForwardStep<
27 Scalar,
28 Options,
29 JointCollectionTpl,
30 ContactMode>>
31 {
32 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
33 typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
34
35 typedef boost::fusion::vector<const Model &, Data &> ArgsType;
36
37 template<typename JointModel>
38 1404 static void algo(
39 const JointModelBase<JointModel> & jmodel,
40 JointDataBase<typename JointModel::JointDataDerived> & jdata,
41 const Model & model,
42 Data & data)
43 {
44 typedef typename Model::JointIndex JointIndex;
45 typedef typename Data::Motion Motion;
46
47
1/2
✓ Branch 1 taken 702 times.
✗ Branch 2 not taken.
 1404 const JointIndex i = jmodel.id();
48 1404 const JointIndex parent = model.parents[i];
49
50 typedef
51 typename SizeDepType<JointModel::NV>::template ColsReturn<typename Data::Matrix6x>::Type
52 ColsBlock;
53
1/2
✓ Branch 1 taken 702 times.
✗ Branch 2 not taken.
 1404 ColsBlock J_cols = jmodel.jointCols(data.J);
54
1/2
✓ Branch 1 taken 702 times.
✗ Branch 2 not taken.
 1404 ColsBlock dAdq_cols = jmodel.jointCols(data.dAdq);
55
56 if (ContactMode)
57 {
58 1188 const Motion & ov = data.ov[i];
59
1/2
✓ Branch 1 taken 594 times.
✗ Branch 2 not taken.
 1188 ColsBlock dJ_cols = jmodel.jointCols(data.dJ);
60
1/2
✓ Branch 1 taken 594 times.
✗ Branch 2 not taken.
 1188 ColsBlock dVdq_cols = jmodel.jointCols(data.dVdq);
61
62
1/2
✓ Branch 1 taken 594 times.
✗ Branch 2 not taken.
 1188 motionSet::motionAction(ov, J_cols, dJ_cols);
63 // TODO: make more efficient
64
2/4
✓ Branch 3 taken 594 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 594 times.
✗ Branch 8 not taken.
 1188 data.v[i] = data.oMi[i].actInv(data.ov[i]);
65
66
2/2
✓ Branch 0 taken 572 times.
✓ Branch 1 taken 22 times.
 1188 if (parent > 0)
67 {
68
1/2
✓ Branch 2 taken 572 times.
✗ Branch 3 not taken.
 1144 motionSet::motionAction(data.ov[parent], J_cols, dVdq_cols);
69 }
70 else
71
1/2
✓ Branch 1 taken 22 times.
✗ Branch 2 not taken.
 44 dVdq_cols.setZero();
72
73 // computes variation of inertias
74
1/2
✓ Branch 2 taken 594 times.
✗ Branch 3 not taken.
 1188 data.doYcrb[i] = data.oinertias[i].variation(ov);
75 typedef impl::ComputeRNEADerivativesForwardStep<
76 Scalar, Options, JointCollectionTpl, typename Data::ConfigVectorType,
77 typename Data::TangentVectorType, typename Data::TangentVectorType>
78 RNEAForwardStepType;
79
1/2
✓ Branch 3 taken 594 times.
✗ Branch 4 not taken.
 1188 RNEAForwardStepType::addForceCrossMatrix(data.oh[i], data.doYcrb[i]);
80 1188 Motion & oa = data.oa[i];
81 1188 Motion & oa_gf = data.oa_gf[i];
82
1/2
✓ Branch 1 taken 594 times.
✗ Branch 2 not taken.
 1188 ColsBlock dAdv_cols = jmodel.jointCols(data.dAdv);
83 1188 const typename Data::TangentVectorType & a = data.ddq;
84
5/9
✓ Branch 1 taken 594 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 572 times.
✓ Branch 5 taken 22 times.
✗ Branch 6 not taken.
✓ Branch 7 taken 572 times.
✗ Branch 8 not taken.
✓ Branch 11 taken 572 times.
✗ Branch 12 not taken.
 2376 data.a[i] =
85
7/14
✓ Branch 1 taken 594 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 594 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 594 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 594 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 594 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 22 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 22 times.
✗ Branch 21 not taken.
 2376 jdata.S() * jmodel.jointVelocitySelector(a) + jdata.c() + (data.v[i] ^ jdata.v());
86
2/2
✓ Branch 0 taken 572 times.
✓ Branch 1 taken 22 times.
 1188 if (parent > 0)
87
2/4
✓ Branch 3 taken 572 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 572 times.
✗ Branch 8 not taken.
 1144 data.a[i] += data.liMi[i].actInv(data.a[parent]);
88
2/4
✓ Branch 3 taken 594 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 594 times.
✗ Branch 7 not taken.
 1188 oa = data.oMi[i].act(data.a[i]);
89
2/4
✓ Branch 1 taken 594 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 594 times.
✗ Branch 5 not taken.
 1188 oa_gf = oa - model.gravity; // add gravity contribution
90
4/8
✓ Branch 2 taken 594 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 594 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 594 times.
✗ Branch 10 not taken.
✓ Branch 13 taken 594 times.
✗ Branch 14 not taken.
 1188 data.of[i] = data.oinertias[i] * oa_gf + ov.cross(data.oh[i]);
91
1/2
✓ Branch 2 taken 594 times.
✗ Branch 3 not taken.
 1188 motionSet::motionAction(data.oa_gf[parent], J_cols, dAdq_cols);
92
1/2
✓ Branch 1 taken 594 times.
✗ Branch 2 not taken.
 1188 dAdv_cols = dJ_cols;
93
2/2
✓ Branch 0 taken 572 times.
✓ Branch 1 taken 22 times.
 1188 if (parent > 0)
94 {
95
1/2
✓ Branch 2 taken 572 times.
✗ Branch 3 not taken.
 1144 motionSet::motionAction<ADDTO>(data.ov[parent], dVdq_cols, dAdq_cols);
96
2/4
✓ Branch 1 taken 572 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 572 times.
✗ Branch 5 not taken.
 1144 dAdv_cols.noalias() += dVdq_cols;
97 }
98 }
99 else
100 {
101 216 Motion & odv = data.oa[i];
102 216 Motion & odvparent = data.oa[parent];
103 216 const typename Data::TangentVectorType & dimpulse = data.ddq;
104 // Temporary calculation of J(dq_after)
105
3/6
✓ Branch 1 taken 108 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 108 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 108 times.
✗ Branch 8 not taken.
 216 odv = J_cols * jmodel.jointVelocitySelector(dimpulse);
106
2/2
✓ Branch 0 taken 104 times.
✓ Branch 1 taken 4 times.
 216 if (parent > 0)
107
1/2
✓ Branch 1 taken 104 times.
✗ Branch 2 not taken.
 208 odv += odvparent;
108
1/2
✓ Branch 1 taken 108 times.
✗ Branch 2 not taken.
 216 motionSet::motionAction(odvparent, J_cols, dAdq_cols);
109
2/4
✓ Branch 2 taken 108 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 108 times.
✗ Branch 7 not taken.
 216 data.of[i] = data.oinertias[i] * odv;
110 }
111 }
112 };
113
114 template<
115 typename Scalar,
116 int Options,
117 template<typename, int> class JointCollectionTpl,
118 bool ContactMode>
119 struct ComputeContactDynamicDerivativesBackwardStep
120 : public fusion::JointUnaryVisitorBase<ComputeContactDynamicDerivativesBackwardStep<
121 Scalar,
122 Options,
123 JointCollectionTpl,
124 ContactMode>>
125 {
126 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
127 typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
128
129 typedef boost::fusion::vector<const Model &, Data &> ArgsType;
130
131 template<typename JointModel>
132 1404 static void algo(const JointModelBase<JointModel> & jmodel, const Model & model, Data & data)
133 {
134 typedef typename Model::JointIndex JointIndex;
135 typedef Eigen::Matrix<
136 Scalar, JointModel::NV, 6, Options, JointModel::NV == Eigen::Dynamic ? 6 : JointModel::NV,
137 6>
138 MatrixNV6;
139 typedef
140 typename SizeDepType<JointModel::NV>::template ColsReturn<typename Data::Matrix6x>::Type
141 ColsBlock;
142
143
1/2
✓ Branch 1 taken 702 times.
✗ Branch 2 not taken.
 1404 const JointIndex i = jmodel.id();
144 1404 const JointIndex parent = model.parents[i];
145
1/2
✓ Branch 1 taken 702 times.
✗ Branch 2 not taken.
 1404 ColsBlock J_cols = jmodel.jointCols(data.J);
146
1/2
✓ Branch 1 taken 702 times.
✗ Branch 2 not taken.
 1404 ColsBlock dVdq_cols = jmodel.jointCols(data.dVdq);
147
1/2
✓ Branch 1 taken 702 times.
✗ Branch 2 not taken.
 1404 ColsBlock dAdq_cols = jmodel.jointCols(data.dAdq);
148
1/2
✓ Branch 1 taken 702 times.
✗ Branch 2 not taken.
 1404 ColsBlock dFdq_cols = jmodel.jointCols(data.dFdq);
149
1/2
✓ Branch 1 taken 702 times.
✗ Branch 2 not taken.
 1404 ColsBlock dFda_cols = jmodel.jointCols(data.dFda);
150
151 1404 typename Data::RowMatrixXs & dtau_dq = data.dtau_dq;
152
153 // Temporary variables
154
2/4
✓ Branch 1 taken 702 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 702 times.
✗ Branch 5 not taken.
 1404 typename PINOCCHIO_EIGEN_PLAIN_ROW_MAJOR_TYPE(MatrixNV6) StdY(jmodel.nv(), 6);
155
156
1/2
✓ Branch 2 taken 702 times.
✗ Branch 3 not taken.
 1404 motionSet::inertiaAction(data.oYcrb[i], dAdq_cols, dFdq_cols);
157 // dtau/dq
158
2/2
✓ Branch 0 taken 676 times.
✓ Branch 1 taken 26 times.
 1404 if (parent > 0)
159 {
160 if (ContactMode)
161 {
162
3/6
✓ Branch 2 taken 572 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 572 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 572 times.
✗ Branch 9 not taken.
 1144 dFdq_cols.noalias() += data.doYcrb[i] * dVdq_cols;
163
4/8
✓ Branch 2 taken 572 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 572 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 572 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 572 times.
✗ Branch 12 not taken.
 1144 StdY.noalias() = J_cols.transpose() * data.doYcrb[i];
164
3/4
✓ Branch 1 taken 572 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5302 times.
✓ Branch 5 taken 572 times.
 11748 for (int j = data.parents_fromRow[(typename Model::Index)jmodel.idx_v()]; j >= 0;
165 10604 j = data.parents_fromRow[(typename Model::Index)j])
166 {
167
7/14
✓ Branch 1 taken 5302 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5302 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 5302 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 5302 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 5302 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 5302 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 5302 times.
✗ Branch 20 not taken.
 10604 dtau_dq.middleRows(jmodel.idx_v(), jmodel.nv()).col(j).noalias() =
168
5/10
✓ Branch 1 taken 5302 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5302 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 5302 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 5302 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 5302 times.
✗ Branch 14 not taken.
 21208 dFda_cols.transpose() * data.dAdq.col(j) + StdY * data.dVdq.col(j);
169 }
170 }
171 else
172 {
173
3/4
✓ Branch 1 taken 104 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 964 times.
✓ Branch 5 taken 104 times.
 2136 for (int j = data.parents_fromRow[(typename Model::Index)jmodel.idx_v()]; j >= 0;
174 1928 j = data.parents_fromRow[(typename Model::Index)j])
175 {
176
6/12
✓ Branch 1 taken 964 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 964 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 964 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 964 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 964 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 964 times.
✗ Branch 17 not taken.
 1928 dtau_dq.middleRows(jmodel.idx_v(), jmodel.nv()).col(j).noalias() =
177
3/6
✓ Branch 1 taken 964 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 964 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 964 times.
✗ Branch 8 not taken.
 3856 dFda_cols.transpose() * data.dAdq.col(j);
178 }
179 }
180 }
181
182
6/12
✓ Branch 2 taken 702 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 702 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 702 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 702 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 702 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 702 times.
✗ Branch 18 not taken.
 1404 dtau_dq.block(jmodel.idx_v(), jmodel.idx_v(), jmodel.nv(), data.nvSubtree[i]).noalias() =
183
4/8
✓ Branch 2 taken 702 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 702 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 702 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 702 times.
✗ Branch 12 not taken.
 1404 J_cols.transpose() * data.dFdq.middleCols(jmodel.idx_v(), data.nvSubtree[i]);
184
1/2
✓ Branch 2 taken 702 times.
✗ Branch 3 not taken.
 1404 motionSet::act<ADDTO>(J_cols, data.of[i], dFdq_cols);
185
186 if (ContactMode)
187 {
188
1/2
✓ Branch 1 taken 594 times.
✗ Branch 2 not taken.
 1188 ColsBlock dAdv_cols = jmodel.jointCols(data.dAdv);
189
1/2
✓ Branch 1 taken 594 times.
✗ Branch 2 not taken.
 1188 ColsBlock dFdv_cols = jmodel.jointCols(data.dFdv);
190
191 1188 typename Data::RowMatrixXs & dtau_dv = data.dtau_dv;
192
3/6
✓ Branch 2 taken 594 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 594 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 594 times.
✗ Branch 9 not taken.
 1188 dFdv_cols.noalias() = data.doYcrb[i] * J_cols;
193
1/2
✓ Branch 2 taken 594 times.
✗ Branch 3 not taken.
 1188 motionSet::inertiaAction<ADDTO>(data.oYcrb[i], dAdv_cols, dFdv_cols);
194
195
6/12
✓ Branch 2 taken 594 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 594 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 594 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 594 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 594 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 594 times.
✗ Branch 18 not taken.
 1188 dtau_dv.block(jmodel.idx_v(), jmodel.idx_v(), jmodel.nv(), data.nvSubtree[i]).noalias() =
196
4/8
✓ Branch 2 taken 594 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 594 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 594 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 594 times.
✗ Branch 12 not taken.
 1188 J_cols.transpose() * data.dFdv.middleCols(jmodel.idx_v(), data.nvSubtree[i]);
197
2/2
✓ Branch 0 taken 572 times.
✓ Branch 1 taken 22 times.
 1188 if (parent > 0)
198 {
199
3/4
✓ Branch 1 taken 572 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5302 times.
✓ Branch 5 taken 572 times.
 11748 for (int j = data.parents_fromRow[(typename Model::Index)jmodel.idx_v()]; j >= 0;
200 10604 j = data.parents_fromRow[(typename Model::Index)j])
201 {
202
7/14
✓ Branch 1 taken 5302 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5302 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 5302 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 5302 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 5302 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 5302 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 5302 times.
✗ Branch 20 not taken.
 10604 dtau_dv.middleRows(jmodel.idx_v(), jmodel.nv()).col(j).noalias() =
203
5/10
✓ Branch 1 taken 5302 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5302 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 5302 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 5302 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 5302 times.
✗ Branch 14 not taken.
 21208 dFda_cols.transpose() * data.dAdv.col(j) + StdY * data.J.col(j);
204 }
205
1/2
✓ Branch 3 taken 572 times.
✗ Branch 4 not taken.
 1144 data.doYcrb[parent] += data.doYcrb[i];
206 }
207 // Restore the status of dAdq_cols (remove gravity)
208
3/4
✓ Branch 1 taken 1298 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 704 times.
✓ Branch 4 taken 594 times.
 2596 for (Eigen::DenseIndex k = 0; k < jmodel.nv(); ++k)
209 {
210 typedef typename ColsBlock::ColXpr ColType;
211
2/4
✓ Branch 1 taken 704 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 704 times.
✗ Branch 5 not taken.
 1408 MotionRef<ColType> min(J_cols.col(k));
212
2/4
✓ Branch 1 taken 704 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 704 times.
✗ Branch 5 not taken.
 1408 MotionRef<ColType> mout(dAdq_cols.col(k));
213
5/10
✓ Branch 1 taken 704 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 704 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 704 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 704 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 704 times.
✗ Branch 14 not taken.
 1408 mout.linear() += model.gravity.linear().cross(min.angular());
214 }
215 }
216
217
2/2
✓ Branch 0 taken 676 times.
✓ Branch 1 taken 26 times.
 1404 if (parent > 0)
218
1/2
✓ Branch 3 taken 676 times.
✗ Branch 4 not taken.
 1352 data.of[parent] += data.of[i];
219 }
220 };
221
222 namespace internal
223 {
224
225 template<typename Scalar>
226 struct ContactForceContribution
227 {
228
229 template<
230 int Options,
231 template<typename, int> class JointCollectionTpl,
232 class ConstraintModelAllocator,
233 class ConstraintDataAllocator>
234 26 static void run(
235 const std::vector<RigidConstraintModelTpl<Scalar, Options>, ConstraintModelAllocator> &
236 contact_models,
237 DataTpl<Scalar, Options, JointCollectionTpl> & data,
238 std::vector<RigidConstraintDataTpl<Scalar, Options>, ConstraintDataAllocator> &
239 contact_data)
240 {
241 typedef RigidConstraintModelTpl<Scalar, Options> RigidConstraintModel;
242 typedef RigidConstraintDataTpl<Scalar, Options> RigidConstraintData;
243 typedef SE3Tpl<Scalar, Options> SE3;
244 typedef ForceTpl<Scalar, Options> Force;
245
246
1/2
✓ Branch 1 taken 26 times.
✗ Branch 2 not taken.
 26 Force of_tmp;
247
248 // Add the contribution of the external forces.
249
2/2
✓ Branch 1 taken 35 times.
✓ Branch 2 taken 26 times.
 61 for (size_t k = 0; k < contact_models.size(); ++k)
250 {
251 35 const RigidConstraintModel & cmodel = contact_models[k];
252 35 const RigidConstraintData & cdata = contact_data[k];
253
254 // TODO: Temporary variable
255 35 const SE3 & oMc1 = cdata.oMc1;
256 35 Force & of1 = data.of[cmodel.joint1_id];
257 35 const SE3 & oMc2 = cdata.oMc2;
258 35 Force & of2 = data.of[cmodel.joint2_id];
259
260
2/3
✓ Branch 0 taken 27 times.
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 35 switch (cmodel.reference_frame)
261 {
262 27 case LOCAL: {
263
2/3
✓ Branch 0 taken 12 times.
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
 27 switch (cmodel.type)
264 {
265 12 case CONTACT_6D: {
266
2/2
✓ Branch 0 taken 11 times.
✓ Branch 1 taken 1 times.
 12 if (cmodel.joint1_id > 0)
267 {
268
2/4
✓ Branch 1 taken 11 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 11 times.
✗ Branch 5 not taken.
 11 of1 -= oMc1.act(cdata.contact_force);
269 }
270
2/2
✓ Branch 0 taken 3 times.
✓ Branch 1 taken 9 times.
 12 if (cmodel.joint2_id > 0)
271 {
272
2/4
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
 3 of_tmp = oMc1.act(cdata.contact_force);
273
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 of2 += of_tmp;
274 }
275 12 break;
276 }
277 15 case CONTACT_3D: {
278
6/12
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 15 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 15 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 15 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 15 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 15 times.
✗ Branch 17 not taken.
 15 of_tmp.linear().noalias() = oMc1.rotation() * cdata.contact_force.linear();
279
280
2/2
✓ Branch 0 taken 14 times.
✓ Branch 1 taken 1 times.
 15 if (cmodel.joint1_id > 0)
281 {
282
4/8
✓ Branch 1 taken 14 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 14 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 14 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 14 times.
✗ Branch 11 not taken.
 14 of1.linear().noalias() -= of_tmp.linear();
283
6/12
✓ Branch 1 taken 14 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 14 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 14 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 14 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 14 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 14 times.
✗ Branch 17 not taken.
 14 of1.angular().noalias() -= oMc1.translation().cross(of_tmp.linear());
284 }
285
2/2
✓ Branch 0 taken 4 times.
✓ Branch 1 taken 11 times.
 15 if (cmodel.joint2_id > 0)
286 {
287
3/6
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4 times.
✗ Branch 8 not taken.
 4 of2.linear() += of_tmp.linear();
288
6/12
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 4 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 4 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 4 times.
✗ Branch 17 not taken.
 4 of2.angular().noalias() += oMc2.translation().cross(of_tmp.linear());
289 }
290 15 break;
291 }
292 ✗ default: {
293 ✗ assert(false && "must never happen");
294 break;
295 }
296 }
297 27 break;
298 }
299 8 case LOCAL_WORLD_ALIGNED: {
300
2/3
✓ Branch 0 taken 4 times.
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 8 switch (cmodel.type)
301 {
302 4 case CONTACT_6D: {
303
1/2
✓ Branch 0 taken 4 times.
✗ Branch 1 not taken.
 4 if (cmodel.joint1_id > 0)
304 {
305
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 4 of1 -= cdata.contact_force;
306
6/12
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 4 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 4 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 4 times.
✗ Branch 17 not taken.
 4 of1.angular().noalias() -= oMc1.translation().cross(cdata.contact_force.linear());
307 }
308
2/2
✓ Branch 0 taken 1 times.
✓ Branch 1 taken 3 times.
 4 if (cmodel.joint2_id > 0)
309 {
310
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 of2 += cdata.contact_force;
311
6/12
✓ 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 taken 1 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
 1 of2.angular().noalias() += oMc1.translation().cross(cdata.contact_force.linear());
312 }
313 4 break;
314 }
315 4 case CONTACT_3D: {
316
1/2
✓ Branch 0 taken 4 times.
✗ Branch 1 not taken.
 4 if (cmodel.joint1_id > 0)
317 {
318
3/6
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4 times.
✗ Branch 8 not taken.
 4 of1.linear() -= cdata.contact_force.linear();
319
6/12
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 4 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 4 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 4 times.
✗ Branch 17 not taken.
 4 of1.angular().noalias() -= oMc1.translation().cross(cdata.contact_force.linear());
320 }
321
2/2
✓ Branch 0 taken 1 times.
✓ Branch 1 taken 3 times.
 4 if (cmodel.joint2_id > 0)
322 {
323
3/6
✓ 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.
 1 of2.linear() += cdata.contact_force.linear();
324
6/12
✓ 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 taken 1 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
 1 of2.angular().noalias() += oMc2.translation().cross(cdata.contact_force.linear());
325 }
326 4 break;
327 }
328 ✗ default: {
329 ✗ assert(false && "must never happen");
330 break;
331 }
332 }
333 8 break;
334 }
335 ✗ default:
336 ✗ assert(false && "Should never happen");
337 break;
338 }
339 }
340 26 }
341 };
342 } // namespace internal
343
344 template<
345 typename Scalar,
346 int Options,
347 template<typename, int> class JointCollectionTpl,
348 class ConstraintModelAllocator,
349 class ConstraintDataAllocator,
350 typename MatrixType1,
351 typename MatrixType2,
352 typename MatrixType3,
353 typename MatrixType4,
354 typename MatrixType5,
355 typename MatrixType6>
356 44 inline void computeConstraintDynamicsDerivatives(
357 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
358 DataTpl<Scalar, Options, JointCollectionTpl> & data,
359 const std::vector<RigidConstraintModelTpl<Scalar, Options>, ConstraintModelAllocator> &
360 contact_models,
361 std::vector<RigidConstraintDataTpl<Scalar, Options>, ConstraintDataAllocator> & contact_data,
362 const ProximalSettingsTpl<Scalar> & settings,
363 const Eigen::MatrixBase<MatrixType1> & ddq_partial_dq,
364 const Eigen::MatrixBase<MatrixType2> & ddq_partial_dv,
365 const Eigen::MatrixBase<MatrixType3> & ddq_partial_dtau,
366 const Eigen::MatrixBase<MatrixType4> & lambda_partial_dq,
367 const Eigen::MatrixBase<MatrixType5> & lambda_partial_dv,
368 const Eigen::MatrixBase<MatrixType6> & lambda_partial_dtau)
369 {
370
1/2
✓ Branch 1 taken 22 times.
✗ Branch 2 not taken.
 44 const Eigen::DenseIndex & nc = data.contact_chol.constraintDim();
371
372
1/4
✗ Branch 2 not taken.
✓ Branch 3 taken 22 times.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
 44 PINOCCHIO_CHECK_INPUT_ARGUMENT(
373 contact_data.size() == contact_models.size(),
374 "contact_data and contact_models do not have the same size");
375
376
1/4
✗ Branch 1 not taken.
✓ Branch 2 taken 22 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
 44 PINOCCHIO_CHECK_INPUT_ARGUMENT(ddq_partial_dq.cols() == model.nv);
377
1/4
✗ Branch 1 not taken.
✓ Branch 2 taken 22 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
 44 PINOCCHIO_CHECK_INPUT_ARGUMENT(ddq_partial_dq.rows() == model.nv);
378
379
1/4
✗ Branch 1 not taken.
✓ Branch 2 taken 22 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
 44 PINOCCHIO_CHECK_INPUT_ARGUMENT(ddq_partial_dv.cols() == model.nv);
380
1/4
✗ Branch 1 not taken.
✓ Branch 2 taken 22 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
 44 PINOCCHIO_CHECK_INPUT_ARGUMENT(ddq_partial_dv.rows() == model.nv);
381
382
1/4
✗ Branch 1 not taken.
✓ Branch 2 taken 22 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
 44 PINOCCHIO_CHECK_INPUT_ARGUMENT(ddq_partial_dtau.cols() == model.nv);
383
1/4
✗ Branch 1 not taken.
✓ Branch 2 taken 22 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
 44 PINOCCHIO_CHECK_INPUT_ARGUMENT(ddq_partial_dtau.rows() == model.nv);
384
385
1/4
✗ Branch 1 not taken.
✓ Branch 2 taken 22 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
 44 PINOCCHIO_CHECK_INPUT_ARGUMENT(lambda_partial_dq.cols() == model.nv);
386
1/4
✗ Branch 1 not taken.
✓ Branch 2 taken 22 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
 44 PINOCCHIO_CHECK_INPUT_ARGUMENT(lambda_partial_dq.rows() == nc);
387
388
1/4
✗ Branch 1 not taken.
✓ Branch 2 taken 22 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
 44 PINOCCHIO_CHECK_INPUT_ARGUMENT(lambda_partial_dv.cols() == model.nv);
389
1/4
✗ Branch 1 not taken.
✓ Branch 2 taken 22 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
 44 PINOCCHIO_CHECK_INPUT_ARGUMENT(lambda_partial_dv.rows() == nc);
390
391
1/4
✗ Branch 1 not taken.
✓ Branch 2 taken 22 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
 44 PINOCCHIO_CHECK_INPUT_ARGUMENT(lambda_partial_dtau.cols() == model.nv);
392
1/4
✗ Branch 1 not taken.
✓ Branch 2 taken 22 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
 44 PINOCCHIO_CHECK_INPUT_ARGUMENT(lambda_partial_dtau.rows() == nc);
393
394
11/41
✓ Branch 1 taken 22 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 22 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 22 times.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✓ Branch 9 taken 22 times.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✓ Branch 12 taken 22 times.
✗ Branch 13 not taken.
✓ Branch 14 taken 22 times.
✗ Branch 15 not taken.
✗ Branch 16 not taken.
✓ Branch 17 taken 22 times.
✗ Branch 18 not taken.
✗ Branch 19 not taken.
✓ Branch 20 taken 22 times.
✗ Branch 21 not taken.
✓ Branch 22 taken 22 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 22 times.
✗ Branch 26 not taken.
✗ Branch 27 not taken.
✓ Branch 28 taken 22 times.
✗ Branch 29 not taken.
✗ Branch 31 not taken.
✗ Branch 32 not taken.
✗ Branch 34 not taken.
✗ Branch 35 not taken.
✗ Branch 37 not taken.
✗ Branch 38 not taken.
✗ Branch 40 not taken.
✗ Branch 41 not taken.
✗ Branch 43 not taken.
✗ Branch 44 not taken.
✗ Branch 56 not taken.
✗ Branch 57 not taken.
✗ Branch 60 not taken.
✗ Branch 61 not taken.
 44 PINOCCHIO_CHECK_INPUT_ARGUMENT(
395 check_expression_if_real<Scalar>(
396 model.gravity.angular()[0] == Scalar(0) && model.gravity.angular()[1] == Scalar(0)
397 && model.gravity.angular()[2] == Scalar(0)),
398 "The gravity must be a pure force vector, no angular part");
399
400
2/4
✓ Branch 1 taken 22 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 22 times.
✗ Branch 4 not taken.
 44 assert(model.check(data) && "data is not consistent with model.");
401
402 // TODO: User should make sure the internal quantities are reset.
403
1/2
✓ Branch 1 taken 22 times.
✗ Branch 2 not taken.
 44 data.dtau_dq.setZero();
404
1/2
✓ Branch 1 taken 22 times.
✗ Branch 2 not taken.
 44 data.dtau_dv.setZero();
405
1/2
✓ Branch 1 taken 22 times.
✗ Branch 2 not taken.
 44 data.dac_dq.setZero();
406
1/2
✓ Branch 1 taken 22 times.
✗ Branch 2 not taken.
 44 data.dac_dv.setZero();
407
408 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
409 typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
410
411 typedef RigidConstraintModelTpl<Scalar, Options> RigidConstraintModel;
412 typedef RigidConstraintDataTpl<Scalar, Options> RigidConstraintData;
413
414 typedef typename ModelTpl<Scalar, Options, JointCollectionTpl>::JointIndex JointIndex;
415 typedef typename Data::SE3 SE3;
416 typedef typename Data::Motion Motion;
417 typedef typename Data::Force Force;
418
2/4
✓ Branch 1 taken 22 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 22 times.
✗ Branch 6 not taken.
 44 data.oa_gf[0] = -model.gravity;
419
420 // TODO: Temp variable
421
1/2
✓ Branch 1 taken 22 times.
✗ Branch 2 not taken.
 44 Motion a_tmp;
422
423 typedef ComputeConstraintDynamicsDerivativesForwardStep<
424 Scalar, Options, JointCollectionTpl, true>
425 Pass1;
426
2/2
✓ Branch 0 taken 594 times.
✓ Branch 1 taken 22 times.
 1232 for (JointIndex i = 1; i < (JointIndex)model.njoints; ++i)
427 {
428
2/4
✓ Branch 1 taken 594 times.
✗ Branch 2 not taken.
✓ Branch 6 taken 594 times.
✗ Branch 7 not taken.
 1188 Pass1::run(model.joints[i], data.joints[i], typename Pass1::ArgsType(model, data));
429 }
430
431
1/2
✓ Branch 1 taken 22 times.
✗ Branch 2 not taken.
 44 internal::ContactForceContribution<Scalar>::run(contact_models, data, contact_data);
432
433 // Backward Pass
434 typedef ComputeContactDynamicDerivativesBackwardStep<Scalar, Options, JointCollectionTpl, true>
435 Pass2;
436
2/2
✓ Branch 0 taken 594 times.
✓ Branch 1 taken 22 times.
 1232 for (JointIndex i = (JointIndex)(model.njoints - 1); i > 0; --i)
437 {
438
2/4
✓ Branch 1 taken 594 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 594 times.
✗ Branch 6 not taken.
 1188 Pass2::run(model.joints[i], typename Pass2::ArgsType(model, data));
439 }
440
441 // Compute the contact frame partial derivatives
442
1/2
✓ Branch 1 taken 22 times.
✗ Branch 2 not taken.
 44 typename Data::SE3::Matrix6 Jlog;
443 44 Eigen::DenseIndex current_row_sol_id = 0;
444
2/2
✓ Branch 1 taken 29 times.
✓ Branch 2 taken 22 times.
 102 for (size_t k = 0; k < contact_models.size(); ++k)
445 {
446 typedef typename RigidConstraintModel::BooleanVector BooleanVector;
447
448 58 const RigidConstraintModel & cmodel = contact_models[k];
449 58 RigidConstraintData & cdata = contact_data[k];
450 // const BooleanVector & joint1_indexes = cmodel.colwise_joint1_sparsity;
451 58 const BooleanVector & joint2_indexes = cmodel.colwise_joint2_sparsity;
452
453
2/3
✓ Branch 0 taken 13 times.
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
 58 switch (cmodel.type)
454 {
455 26 case CONTACT_6D: {
456 typedef
457 typename SizeDepType<6>::template RowsReturn<typename Data::MatrixXs>::Type RowsBlock;
458
1/2
✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
 26 RowsBlock contact_dvc_dq = SizeDepType<6>::middleRows(data.dvc_dq, current_row_sol_id);
459
1/2
✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
 26 RowsBlock contact_dac_dq = SizeDepType<6>::middleRows(data.dac_dq, current_row_sol_id);
460
1/2
✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
 26 RowsBlock contact_dac_dv = SizeDepType<6>::middleRows(data.dac_dv, current_row_sol_id);
461
1/2
✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
 26 RowsBlock contact_dac_da = SizeDepType<6>::middleRows(data.dac_da, current_row_sol_id);
462
463
2/2
✓ Branch 0 taken 12 times.
✓ Branch 1 taken 1 times.
 26 if (cmodel.joint1_id > 0)
464 {
465
1/2
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
 24 cdata.dv1_dq.setZero();
466
1/2
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
 24 cdata.da1_dq.setZero();
467
1/2
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
 24 cdata.da1_dv.setZero();
468
1/2
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
 24 cdata.da1_da.setZero();
469
470 24 getFrameAccelerationDerivatives(
471 24 model, data, cmodel.joint1_id, cmodel.joint1_placement, cmodel.reference_frame,
472
1/2
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
 24 cdata.dv1_dq, cdata.da1_dq, cdata.da1_dv, cdata.da1_da);
473
474
1/2
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
 24 contact_dvc_dq = cdata.dv1_dq;
475
1/2
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
 24 contact_dac_dq = cdata.da1_dq;
476
1/2
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
 24 contact_dac_dv = cdata.da1_dv;
477
1/2
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
 24 contact_dac_da = cdata.da1_da;
478 }
479
480
2/2
✓ Branch 0 taken 4 times.
✓ Branch 1 taken 9 times.
 26 if (cmodel.joint2_id > 0)
481 {
482
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 8 cdata.dv2_dq.setZero();
483
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 8 cdata.da2_dq.setZero();
484
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 8 cdata.da2_dv.setZero();
485
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 8 cdata.da2_da.setZero();
486
2/4
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
 8 const SE3 joint2_M_c1 = cmodel.joint2_placement * cdata.c1Mc2.inverse();
487
488 8 getFrameAccelerationDerivatives(
489 8 model, data, cmodel.joint2_id, joint2_M_c1,
490 // cmodel.joint2_placement,
491
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 8 cmodel.reference_frame, cdata.dv2_dq, cdata.da2_dq, cdata.da2_dv, cdata.da2_da);
492
493 // TODO: This is only in case reference_frame is LOCAL
494 // TODO(jcarpent):to do colwise
495
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 8 contact_dvc_dq -= cdata.dv2_dq;
496
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 8 contact_dac_dq -= cdata.da2_dq;
497
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 8 contact_dac_dv -= cdata.da2_dv;
498
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 8 contact_dac_da -= cdata.da2_da;
499
500
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 8 const Motion v2_in_c1 = cdata.c1Mc2.act(cdata.contact2_velocity);
501
1/2
✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
 8 const Motion a2_in_c1 = cdata.oMc1.actInv(data.oa[cmodel.joint2_id]);
502
503 8 Eigen::DenseIndex k = Eigen::DenseIndex(cmodel.colwise_span_indexes.size()) - 1;
504 8 Eigen::DenseIndex col_id(0);
505
5/6
✓ Branch 0 taken 54 times.
✓ Branch 1 taken 1 times.
✓ Branch 3 taken 54 times.
✗ Branch 4 not taken.
✓ Branch 5 taken 54 times.
✓ Branch 6 taken 1 times.
 110 while (cmodel.reference_frame == LOCAL && cmodel.colwise_span_indexes.size() > 0)
506 {
507
2/2
✓ Branch 0 taken 35 times.
✓ Branch 1 taken 19 times.
 108 if (k >= 0)
508 {
509 70 col_id = cmodel.colwise_span_indexes[size_t(k)];
510 70 k--;
511 }
512 else
513 {
514 38 col_id = data.parents_fromRow[size_t(col_id)];
515
2/2
✓ Branch 0 taken 3 times.
✓ Branch 1 taken 16 times.
 38 if (col_id < 0)
516 6 break;
517 }
518
519
2/4
✓ Branch 1 taken 51 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 51 times.
✗ Branch 5 not taken.
 102 const MotionRef<typename RowsBlock::ColXpr> dvc_dv_col(contact_dac_da.col(col_id));
520
1/2
✓ Branch 1 taken 51 times.
✗ Branch 2 not taken.
 102 const MotionRef<typename RigidConstraintData::Matrix6x::ColXpr> da2_da_col(
521
1/2
✓ Branch 1 taken 51 times.
✗ Branch 2 not taken.
 102 cdata.da2_da.col(col_id));
522
1/2
✓ Branch 1 taken 51 times.
✗ Branch 2 not taken.
 102 const MotionRef<typename RigidConstraintData::Matrix6x::ColXpr> dv2_dq_col(
523
1/2
✓ Branch 1 taken 51 times.
✗ Branch 2 not taken.
 102 cdata.dv2_dq.col(col_id));
524
525 // dv/dq
526
1/2
✓ Branch 1 taken 51 times.
✗ Branch 2 not taken.
 102 const Motion v2_in_c1_cross_dvc_dv_col = v2_in_c1.cross(dvc_dv_col);
527
3/6
✓ Branch 1 taken 51 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 51 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 51 times.
✗ Branch 8 not taken.
 102 contact_dvc_dq.col(col_id) -= v2_in_c1_cross_dvc_dv_col.toVector();
528
529 // da/dv
530
3/6
✓ Branch 1 taken 51 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 51 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 51 times.
✗ Branch 8 not taken.
 102 contact_dac_dv.col(col_id) -= v2_in_c1_cross_dvc_dv_col.toVector();
531
4/8
✓ Branch 1 taken 51 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 51 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 51 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 51 times.
✗ Branch 11 not taken.
 102 contact_dac_dv.col(col_id) += cdata.contact_velocity_error.cross(da2_da_col).toVector();
532
533 // da/dq
534
2/4
✓ Branch 1 taken 51 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 51 times.
✗ Branch 5 not taken.
 102 const MotionRef<typename RowsBlock::ColXpr> dvc_dq_col(contact_dvc_dq.col(col_id));
535
536
4/8
✓ Branch 1 taken 51 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 51 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 51 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 51 times.
✗ Branch 11 not taken.
 102 contact_dac_dq.col(col_id) -= a2_in_c1.cross(dvc_dv_col).toVector();
537
4/8
✓ Branch 1 taken 51 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 51 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 51 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 51 times.
✗ Branch 11 not taken.
 102 contact_dac_dq.col(col_id) -= v2_in_c1.cross(dvc_dq_col).toVector();
538
1/2
✓ Branch 1 taken 51 times.
✗ Branch 2 not taken.
 102 contact_dac_dq.col(col_id) +=
539
4/8
✓ Branch 1 taken 51 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 51 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 51 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 51 times.
✗ Branch 11 not taken.
 204 cdata.contact_velocity_error.cross(v2_in_c1_cross_dvc_dv_col + dv2_dq_col).toVector();
540 }
541
542
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 8 cdata.dvc_dq = contact_dvc_dq;
543
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 8 cdata.dac_dq = contact_dac_dq;
544
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 8 cdata.dac_dv = contact_dac_dv;
545
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 8 cdata.dac_da = contact_dac_da;
546 }
547
548 26 break;
549 }
550 32 case CONTACT_3D: {
551 typedef
552 typename SizeDepType<3>::template RowsReturn<typename Data::MatrixXs>::Type RowsBlock;
553
554
1/2
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
 32 RowsBlock contact_dvc_dq = SizeDepType<3>::middleRows(data.dvc_dq, current_row_sol_id);
555
1/2
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
 32 RowsBlock contact_dac_dq = SizeDepType<3>::middleRows(data.dac_dq, current_row_sol_id);
556
1/2
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
 32 RowsBlock contact_dac_dv = SizeDepType<3>::middleRows(data.dac_dv, current_row_sol_id);
557
1/2
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
 32 RowsBlock contact_dac_da = SizeDepType<3>::middleRows(data.dac_da, current_row_sol_id);
558
559
2/2
✓ Branch 0 taken 15 times.
✓ Branch 1 taken 1 times.
 32 if (cmodel.joint1_id > 0)
560 {
561
1/2
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
 30 cdata.dv1_dq.setZero();
562
1/2
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
 30 cdata.da1_dq.setZero();
563
1/2
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
 30 cdata.da1_dv.setZero();
564
1/2
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
 30 cdata.da1_da.setZero();
565
566 30 getPointClassicAccelerationDerivatives(
567
1/2
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
 30 model, data, cmodel.joint1_id, cmodel.joint1_placement, cmodel.reference_frame,
568
2/4
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 15 times.
✗ Branch 5 not taken.
 30 cdata.dv1_dq.template bottomRows<3>(), cdata.da1_dq.template bottomRows<3>(),
569
2/4
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 15 times.
✗ Branch 5 not taken.
 30 cdata.da1_dv.template bottomRows<3>(), cdata.da1_da.template bottomRows<3>());
570
571
2/4
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 15 times.
✗ Branch 5 not taken.
 30 contact_dvc_dq = cdata.dv1_dq.template bottomRows<3>();
572
2/4
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 15 times.
✗ Branch 5 not taken.
 30 contact_dac_dq = cdata.da1_dq.template bottomRows<3>();
573
2/4
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 15 times.
✗ Branch 5 not taken.
 30 contact_dac_dv = cdata.da1_dv.template bottomRows<3>();
574
2/4
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 15 times.
✗ Branch 5 not taken.
 30 contact_dac_da = cdata.da1_da.template bottomRows<3>();
575 }
576
577
2/2
✓ Branch 0 taken 5 times.
✓ Branch 1 taken 11 times.
 32 if (cmodel.joint2_id > 0)
578 {
579
1/2
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
 10 cdata.dv2_dq.setZero();
580
1/2
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
 10 cdata.da2_dq.setZero();
581
1/2
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
 10 cdata.da2_dv.setZero();
582
1/2
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
 10 cdata.da2_da.setZero();
583
1/2
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
 10 const SE3 joint2_M_c1(
584
3/6
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 5 times.
✗ Branch 8 not taken.
 10 cmodel.joint2_placement.rotation() * cdata.c1Mc2.rotation().transpose(),
585
2/4
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
 10 cmodel.joint2_placement.translation());
586
587 10 getPointClassicAccelerationDerivatives(
588
1/2
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
 10 model, data, cmodel.joint2_id, joint2_M_c1, cmodel.reference_frame,
589
2/4
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
 10 cdata.dv2_dq.template bottomRows<3>(), cdata.da2_dq.template bottomRows<3>(),
590
2/4
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
 10 cdata.da2_dv.template bottomRows<3>(), cdata.da2_da.template bottomRows<3>());
591
592 // TODO: This is only in case reference_frame is LOCAL
593
2/4
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
 10 contact_dvc_dq -= cdata.dv2_dq.template bottomRows<3>();
594
2/4
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
 10 contact_dac_dq -= cdata.da2_dq.template bottomRows<3>();
595
2/4
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
 10 contact_dac_dv -= cdata.da2_dv.template bottomRows<3>();
596
2/4
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
 10 contact_dac_da -= cdata.da2_da.template bottomRows<3>();
597 }
598 32 break;
599 }
600 ✗ default:
601 ✗ assert(false && "must never happen");
602 break;
603 }
604
605
1/2
✓ Branch 1 taken 29 times.
✗ Branch 2 not taken.
 58 assert(
606 cmodel.loop_span_indexes.size() > 0
607 && "Must never happened, the sparsity pattern is empty");
608 // Derivative of closed loop kinematic tree
609
2/2
✓ Branch 0 taken 9 times.
✓ Branch 1 taken 20 times.
 58 if (cmodel.joint2_id > 0)
610 {
611
2/3
✓ Branch 0 taken 4 times.
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
 18 switch (cmodel.type)
612 {
613 8 case CONTACT_6D: {
614 // TODO: THIS IS FOR THE LOCAL FRAME ONLY
615 8 const typename Model::JointIndex joint2_id = cmodel.joint2_id;
616 8 const Eigen::DenseIndex colRef2 =
617
2/4
✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 4 times.
✗ Branch 7 not taken.
 8 nv(model.joints[joint2_id]) + idx_v(model.joints[joint2_id]) - 1;
618
619
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 8 Force contact_force_in_WORLD;
620
2/3
✓ Branch 0 taken 3 times.
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 8 switch (cmodel.reference_frame)
621 {
622 6 case LOCAL: {
623
2/4
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
 6 contact_force_in_WORLD = cdata.oMc1.act(cdata.contact_force);
624 6 break;
625 }
626 2 case LOCAL_WORLD_ALIGNED: {
627
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 contact_force_in_WORLD = cdata.contact_force;
628
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 contact_force_in_WORLD.angular().noalias() +=
629
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 cdata.oMc1.translation().cross(cdata.contact_force.linear());
630 2 break;
631 }
632 ✗ default: {
633 ✗ assert(false && "must never happen");
634 break;
635 }
636 }
637
638 // d./dq
639
2/3
✓ Branch 1 taken 45 times.
✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
 98 for (Eigen::DenseIndex k = 0; k < Eigen::DenseIndex(cmodel.colwise_span_indexes.size());
640 ++k)
641 {
642 90 const Eigen::DenseIndex col_id = cmodel.colwise_span_indexes[size_t(k)];
643
644
2/4
✓ Branch 1 taken 45 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 45 times.
✗ Branch 5 not taken.
 90 const MotionRef<typename Data::Matrix6x::ColXpr> J_col(data.J.col(col_id));
645
1/2
✓ Branch 1 taken 45 times.
✗ Branch 2 not taken.
 90 const Force J_col_cross_contact_force_in_WORLD = J_col.cross(contact_force_in_WORLD);
646
2/2
✓ Branch 1 taken 597 times.
✓ Branch 2 taken 45 times.
 1284 for (Eigen::DenseIndex j = colRef2; j >= 0; j = data.parents_fromRow[(size_t)j])
647 {
648
3/4
✓ Branch 1 taken 597 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 383 times.
✓ Branch 4 taken 214 times.
 1194 if (joint2_indexes[col_id])
649 {
650
0/4
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
 1532 data.dtau_dq(j, col_id) -=
651
4/8
✓ Branch 1 taken 383 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 383 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 383 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 383 times.
✗ Branch 11 not taken.
 766 data.J.col(j).dot(J_col_cross_contact_force_in_WORLD.toVector());
652 }
653 else
654 {
655
0/4
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
 856 data.dtau_dq(j, col_id) +=
656
4/8
✓ Branch 1 taken 214 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 214 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 214 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 214 times.
✗ Branch 11 not taken.
 428 data.J.col(j).dot(J_col_cross_contact_force_in_WORLD.toVector());
657 }
658 }
659 }
660 8 break;
661 }
662 10 case CONTACT_3D: {
663
664 typedef
665 typename SizeDepType<3>::template RowsReturn<typename Data::MatrixXs>::Type RowsBlock;
666
1/2
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
 10 RowsBlock contact_dac_dq = SizeDepType<3>::middleRows(data.dac_dq, current_row_sol_id);
667 10 const typename Model::JointIndex joint2_id = cmodel.joint2_id;
668 10 const Eigen::DenseIndex colRef2 =
669
2/4
✓ Branch 2 taken 5 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 5 times.
✗ Branch 7 not taken.
 10 nv(model.joints[joint2_id]) + idx_v(model.joints[joint2_id]) - 1;
670
671
2/4
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
 10 Force of_tmp, of_tmp2; // temporary Force variables
672
673
2/3
✓ Branch 0 taken 4 times.
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 10 switch (cmodel.reference_frame)
674 {
675 8 case LOCAL: {
676
6/12
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 4 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 4 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 4 times.
✗ Branch 17 not taken.
 8 of_tmp.linear().noalias() = cdata.oMc1.rotation() * cdata.contact_force.linear();
677 16 const Motion & c2_acc_c2 = getFrameClassicalAcceleration(
678
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 8 model, data, cmodel.joint2_id, cmodel.joint2_placement, cmodel.reference_frame);
679
6/12
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 4 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 4 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 4 times.
✗ Branch 17 not taken.
 8 a_tmp.angular().noalias() = cdata.oMc2.rotation() * c2_acc_c2.linear();
680 8 break;
681 }
682 2 case LOCAL_WORLD_ALIGNED: {
683
3/6
✓ 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.
 2 of_tmp.linear() = cdata.contact_force.linear();
684 2 break;
685 }
686 ✗ default: {
687 ✗ assert(false && "must never happen");
688 break;
689 }
690 }
691
692 // d./dq
693
2/2
✓ Branch 1 taken 217 times.
✓ Branch 2 taken 5 times.
 444 for (Eigen::DenseIndex k = 0; k < Eigen::DenseIndex(cmodel.loop_span_indexes.size()); ++k)
694 {
695 434 const Eigen::DenseIndex col_id = cmodel.loop_span_indexes[size_t(k)];
696
697
2/4
✓ Branch 1 taken 217 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 217 times.
✗ Branch 5 not taken.
 434 const MotionRef<typename Data::Matrix6x::ColXpr> J_col(data.J.col(col_id));
698
699
2/3
✓ Branch 0 taken 175 times.
✓ Branch 1 taken 42 times.
✗ Branch 2 not taken.
 434 switch (cmodel.reference_frame)
700 {
701 350 case LOCAL: {
702
6/12
✓ Branch 1 taken 175 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 175 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 175 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 175 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 175 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 175 times.
✗ Branch 17 not taken.
 350 a_tmp.linear().noalias() = a_tmp.angular().cross(J_col.angular());
703
3/4
✓ Branch 1 taken 175 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 158 times.
✓ Branch 4 taken 17 times.
 350 if (joint2_indexes[col_id])
704 {
705
3/6
✓ Branch 1 taken 158 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 158 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 158 times.
✗ Branch 8 not taken.
 316 contact_dac_dq.col(col_id).noalias() +=
706
4/8
✓ Branch 1 taken 158 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 158 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 158 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 158 times.
✗ Branch 11 not taken.
 632 cdata.oMc1.rotation().transpose() * a_tmp.linear();
707 }
708 else
709 {
710
3/6
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 17 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 17 times.
✗ Branch 8 not taken.
 34 contact_dac_dq.col(col_id).noalias() -=
711
4/8
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 17 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 17 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 17 times.
✗ Branch 11 not taken.
 68 cdata.oMc1.rotation().transpose() * a_tmp.linear();
712 }
713 350 break;
714 }
715 84 case LOCAL_WORLD_ALIGNED: {
716 // Do nothing
717 84 break;
718 }
719 ✗ default: {
720 ✗ assert(false && "must never happen");
721 break;
722 }
723 }
724
725
6/12
✓ Branch 1 taken 217 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 217 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 217 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 217 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 217 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 217 times.
✗ Branch 17 not taken.
 434 of_tmp2.linear().noalias() = of_tmp.linear().cross(J_col.angular());
726
2/2
✓ Branch 1 taken 2733 times.
✓ Branch 2 taken 217 times.
 5900 for (Eigen::DenseIndex j = colRef2; j >= 0; j = data.parents_fromRow[(size_t)j])
727 {
728
2/4
✓ Branch 1 taken 2733 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2733 times.
✗ Branch 5 not taken.
 5466 const MotionRef<typename Data::Matrix6x::ColXpr> J2_col(data.J.col(j));
729
730 // Temporary assignment
731
5/10
✓ Branch 1 taken 2733 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2733 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2733 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 2733 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 2733 times.
✗ Branch 14 not taken.
 10932 of_tmp2.angular().noalias() =
732
3/6
✓ Branch 1 taken 2733 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2733 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2733 times.
✗ Branch 8 not taken.
 10932 J2_col.linear() - cdata.oMc2.translation().cross(J2_col.angular());
733
3/4
✓ Branch 1 taken 2733 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 2459 times.
✓ Branch 4 taken 274 times.
 5466 if (joint2_indexes[col_id])
734 {
735
4/10
✓ Branch 1 taken 2459 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2459 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2459 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 2459 times.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
 4918 data.dtau_dq(j, col_id) += of_tmp2.angular().dot(of_tmp2.linear());
736 }
737 else
738 {
739
4/10
✓ Branch 1 taken 274 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 274 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 274 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 274 times.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
 548 data.dtau_dq(j, col_id) -= of_tmp2.angular().dot(of_tmp2.linear());
740 }
741 }
742 }
743 10 break;
744 }
745 ✗ default: {
746 ✗ assert(false && "must never happen");
747 break;
748 }
749 }
750 }
751
752 // Add the contribution of the corrector
753 58 if (
754
2/8
✓ Branch 1 taken 29 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 29 times.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
✗ Branch 10 not taken.
 58 check_expression_if_real<Scalar>(!isZero(cmodel.corrector.Kp, static_cast<Scalar>(0.)))
755
8/21
✓ Branch 0 taken 6 times.
✓ Branch 1 taken 23 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 6 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 6 times.
✗ Branch 7 not taken.
✓ Branch 8 taken 2 times.
✓ Branch 9 taken 4 times.
✓ Branch 10 taken 25 times.
✓ Branch 11 taken 4 times.
✗ Branch 12 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 15 not taken.
✗ Branch 16 not taken.
✗ Branch 17 not taken.
✗ Branch 19 not taken.
✗ Branch 20 not taken.
✗ Branch 21 not taken.
✗ Branch 22 not taken.
 58 || check_expression_if_real<Scalar>(!isZero(cmodel.corrector.Kd, static_cast<Scalar>(0.))))
756 {
757
2/4
✓ Branch 1 taken 25 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 25 times.
✗ Branch 5 not taken.
 50 Jlog6(cdata.c1Mc2.inverse(), Jlog);
758
759
2/3
✓ Branch 0 taken 10 times.
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
 50 switch (cmodel.type)
760 {
761 20 case CONTACT_6D: {
762 typedef
763 typename SizeDepType<6>::template RowsReturn<typename Data::MatrixXs>::Type RowsBlock;
764 const RowsBlock contact_dvc_dq =
765
1/2
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
 20 SizeDepType<6>::middleRows(data.dvc_dq, current_row_sol_id);
766
1/2
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
 20 RowsBlock contact_dac_dq = SizeDepType<6>::middleRows(data.dac_dq, current_row_sol_id);
767
1/2
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
 20 RowsBlock contact_dac_dv = SizeDepType<6>::middleRows(data.dac_dv, current_row_sol_id);
768 const RowsBlock contact_dac_da =
769
1/2
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
 20 SizeDepType<6>::middleRows(data.dac_da, current_row_sol_id);
770
3/6
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 10 times.
✗ Branch 8 not taken.
 20 contact_dac_dq += cmodel.corrector.Kd.asDiagonal() * contact_dvc_dq;
771
3/6
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 10 times.
✗ Branch 8 not taken.
 20 contact_dac_dv += cmodel.corrector.Kd.asDiagonal() * contact_dac_da;
772 // d./dq
773
2/2
✓ Branch 1 taken 119 times.
✓ Branch 2 taken 10 times.
 258 for (Eigen::DenseIndex k = 0; k < Eigen::DenseIndex(cmodel.colwise_span_indexes.size());
774 ++k)
775 {
776 238 const Eigen::DenseIndex row_id = cmodel.colwise_span_indexes[size_t(k)];
777 // contact_dac_dq.col(row_id) += cmodel.corrector.Kd * contact_dvc_dq.col(row_id);
778
3/6
✓ Branch 1 taken 119 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 119 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 119 times.
✗ Branch 8 not taken.
 238 contact_dac_dq.col(row_id).noalias() +=
779
4/8
✓ Branch 1 taken 119 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 119 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 119 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 119 times.
✗ Branch 11 not taken.
 476 cmodel.corrector.Kp.asDiagonal() * Jlog * contact_dac_da.col(row_id);
780 }
781 20 break;
782 }
783 30 case CONTACT_3D: {
784 typedef
785 typename SizeDepType<3>::template RowsReturn<typename Data::MatrixXs>::Type RowsBlock;
786 const RowsBlock contact_dvc_dq =
787
1/2
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
 30 SizeDepType<3>::middleRows(data.dvc_dq, current_row_sol_id);
788
1/2
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
 30 RowsBlock contact_dac_dq = SizeDepType<3>::middleRows(data.dac_dq, current_row_sol_id);
789
1/2
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
 30 RowsBlock contact_dac_dv = SizeDepType<3>::middleRows(data.dac_dv, current_row_sol_id);
790 const RowsBlock contact_dac_da =
791
1/2
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
 30 SizeDepType<3>::middleRows(data.dac_da, current_row_sol_id);
792
2/2
✓ Branch 0 taken 12 times.
✓ Branch 1 taken 3 times.
 30 if (cmodel.reference_frame == LOCAL)
793 {
794
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.
 48 a_tmp.linear() = cmodel.corrector.Kd.asDiagonal() * cdata.oMc2.rotation()
795
2/4
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12 times.
✗ Branch 5 not taken.
 48 * cdata.contact2_velocity.linear();
796
2/4
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12 times.
✗ Branch 5 not taken.
 24 typename SE3::Matrix3 vc2_cross_in_c1, vc2_cross_in_world;
797
2/4
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12 times.
✗ Branch 5 not taken.
 24 skew(a_tmp.linear(), vc2_cross_in_world);
798
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 vc2_cross_in_c1.noalias() = cdata.oMc1.rotation().transpose() * vc2_cross_in_world;
799
2/2
✓ Branch 1 taken 529 times.
✓ Branch 2 taken 12 times.
 1082 for (Eigen::DenseIndex k = 0; k < Eigen::DenseIndex(cmodel.loop_span_indexes.size());
800 ++k)
801 {
802 1058 const Eigen::DenseIndex row_id = cmodel.loop_span_indexes[size_t(k)];
803
2/4
✓ Branch 1 taken 529 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 529 times.
✗ Branch 5 not taken.
 1058 const MotionRef<typename Data::Matrix6x::ColXpr> J_col(data.J.col(row_id));
804
3/4
✓ Branch 1 taken 529 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 158 times.
✓ Branch 4 taken 371 times.
 1058 if (joint2_indexes[row_id])
805 {
806
5/10
✓ Branch 1 taken 158 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 158 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 158 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 158 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 158 times.
✗ Branch 14 not taken.
 316 contact_dac_dq.col(row_id).noalias() += vc2_cross_in_c1 * J_col.angular();
807 }
808 else
809 {
810
5/10
✓ Branch 1 taken 371 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 371 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 371 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 371 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 371 times.
✗ Branch 14 not taken.
 742 contact_dac_dq.col(row_id).noalias() -= vc2_cross_in_c1 * J_col.angular();
811 }
812 }
813 24 const int colRef =
814
2/4
✓ Branch 2 taken 12 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 12 times.
✗ Branch 7 not taken.
 24 nv(model.joints[cmodel.joint1_id]) + idx_v(model.joints[cmodel.joint1_id]) - 1;
815
2/2
✓ Branch 0 taken 135 times.
✓ Branch 1 taken 12 times.
 294 for (Eigen::DenseIndex j = colRef; j >= 0; j = data.parents_fromRow[(size_t)j])
816 {
817 typedef typename Data::Matrix6x::ColXpr ColType;
818
2/4
✓ Branch 1 taken 135 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 135 times.
✗ Branch 5 not taken.
 270 const MotionRef<ColType> J_col(data.J.col(j));
819
6/12
✓ Branch 1 taken 135 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 135 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 135 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 135 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 135 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 135 times.
✗ Branch 17 not taken.
 270 a_tmp.angular() = cdata.oMc1.rotation().transpose() * J_col.angular();
820
3/6
✓ Branch 1 taken 135 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 135 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 135 times.
✗ Branch 8 not taken.
 270 contact_dac_dq.col(j).noalias() +=
821
1/2
✓ Branch 1 taken 135 times.
✗ Branch 2 not taken.
 270 cmodel.corrector.Kp.asDiagonal()
822
4/8
✓ Branch 1 taken 135 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 135 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 135 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 135 times.
✗ Branch 11 not taken.
 810 * cdata.contact_placement_error.linear().cross(a_tmp.angular());
823 }
824 }
825
4/8
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 15 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 15 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 15 times.
✗ Branch 11 not taken.
 30 contact_dac_dq.noalias() += cmodel.corrector.Kd.asDiagonal() * contact_dvc_dq;
826
4/8
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 15 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 15 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 15 times.
✗ Branch 11 not taken.
 30 contact_dac_dq.noalias() += cmodel.corrector.Kp.asDiagonal() * contact_dac_da;
827
4/8
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 15 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 15 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 15 times.
✗ Branch 11 not taken.
 30 contact_dac_dv.noalias() += cmodel.corrector.Kd.asDiagonal() * contact_dac_da;
828 30 break;
829 }
830 ✗ default:
831 ✗ assert(false && "must never happen");
832 break;
833 }
834 }
835
836 58 current_row_sol_id += cmodel.size();
837 }
838
839
1/2
✓ Branch 1 taken 22 times.
✗ Branch 2 not taken.
 44 data.contact_chol.getOperationalSpaceInertiaMatrix(data.osim);
840
1/2
✓ Branch 1 taken 22 times.
✗ Branch 2 not taken.
 44 data.contact_chol.getInverseMassMatrix(data.Minv);
841
842 // Temporary: dlambda_dv stores J*Minv
843 44 typename Data::MatrixXs & JMinv = data.dlambda_dv;
844
845
3/6
✓ Branch 1 taken 22 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 22 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 22 times.
✗ Branch 8 not taken.
 44 JMinv.noalias() = data.dac_da * data.Minv;
846 44 MatrixType3 & ddq_partial_dtau_ = PINOCCHIO_EIGEN_CONST_CAST(MatrixType3, ddq_partial_dtau);
847 MatrixType6 & lambda_partial_dtau_ =
848 44 PINOCCHIO_EIGEN_CONST_CAST(MatrixType6, lambda_partial_dtau);
849 44 typename Data::MatrixXs & dlambda_dx_prox = data.dlambda_dx_prox;
850 44 typename Data::MatrixXs & drhs_prox = data.drhs_prox;
851 {
852
4/8
✓ Branch 1 taken 22 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 22 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 22 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 22 times.
✗ Branch 11 not taken.
 44 lambda_partial_dtau_.noalias() = -data.osim * JMinv; // OUTPUT
853
2/2
✓ Branch 0 taken 20 times.
✓ Branch 1 taken 22 times.
 84 for (int it = 1; it < settings.iter; ++it)
854 {
855
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 40 lambda_partial_dtau_.swap(dlambda_dx_prox);
856
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 40 dlambda_dx_prox *= settings.mu;
857
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 40 dlambda_dx_prox -= JMinv;
858
3/6
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 20 times.
✗ Branch 8 not taken.
 40 lambda_partial_dtau_.noalias() = data.osim * dlambda_dx_prox;
859 }
860
861
3/4
✓ Branch 0 taken 4 times.
✓ Branch 1 taken 18 times.
✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
 44 if (settings.iter % 2 == 0 && settings.iter > 0)
862 {
863
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 8 lambda_partial_dtau_.swap(dlambda_dx_prox); // restore previous memory address
864
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 8 lambda_partial_dtau_ = dlambda_dx_prox;
865 }
866 }
867
868
4/8
✓ Branch 1 taken 22 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 22 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 22 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 22 times.
✗ Branch 11 not taken.
 44 ddq_partial_dtau_.noalias() = JMinv.transpose() * lambda_partial_dtau;
869
1/2
✓ Branch 1 taken 22 times.
✗ Branch 2 not taken.
 44 ddq_partial_dtau_ += data.Minv; // OUTPUT
870
871 44 MatrixType4 & lambda_partial_dq_ = PINOCCHIO_EIGEN_CONST_CAST(MatrixType4, lambda_partial_dq);
872
4/8
✓ Branch 1 taken 22 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 22 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 22 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 22 times.
✗ Branch 11 not taken.
 44 drhs_prox.noalias() = -JMinv * data.dtau_dq;
873
1/2
✓ Branch 1 taken 22 times.
✗ Branch 2 not taken.
 44 drhs_prox += data.dac_dq;
874 {
875
4/8
✓ Branch 1 taken 22 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 22 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 22 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 22 times.
✗ Branch 11 not taken.
 44 lambda_partial_dq_.noalias() = -data.osim * data.drhs_prox; // OUTPUT
876
2/2
✓ Branch 0 taken 20 times.
✓ Branch 1 taken 22 times.
 84 for (int it = 1; it < settings.iter; ++it)
877 {
878
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 40 lambda_partial_dq_.swap(dlambda_dx_prox);
879
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 40 dlambda_dx_prox *= settings.mu;
880
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 40 dlambda_dx_prox -= drhs_prox;
881
3/6
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 20 times.
✗ Branch 8 not taken.
 40 lambda_partial_dq_.noalias() = data.osim * dlambda_dx_prox;
882 }
883
884
3/4
✓ Branch 0 taken 4 times.
✓ Branch 1 taken 18 times.
✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
 44 if (settings.iter % 2 == 0 && settings.iter > 0)
885 {
886
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 8 lambda_partial_dq_.swap(dlambda_dx_prox); // restore previous memory address
887
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 8 lambda_partial_dq_ = dlambda_dx_prox;
888 }
889 }
890
891 44 MatrixType5 & lambda_partial_dv_ = PINOCCHIO_EIGEN_CONST_CAST(MatrixType5, lambda_partial_dv);
892
4/8
✓ Branch 1 taken 22 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 22 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 22 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 22 times.
✗ Branch 11 not taken.
 44 drhs_prox.noalias() = -JMinv * data.dtau_dv;
893
1/2
✓ Branch 1 taken 22 times.
✗ Branch 2 not taken.
 44 drhs_prox += data.dac_dv;
894 {
895
4/8
✓ Branch 1 taken 22 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 22 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 22 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 22 times.
✗ Branch 11 not taken.
 44 lambda_partial_dv_.noalias() = -data.osim * data.drhs_prox; // OUTPUT
896
2/2
✓ Branch 0 taken 20 times.
✓ Branch 1 taken 22 times.
 84 for (int it = 1; it < settings.iter; ++it)
897 {
898
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 40 lambda_partial_dv_.swap(dlambda_dx_prox);
899
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 40 dlambda_dx_prox *= settings.mu;
900
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 40 dlambda_dx_prox -= drhs_prox;
901
3/6
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 20 times.
✗ Branch 8 not taken.
 40 lambda_partial_dv_.noalias() = data.osim * dlambda_dx_prox;
902 }
903
904
3/4
✓ Branch 0 taken 4 times.
✓ Branch 1 taken 18 times.
✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
 44 if (settings.iter % 2 == 0 && settings.iter > 0)
905 {
906
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 8 lambda_partial_dv_.swap(dlambda_dx_prox); // restore previous memory address
907
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 8 lambda_partial_dv_ = dlambda_dx_prox;
908 }
909 }
910
911 44 current_row_sol_id = 0;
912
2/2
✓ Branch 1 taken 29 times.
✓ Branch 2 taken 22 times.
 102 for (size_t k = 0; k < contact_models.size(); ++k)
913 {
914 58 const RigidConstraintModel & cmodel = contact_models[k];
915
916
2/3
✓ Branch 0 taken 13 times.
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
 58 switch (cmodel.type)
917 {
918 26 case CONTACT_6D: {
919
920 typedef
921 typename SizeDepType<6>::template RowsReturn<typename Data::MatrixXs>::Type RowsBlock;
922 typedef typename SizeDepType<6>::template RowsReturn<typename Data::MatrixXs>::ConstType
923 ConstRowsBlock;
924
925 // TODO: replace with contact_model::nc
926
1/2
✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
 26 RowsBlock contact_dac_da = SizeDepType<6>::middleRows(data.dac_da, current_row_sol_id);
927
928 ConstRowsBlock contact_dlambda_dq =
929
1/2
✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
 26 SizeDepType<6>::middleRows(lambda_partial_dq, current_row_sol_id);
930 ConstRowsBlock contact_dlambda_dv =
931
1/2
✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
 26 SizeDepType<6>::middleRows(lambda_partial_dv, current_row_sol_id);
932
933 // TODO: Sparsity in dac_da with loop joints?
934
935
4/8
✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 13 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 13 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 13 times.
✗ Branch 11 not taken.
 26 data.dtau_dq.noalias() -= contact_dac_da.transpose() * contact_dlambda_dq;
936
4/8
✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 13 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 13 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 13 times.
✗ Branch 11 not taken.
 26 data.dtau_dv.noalias() -= contact_dac_da.transpose() * contact_dlambda_dv;
937
938 // END TODO
939
940 /*
941
942 for(Eigen::DenseIndex j=colRef;j>=0;j=data.parents_fromRow[(size_t)j])
943 {
944 data.dtau_dq.row(j).noalias() -= contact_dac_da.col(j).transpose() * contact_dlambda_dq;
945 data.dtau_dv.row(j).noalias() -= contact_dac_da.col(j).transpose() * contact_dlambda_dv;
946 }
947 */
948 26 break;
949 }
950 32 case CONTACT_3D: {
951
952 typedef
953 typename SizeDepType<3>::template RowsReturn<typename Data::MatrixXs>::Type RowsBlock;
954 typedef typename SizeDepType<3>::template RowsReturn<typename Data::MatrixXs>::ConstType
955 ConstRowsBlock;
956
957
1/2
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
 32 RowsBlock contact_dac_da = SizeDepType<3>::middleRows(data.dac_da, current_row_sol_id);
958
959 ConstRowsBlock contact_dlambda_dq =
960
1/2
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
 32 SizeDepType<3>::middleRows(lambda_partial_dq, current_row_sol_id);
961 ConstRowsBlock contact_dlambda_dv =
962
1/2
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
 32 SizeDepType<3>::middleRows(lambda_partial_dv, current_row_sol_id);
963
964 // TODO: Sparsity in dac_da with loop joints?
965
966
4/8
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 16 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 16 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 16 times.
✗ Branch 11 not taken.
 32 data.dtau_dq.noalias() -= contact_dac_da.transpose() * contact_dlambda_dq;
967
4/8
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 16 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 16 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 16 times.
✗ Branch 11 not taken.
 32 data.dtau_dv.noalias() -= contact_dac_da.transpose() * contact_dlambda_dv;
968
969 // END TODO
970 /*
971
972
973
974 for(Eigen::DenseIndex j=colRef;j>=0;j=data.parents_fromRow[(size_t)j])
975 {
976 data.dtau_dq.row(j).noalias() -= contact_dac_da.col(j).transpose() * contact_dlambda_dq;
977 data.dtau_dv.row(j).noalias() -= contact_dac_da.col(j).transpose() * contact_dlambda_dv;
978 }
979 */
980 32 break;
981 }
982
983 ✗ default:
984 ✗ assert(false && "must never happen");
985 break;
986 }
987 58 current_row_sol_id += cmodel.size();
988 }
989
990
3/6
✓ Branch 1 taken 22 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 22 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 22 times.
✗ Branch 9 not taken.
 44 PINOCCHIO_EIGEN_CONST_CAST(MatrixType1, ddq_partial_dq).noalias() =
991
1/2
✓ Branch 1 taken 22 times.
✗ Branch 2 not taken.
 44 -data.Minv * data.dtau_dq; // OUTPUT
992
3/6
✓ Branch 1 taken 22 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 22 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 22 times.
✗ Branch 9 not taken.
 44 PINOCCHIO_EIGEN_CONST_CAST(MatrixType2, ddq_partial_dv).noalias() =
993
1/2
✓ Branch 1 taken 22 times.
✗ Branch 2 not taken.
 44 -data.Minv * data.dtau_dv; // OUTPUT
994
995 44 MatrixType4 & dfc_dq = PINOCCHIO_EIGEN_CONST_CAST(MatrixType4, lambda_partial_dq);
996 typedef typename SizeDepType<6>::template RowsReturn<typename Data::MatrixXs>::Type Rows6Block;
997 typedef typename SizeDepType<3>::template RowsReturn<typename Data::MatrixXs>::Type Rows3Block;
998
999 44 current_row_sol_id = 0;
1000
2/2
✓ Branch 1 taken 29 times.
✓ Branch 2 taken 22 times.
 102 for (size_t k = 0; k < contact_models.size(); ++k)
1001 {
1002 58 const RigidConstraintModel & cmodel = contact_models[k];
1003 58 const RigidConstraintData & cdata = contact_data[k];
1004 58 const typename Model::JointIndex joint1_id = cmodel.joint1_id;
1005
2/4
✓ Branch 2 taken 29 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 29 times.
✗ Branch 7 not taken.
 58 const int colRef = nv(model.joints[joint1_id]) + idx_v(model.joints[joint1_id]) - 1;
1006
1007
2/3
✓ Branch 0 taken 23 times.
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
 58 switch (cmodel.reference_frame)
1008 {
1009 46 case LOCAL:
1010 46 break;
1011 12 case LOCAL_WORLD_ALIGNED: {
1012 12 const Force & of = cdata.contact_force;
1013
2/3
✓ Branch 0 taken 3 times.
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 12 switch (cmodel.type)
1014 {
1015 6 case CONTACT_6D: {
1016
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 6 Rows6Block contact_dfc_dq = SizeDepType<6>::middleRows(dfc_dq, current_row_sol_id);
1017
2/2
✓ Branch 0 taken 37 times.
✓ Branch 1 taken 3 times.
 80 for (Eigen::DenseIndex j = colRef; j >= 0; j = data.parents_fromRow[(size_t)j])
1018 {
1019 typedef typename Data::Matrix6x::ColXpr ColType;
1020 typedef typename Rows6Block::ColXpr ColTypeOut;
1021
2/4
✓ Branch 1 taken 37 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 37 times.
✗ Branch 5 not taken.
 74 const MotionRef<ColType> J_col(data.J.col(j));
1022
2/4
✓ Branch 1 taken 37 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 37 times.
✗ Branch 5 not taken.
 74 ForceRef<ColTypeOut> fout(contact_dfc_dq.col(j));
1023
6/12
✓ Branch 1 taken 37 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 37 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 37 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 37 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 37 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 37 times.
✗ Branch 17 not taken.
 74 fout.linear().noalias() += J_col.angular().cross(of.linear());
1024
6/12
✓ Branch 1 taken 37 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 37 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 37 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 37 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 37 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 37 times.
✗ Branch 17 not taken.
 74 fout.angular().noalias() += J_col.angular().cross(of.angular());
1025 }
1026 6 break;
1027 }
1028 6 case CONTACT_3D: {
1029
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 6 Rows3Block contact_dfc_dq = SizeDepType<3>::middleRows(dfc_dq, current_row_sol_id);
1030
2/2
✓ Branch 0 taken 39 times.
✓ Branch 1 taken 3 times.
 84 for (Eigen::DenseIndex j = colRef; j >= 0; j = data.parents_fromRow[(size_t)j])
1031 {
1032 typedef typename Data::Matrix6x::ColXpr ColType;
1033
2/4
✓ Branch 1 taken 39 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 39 times.
✗ Branch 5 not taken.
 78 const MotionRef<ColType> J_col(data.J.col(j));
1034
6/12
✓ Branch 1 taken 39 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 39 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 39 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 39 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 39 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 39 times.
✗ Branch 17 not taken.
 78 contact_dfc_dq.col(j).noalias() += J_col.angular().cross(of.linear());
1035 }
1036 6 break;
1037 }
1038 ✗ default:
1039 ✗ assert(false && "must never happen");
1040 break;
1041 }
1042 12 break;
1043 }
1044 ✗ default:
1045 ✗ assert(false && "must never happen");
1046 break;
1047 }
1048 58 current_row_sol_id += cmodel.size();
1049 }
1050 44 }
1051
1052 } // namespace pinocchio
1053
1054 #endif // ifndef __pinocchio_algorithm_constraint_dynamics_derivatives_hxx__
1055