GCC Code Coverage Report

Directory: ./
File: include/pinocchio/algorithm/constrained-dynamics-derivatives.hxx
Date: 2025-04-30 16:14:33
Exec Total Coverage
Lines: 473 497 95.2%
Branches: 783 1522 51.4%
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/14
✓ 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 10 not taken.
✓ Branch 11 taken 572 times.
✗ Branch 12 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 17 not taken.
✗ Branch 18 not taken.
 2376 data.a[i] =
85
7/15
✓ Branch 1 taken 594 times.
✗ Branch 2 not taken.
✗ 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 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
2/4
✓ Branch 1 taken 22 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 22 times.
✗ Branch 4 not taken.
 44 assert(model.check(MimicChecker()) && "Function does not support mimic joints");
402
403 // TODO: User should make sure the internal quantities are reset.
404
1/2
✓ Branch 1 taken 22 times.
✗ Branch 2 not taken.
 44 data.dtau_dq.setZero();
405
1/2
✓ Branch 1 taken 22 times.
✗ Branch 2 not taken.
 44 data.dtau_dv.setZero();
406
1/2
✓ Branch 1 taken 22 times.
✗ Branch 2 not taken.
 44 data.dac_dq.setZero();
407
1/2
✓ Branch 1 taken 22 times.
✗ Branch 2 not taken.
 44 data.dac_dv.setZero();
408
409 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
410 typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
411
412 typedef RigidConstraintModelTpl<Scalar, Options> RigidConstraintModel;
413 typedef RigidConstraintDataTpl<Scalar, Options> RigidConstraintData;
414
415 typedef typename ModelTpl<Scalar, Options, JointCollectionTpl>::JointIndex JointIndex;
416 typedef typename Data::SE3 SE3;
417 typedef typename Data::Motion Motion;
418 typedef typename Data::Force Force;
419
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;
420
421 // TODO: Temp variable
422
1/2
✓ Branch 1 taken 22 times.
✗ Branch 2 not taken.
 44 Motion a_tmp;
423
424 typedef ComputeConstraintDynamicsDerivativesForwardStep<
425 Scalar, Options, JointCollectionTpl, true>
426 Pass1;
427
2/2
✓ Branch 0 taken 594 times.
✓ Branch 1 taken 22 times.
 1232 for (JointIndex i = 1; i < (JointIndex)model.njoints; ++i)
428 {
429
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));
430 }
431
432
1/2
✓ Branch 1 taken 22 times.
✗ Branch 2 not taken.
 44 internal::ContactForceContribution<Scalar>::run(contact_models, data, contact_data);
433
434 // Backward Pass
435 typedef ComputeContactDynamicDerivativesBackwardStep<Scalar, Options, JointCollectionTpl, true>
436 Pass2;
437
2/2
✓ Branch 0 taken 594 times.
✓ Branch 1 taken 22 times.
 1232 for (JointIndex i = (JointIndex)(model.njoints - 1); i > 0; --i)
438 {
439
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));
440 }
441
442 // Compute the contact frame partial derivatives
443
1/2
✓ Branch 1 taken 22 times.
✗ Branch 2 not taken.
 44 typename Data::SE3::Matrix6 Jlog;
444 44 Eigen::DenseIndex current_row_sol_id = 0;
445
2/2
✓ Branch 1 taken 29 times.
✓ Branch 2 taken 22 times.
 102 for (size_t k = 0; k < contact_models.size(); ++k)
446 {
447 typedef typename RigidConstraintModel::BooleanVector BooleanVector;
448
449 58 const RigidConstraintModel & cmodel = contact_models[k];
450 58 RigidConstraintData & cdata = contact_data[k];
451 // const BooleanVector & joint1_indexes = cmodel.colwise_joint1_sparsity;
452 58 const BooleanVector & joint2_indexes = cmodel.colwise_joint2_sparsity;
453
454
2/3
✓ Branch 0 taken 13 times.
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
 58 switch (cmodel.type)
455 {
456 26 case CONTACT_6D: {
457 typedef
458 typename SizeDepType<6>::template RowsReturn<typename Data::MatrixXs>::Type RowsBlock;
459
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);
460
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);
461
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);
462
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);
463
464
2/2
✓ Branch 0 taken 12 times.
✓ Branch 1 taken 1 times.
 26 if (cmodel.joint1_id > 0)
