GCC Code Coverage Report

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