465 {
466
1/2
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
 24 cdata.dv1_dq.setZero();
467
1/2
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
 24 cdata.da1_dq.setZero();
468
1/2
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
 24 cdata.da1_dv.setZero();
469
1/2
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
 24 cdata.da1_da.setZero();
470
471 24 getFrameAccelerationDerivatives(
472 24 model, data, cmodel.joint1_id, cmodel.joint1_placement, cmodel.reference_frame,
473
1/2
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
 24 cdata.dv1_dq, cdata.da1_dq, cdata.da1_dv, cdata.da1_da);
474
475
1/2
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
 24 contact_dvc_dq = cdata.dv1_dq;
476
1/2
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
 24 contact_dac_dq = cdata.da1_dq;
477
1/2
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
 24 contact_dac_dv = cdata.da1_dv;
478
1/2
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
 24 contact_dac_da = cdata.da1_da;
479 }
480
481
2/2
✓ Branch 0 taken 4 times.
✓ Branch 1 taken 9 times.
 26 if (cmodel.joint2_id > 0)
482 {
483
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 8 cdata.dv2_dq.setZero();
484
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 8 cdata.da2_dq.setZero();
485
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 8 cdata.da2_dv.setZero();
486
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 8 cdata.da2_da.setZero();
487
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();
488
489 8 getFrameAccelerationDerivatives(
490 8 model, data, cmodel.joint2_id, joint2_M_c1,
491 // cmodel.joint2_placement,
492
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);
493
494 // TODO: This is only in case reference_frame is LOCAL
495 // TODO(jcarpent):to do colwise
496
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 8 contact_dvc_dq -= cdata.dv2_dq;
497
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 8 contact_dac_dq -= cdata.da2_dq;
498
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 8 contact_dac_dv -= cdata.da2_dv;
499
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 8 contact_dac_da -= cdata.da2_da;
500
501
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 8 const Motion v2_in_c1 = cdata.c1Mc2.act(cdata.contact2_velocity);
502
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]);
503
504 8 Eigen::DenseIndex k = Eigen::DenseIndex(cmodel.colwise_span_indexes.size()) - 1;
505 8 Eigen::DenseIndex col_id(0);
506
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)
507 {
508
2/2
✓ Branch 0 taken 35 times.
✓ Branch 1 taken 19 times.
 108 if (k >= 0)
509 {
510 70 col_id = cmodel.colwise_span_indexes[size_t(k)];
511 70 k--;
512 }
513 else
514 {
515 38 col_id = data.parents_fromRow[size_t(col_id)];
516
2/2
✓ Branch 0 taken 3 times.
✓ Branch 1 taken 16 times.
 38 if (col_id < 0)
517 6 break;
518 }
519
520
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));
521
1/2
✓ Branch 1 taken 51 times.
✗ Branch 2 not taken.
 102 const MotionRef<typename RigidConstraintData::Matrix6x::ColXpr> da2_da_col(
522
1/2
✓ Branch 1 taken 51 times.
✗ Branch 2 not taken.
 102 cdata.da2_da.col(col_id));
523
1/2
✓ Branch 1 taken 51 times.
✗ Branch 2 not taken.
 102 const MotionRef<typename RigidConstraintData::Matrix6x::ColXpr> dv2_dq_col(
524
1/2
✓ Branch 1 taken 51 times.
✗ Branch 2 not taken.
 102 cdata.dv2_dq.col(col_id));
525
526 // dv/dq
527
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);
528
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();
529
530 // da/dv
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_dac_dv.col(col_id) -= v2_in_c1_cross_dvc_dv_col.toVector();
532
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();
533
534 // da/dq
535
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));
536
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) -= a2_in_c1.cross(dvc_dv_col).toVector();
538
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();
539
1/2
✓ Branch 1 taken 51 times.
✗ Branch 2 not taken.
 102 contact_dac_dq.col(col_id) +=
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.
 204 cdata.contact_velocity_error.cross(v2_in_c1_cross_dvc_dv_col + dv2_dq_col).toVector();
541 }
542
543
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 8 cdata.dvc_dq = contact_dvc_dq;
544
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 8 cdata.dac_dq = contact_dac_dq;
545
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 8 cdata.dac_dv = contact_dac_dv;
546
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 8 cdata.dac_da = contact_dac_da;
547 }
548
549 26 break;
550 }
551 32 case CONTACT_3D: {
552 typedef
553 typename SizeDepType<3>::template RowsReturn<typename Data::MatrixXs>::Type RowsBlock;
554
555
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);
556
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);
557
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);
558
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);
559
560
2/2
✓ Branch 0 taken 15 times.
✓ Branch 1 taken 1 times.
 32 if (cmodel.joint1_id > 0)
561 {
562
1/2
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
 30 cdata.dv1_dq.setZero();
563
1/2
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
 30 cdata.da1_dq.setZero();
564
1/2
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
 30 cdata.da1_dv.setZero();
565
1/2
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
 30 cdata.da1_da.setZero();
566
567 30 getPointClassicAccelerationDerivatives(
568
1/2
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
 30 model, data, cmodel.joint1_id, cmodel.joint1_placement, cmodel.reference_frame,
569
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>(),
570
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>());
571
572
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>();
573
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>();
574
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>();
575
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>();
576 }
577
578
2/2
✓ Branch 0 taken 5 times.
✓ Branch 1 taken 11 times.
 32 if (cmodel.joint2_id > 0)
579 {
580
1/2
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
 10 cdata.dv2_dq.setZero();
581
1/2
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
 10 cdata.da2_dq.setZero();
582
1/2
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
 10 cdata.da2_dv.setZero();
583
1/2
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
 10 cdata.da2_da.setZero();
584
1/2
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
 10 const SE3 joint2_M_c1(
585
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(),
586
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());
587
588 10 getPointClassicAccelerationDerivatives(
589
1/2
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
 10 model, data, cmodel.joint2_id, joint2_M_c1, cmodel.reference_frame,
590
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>(),
591
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>());
592
593 // TODO: This is only in case reference_frame is LOCAL
594
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>();
595
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>();
596
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>();
597
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>();
598 }
599 32 break;
600 }
601 ✗ default:
602 ✗ assert(false && "must never happen");
603 break;
604 }
605
606
1/2
✓ Branch 1 taken 29 times.
✗ Branch 2 not taken.
 58 assert(
607 cmodel.loop_span_indexes.size() > 0
608 && "Must never happened, the sparsity pattern is empty");
609 // Derivative of closed loop kinematic tree
610
2/2
✓ Branch 0 taken 9 times.
✓ Branch 1 taken 20 times.
 58 if (cmodel.joint2_id > 0)
611 {
612
2/3
✓ Branch 0 taken 4 times.
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
 18 switch (cmodel.type)
613 {
614 8 case CONTACT_6D: {
615 // TODO: THIS IS FOR THE LOCAL FRAME ONLY
616 8 const typename Model::JointIndex joint2_id = cmodel.joint2_id;
617 8 const Eigen::DenseIndex colRef2 =
618
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;
619
620
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 8 Force contact_force_in_WORLD;
621
2/3
✓ Branch 0 taken 3 times.
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 8 switch (cmodel.reference_frame)
622 {
623 6 case LOCAL: {
624
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);
625 6 break;
626 }
627 2 case LOCAL_WORLD_ALIGNED: {
628
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 contact_force_in_WORLD = cdata.contact_force;
629
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() +=
630
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());
631 2 break;
632 }
633 ✗ default: {
634 ✗ assert(false && "must never happen");
635 break;
636 }
637 }
638
639 // d./dq
640
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());
641 ++k)
642 {
643 90 const Eigen::DenseIndex col_id = cmodel.colwise_span_indexes[size_t(k)];
644
645
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));
646
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);
647
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])
648 {
649
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])
650 {
651
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) -=
652
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());
653 }
654 else
655 {
656
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) +=
657
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());
658 }
659 }
660 }
661 8 break;
662 }
663 10 case CONTACT_3D: {
664
665 typedef
666 typename SizeDepType<3>::template RowsReturn<typename Data::MatrixXs>::Type RowsBlock;
667
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);
668 10 const typename Model::JointIndex joint2_id = cmodel.joint2_id;
669 10 const Eigen::DenseIndex colRef2 =
670
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;
671
672
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
673
674
2/3
✓ Branch 0 taken 4 times.
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 10 switch (cmodel.reference_frame)
675 {
676 8 case LOCAL: {
677
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();
678 16 const Motion & c2_acc_c2 = getFrameClassicalAcceleration(
679
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 8 model, data, cmodel.joint2_id, cmodel.joint2_placement, cmodel.reference_frame);
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 a_tmp.angular().noalias() = cdata.oMc2.rotation() * c2_acc_c2.linear();
681 8 break;
682 }
683 2 case LOCAL_WORLD_ALIGNED: {
684
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();
685 2 break;
686 }
687 ✗ default: {
688 ✗ assert(false && "must never happen");
689 break;
690 }
691 }
692
693 // d./dq
694
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)
695 {
696 434 const Eigen::DenseIndex col_id = cmodel.loop_span_indexes[size_t(k)];
697
698
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));
699
700
2/3
✓ Branch 0 taken 175 times.
✓ Branch 1 taken 42 times.
✗ Branch 2 not taken.
 434 switch (cmodel.reference_frame)
701 {
702 350 case LOCAL: {
703
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());
704
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])
705 {
706
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() +=
707
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();
708 }
709 else
710 {
711
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() -=
712
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();
713 }
714 350 break;
715 }
716 84 case LOCAL_WORLD_ALIGNED: {
717 // Do nothing
718 84 break;
719 }
720 ✗ default: {
721 ✗ assert(false && "must never happen");
722 break;
723 }
724 }
725
726
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());
727
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])
728 {
729
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));
730
731 // Temporary assignment
732
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() =
733
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());
734
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])
735 {
736
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());
737 }
738 else
739 {
740
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());
741 }
742 }
743 }
744 10 break;
745 }
746 ✗ default: {
747 ✗ assert(false && "must never happen");
748 break;
749 }
750 }
751 }
752
753 // Add the contribution of the corrector
754 58 if (
755
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.)))
756
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.))))
757 {
758
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);
759
760
2/3
✓ Branch 0 taken 10 times.
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
 50 switch (cmodel.type)
761 {
762 20 case CONTACT_6D: {
763 typedef
764 typename SizeDepType<6>::template RowsReturn<typename Data::MatrixXs>::Type RowsBlock;
765 const RowsBlock contact_dvc_dq =
766
1/2
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
 20 SizeDepType<6>::middleRows(data.dvc_dq, current_row_sol_id);
767
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);
768
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);
769 const RowsBlock contact_dac_da =
770
1/2
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
 20 SizeDepType<6>::middleRows(data.dac_da, current_row_sol_id);
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_dq += cmodel.corrector.Kd.asDiagonal() * contact_dvc_dq;
772
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;
773 // d./dq
774
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());
775 ++k)
776 {
777 238 const Eigen::DenseIndex row_id = cmodel.colwise_span_indexes[size_t(k)];
778 // contact_dac_dq.col(row_id) += cmodel.corrector.Kd * contact_dvc_dq.col(row_id);
779
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() +=
780
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);
781 }
782 20 break;
783 }
784 30 case CONTACT_3D: {
785 typedef
786 typename SizeDepType<3>::template RowsReturn<typename Data::MatrixXs>::Type RowsBlock;
787 const RowsBlock contact_dvc_dq =
788
1/2
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
 30 SizeDepType<3>::middleRows(data.dvc_dq, current_row_sol_id);
789
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);
790
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);
791 const RowsBlock contact_dac_da =
792
1/2
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
 30 SizeDepType<3>::middleRows(data.dac_da, current_row_sol_id);
793
2/2
✓ Branch 0 taken 12 times.
✓ Branch 1 taken 3 times.
 30 if (cmodel.reference_frame == LOCAL)
794 {
795
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()
796
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();
797
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;
798
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);
799
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;
800
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());
801 ++k)
802 {
803 1058 const Eigen::DenseIndex row_id = cmodel.loop_span_indexes[size_t(k)];
804
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));
805
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])
806 {
807
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();
808 }
809 else
810 {
811
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();
812 }
813 }
814 24 const int colRef =
815
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;
816
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])
817 {
818 typedef typename Data::Matrix6x::ColXpr ColType;
819
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));
820
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();
821
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() +=
822
1/2
✓ Branch 1 taken 135 times.
✗ Branch 2 not taken.
 270 cmodel.corrector.Kp.asDiagonal()
823
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());
824 }
825 }
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.Kd.asDiagonal() * contact_dvc_dq;
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_dq.noalias() += cmodel.corrector.Kp.asDiagonal() * contact_dac_da;
828
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;
829 30 break;
830 }
831 ✗ default:
832 ✗ assert(false && "must never happen");
833 break;
834 }
835 }
836
837 58 current_row_sol_id += cmodel.size();
838 }
839
840
1/2
✓ Branch 1 taken 22 times.
✗ Branch 2 not taken.
 44 data.contact_chol.getOperationalSpaceInertiaMatrix(data.osim);
841
1/2
✓ Branch 1 taken 22 times.
✗ Branch 2 not taken.
 44 data.contact_chol.getInverseMassMatrix(data.Minv);
842
843 // Temporary: dlambda_dv stores J*Minv
844 44 typename Data::MatrixXs & JMinv = data.dlambda_dv;
845
846
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;
847 44 MatrixType3 & ddq_partial_dtau_ = PINOCCHIO_EIGEN_CONST_CAST(MatrixType3, ddq_partial_dtau);
848 MatrixType6 & lambda_partial_dtau_ =
849 44 PINOCCHIO_EIGEN_CONST_CAST(MatrixType6, lambda_partial_dtau);
850 44 typename Data::MatrixXs & dlambda_dx_prox = data.dlambda_dx_prox;
851 44 typename Data::MatrixXs & drhs_prox = data.drhs_prox;
852 {
853
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
854
2/2
✓ Branch 0 taken 20 times.
✓ Branch 1 taken 22 times.
 84 for (int it = 1; it < settings.iter; ++it)
855 {
856
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 40 lambda_partial_dtau_.swap(dlambda_dx_prox);
857
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 40 dlambda_dx_prox *= settings.mu;
858
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 40 dlambda_dx_prox -= JMinv;
859
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;
860 }
861
862
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)
863 {
864
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 8 lambda_partial_dtau_.swap(dlambda_dx_prox); // restore previous memory address
865
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 8 lambda_partial_dtau_ = dlambda_dx_prox;
866 }
867 }
868
869
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;
870
1/2
✓ Branch 1 taken 22 times.
✗ Branch 2 not taken.
 44 ddq_partial_dtau_ += data.Minv; // OUTPUT
871
872 44 MatrixType4 & lambda_partial_dq_ = PINOCCHIO_EIGEN_CONST_CAST(MatrixType4, lambda_partial_dq);
873
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;
874
1/2
✓ Branch 1 taken 22 times.
✗ Branch 2 not taken.
 44 drhs_prox += data.dac_dq;
875 {
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 lambda_partial_dq_.noalias() = -data.osim * data.drhs_prox; // OUTPUT
877
2/2
✓ Branch 0 taken 20 times.
✓ Branch 1 taken 22 times.
 84 for (int it = 1; it < settings.iter; ++it)
878 {
879
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 40 lambda_partial_dq_.swap(dlambda_dx_prox);
880
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 40 dlambda_dx_prox *= settings.mu;
881
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 40 dlambda_dx_prox -= drhs_prox;
882
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;
883 }
884
885
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)
886 {
887
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 8 lambda_partial_dq_.swap(dlambda_dx_prox); // restore previous memory address
888
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 8 lambda_partial_dq_ = dlambda_dx_prox;
889 }
890 }
891
892 44 MatrixType5 & lambda_partial_dv_ = PINOCCHIO_EIGEN_CONST_CAST(MatrixType5, lambda_partial_dv);
893
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;
894
1/2
✓ Branch 1 taken 22 times.
✗ Branch 2 not taken.
 44 drhs_prox += data.dac_dv;
895 {
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 lambda_partial_dv_.noalias() = -data.osim * data.drhs_prox; // OUTPUT
897
2/2
✓ Branch 0 taken 20 times.
✓ Branch 1 taken 22 times.
 84 for (int it = 1; it < settings.iter; ++it)
898 {
899
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 40 lambda_partial_dv_.swap(dlambda_dx_prox);
900
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 40 dlambda_dx_prox *= settings.mu;
901
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 40 dlambda_dx_prox -= drhs_prox;
902
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;
903 }
904
905
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)
906 {
907
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 8 lambda_partial_dv_.swap(dlambda_dx_prox); // restore previous memory address
908
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 8 lambda_partial_dv_ = dlambda_dx_prox;
909 }
910 }
911
912 44 current_row_sol_id = 0;
913
2/2
✓ Branch 1 taken 29 times.
✓ Branch 2 taken 22 times.
 102 for (size_t k = 0; k < contact_models.size(); ++k)
914 {
915 58 const RigidConstraintModel & cmodel = contact_models[k];
916
917
2/3
✓ Branch 0 taken 13 times.
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
 58 switch (cmodel.type)
918 {
919 26 case CONTACT_6D: {
920
921 typedef
922 typename SizeDepType<6>::template RowsReturn<typename Data::MatrixXs>::Type RowsBlock;
923 typedef typename SizeDepType<6>::template RowsReturn<typename Data::MatrixXs>::ConstType
924 ConstRowsBlock;
925
926 // TODO: replace with contact_model::nc
927
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);
928
929 ConstRowsBlock contact_dlambda_dq =
930
1/2
✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
 26 SizeDepType<6>::middleRows(lambda_partial_dq, current_row_sol_id);
931 ConstRowsBlock contact_dlambda_dv =
932
1/2
✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
 26 SizeDepType<6>::middleRows(lambda_partial_dv, current_row_sol_id);
933
934 // TODO: Sparsity in dac_da with loop joints?
935
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_dq.noalias() -= contact_dac_da.transpose() * contact_dlambda_dq;
937
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;
938
939 // END TODO
940
941 /*
942
943 for(Eigen::DenseIndex j=colRef;j>=0;j=data.parents_fromRow[(size_t)j])
944 {
945 data.dtau_dq.row(j).noalias() -= contact_dac_da.col(j).transpose() * contact_dlambda_dq;
946 data.dtau_dv.row(j).noalias() -= contact_dac_da.col(j).transpose() * contact_dlambda_dv;
947 }
948 */
949 26 break;
950 }
951 32 case CONTACT_3D: {
952
953 typedef
954 typename SizeDepType<3>::template RowsReturn<typename Data::MatrixXs>::Type RowsBlock;
955 typedef typename SizeDepType<3>::template RowsReturn<typename Data::MatrixXs>::ConstType
956 ConstRowsBlock;
957
958
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);
959
960 ConstRowsBlock contact_dlambda_dq =
961
1/2
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
 32 SizeDepType<3>::middleRows(lambda_partial_dq, current_row_sol_id);
962 ConstRowsBlock contact_dlambda_dv =
963
1/2
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
 32 SizeDepType<3>::middleRows(lambda_partial_dv, current_row_sol_id);
964
965 // TODO: Sparsity in dac_da with loop joints?
966
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_dq.noalias() -= contact_dac_da.transpose() * contact_dlambda_dq;
968
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;
969
970 // END TODO
971 /*
972
973
974
975 for(Eigen::DenseIndex j=colRef;j>=0;j=data.parents_fromRow[(size_t)j])
976 {
977 data.dtau_dq.row(j).noalias() -= contact_dac_da.col(j).transpose() * contact_dlambda_dq;
978 data.dtau_dv.row(j).noalias() -= contact_dac_da.col(j).transpose() * contact_dlambda_dv;
979 }
980 */
981 32 break;
982 }
983
984 ✗ default:
985 ✗ assert(false && "must never happen");
986 break;
987 }
988 58 current_row_sol_id += cmodel.size();
989 }
990
991
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() =
992
1/2
✓ Branch 1 taken 22 times.
✗ Branch 2 not taken.
 44 -data.Minv * data.dtau_dq; // OUTPUT
993
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() =
994
1/2
✓ Branch 1 taken 22 times.
✗ Branch 2 not taken.
 44 -data.Minv * data.dtau_dv; // OUTPUT
995
996 44 MatrixType4 & dfc_dq = PINOCCHIO_EIGEN_CONST_CAST(MatrixType4, lambda_partial_dq);
997 typedef typename SizeDepType<6>::template RowsReturn<typename Data::MatrixXs>::Type Rows6Block;
998 typedef typename SizeDepType<3>::template RowsReturn<typename Data::MatrixXs>::Type Rows3Block;
999
1000 44 current_row_sol_id = 0;
1001
2/2
✓ Branch 1 taken 29 times.
✓ Branch 2 taken 22 times.
 102 for (size_t k = 0; k < contact_models.size(); ++k)
1002 {
1003 58 const RigidConstraintModel & cmodel = contact_models[k];
1004 58 const RigidConstraintData & cdata = contact_data[k];
1005 58 const typename Model::JointIndex joint1_id = cmodel.joint1_id;
1006
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;
1007
1008
2/3
✓ Branch 0 taken 23 times.
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
 58 switch (cmodel.reference_frame)
1009 {
1010 46 case LOCAL:
1011 46 break;
1012 12 case LOCAL_WORLD_ALIGNED: {
1013 12 const Force & of = cdata.contact_force;
1014
2/3
✓ Branch 0 taken 3 times.
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 12 switch (cmodel.type)
1015 {
1016 6 case CONTACT_6D: {
1017
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);
1018
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])
1019 {
1020 typedef typename Data::Matrix6x::ColXpr ColType;
1021 typedef typename Rows6Block::ColXpr ColTypeOut;
1022
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));
1023
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));
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.linear().noalias() += J_col.angular().cross(of.linear());
1025
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());
1026 }
1027 6 break;
1028 }
1029 6 case CONTACT_3D: {
1030
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);
1031
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])
1032 {
1033 typedef typename Data::Matrix6x::ColXpr ColType;
1034
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));
1035
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());
1036 }
1037 6 break;
1038 }
1039 ✗ default:
1040 ✗ assert(false && "must never happen");
1041 break;
1042 }
1043 12 break;
1044 }
1045 ✗ default:
1046 ✗ assert(false && "must never happen");
1047 break;
1048 }
1049 58 current_row_sol_id += cmodel.size();
1050 }
1051 44 }
1052
1053 } // namespace pinocchio
1054
1055 #endif // ifndef __pinocchio_algorithm_constraint_dynamics_derivatives_hxx__
1056