GCC Code Coverage Report

Directory: ./
File: include/pinocchio/algorithm/constrained-dynamics.hxx
Date: 2024-08-27 18:20:05
Exec Total Coverage
Lines: 411 418 98.3%
Branches: 621 1323 46.9%
Line Branch Exec Source
1 //
2 // Copyright (c) 2019-2022 INRIA CNRS
3 //
4
5 #ifndef __pinocchio_algorithm_constraint_dynamics_hxx__
6 #define __pinocchio_algorithm_constraint_dynamics_hxx__
7
8 #include "pinocchio/spatial/classic-acceleration.hpp"
9 #include "pinocchio/spatial/explog.hpp"
10
11 #include "pinocchio/algorithm/check.hpp"
12 #include "pinocchio/algorithm/aba.hpp"
13 #include "pinocchio/algorithm/contact-cholesky.hxx"
14 #include "pinocchio/algorithm/crba.hpp"
15 #include "pinocchio/algorithm/cholesky.hpp"
16 #include <limits>
17
18 namespace pinocchio
19 {
20
21 template<
22 typename Scalar,
23 int Options,
24 template<typename, int>
25 class JointCollectionTpl,
26 class Allocator>
27 68 inline void initConstraintDynamics(
28 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
29 DataTpl<Scalar, Options, JointCollectionTpl> & data,
30 const std::vector<RigidConstraintModelTpl<Scalar, Options>, Allocator> & contact_models)
31 {
32 68 data.contact_chol.allocate(model, contact_models);
33 68 data.primal_dual_contact_solution.resize(data.contact_chol.size());
34 68 data.primal_rhs_contact.resize(data.contact_chol.constraintDim());
35
36 68 data.lambda_c.resize(data.contact_chol.constraintDim());
37 68 data.lambda_c_prox.resize(data.contact_chol.constraintDim());
38 68 data.impulse_c.resize(data.contact_chol.constraintDim());
39
40 // TODO: should be moved elsewhere
41 68 data.dlambda_dq.resize(data.contact_chol.constraintDim(), model.nv);
42 68 data.dlambda_dx_prox.resize(data.contact_chol.constraintDim(), model.nv);
43 68 data.drhs_prox.resize(data.contact_chol.constraintDim(), model.nv);
44 68 data.dlambda_dv.resize(data.contact_chol.constraintDim(), model.nv);
45 68 data.dlambda_dtau.resize(data.contact_chol.constraintDim(), model.nv);
46 68 data.dvc_dq.resize(data.contact_chol.constraintDim(), model.nv);
47 68 data.dac_dq.resize(data.contact_chol.constraintDim(), model.nv);
48 68 data.dac_dv.resize(data.contact_chol.constraintDim(), model.nv);
49 68 data.dac_da.resize(data.contact_chol.constraintDim(), model.nv);
50 68 data.osim.resize(data.contact_chol.constraintDim(), data.contact_chol.constraintDim());
51
52 68 data.lambda_c.setZero();
53 68 data.lambda_c_prox.setZero();
54 68 data.dlambda_dq.setZero();
55 68 data.dlambda_dv.setZero();
56 68 data.dlambda_dtau.setZero();
57 68 data.dvc_dq.setZero();
58 68 data.dac_dq.setZero();
59 68 data.dac_dv.setZero();
60 68 data.dac_da.setZero();
61 68 data.osim.setZero();
62 68 }
63
64 template<
65 typename Scalar,
66 int Options,
67 template<typename, int>
68 class JointCollectionTpl,
69 typename ConfigVectorType,
70 typename TangentVectorType,
71 bool ContactMode>
72 struct ContactAndImpulseDynamicsForwardStep
73 : public fusion::JointUnaryVisitorBase<ContactAndImpulseDynamicsForwardStep<
74 Scalar,
75 Options,
76 JointCollectionTpl,
77 ConfigVectorType,
78 TangentVectorType,
79 ContactMode>>
80 {
81 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
82 typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
83
84 typedef boost::fusion::
85 vector<const Model &, Data &, const ConfigVectorType &, const TangentVectorType &>
86 ArgsType;
87
88 template<typename JointModel>
89 85162 static void algo(
90 const JointModelBase<JointModel> & jmodel,
91 JointDataBase<typename JointModel::JointDataDerived> & jdata,
92 const Model & model,
93 Data & data,
94 const Eigen::MatrixBase<ConfigVectorType> & q,
95 const Eigen::MatrixBase<TangentVectorType> & v)
96 {
97 typedef typename Model::JointIndex JointIndex;
98 typedef typename Data::Motion Motion;
99 typedef typename Data::Force Force;
100 typedef typename Data::Inertia Inertia;
101
102
1/2
✓ Branch 1 taken 42581 times.
✗ Branch 2 not taken.
 85162 const JointIndex & i = jmodel.id();
103 85162 const JointIndex & parent = model.parents[i];
104
105 85162 Motion & ov = data.ov[i];
106 85162 Inertia & oinertias = data.oinertias[i];
107
108
1/2
✓ Branch 4 taken 42581 times.
✗ Branch 5 not taken.
 85162 jmodel.calc(jdata.derived(), q.derived(), v.derived());
109
110
6/10
✓ Branch 1 taken 42581 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 40612 times.
✓ Branch 5 taken 1969 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 40612 times.
✓ Branch 9 taken 1969 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 40612 times.
✗ Branch 13 not taken.
 85162 data.liMi[i] = model.jointPlacements[i] * jdata.M();
111
2/2
✓ Branch 0 taken 40612 times.
✓ Branch 1 taken 1969 times.
 85162 if (parent > 0)
112
2/4
✓ Branch 3 taken 40612 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 40612 times.
✗ Branch 8 not taken.
 81224 data.oMi[i] = data.oMi[parent] * data.liMi[i];
113 else
114
1/2
✓ Branch 3 taken 1969 times.
✗ Branch 4 not taken.
 3938 data.oMi[i] = data.liMi[i];
115
116
3/6
✓ Branch 2 taken 42581 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 42581 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 42581 times.
✗ Branch 9 not taken.
 85162 ov = data.oMi[i].act(jdata.v());
117
2/2
✓ Branch 0 taken 40612 times.
✓ Branch 1 taken 1969 times.
 85162 if (parent > 0)
118
1/2
✓ Branch 2 taken 40612 times.
✗ Branch 3 not taken.
 81224 ov += data.ov[parent];
119
120
4/8
✓ Branch 2 taken 42581 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 42581 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 42581 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 42581 times.
✗ Branch 12 not taken.
 85162 jmodel.jointCols(data.J) = data.oMi[i].act(jdata.S());
121
2/4
✓ Branch 3 taken 42581 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 42581 times.
✗ Branch 7 not taken.
 85162 oinertias = data.oMi[i].act(model.inertias[i]);
122
1/2
✓ Branch 3 taken 42581 times.
✗ Branch 4 not taken.
 85162 data.oYcrb[i] = data.oinertias[i];
123 if (ContactMode)
124 {
125 77710 Force & oh = data.oh[i];
126 77710 Force & of = data.of[i];
127 77710 Motion & oa = data.oa[i];
128 77710 Motion & oa_gf = data.oa_gf[i];
129
2/4
✓ Branch 1 taken 38855 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 38855 times.
✗ Branch 5 not taken.
 77710 oh = oinertias * ov;
130
3/6
✓ Branch 2 taken 38855 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 38855 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 38855 times.
✗ Branch 9 not taken.
 77710 oa = data.oMi[i].act(jdata.c());
131
2/2
✓ Branch 0 taken 37024 times.
✓ Branch 1 taken 1831 times.
 77710 if (parent > 0)
132 {
133
2/4
✓ Branch 2 taken 37024 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 37024 times.
✗ Branch 6 not taken.
 74048 oa += (data.ov[parent] ^ ov);
134
1/2
✓ Branch 2 taken 37024 times.
✗ Branch 3 not taken.
 74048 oa += data.oa[parent];
135 }
136
2/4
✓ Branch 1 taken 38855 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 38855 times.
✗ Branch 5 not taken.
 77710 oa_gf = oa - model.gravity; // add gravity contribution
137
4/8
✓ Branch 1 taken 38855 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 38855 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 38855 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 38855 times.
✗ Branch 11 not taken.
 77710 of = oinertias * oa_gf + ov.cross(oh);
138 }
139 }
140 };
141
142 template<
143 typename Scalar,
144 int Options,
145 template<typename, int>
146 class JointCollectionTpl,
147 bool ContactMode>
148 struct ContactAndImpulseDynamicsBackwardStep
149 : public fusion::JointUnaryVisitorBase<
150 ContactAndImpulseDynamicsBackwardStep<Scalar, Options, JointCollectionTpl, ContactMode>>
151 {
152 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
153 typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
154
155 typedef boost::fusion::vector<const Model &, Data &> ArgsType;
156
157 template<typename JointModel>
158 85162 static void algo(const JointModelBase<JointModel> & jmodel, const Model & model, Data & data)
159 {
160 typedef typename Model::JointIndex JointIndex;
161 typedef
162 typename SizeDepType<JointModel::NV>::template ColsReturn<typename Data::Matrix6x>::Type
163 ColsBlock;
164
165
1/2
✓ Branch 1 taken 42581 times.
✗ Branch 2 not taken.
 85162 const JointIndex i = jmodel.id();
166 85162 const JointIndex parent = model.parents[i];
167
168
1/2
✓ Branch 1 taken 42581 times.
✗ Branch 2 not taken.
 85162 ColsBlock dFda_cols = jmodel.jointCols(data.dFda);
169
1/2
✓ Branch 1 taken 42581 times.
✗ Branch 2 not taken.
 85162 const ColsBlock J_cols = jmodel.jointCols(data.J);
170
1/2
✓ Branch 2 taken 42581 times.
✗ Branch 3 not taken.
 85162 motionSet::inertiaAction(data.oYcrb[i], J_cols, dFda_cols);
171
172
6/12
✓ Branch 2 taken 42581 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 42581 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 42581 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 42581 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 42581 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 42581 times.
✗ Branch 18 not taken.
 85162 data.M.block(jmodel.idx_v(), jmodel.idx_v(), jmodel.nv(), data.nvSubtree[i]).noalias() =
173
4/8
✓ Branch 2 taken 42581 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 42581 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 42581 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 42581 times.
✗ Branch 12 not taken.
 85162 J_cols.transpose() * data.dFda.middleCols(jmodel.idx_v(), data.nvSubtree[i]);
174
1/2
✓ Branch 3 taken 42581 times.
✗ Branch 4 not taken.
 85162 data.oYcrb[parent] += data.oYcrb[i];
175
176 if (ContactMode)
177 {
178
3/6
✓ Branch 1 taken 38855 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 38855 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 38855 times.
✗ Branch 8 not taken.
 77710 jmodel.jointVelocitySelector(data.nle).noalias() =
179
3/6
✓ Branch 2 taken 38855 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 38855 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 38855 times.
✗ Branch 9 not taken.
 77710 J_cols.transpose() * data.of[i].toVector();
180
1/2
✓ Branch 3 taken 38855 times.
✗ Branch 4 not taken.
 77710 data.of[parent] += data.of[i];
181 }
182 }
183 };
184
185 template<
186 typename Scalar,
187 int Options,
188 template<typename, int>
189 class JointCollectionTpl,
190 typename ConfigVectorType,
191 typename TangentVectorType1,
192 typename TangentVectorType2,
193 class ConstraintModelAllocator,
194 class ConstraintDataAllocator>
195 inline const typename DataTpl<Scalar, Options, JointCollectionTpl>::TangentVectorType &
196 1831 constraintDynamics(
197 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
198 DataTpl<Scalar, Options, JointCollectionTpl> & data,
199 const Eigen::MatrixBase<ConfigVectorType> & q,
200 const Eigen::MatrixBase<TangentVectorType1> & v,
201 const Eigen::MatrixBase<TangentVectorType2> & tau,
202 const std::vector<RigidConstraintModelTpl<Scalar, Options>, ConstraintModelAllocator> &
203 contact_models,
204 std::vector<RigidConstraintDataTpl<Scalar, Options>, ConstraintDataAllocator> & contact_datas,
205 ProximalSettingsTpl<Scalar> & settings)
206 {
207 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
208 typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
209 typedef typename Data::Motion Motion;
210
211 typedef RigidConstraintModelTpl<Scalar, Options> RigidConstraintModel;
212 typedef std::vector<RigidConstraintModel, ConstraintModelAllocator> VectorRigidConstraintModel;
213 typedef RigidConstraintDataTpl<Scalar, Options> RigidConstraintData;
214
215
2/4
✓ Branch 1 taken 1831 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 1831 times.
✗ Branch 4 not taken.
 1831 assert(model.check(data) && "data is not consistent with model.");
216
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 1831 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 17 not taken.
✗ Branch 18 not taken.
✗ Branch 20 not taken.
✗ Branch 21 not taken.
✗ Branch 23 not taken.
✗ Branch 24 not taken.
✗ Branch 26 not taken.
✗ Branch 27 not taken.
✗ Branch 29 not taken.
✗ Branch 30 not taken.
✗ Branch 33 not taken.
✗ Branch 34 not taken.
✗ Branch 36 not taken.
✗ Branch 37 not taken.
 1831 PINOCCHIO_CHECK_ARGUMENT_SIZE(
217 q.size(), model.nq, "The joint configuration vector is not of right size");
218
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 1831 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 17 not taken.
✗ Branch 18 not taken.
✗ Branch 20 not taken.
✗ Branch 21 not taken.
✗ Branch 23 not taken.
✗ Branch 24 not taken.
✗ Branch 26 not taken.
✗ Branch 27 not taken.
✗ Branch 29 not taken.
✗ Branch 30 not taken.
✗ Branch 33 not taken.
✗ Branch 34 not taken.
✗ Branch 36 not taken.
✗ Branch 37 not taken.
 1831 PINOCCHIO_CHECK_ARGUMENT_SIZE(
219 v.size(), model.nv, "The joint velocity vector is not of right size");
220
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 1831 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 17 not taken.
✗ Branch 18 not taken.
✗ Branch 20 not taken.
✗ Branch 21 not taken.
✗ Branch 23 not taken.
✗ Branch 24 not taken.
✗ Branch 26 not taken.
✗ Branch 27 not taken.
✗ Branch 29 not taken.
✗ Branch 30 not taken.
✗ Branch 33 not taken.
✗ Branch 34 not taken.
✗ Branch 36 not taken.
✗ Branch 37 not taken.
 1831 PINOCCHIO_CHECK_ARGUMENT_SIZE(
221 tau.size(), model.nv, "The joint torque vector is not of right size");
222
2/6
✓ Branch 1 taken 1831 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 1831 times.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
 1831 PINOCCHIO_CHECK_INPUT_ARGUMENT(
223 check_expression_if_real<Scalar>(settings.mu >= Scalar(0)), "mu has to be positive");
224
1/24
✗ Branch 2 not taken.
✓ Branch 3 taken 1831 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
✗ Branch 12 not taken.
✗ Branch 13 not taken.
✗ Branch 15 not taken.
✗ Branch 16 not taken.
✗ Branch 19 not taken.
✗ Branch 20 not taken.
✗ Branch 22 not taken.
✗ Branch 23 not taken.
✗ Branch 25 not taken.
✗ Branch 26 not taken.
✗ Branch 28 not taken.
✗ Branch 29 not taken.
✗ Branch 31 not taken.
✗ Branch 32 not taken.
✗ Branch 35 not taken.
✗ Branch 36 not taken.
✗ Branch 38 not taken.
✗ Branch 39 not taken.
 1831 PINOCCHIO_CHECK_ARGUMENT_SIZE(
225 contact_models.size(), contact_datas.size(),
226 "The contact models and data do not have the same vector size.");
227
228 // Check that all the frames are related to LOCAL or LOCAL_WORLD_ALIGNED reference frames
229 1831 for (typename VectorRigidConstraintModel::const_iterator cm_it = contact_models.begin();
230
2/2
✓ Branch 3 taken 2847 times.
✓ Branch 4 taken 1831 times.
 4678 cm_it != contact_models.end(); ++cm_it)
231 {
232
1/8
✗ Branch 1 not taken.
✓ Branch 2 taken 2847 times.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✗ Branch 9 not taken.
✗ Branch 10 not taken.
✗ Branch 12 not taken.
✗ Branch 13 not taken.
 2847 PINOCCHIO_CHECK_INPUT_ARGUMENT(
233 cm_it->reference_frame != WORLD, "Contact model with name " + cm_it->name
234 + " has reference_frame equals to WORLD. "
235 "constraintDynamics is only operating from LOCAL or "
236 "LOCAL_WORLD_ALIGNED reference frames.")
237 }
238
239 1831 typename Data::TangentVectorType & a = data.ddq;
240 1831 typename Data::ContactCholeskyDecomposition & contact_chol = data.contact_chol;
241 1831 typename Data::VectorXs & primal_dual_contact_solution = data.primal_dual_contact_solution;
242 1831 typename Data::VectorXs & primal_rhs_contact = data.primal_rhs_contact;
243
244
1/2
✓ Branch 2 taken 1831 times.
✗ Branch 3 not taken.
 1831 data.oYcrb[0].setZero();
245
1/2
✓ Branch 2 taken 1831 times.
✗ Branch 3 not taken.
 1831 data.of[0].setZero();
246
1/2
✓ Branch 2 taken 1831 times.
✗ Branch 3 not taken.
 1831 data.oa[0].setZero();
247
1/2
✓ Branch 2 taken 1831 times.
✗ Branch 3 not taken.
 1831 data.ov[0].setZero();
248 typedef ContactAndImpulseDynamicsForwardStep<
249 Scalar, Options, JointCollectionTpl, ConfigVectorType, TangentVectorType1, true>
250 Pass1;
251
2/2
✓ Branch 0 taken 38855 times.
✓ Branch 1 taken 1831 times.
 40686 for (JointIndex i = 1; i < (JointIndex)model.njoints; ++i)
252 {
253
1/2
✓ Branch 1 taken 38855 times.
✗ Branch 2 not taken.
 38855 Pass1::run(
254 38855 model.joints[i], data.joints[i],
255
1/2
✓ Branch 3 taken 38855 times.
✗ Branch 4 not taken.
 77710 typename Pass1::ArgsType(model, data, q.derived(), v.derived()));
256 }
257
258 typedef ContactAndImpulseDynamicsBackwardStep<Scalar, Options, JointCollectionTpl, true> Pass2;
259
2/2
✓ Branch 0 taken 38855 times.
✓ Branch 1 taken 1831 times.
 40686 for (JointIndex i = (JointIndex)(model.njoints - 1); i > 0; --i)
260 {
261
2/4
✓ Branch 1 taken 38855 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 38855 times.
✗ Branch 6 not taken.
 38855 Pass2::run(model.joints[i], typename Pass2::ArgsType(model, data));
262 }
263
264 // Add the armature contribution
265
2/4
✓ Branch 1 taken 1831 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1831 times.
✗ Branch 5 not taken.
 1831 data.M.diagonal() += model.armature;
266
267 // Retrieve the Centroidal Momemtum map
268 typedef typename Data::Force Force;
269 typedef Eigen::Block<typename Data::Matrix6x, 3, -1> Block3x;
270
271
1/2
✓ Branch 4 taken 1831 times.
✗ Branch 5 not taken.
 1831 data.com[0] = data.oYcrb[0].lever();
272
273
1/2
✓ Branch 1 taken 1831 times.
✗ Branch 2 not taken.
 1831 data.Ag = data.dFda;
274
1/2
✓ Branch 1 taken 1831 times.
✗ Branch 2 not taken.
 1831 const Block3x Ag_lin = data.Ag.template middleRows<3>(Force::LINEAR);
275
1/2
✓ Branch 1 taken 1831 times.
✗ Branch 2 not taken.
 1831 Block3x Ag_ang = data.Ag.template middleRows<3>(Force::ANGULAR);
276
2/2
✓ Branch 0 taken 48010 times.
✓ Branch 1 taken 1831 times.
 49841 for (long i = 0; i < model.nv; ++i)
277
4/8
✓ Branch 1 taken 48010 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 48010 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 48010 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 48010 times.
✗ Branch 12 not taken.
 48010 Ag_ang.col(i) += Ag_lin.col(i).cross(data.com[0]);
278
279 // Computes the Cholesky decomposition
280 1831 const Scalar mu = settings.mu;
281
1/2
✓ Branch 1 taken 1831 times.
✗ Branch 2 not taken.
 1831 contact_chol.compute(model, data, contact_models, contact_datas, mu);
282
283
3/6
✓ Branch 1 taken 1831 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1831 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1831 times.
✗ Branch 8 not taken.
 1831 primal_dual_contact_solution.tail(model.nv) = tau - data.nle;
284
285 // Temporary variables
286 // Motion coriolis_centrifugal_acc1, coriolis_centrifugal_acc2; // Coriolis/centrifugal
287 // acceleration of the contact frame.
288
1/2
✓ Branch 1 taken 1831 times.
✗ Branch 2 not taken.
 1831 typename Motion::Vector3 coriolis_centrifugal_acc1_local;
289
1/2
✓ Branch 1 taken 1831 times.
✗ Branch 2 not taken.
 1831 typename Motion::Vector3 coriolis_centrifugal_acc2_local;
290
291 1831 Eigen::DenseIndex current_row_id = 0;
292
2/2
✓ Branch 1 taken 2847 times.
✓ Branch 2 taken 1831 times.
 4678 for (size_t contact_id = 0; contact_id < contact_models.size(); ++contact_id)
293 {
294 2847 const RigidConstraintModel & contact_model = contact_models[contact_id];
295 2847 RigidConstraintData & contact_data = contact_datas[contact_id];
296 2847 const int contact_dim = contact_model.size();
297
298 2847 const typename RigidConstraintModel::BaumgarteCorrectorParameters & corrector =
299 contact_model.corrector;
300 2847 const typename RigidConstraintData::Motion & contact_acceleration_desired =
301 contact_data.contact_acceleration_desired;
302 2847 typename RigidConstraintData::Motion & contact_acceleration_error =
303 contact_data.contact_acceleration_error;
304
305 2847 const typename Model::JointIndex joint1_id = contact_model.joint1_id;
306 2847 typename RigidConstraintData::SE3 & oMc1 = contact_data.oMc1;
307 2847 typename RigidConstraintData::Motion & vc1 = contact_data.contact1_velocity;
308 2847 typename RigidConstraintData::Motion & coriolis_centrifugal_acc1 =
309 contact_data.contact1_acceleration_drift;
310
311 2847 const typename Model::JointIndex joint2_id = contact_model.joint2_id;
312 2847 typename RigidConstraintData::SE3 & oMc2 = contact_data.oMc2;
313 2847 typename RigidConstraintData::Motion & vc2 = contact_data.contact2_velocity;
314 2847 typename RigidConstraintData::Motion & coriolis_centrifugal_acc2 =
315 contact_data.contact2_acceleration_drift;
316
317 2847 const typename RigidConstraintData::SE3 & c1Mc2 = contact_data.c1Mc2;
318
319 // Compute contact placement and velocities
320
2/2
✓ Branch 0 taken 2646 times.
✓ Branch 1 taken 201 times.
 2847 if (joint1_id > 0)
321
2/4
✓ Branch 2 taken 2646 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 2646 times.
✗ Branch 6 not taken.
 2646 vc1 = oMc1.actInv(data.ov[joint1_id]);
322 else
323
1/2
✓ Branch 1 taken 201 times.
✗ Branch 2 not taken.
 201 vc1.setZero();
324
2/2
✓ Branch 0 taken 727 times.
✓ Branch 1 taken 2120 times.
 2847 if (joint2_id > 0)
325
2/4
✓ Branch 2 taken 727 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 727 times.
✗ Branch 6 not taken.
 727 vc2 = oMc2.actInv(data.ov[joint2_id]);
326 else
327
1/2
✓ Branch 1 taken 2120 times.
✗ Branch 2 not taken.
 2120 vc2.setZero();
328
329
1/2
✓ Branch 1 taken 2847 times.
✗ Branch 2 not taken.
 2847 const Motion vc2_in_frame1 = c1Mc2.act(vc2);
330 // Compute placement and velocity errors
331
2/2
✓ Branch 0 taken 1044 times.
✓ Branch 1 taken 1803 times.
 2847 if (contact_model.type == CONTACT_6D)
332 {
333
3/6
✓ Branch 1 taken 1044 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1044 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1044 times.
✗ Branch 8 not taken.
 1044 contact_data.contact_placement_error = -log6(c1Mc2);
334
4/8
✓ Branch 1 taken 1044 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1044 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1044 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1044 times.
✗ Branch 11 not taken.
 1044 contact_data.contact_velocity_error.toVector() = (vc1 - vc2_in_frame1).toVector();
335 }
336 else
337 {
338
4/8
✓ Branch 1 taken 1803 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1803 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1803 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1803 times.
✗ Branch 11 not taken.
 1803 contact_data.contact_placement_error.linear() = -c1Mc2.translation();
339
2/4
✓ Branch 1 taken 1803 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1803 times.
✗ Branch 5 not taken.
 1803 contact_data.contact_placement_error.angular().setZero();
340
341
3/6
✓ Branch 1 taken 1803 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1803 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1803 times.
✗ Branch 8 not taken.
 1803 contact_data.contact_velocity_error.linear() =
342
4/8
✓ Branch 1 taken 1803 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1803 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1803 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1803 times.
✗ Branch 11 not taken.
 1803 vc1.linear() - c1Mc2.rotation() * vc2.linear();
343
2/4
✓ Branch 1 taken 1803 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1803 times.
✗ Branch 5 not taken.
 1803 contact_data.contact_velocity_error.angular().setZero();
344 }
345
346 5694 if (check_expression_if_real<Scalar, false>(
347
1/2
✓ Branch 1 taken 2847 times.
✗ Branch 2 not taken.
 2847 isZero(corrector.Kp, static_cast<Scalar>(0.))
348
8/10
✓ Branch 0 taken 357 times.
✓ Branch 1 taken 2490 times.
✓ Branch 3 taken 357 times.
✗ Branch 4 not taken.
✓ Branch 5 taken 161 times.
✓ Branch 6 taken 196 times.
✓ Branch 8 taken 2847 times.
✗ Branch 9 not taken.
✓ Branch 10 taken 161 times.
✓ Branch 11 taken 2686 times.
 2847 && isZero(corrector.Kd, static_cast<Scalar>(0.))))
349 {
350
1/2
✓ Branch 1 taken 161 times.
✗ Branch 2 not taken.
 161 contact_acceleration_error.setZero();
351 }
352 else
353 {
354
2/2
✓ Branch 0 taken 892 times.
✓ Branch 1 taken 1794 times.
 2686 if (contact_model.type == CONTACT_6D)
355
5/10
✓ Branch 1 taken 892 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 892 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 892 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 892 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 892 times.
✗ Branch 14 not taken.
 892 contact_acceleration_error.toVector().noalias() =
356 ✗ -(corrector.Kp.asDiagonal() /* * Jexp6(contact_data.contact_placement_error) */
357
3/6
✓ Branch 1 taken 892 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 892 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 892 times.
✗ Branch 8 not taken.
 892 * contact_data.contact_placement_error.toVector())
358
3/6
✓ Branch 1 taken 892 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 892 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 892 times.
✗ Branch 8 not taken.
 1784 - (corrector.Kd.asDiagonal() * contact_data.contact_velocity_error.toVector());
359 else
360 {
361
5/10
✓ Branch 1 taken 1794 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1794 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1794 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1794 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1794 times.
✗ Branch 14 not taken.
 1794 contact_acceleration_error.linear().noalias() =
362
3/6
✓ Branch 1 taken 1794 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1794 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1794 times.
✗ Branch 8 not taken.
 1794 -(corrector.Kp.asDiagonal() * contact_data.contact_placement_error.linear())
363
3/6
✓ Branch 1 taken 1794 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1794 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1794 times.
✗ Branch 8 not taken.
 1794 - (corrector.Kd.asDiagonal() * contact_data.contact_velocity_error.linear());
364
2/4
✓ Branch 1 taken 1794 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1794 times.
✗ Branch 5 not taken.
 1794 contact_acceleration_error.angular().setZero();
365 }
366 }
367
368
2/3
✓ Branch 0 taken 594 times.
✓ Branch 1 taken 2253 times.
✗ Branch 2 not taken.
 2847 switch (contact_model.reference_frame)
369 {
370 594 case LOCAL_WORLD_ALIGNED: {
371 // LINEAR
372
4/8
✓ Branch 1 taken 594 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 594 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 594 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 594 times.
✗ Branch 11 not taken.
 594 coriolis_centrifugal_acc1.linear().noalias() =
373
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 12 taken 594 times.
✗ Branch 13 not taken.
 594 data.oa[joint1_id].linear() + data.oa[joint1_id].angular().cross(oMc1.translation());
374
2/2
✓ Branch 0 taken 297 times.
✓ Branch 1 taken 297 times.
 594 if (contact_model.type == CONTACT_3D)
375
5/10
✓ Branch 2 taken 297 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 297 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 297 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 297 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 297 times.
✗ Branch 15 not taken.
 594 coriolis_centrifugal_acc1.linear() += data.ov[joint1_id].angular().cross(
376
4/8
✓ Branch 2 taken 297 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 297 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 297 times.
✗ Branch 9 not taken.
✓ Branch 12 taken 297 times.
✗ Branch 13 not taken.
 594 data.ov[joint1_id].linear() + data.ov[joint1_id].angular().cross(oMc1.translation()));
377 // ANGULAR
378
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.
 594 coriolis_centrifugal_acc1.angular() = data.oa[joint1_id].angular();
379
380 // LINEAR
381
2/2
✓ Branch 0 taken 297 times.
✓ Branch 1 taken 297 times.
 594 if (contact_model.type == CONTACT_3D)
382 {
383
7/14
✓ Branch 1 taken 297 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 297 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 297 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 297 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 297 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 297 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 297 times.
✗ Branch 20 not taken.
 594 coriolis_centrifugal_acc2.linear().noalias() =
384
4/8
✓ Branch 2 taken 297 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 297 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 297 times.
✗ Branch 9 not taken.
✓ Branch 12 taken 297 times.
✗ Branch 13 not taken.
 297 data.oa[joint2_id].linear() + data.oa[joint2_id].angular().cross(oMc2.translation())
385
1/2
✓ Branch 2 taken 297 times.
✗ Branch 3 not taken.
 297 + data.ov[joint2_id].angular().cross(
386
4/8
✓ Branch 2 taken 297 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 297 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 297 times.
✗ Branch 9 not taken.
✓ Branch 12 taken 297 times.
✗ Branch 13 not taken.
 297 data.ov[joint2_id].linear() + data.ov[joint2_id].angular().cross(oMc2.translation()));
387
2/4
✓ Branch 1 taken 297 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 297 times.
✗ Branch 5 not taken.
 297 coriolis_centrifugal_acc2.angular().setZero();
388 }
389 else
390 {
391
3/6
✓ Branch 1 taken 297 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 297 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 297 times.
✗ Branch 8 not taken.
 297 coriolis_centrifugal_acc2.linear() =
392
4/8
✓ Branch 2 taken 297 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 297 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 297 times.
✗ Branch 9 not taken.
✓ Branch 12 taken 297 times.
✗ Branch 13 not taken.
 297 data.oa[joint2_id].linear() + data.oa[joint2_id].angular().cross(oMc1.translation());
393
3/6
✓ Branch 2 taken 297 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 297 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 297 times.
✗ Branch 9 not taken.
 297 coriolis_centrifugal_acc2.angular() = data.oa[joint2_id].angular();
394 }
395
396
5/10
✓ Branch 1 taken 594 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 594 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 594 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 594 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 594 times.
✗ Branch 14 not taken.
 594 contact_acceleration_error.linear() = oMc1.rotation() * contact_acceleration_error.linear();
397
2/4
✓ Branch 1 taken 594 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 594 times.
✗ Branch 5 not taken.
 594 contact_acceleration_error.angular() =
398
3/6
✓ Branch 1 taken 594 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 594 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 594 times.
✗ Branch 8 not taken.
 594 oMc1.rotation() * contact_acceleration_error.angular();
399 594 break;
400 }
401 2253 case LOCAL: {
402
2/4
✓ Branch 2 taken 2253 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 2253 times.
✗ Branch 6 not taken.
 2253 coriolis_centrifugal_acc1 = oMc1.actInv(data.oa[joint1_id]);
403
2/2
✓ Branch 0 taken 1506 times.
✓ Branch 1 taken 747 times.
 2253 if (contact_model.type == CONTACT_3D)
404 {
405
5/10
✓ Branch 1 taken 1506 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1506 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1506 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1506 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1506 times.
✗ Branch 14 not taken.
 1506 coriolis_centrifugal_acc1.linear() += vc1.angular().cross(vc1.linear());
406
2/4
✓ Branch 1 taken 1506 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1506 times.
✗ Branch 5 not taken.
 1506 coriolis_centrifugal_acc1.angular().setZero();
407 }
408 else
409
2/4
✓ Branch 1 taken 747 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 747 times.
✗ Branch 5 not taken.
 747 coriolis_centrifugal_acc1 += contact_data.contact_velocity_error.cross(vc2_in_frame1);
410
411
2/2
✓ Branch 0 taken 1506 times.
✓ Branch 1 taken 747 times.
 2253 if (contact_model.type == CONTACT_3D)
412 {
413
8/16
✓ Branch 1 taken 1506 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1506 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1506 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1506 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1506 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1506 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 1506 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 1506 times.
✗ Branch 23 not taken.
 4518 coriolis_centrifugal_acc2.linear().noalias() =
414
2/4
✓ Branch 1 taken 1506 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1506 times.
✗ Branch 5 not taken.
 1506 oMc1.rotation().transpose()
415
4/8
✓ Branch 2 taken 1506 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1506 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1506 times.
✗ Branch 9 not taken.
✓ Branch 12 taken 1506 times.
✗ Branch 13 not taken.
 1506 * (data.oa[joint2_id].linear() + data.oa[joint2_id].angular().cross(oMc2.translation())
416
1/2
✓ Branch 2 taken 1506 times.
✗ Branch 3 not taken.
 1506 + data.ov[joint2_id].angular().cross(
417
1/2
✓ Branch 2 taken 1506 times.
✗ Branch 3 not taken.
 1506 data.ov[joint2_id].linear()
418
3/6
✓ Branch 2 taken 1506 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1506 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1506 times.
✗ Branch 9 not taken.
 1506 + data.ov[joint2_id].angular().cross(oMc2.translation())));
419
2/4
✓ Branch 1 taken 1506 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1506 times.
✗ Branch 5 not taken.
 1506 coriolis_centrifugal_acc2.angular().setZero();
420 }
421 else
422
2/4
✓ Branch 2 taken 747 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 747 times.
✗ Branch 6 not taken.
 747 coriolis_centrifugal_acc2 = oMc1.actInv(data.oa[joint2_id]);
423 2253 break;
424 }
425 ✗ default:
426 ✗ assert(false && "must never happened");
427 break;
428 }
429
430
1/2
✓ Branch 1 taken 2847 times.
✗ Branch 2 not taken.
 2847 contact_data.contact_acceleration = coriolis_centrifugal_acc2;
431
2/3
✓ Branch 0 taken 1803 times.
✓ Branch 1 taken 1044 times.
✗ Branch 2 not taken.
 2847 switch (contact_model.type)
432 {
433 1803 case CONTACT_3D:
434
6/12
✓ Branch 1 taken 1803 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1803 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1803 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1803 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1803 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1803 times.
✗ Branch 17 not taken.
 1803 primal_rhs_contact.segment(current_row_id, contact_dim) =
435
2/4
✓ Branch 1 taken 1803 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1803 times.
✗ Branch 5 not taken.
 1803 -coriolis_centrifugal_acc1.linear() + coriolis_centrifugal_acc2.linear()
436
2/4
✓ Branch 1 taken 1803 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1803 times.
✗ Branch 5 not taken.
 1803 + contact_acceleration_error.linear() + contact_acceleration_desired.linear();
437 1803 break;
438 1044 case CONTACT_6D:
439
5/10
✓ Branch 1 taken 1044 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1044 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1044 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1044 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1044 times.
✗ Branch 14 not taken.
 1044 primal_rhs_contact.segment(current_row_id, contact_dim) =
440
2/4
✓ Branch 1 taken 1044 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1044 times.
✗ Branch 5 not taken.
 1044 -coriolis_centrifugal_acc1.toVector() + coriolis_centrifugal_acc2.toVector()
441
3/6
✓ Branch 1 taken 1044 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1044 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1044 times.
✗ Branch 8 not taken.
 1044 + contact_acceleration_error.toVector() + contact_acceleration_desired.toVector();
442 1044 break;
443 ✗ default:
444 ✗ assert(false && "must never happened");
445 break;
446 }
447
448 2847 current_row_id += contact_dim;
449 }
450
451 // Solve the system
452 // Scalar primal_infeasibility = Scalar(0);
453 1831 int it = 0;
454
1/2
✓ Branch 1 taken 1831 times.
✗ Branch 2 not taken.
 1831 data.lambda_c_prox.setZero();
455
1/2
✓ Branch 1 taken 1831 times.
✗ Branch 2 not taken.
 1831 const Eigen::DenseIndex constraint_dim = contact_chol.constraintDim();
456
2/2
✓ Branch 0 taken 2909 times.
✓ Branch 1 taken 1436 times.
 4345 for (; it < settings.max_iter;)
457 {
458 2909 it++;
459
3/6
✓ Branch 1 taken 2909 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2909 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2909 times.
✗ Branch 8 not taken.
 2909 primal_dual_contact_solution.head(constraint_dim) =
460
1/2
✓ Branch 1 taken 2909 times.
✗ Branch 2 not taken.
 2909 primal_rhs_contact + data.lambda_c_prox * settings.mu;
461
3/6
✓ Branch 1 taken 2909 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2909 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2909 times.
✗ Branch 8 not taken.
 2909 primal_dual_contact_solution.tail(model.nv) = tau - data.nle;
462
1/2
✓ Branch 1 taken 2909 times.
✗ Branch 2 not taken.
 2909 contact_chol.solveInPlace(primal_dual_contact_solution);
463
464 // Use data.lambda_c as tmp variable for computing the constraint residual
465
2/4
✓ Branch 1 taken 2909 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2909 times.
✗ Branch 5 not taken.
 5818 contact_chol.getDelassusCholeskyExpression().applyOnTheRight(
466
1/2
✓ Branch 1 taken 2909 times.
✗ Branch 2 not taken.
 5818 primal_dual_contact_solution.head(constraint_dim), data.lambda_c);
467
4/8
✓ Branch 1 taken 2909 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2909 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2909 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 2909 times.
✗ Branch 11 not taken.
 2909 data.lambda_c -= mu * primal_dual_contact_solution.head(constraint_dim)
468
1/2
✓ Branch 1 taken 2909 times.
✗ Branch 2 not taken.
 2909 + primal_rhs_contact.head(constraint_dim);
469
470
1/2
✓ Branch 1 taken 2909 times.
✗ Branch 2 not taken.
 2909 settings.absolute_residual = data.lambda_c.template lpNorm<Eigen::Infinity>();
471 2909 settings.relative_residual =
472
1/2
✓ Branch 1 taken 2909 times.
✗ Branch 2 not taken.
 2909 (primal_dual_contact_solution.head(constraint_dim) + data.lambda_c_prox)
473
2/4
✓ Branch 1 taken 2909 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2909 times.
✗ Branch 5 not taken.
 2909 .template lpNorm<Eigen::Infinity>();
474
475
3/6
✓ Branch 1 taken 2909 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2909 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2909 times.
✗ Branch 8 not taken.
 2909 data.lambda_c_prox = -primal_dual_contact_solution.head(constraint_dim);
476
477 2909 const bool convergence_criteria_reached =
478 5818 check_expression_if_real<Scalar, false>(
479
1/2
✓ Branch 1 taken 2909 times.
✗ Branch 2 not taken.
 2909 settings.absolute_residual <= settings.absolute_accuracy)
480
4/4
✓ Branch 0 taken 2906 times.
✓ Branch 1 taken 3 times.
✓ Branch 2 taken 392 times.
✓ Branch 3 taken 2514 times.
 5815 || check_expression_if_real<Scalar, false>(
481
1/2
✓ Branch 1 taken 2906 times.
✗ Branch 2 not taken.
 2906 settings.relative_residual <= settings.relative_accuracy);
482
2/2
✓ Branch 0 taken 395 times.
✓ Branch 1 taken 2514 times.
 2909 if (convergence_criteria_reached) // In the case where Scalar is not double, this will iterate
483 // for max_it.
484 395 break;
485 }
486 1831 settings.iter = it;
487
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 1831 times.
 1831 assert(settings.iter <= settings.max_iter && "must never happened");
488
489 // Retrieve the joint space acceleration
490
2/4
✓ Branch 1 taken 1831 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1831 times.
✗ Branch 5 not taken.
 1831 a = primal_dual_contact_solution.tail(model.nv);
491
3/6
✓ Branch 1 taken 1831 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1831 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1831 times.
✗ Branch 8 not taken.
 1831 data.lambda_c = -primal_dual_contact_solution.head(constraint_dim);
492
493 // Retrieve the contact forces
494 1831 Eigen::DenseIndex current_row_sol_id = 0;
495
2/2
✓ Branch 1 taken 2847 times.
✓ Branch 2 taken 1831 times.
 4678 for (size_t contact_id = 0; contact_id < contact_models.size(); ++contact_id)
496 {
497 2847 const RigidConstraintModel & contact_model = contact_models[contact_id];
498 2847 RigidConstraintData & contact_data = contact_datas[contact_id];
499 2847 typename RigidConstraintData::Force & fext = contact_data.contact_force;
500 2847 const int contact_dim = contact_model.size();
501
502
2/3
✓ Branch 0 taken 1803 times.
✓ Branch 1 taken 1044 times.
✗ Branch 2 not taken.
 2847 switch (contact_model.type)
503 {
504 1803 case CONTACT_3D: {
505
4/8
✓ Branch 1 taken 1803 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1803 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1803 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1803 times.
✗ Branch 11 not taken.
 1803 fext.linear() = -primal_dual_contact_solution.template segment<3>(current_row_sol_id);
506
2/4
✓ Branch 1 taken 1803 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1803 times.
✗ Branch 5 not taken.
 1803 fext.angular().setZero();
507 1803 break;
508 }
509 1044 case CONTACT_6D: {
510 typedef typename Data::VectorXs::template FixedSegmentReturnType<6>::Type Segment6d;
511
2/4
✓ Branch 1 taken 1044 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1044 times.
✗ Branch 5 not taken.
 1044 const ForceRef<Segment6d> f_sol(
512 primal_dual_contact_solution.template segment<6>(current_row_sol_id));
513
2/4
✓ Branch 1 taken 1044 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1044 times.
✗ Branch 5 not taken.
 1044 fext = -f_sol;
514 1044 break;
515 }
516 ✗ default:
517 ✗ assert(false && "must never happened");
518 break;
519 }
520
521 2847 current_row_sol_id += contact_dim;
522 }
523
524 1831 return a;
525 }
526
527 template<
528 typename Scalar,
529 int Options,
530 template<typename, int>
531 class JointCollectionTpl,
532 typename ConfigVectorType,
533 typename TangentVectorType>
534 struct ContactABAForwardStep1
535 : public fusion::JointUnaryVisitorBase<ContactABAForwardStep1<
536 Scalar,
537 Options,
538 JointCollectionTpl,
539 ConfigVectorType,
540 TangentVectorType>>
541 {
542 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
543 typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
544
545 typedef boost::fusion::
546 vector<const Model &, Data &, const ConfigVectorType &, const TangentVectorType &>
547 ArgsType;
548
549 template<typename JointModel>
550 378 static void algo(
551 const pinocchio::JointModelBase<JointModel> & jmodel,
552 pinocchio::JointDataBase<typename JointModel::JointDataDerived> & jdata,
553 const Model & model,
554 Data & data,
555 const Eigen::MatrixBase<ConfigVectorType> & q,
556 const Eigen::MatrixBase<TangentVectorType> & v)
557 {
558 typedef typename Model::JointIndex JointIndex;
559
560
1/2
✓ Branch 1 taken 189 times.
✗ Branch 2 not taken.
 378 const JointIndex & i = jmodel.id();
561
1/2
✓ Branch 4 taken 189 times.
✗ Branch 5 not taken.
 378 jmodel.calc(jdata.derived(), q.derived(), v.derived());
562
563 378 const JointIndex & parent = model.parents[i];
564
6/10
✓ Branch 1 taken 189 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 182 times.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 182 times.
✓ Branch 9 taken 7 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 182 times.
✗ Branch 13 not taken.
 378 data.liMi[i] = model.jointPlacements[i] * jdata.M();
565
2/2
✓ Branch 0 taken 182 times.
✓ Branch 1 taken 7 times.
 378 if (parent > 0)
566
2/4
✓ Branch 3 taken 182 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 182 times.
✗ Branch 8 not taken.
 364 data.oMi[i] = data.oMi[parent] * data.liMi[i];
567 else
568
1/2
✓ Branch 3 taken 7 times.
✗ Branch 4 not taken.
 14 data.oMi[i] = data.liMi[i];
569
570
4/8
✓ Branch 2 taken 189 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 189 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 189 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 189 times.
✗ Branch 12 not taken.
 378 jmodel.jointCols(data.J) = data.oMi[i].act(jdata.S());
571
572
3/6
✓ Branch 2 taken 189 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 189 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 189 times.
✗ Branch 10 not taken.
 378 data.ov[i] = data.oMi[i].act(jdata.v());
573
2/2
✓ Branch 0 taken 182 times.
✓ Branch 1 taken 7 times.
 378 if (parent > 0)
574
1/2
✓ Branch 3 taken 182 times.
✗ Branch 4 not taken.
 364 data.ov[i] += data.ov[parent];
575
576
3/6
✓ Branch 2 taken 189 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 189 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 189 times.
✗ Branch 10 not taken.
 378 data.oa[i] = data.oMi[i].act(jdata.c());
577
2/2
✓ Branch 0 taken 182 times.
✓ Branch 1 taken 7 times.
 378 if (parent > 0)
578 {
579
2/4
✓ Branch 3 taken 182 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 182 times.
✗ Branch 8 not taken.
 364 data.oa[i] += (data.ov[parent] ^ data.ov[i]);
580 }
581
582
1/2
✓ Branch 3 taken 189 times.
✗ Branch 4 not taken.
 378 data.oa_drift[i] = data.oa[i];
583
2/2
✓ Branch 0 taken 182 times.
✓ Branch 1 taken 7 times.
 378 if (parent > 0)
584 {
585
1/2
✓ Branch 3 taken 182 times.
✗ Branch 4 not taken.
 364 data.oa_drift[i] += data.oa_drift[parent];
586 }
587
588
2/4
✓ Branch 3 taken 189 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 189 times.
✗ Branch 8 not taken.
 378 data.oYcrb[i] = data.oMi[i].act(model.inertias[i]);
589
1/2
✓ Branch 2 taken 189 times.
✗ Branch 3 not taken.
 378 data.oYaba[i] = data.oYcrb[i].matrix();
590
4/8
✓ Branch 2 taken 189 times.
✗ Branch 3 not taken.
✓ Branch 7 taken 189 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 189 times.
✗ Branch 11 not taken.
✓ Branch 14 taken 189 times.
✗ Branch 15 not taken.
 378 data.of[i] = data.oYcrb[i].vxiv(data.ov[i]) - data.oYcrb[i] * model.gravity; // -f_ext
591 }
592 };
593
594 template<
595 typename Scalar,
596 int Options,
597 template<typename, int>
598 class JointCollectionTpl,
599 typename TangentVectorType>
600 struct ContactABABackwardStep1
601 : public fusion::JointUnaryVisitorBase<
602 ContactABABackwardStep1<Scalar, Options, JointCollectionTpl, TangentVectorType>>
603 {
604 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
605 typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
606
607 typedef boost::fusion::vector<const Model &, Data &, const TangentVectorType &> ArgsType;
608
609 template<typename JointModel>
610 378 static void algo(
611 const JointModelBase<JointModel> & jmodel,
612 JointDataBase<typename JointModel::JointDataDerived> & jdata,
613 const Model & model,
614 Data & data,
615 const Eigen::MatrixBase<TangentVectorType> & tau)
616 {
617 typedef typename Model::JointIndex JointIndex;
618 typedef typename Data::Inertia Inertia;
619 typedef typename Data::Force Force;
620 typedef typename Data::Matrix6x Matrix6x;
621
622
1/2
✓ Branch 1 taken 189 times.
✗ Branch 2 not taken.
 378 const JointIndex & i = jmodel.id();
623 378 const JointIndex & parent = model.parents[i];
624 378 typename Inertia::Matrix6 & Ia = data.oYaba[i];
625
626 typedef typename SizeDepType<JointModel::NV>::template ColsReturn<Matrix6x>::Type ColBlock;
627
1/2
✓ Branch 1 taken 189 times.
✗ Branch 2 not taken.
 378 ColBlock Jcols = jmodel.jointCols(data.J);
628
629 378 Force & fi_augmented = data.of_augmented[i];
630 378 const Force & fi = data.of[i];
631
632
1/2
✓ Branch 1 taken 189 times.
✗ Branch 2 not taken.
 378 fi_augmented += fi;
633
4/8
✓ Branch 1 taken 189 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 189 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 189 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 189 times.
✗ Branch 11 not taken.
 378 jmodel.jointVelocitySelector(data.u).noalias() =
634
4/8
✓ Branch 1 taken 189 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 189 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 189 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 189 times.
✗ Branch 11 not taken.
 378 jmodel.jointVelocitySelector(tau) - Jcols.transpose() * fi_augmented.toVector();
635
636
4/8
✓ Branch 1 taken 189 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 189 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 189 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 189 times.
✗ Branch 11 not taken.
 378 jdata.U().noalias() = Ia * Jcols;
637
6/12
✓ Branch 1 taken 189 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 189 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 189 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 189 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 189 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 189 times.
✗ Branch 17 not taken.
 378 jdata.StU().noalias() = Jcols.transpose() * jdata.U();
638
639 // Account for the rotor inertia contribution
640
4/8
✓ Branch 1 taken 189 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 189 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 189 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 189 times.
✗ Branch 11 not taken.
 378 jdata.StU().diagonal() += jmodel.jointVelocitySelector(model.armature);
641
642
3/6
✓ Branch 1 taken 189 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 189 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 189 times.
✗ Branch 8 not taken.
 378 internal::PerformStYSInversion<Scalar>::run(jdata.StU(), jdata.Dinv());
643
6/12
✓ Branch 1 taken 189 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 189 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 189 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 189 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 189 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 189 times.
✗ Branch 17 not taken.
 378 jdata.UDinv().noalias() = jdata.U() * jdata.Dinv();
644
645
2/2
✓ Branch 0 taken 182 times.
✓ Branch 1 taken 7 times.
 378 if (parent > 0)
646 {
647
6/12
✓ Branch 1 taken 182 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 182 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 182 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 182 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 182 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 182 times.
✗ Branch 17 not taken.
 364 Ia.noalias() -= jdata.UDinv() * jdata.U().transpose();
648
649
4/8
✓ Branch 1 taken 182 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 182 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 182 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 182 times.
✗ Branch 11 not taken.
 364 fi_augmented.toVector().noalias() +=
650
5/10
✓ Branch 1 taken 182 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 182 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 182 times.
✗ Branch 8 not taken.
✓ Branch 11 taken 182 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 182 times.
✗ Branch 15 not taken.
 364 Ia * data.oa[i].toVector() + jdata.UDinv() * jmodel.jointVelocitySelector(data.u);
651
1/2
✓ Branch 2 taken 182 times.
✗ Branch 3 not taken.
 364 data.oYaba[parent] += Ia;
652
1/2
✓ Branch 2 taken 182 times.
✗ Branch 3 not taken.
 364 data.of_augmented[parent] += fi_augmented;
653 }
654 }
655 };
656
657 template<
658 typename Scalar,
659 int Options,
660 template<typename, int>
661 class JointCollectionTpl,
662 typename TangentVectorType>
663 struct ContactABABackwardStepAugmented
664 : public fusion::JointUnaryVisitorBase<
665 ContactABABackwardStepAugmented<Scalar, Options, JointCollectionTpl, TangentVectorType>>
666 {
667 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
668 typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
669
670 typedef boost::fusion::vector<const Model &, Data &, const TangentVectorType &> ArgsType;
671
672 template<typename JointModel>
673 324 static void algo(
674 const JointModelBase<JointModel> & jmodel,
675 JointDataBase<typename JointModel::JointDataDerived> & jdata,
676 const Model & model,
677 Data & data,
678 const Eigen::MatrixBase<TangentVectorType> & tau)
679 {
680 typedef typename Model::JointIndex JointIndex;
681 typedef typename Data::Inertia Inertia;
682 typedef typename Data::Force Force;
683 typedef typename Data::Matrix6x Matrix6x;
684
685
1/2
✓ Branch 1 taken 162 times.
✗ Branch 2 not taken.
 324 const JointIndex & i = jmodel.id();
686 324 const JointIndex & parent = model.parents[i];
687 324 typename Inertia::Matrix6 & Ia = data.oYaba[i];
688
689 typedef typename SizeDepType<JointModel::NV>::template ColsReturn<Matrix6x>::Type ColBlock;
690
1/2
✓ Branch 1 taken 162 times.
✗ Branch 2 not taken.
 324 ColBlock Jcols = jmodel.jointCols(data.J);
691
692 324 Force & fi_augmented = data.of_augmented[i];
693 324 const Force & fi = data.of[i];
694
695
1/2
✓ Branch 1 taken 162 times.
✗ Branch 2 not taken.
 324 fi_augmented += fi;
696
4/8
✓ Branch 1 taken 162 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 162 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 162 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 162 times.
✗ Branch 11 not taken.
 324 jmodel.jointVelocitySelector(data.u).noalias() =
697
4/8
✓ Branch 1 taken 162 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 162 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 162 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 162 times.
✗ Branch 11 not taken.
 324 jmodel.jointVelocitySelector(tau) - Jcols.transpose() * fi_augmented.toVector();
698
699
2/2
✓ Branch 0 taken 156 times.
✓ Branch 1 taken 6 times.
 324 if (parent > 0)
700 {
701
4/8
✓ Branch 1 taken 156 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 156 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 156 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 156 times.
✗ Branch 11 not taken.
 312 fi_augmented.toVector().noalias() +=
702
5/10
✓ Branch 1 taken 156 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 156 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 156 times.
✗ Branch 8 not taken.
✓ Branch 11 taken 156 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 156 times.
✗ Branch 15 not taken.
 312 Ia * data.oa[i].toVector() + jdata.UDinv() * jmodel.jointVelocitySelector(data.u);
703
1/2
✓ Branch 2 taken 156 times.
✗ Branch 3 not taken.
 312 data.of_augmented[parent] += fi_augmented;
704 }
705 }
706 };
707
708 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
709 struct ContactABAForwardStep2
710 : public fusion::JointUnaryVisitorBase<
711 ContactABAForwardStep2<Scalar, Options, JointCollectionTpl>>
712 {
713 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
714 typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
715
716 typedef boost::fusion::vector<const Model &, Data &> ArgsType;
717
718 template<typename JointModel>
719 702 static void algo(
720 const pinocchio::JointModelBase<JointModel> & jmodel,
721 pinocchio::JointDataBase<typename JointModel::JointDataDerived> & jdata,
722 const Model & model,
723 Data & data)
724 {
725 typedef typename Model::JointIndex JointIndex;
726 typedef typename Data::Matrix6x Matrix6x;
727
728 typedef typename SizeDepType<JointModel::NV>::template ColsReturn<Matrix6x>::Type ColBlock;
729
1/2
✓ Branch 1 taken 351 times.
✗ Branch 2 not taken.
 702 ColBlock Jcols = jmodel.jointCols(data.J);
730
731
1/2
✓ Branch 1 taken 351 times.
✗ Branch 2 not taken.
 702 const JointIndex & i = jmodel.id();
732 702 const JointIndex & parent = model.parents[i];
733
734
1/2
✓ Branch 3 taken 351 times.
✗ Branch 4 not taken.
 702 data.oa_augmented[i] = data.oa[i];
735
2/2
✓ Branch 0 taken 338 times.
✓ Branch 1 taken 13 times.
 702 if (parent > 0)
736
1/2
✓ Branch 2 taken 338 times.
✗ Branch 3 not taken.
 676 data.oa_augmented[i] +=
737 676 data.oa_augmented[parent]; // does not take into account the gravity field
738
4/8
✓ Branch 1 taken 351 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 351 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 351 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 351 times.
✗ Branch 11 not taken.
 702 jmodel.jointVelocitySelector(data.ddq).noalias() =
739
3/6
✓ Branch 1 taken 351 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 351 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 351 times.
✗ Branch 8 not taken.
 702 jdata.Dinv() * jmodel.jointVelocitySelector(data.u)
740
4/8
✓ Branch 2 taken 351 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 351 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 351 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 351 times.
✗ Branch 12 not taken.
 702 - jdata.UDinv().transpose() * data.oa_augmented[i].toVector();
741
4/8
✓ Branch 1 taken 351 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 351 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 351 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 351 times.
✗ Branch 12 not taken.
 702 data.oa_augmented[i].toVector() += Jcols * jmodel.jointVelocitySelector(data.ddq);
742 }
743 };
744
745 template<
746 typename Scalar,
747 int Options,
748 template<typename, int>
749 class JointCollectionTpl,
750 typename ConfigVectorType,
751 typename TangentVectorType1,
752 typename TangentVectorType2,
753 class ModelAllocator,
754 class DataAllocator>
755 inline const typename DataTpl<Scalar, Options, JointCollectionTpl>::TangentVectorType &
756 7 contactABA(
757 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
758 DataTpl<Scalar, Options, JointCollectionTpl> & data,
759 const Eigen::MatrixBase<ConfigVectorType> & q,
760 const Eigen::MatrixBase<TangentVectorType1> & v,
761 const Eigen::MatrixBase<TangentVectorType2> & tau,
762 const std::vector<RigidConstraintModelTpl<Scalar, Options>, ModelAllocator> & contact_models,
763 std::vector<RigidConstraintDataTpl<Scalar, Options>, DataAllocator> & contact_data,
764 ProximalSettingsTpl<Scalar> & settings)
765 {
766
2/4
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 7 times.
✗ Branch 4 not taken.
 7 assert(model.check(data) && "data is not consistent with model.");
767
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 7 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 17 not taken.
✗ Branch 18 not taken.
✗ Branch 20 not taken.
✗ Branch 21 not taken.
✗ Branch 23 not taken.
✗ Branch 24 not taken.
✗ Branch 26 not taken.
✗ Branch 27 not taken.
✗ Branch 29 not taken.
✗ Branch 30 not taken.
✗ Branch 33 not taken.
✗ Branch 34 not taken.
✗ Branch 36 not taken.
✗ Branch 37 not taken.
 7 PINOCCHIO_CHECK_ARGUMENT_SIZE(
768 q.size(), model.nq, "The joint configuration vector is not of right size");
769
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 7 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 17 not taken.
✗ Branch 18 not taken.
✗ Branch 20 not taken.
✗ Branch 21 not taken.
✗ Branch 23 not taken.
✗ Branch 24 not taken.
✗ Branch 26 not taken.
✗ Branch 27 not taken.
✗ Branch 29 not taken.
✗ Branch 30 not taken.
✗ Branch 33 not taken.
✗ Branch 34 not taken.
✗ Branch 36 not taken.
✗ Branch 37 not taken.
 7 PINOCCHIO_CHECK_ARGUMENT_SIZE(
770 v.size(), model.nv, "The joint velocity vector is not of right size");
771
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 7 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 17 not taken.
✗ Branch 18 not taken.
✗ Branch 20 not taken.
✗ Branch 21 not taken.
✗ Branch 23 not taken.
✗ Branch 24 not taken.
✗ Branch 26 not taken.
✗ Branch 27 not taken.
✗ Branch 29 not taken.
✗ Branch 30 not taken.
✗ Branch 33 not taken.
✗ Branch 34 not taken.
✗ Branch 36 not taken.
✗ Branch 37 not taken.
 7 PINOCCHIO_CHECK_ARGUMENT_SIZE(
772 tau.size(), model.nv, "The joint torque vector is not of right size");
773
2/6
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 7 times.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
 7 PINOCCHIO_CHECK_INPUT_ARGUMENT(
774 check_expression_if_real<Scalar>(settings.mu >= Scalar(0)), "mu has to be positive");
775
1/24
✗ Branch 2 not taken.
✓ Branch 3 taken 7 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
✗ Branch 12 not taken.
✗ Branch 13 not taken.
✗ Branch 15 not taken.
✗ Branch 16 not taken.
✗ Branch 19 not taken.
✗ Branch 20 not taken.
✗ Branch 22 not taken.
✗ Branch 23 not taken.
✗ Branch 25 not taken.
✗ Branch 26 not taken.
✗ Branch 28 not taken.
✗ Branch 29 not taken.
✗ Branch 31 not taken.
✗ Branch 32 not taken.
✗ Branch 35 not taken.
✗ Branch 36 not taken.
✗ Branch 38 not taken.
✗ Branch 39 not taken.
 7 PINOCCHIO_CHECK_ARGUMENT_SIZE(
776 contact_models.size(), contact_data.size(), "contact models and data size are not the same");
777
778 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
779 typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
780
781 typedef typename Data::Motion Motion;
782
783 typedef typename Model::JointIndex JointIndex;
784 typedef RigidConstraintModelTpl<Scalar, Options> RigidConstraintModel;
785 typedef RigidConstraintDataTpl<Scalar, Options> RigidConstraintData;
786 typedef typename Data::Force Force;
787
788 typedef ContactABAForwardStep1<
789 Scalar, Options, JointCollectionTpl, ConfigVectorType, TangentVectorType1>
790 Pass1;
791
2/2
✓ Branch 0 taken 189 times.
✓ Branch 1 taken 7 times.
 196 for (JointIndex i = 1; i < (JointIndex)model.njoints; ++i)
792 {
793
1/2
✓ Branch 1 taken 189 times.
✗ Branch 2 not taken.
 189 Pass1::run(
794 189 model.joints[i], data.joints[i],
795
1/2
✓ Branch 3 taken 189 times.
✗ Branch 4 not taken.
 189 typename Pass1::ArgsType(model, data, q.derived(), v.derived()));
796
1/2
✓ Branch 2 taken 189 times.
✗ Branch 3 not taken.
 189 data.of_augmented[i].setZero();
797 }
798
799
2/2
✓ Branch 1 taken 8 times.
✓ Branch 2 taken 7 times.
 15 for (size_t k = 0; k < contact_models.size(); ++k)
800 {
801 8 const RigidConstraintModel & cmodel = contact_models[k];
802 8 RigidConstraintData & cdata = contact_data[k];
803
804 8 const typename Model::JointIndex joint1_id = cmodel.joint1_id;
805
806 // Compute relative placement between the joint and the contact frame
807 8 SE3 & oMc = cdata.oMc1;
808
2/4
✓ Branch 2 taken 8 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 8 times.
✗ Branch 6 not taken.
 8 oMc = data.oMi[joint1_id] * cmodel.joint1_placement; // contact placement
809
810 typedef typename Data::Inertia Inertia;
811 typedef typename Inertia::Symmetric3 Symmetric3;
812
813 // Add contact inertia to the joint articulated inertia
814
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 Symmetric3 S(Symmetric3::Zero());
815
2/2
✓ Branch 0 taken 4 times.
✓ Branch 1 taken 4 times.
 8 if (cmodel.type == CONTACT_6D)
816
2/4
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
 4 S.setDiagonal(Symmetric3::Vector3::Constant(settings.mu));
817
818
2/4
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
 8 const Inertia contact_inertia(settings.mu, oMc.translation(), S);
819
2/4
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 8 times.
✗ Branch 6 not taken.
 8 data.oYaba[joint1_id] += contact_inertia.matrix();
820
821 8 typename Data::Motion & joint_velocity = data.ov[joint1_id];
822 8 Motion & contact1_velocity = cdata.contact1_velocity;
823
2/4
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
 8 contact1_velocity = oMc.actInv(joint_velocity);
824
825 8 typename Data::Motion & joint_spatial_acceleration_drift = data.oa_drift[joint1_id];
826 8 Motion & contact_acceleration_drift = cdata.contact1_acceleration_drift;
827
2/4
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
 16 contact_acceleration_drift =
828
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 cmodel.desired_contact_acceleration - oMc.actInv(joint_spatial_acceleration_drift);
829
830 // Handle the classic acceleration term
831
2/2
✓ Branch 0 taken 4 times.
✓ Branch 1 taken 4 times.
 8 if (cmodel.type == CONTACT_3D)
832
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 contact_acceleration_drift.linear() -=
833
2/4
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
 8 contact1_velocity.angular().cross(contact1_velocity.linear());
834
835 // Init contact force
836 // cdata.contact_force.setZero();
837
838 // Add the contribution of the constraints to the force vector
839
2/4
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 8 times.
✗ Branch 6 not taken.
 8 data.of_augmented[joint1_id] = oMc.act(cdata.contact_force);
840
2/2
✓ Branch 0 taken 4 times.
✓ Branch 1 taken 4 times.
 8 if (cmodel.type == CONTACT_3D)
841 {
842
3/6
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 4 times.
✗ Branch 9 not taken.
 4 data.of_augmented[joint1_id] -=
843
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.
 8 settings.mu * oMc.act(Force(contact_acceleration_drift.linear(), Force::Vector3::Zero()));
844 }
845 else
846 {
847
2/4
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 4 times.
✗ Branch 6 not taken.
 4 data.of_augmented[joint1_id] -=
848
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.
 8 oMc.act(Force(settings.mu * contact_acceleration_drift.toVector()));
849 }
850 }
851
852 typedef ContactABABackwardStep1<Scalar, Options, JointCollectionTpl, TangentVectorType2> Pass2;
853
2/2
✓ Branch 0 taken 189 times.
✓ Branch 1 taken 7 times.
 196 for (JointIndex i = (JointIndex)model.njoints - 1; i > 0; --i)
854 {
855
1/2
✓ Branch 1 taken 189 times.
✗ Branch 2 not taken.
 189 Pass2::run(
856
1/2
✓ Branch 2 taken 189 times.
✗ Branch 3 not taken.
 378 model.joints[i], data.joints[i], typename Pass2::ArgsType(model, data, tau.derived()));
857 }
858
859 typedef ContactABAForwardStep2<Scalar, Options, JointCollectionTpl> Pass3;
860
2/2
✓ Branch 0 taken 189 times.
✓ Branch 1 taken 7 times.
 196 for (JointIndex i = 1; i < (JointIndex)model.njoints; ++i)
861 {
862
2/4
✓ Branch 1 taken 189 times.
✗ Branch 2 not taken.
✓ Branch 6 taken 189 times.
✗ Branch 7 not taken.
 189 Pass3::run(model.joints[i], data.joints[i], typename Pass3::ArgsType(model, data));
863
1/2
✓ Branch 2 taken 189 times.
✗ Branch 3 not taken.
 189 data.of_augmented[i].setZero();
864 }
865
866 7 settings.iter = 0;
867 7 bool optimal_solution_found = false;
868
2/2
✓ Branch 1 taken 3 times.
✓ Branch 2 taken 4 times.
 7 if (contact_models.size() == 0)
869 {
870 3 return data.ddq;
871 }
872
873 4 Scalar primal_infeasibility = Scalar(0);
874 4 int it = 0;
875
2/2
✓ Branch 0 taken 8 times.
✓ Branch 1 taken 2 times.
 10 for (int it = 0; it < settings.max_iter; ++it)
876 {
877 // Compute contact acceleration errors and max contact errors, aka primal_infeasibility
878 8 primal_infeasibility = Scalar(0);
879
2/2
✓ Branch 1 taken 16 times.
✓ Branch 2 taken 8 times.
 24 for (size_t contact_id = 0; contact_id < contact_models.size(); ++contact_id)
880 {
881 16 const RigidConstraintModel & cmodel = contact_models[contact_id];
882 16 RigidConstraintData & cdata = contact_data[contact_id];
883
884 16 const typename Model::JointIndex & joint1_id = cmodel.joint1_id;
885
886 16 const SE3 & oMc = cdata.oMc1;
887 16 const Motion & contact1_velocity = cdata.contact1_velocity;
888
889 // Compute contact acceleration error (drift)
890 16 const typename Data::Motion & joint_spatial_acceleration = data.oa_augmented[joint1_id];
891
2/4
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 16 times.
✗ Branch 5 not taken.
 16 cdata.contact_acceleration_deviation =
892
1/2
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
 16 oMc.actInv(joint_spatial_acceleration) - cmodel.desired_contact_acceleration;
893
2/2
✓ Branch 0 taken 8 times.
✓ Branch 1 taken 8 times.
 16 if (cmodel.type == CONTACT_3D)
894
3/6
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 8 times.
✗ Branch 8 not taken.
 8 cdata.contact_acceleration_deviation.linear() +=
895
2/4
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
 16 contact1_velocity.angular().cross(contact1_velocity.linear());
896
897 using std::max;
898
2/2
✓ Branch 0 taken 8 times.
✓ Branch 1 taken 8 times.
 16 if (cmodel.type == CONTACT_3D)
899 {
900 16 primal_infeasibility = max<Scalar>(
901 primal_infeasibility,
902
2/4
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
 8 cdata.contact_acceleration_deviation.linear().template lpNorm<Eigen::Infinity>());
903 }
904 else
905 {
906 16 primal_infeasibility = max<Scalar>(
907 primal_infeasibility,
908
2/4
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
 8 cdata.contact_acceleration_deviation.toVector().template lpNorm<Eigen::Infinity>());
909 }
910 }
911
912 16 if (check_expression_if_real<Scalar, false>(
913
3/4
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 2 times.
✓ Branch 4 taken 6 times.
 8 primal_infeasibility < settings.absolute_accuracy))
914 {
915 2 optimal_solution_found = true;
916 2 break;
917 }
918
919 // Update contact forces
920
2/2
✓ Branch 1 taken 12 times.
✓ Branch 2 taken 6 times.
 18 for (size_t contact_id = 0; contact_id < contact_models.size(); ++contact_id)
921 {
922 12 const RigidConstraintModel & cmodel = contact_models[contact_id];
923 12 RigidConstraintData & cdata = contact_data[contact_id];
924
925 12 const typename Model::JointIndex & joint1_id = cmodel.joint1_id;
926
927 12 const SE3 & oMc = cdata.oMc1;
928
929 // Update contact force value
930
2/2
✓ Branch 0 taken 6 times.
✓ Branch 1 taken 6 times.
 12 if (cmodel.type == CONTACT_3D)
931
3/6
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 6 times.
✗ Branch 8 not taken.
 6 cdata.contact_force.linear().noalias() +=
932
2/4
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
 12 settings.mu * cdata.contact_acceleration_deviation.linear();
933 else
934
3/6
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 6 times.
✗ Branch 8 not taken.
 6 cdata.contact_force.toVector().noalias() +=
935
2/4
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
 6 settings.mu * cdata.contact_acceleration_deviation.toVector();
936
937 // Add the contribution of the constraints to the force vector
938 12 const Motion & contact_acceleration_drift = cdata.contact1_acceleration_drift;
939
2/4
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 12 times.
✗ Branch 6 not taken.
 12 data.of_augmented[joint1_id] = oMc.act(cdata.contact_force);
940
2/2
✓ Branch 0 taken 6 times.
✓ Branch 1 taken 6 times.
 12 if (cmodel.type == CONTACT_3D)
941 {
942
3/6
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 6 times.
✗ Branch 9 not taken.
 6 data.of_augmented[joint1_id] -=
943 settings.mu
944
3/6
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 6 times.
✗ Branch 8 not taken.
 12 * oMc.act(Force(contact_acceleration_drift.linear(), Force::Vector3::Zero()));
945 }
946 else
947 {
948
2/4
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 6 times.
✗ Branch 6 not taken.
 6 data.of_augmented[joint1_id] -=
949
3/6
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 6 times.
✗ Branch 8 not taken.
 12 oMc.act(Force(settings.mu * contact_acceleration_drift.toVector()));
950 }
951 }
952
953 typedef ContactABABackwardStepAugmented<
954 Scalar, Options, JointCollectionTpl, TangentVectorType2>
955 Pass2Augmented;
956
2/2
✓ Branch 0 taken 162 times.
✓ Branch 1 taken 6 times.
 168 for (JointIndex i = (JointIndex)model.njoints - 1; i > 0; --i)
957 {
958
1/2
✓ Branch 1 taken 162 times.
✗ Branch 2 not taken.
 162 Pass2Augmented::run(
959
1/2
✓ Branch 2 taken 162 times.
✗ Branch 3 not taken.
 324 model.joints[i], data.joints[i], typename Pass2::ArgsType(model, data, tau.derived()));
960 }
961
962 typedef ContactABAForwardStep2<Scalar, Options, JointCollectionTpl> Pass3;
963
2/2
✓ Branch 0 taken 162 times.
✓ Branch 1 taken 6 times.
 168 for (JointIndex i = 1; i < (JointIndex)model.njoints; ++i)
964 {
965
2/4
✓ Branch 1 taken 162 times.
✗ Branch 2 not taken.
✓ Branch 6 taken 162 times.
✗ Branch 7 not taken.
 162 Pass3::run(model.joints[i], data.joints[i], typename Pass3::ArgsType(model, data));
966
1/2
✓ Branch 2 taken 162 times.
✗ Branch 3 not taken.
 162 data.of_augmented[i].setZero();
967 }
968 }
969
970 4 settings.iter = it;
971 4 settings.absolute_residual = primal_infeasibility;
972
973 4 return data.ddq;
974 }
975
976 template<
977 typename Scalar,
978 int Options,
979 template<typename, int>
980 class JointCollectionTpl,
981 typename ConfigVectorType,
982 typename TangentVectorType1,
983 typename TangentVectorType2,
984 class ModelAllocator,
985 class DataAllocator>
986 inline const typename DataTpl<Scalar, Options, JointCollectionTpl>::TangentVectorType & proxLTLs(
987 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
988 DataTpl<Scalar, Options, JointCollectionTpl> & data,
989 const Eigen::MatrixBase<ConfigVectorType> & q,
990 const Eigen::MatrixBase<TangentVectorType1> & v,
991 const Eigen::MatrixBase<TangentVectorType2> & tau,
992 const std::vector<RigidConstraintModelTpl<Scalar, Options>, ModelAllocator> & contact_models,
993 std::vector<RigidConstraintDataTpl<Scalar, Options>, DataAllocator> & contact_data,
994 ProximalSettingsTpl<Scalar> & settings)
995 {
996 // TODO: wip not yet tested.
997 using namespace Eigen;
998
999 assert(model.check(data) && "data is not consistent with model.");
1000 PINOCCHIO_CHECK_ARGUMENT_SIZE(
1001 q.size(), model.nq, "The joint configuration vector is not of right size");
1002 PINOCCHIO_CHECK_ARGUMENT_SIZE(
1003 v.size(), model.nv, "The joint velocity vector is not of right size");
1004 PINOCCHIO_CHECK_ARGUMENT_SIZE(
1005 tau.size(), model.nv, "The joint torque vector is not of right size");
1006 PINOCCHIO_CHECK_INPUT_ARGUMENT(
1007 check_expression_if_real<Scalar>(settings.mu >= Scalar(0)), "mu has to be positive");
1008 PINOCCHIO_CHECK_ARGUMENT_SIZE(
1009 contact_models.size(), contact_data.size(), "contact models and data size are not the same");
1010
1011 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
1012 typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
1013
1014 typedef typename Data::Motion Motion;
1015
1016 typedef typename Model::JointIndex JointIndex;
1017 typedef RigidConstraintModelTpl<Scalar, Options> RigidConstraintModel;
1018 typedef RigidConstraintDataTpl<Scalar, Options> RigidConstraintData;
1019 typedef typename Data::Force Force;
1020
1021 // Forward pass to compute the Jacobian and the inertia matrices
1022 typedef ContactABAForwardStep1<
1023 Scalar, Options, JointCollectionTpl, ConfigVectorType, TangentVectorType1>
1024 Pass1;
1025 data.tau = tau;
1026 for (JointIndex i = 1; i < (JointIndex)model.njoints; ++i)
1027 {
1028 Pass1::run(
1029 model.joints[i], data.joints[i],
1030 typename Pass1::ArgsType(model, data, q.derived(), v.derived()));
1031 data.of_augmented[i].setZero();
1032 data.of[i] += data.oYcrb[i] * data.oa_drift[i];
1033 }
1034
1035 // Update the inertia matrix of the constrained links
1036 for (size_t k = 0; k < contact_models.size(); ++k)
1037 {
1038 const RigidConstraintModel & cmodel = contact_models[k];
1039 RigidConstraintData & cdata = contact_data[k];
1040
1041 const typename Model::JointIndex joint1_id = cmodel.joint1_id;
1042
1043 // Compute relative placement between the joint and the contact frame
1044 SE3 & oMc = cdata.oMc1;
1045 oMc = data.oMi[joint1_id] * cmodel.joint1_placement; // contact placement
1046
1047 typedef typename Data::Inertia Inertia;
1048 typedef typename Inertia::Symmetric3 Symmetric3;
1049
1050 // Add contact inertia to the joint articulated inertia
1051 Symmetric3 S(Symmetric3::Zero());
1052 if (cmodel.type == CONTACT_6D)
1053 S.setDiagonal(Symmetric3::Vector3::Constant(settings.mu));
1054
1055 const Inertia contact_inertia(settings.mu, oMc.translation(), S);
1056 data.oYcrb[joint1_id] += contact_inertia;
1057
1058 typename Data::Motion & joint_velocity = data.ov[joint1_id];
1059 Motion & contact1_velocity = cdata.contact1_velocity;
1060 contact1_velocity = oMc.actInv(joint_velocity);
1061
1062 typename Data::Motion & joint_spatial_acceleration_drift = data.oa_drift[joint1_id];
1063 Motion & contact_acceleration_drift = cdata.contact1_acceleration_drift;
1064 contact_acceleration_drift =
1065 cmodel.desired_contact_acceleration - oMc.actInv(joint_spatial_acceleration_drift);
1066
1067 // Handle the classic acceleration term
1068 if (cmodel.type == CONTACT_3D)
1069 contact_acceleration_drift.linear() -=
1070 contact1_velocity.angular().cross(contact1_velocity.linear());
1071
1072 // Init contact force
1073 // cdata.contact_force.setZero();
1074
1075 // Add the contribution of the constraints to the force vector
1076 // data.of_augmented[joint1_id] = oMc.act(cdata.contact_force);
1077 if (cmodel.type == CONTACT_3D)
1078 {
1079 data.of_augmented[joint1_id] -=
1080 settings.mu * oMc.act(Force(contact_acceleration_drift.linear(), Force::Vector3::Zero()));
1081 }
1082 else
1083 {
1084 data.of_augmented[joint1_id] -=
1085 oMc.act(Force(settings.mu * contact_acceleration_drift.toVector()));
1086 }
1087 }
1088
1089 // Backward pass to compute the modified CRBA
1090 typedef impl::CrbaWorldConventionBackwardStep<Scalar, Options, JointCollectionTpl> Pass2;
1091 for (JointIndex i = (JointIndex)(model.njoints - 1); i > 0; --i)
1092 {
1093 Pass2::run(model.joints[i], typename Pass2::ArgsType(model, data));
1094
1095 // Compute data.oF in RNEA style to get bias forces
1096 const JointIndex & parent = model.parents[i];
1097 const JointModel & jmodel = model.joints[i];
1098 data.of[i] += data.of_augmented[i];
1099 data.of[parent] += data.of[i];
1100
1101 // subtract the bias forces from the torque to get Mv_dot_free
1102 jmodel.jointVelocitySelector(data.tau).noalias() -=
1103 jmodel.jointCols(data.J).transpose() * (data.of[i].toVector());
1104 data.of_augmented[i].toVector().setZero();
1105 }
1106
1107 // Factorize the CRBA
1108 // crba(model, data, q);
1109 pinocchio::cholesky::decompose(model, data);
1110 data.ddq = pinocchio::cholesky::solve(model, data, data.tau);
1111
1112 data.u = data.ddq;
1113 // data.tau.setZero();
1114 // Proximal iterations
1115 for (int it = 1; it < settings.max_iter; it++)
1116 {
1117 data.tau.setZero();
1118 // Compute accelerations and constraint residual
1119 data.oa_augmented[0].setZero();
1120 for (JointIndex i = 1; i < (JointIndex)model.njoints; ++i)
1121 {
1122 if (data.constraints_supported_dim[i] > 0)
1123 {
1124 const JointModel & jmodel = model.joints[i];
1125 data.oa_augmented[i].toVector().noalias() =
1126 data.oa_augmented[model.parents[i]].toVector()
1127 + jmodel.jointCols(data.J) * jmodel.jointVelocitySelector(data.u);
1128 data.of_augmented[i].toVector().setZero();
1129 }
1130 }
1131
1132 // Check convergence
1133 for (size_t k = 0; k < contact_models.size(); ++k)
1134 {
1135 const RigidConstraintModel & cmodel = contact_models[k];
1136 RigidConstraintData & cdata = contact_data[k];
1137
1138 const typename Model::JointIndex joint1_id = cmodel.joint1_id;
1139
1140 // Compute relative placement between the joint and the contact frame
1141 SE3 & oMc = cdata.oMc1;
1142 oMc = data.oMi[joint1_id] * cmodel.joint1_placement; // contact placement
1143
1144 Motion & contact_acceleration_drift = cdata.contact1_acceleration_drift;
1145 contact_acceleration_drift -= oMc.actInv(data.oa_augmented[joint1_id]);
1146
1147 // Add the contribution of the constraints to the force vector
1148 if (cmodel.type == CONTACT_3D)
1149 {
1150 data.of_augmented[joint1_id] -=
1151 settings.mu
1152 * oMc.act(Force(contact_acceleration_drift.linear(), Force::Vector3::Zero()));
1153 }
1154 else
1155 {
1156 data.of_augmented[joint1_id] -=
1157 oMc.act(Force(settings.mu * contact_acceleration_drift.toVector()));
1158 }
1159 }
1160
1161 for (JointIndex i = (JointIndex)(model.njoints - 1); i > 0; --i)
1162 {
1163 if (data.constraints_supported_dim[i] > 0)
1164 {
1165
1166 const JointIndex & parent = model.parents[i];
1167 const JointModel & jmodel = model.joints[i];
1168 data.of_augmented[parent] += data.of_augmented[i];
1169
1170 jmodel.jointVelocitySelector(data.tau).noalias() =
1171 -jmodel.jointCols(data.J).transpose() * (data.of_augmented[i].toVector());
1172 }
1173 }
1174
1175 data.u = cholesky::solve(model, data, data.tau);
1176 data.ddq.noalias() += data.u;
1177 }
1178
1179 return data.ddq;
1180 }
1181
1182 } // namespace pinocchio
1183
1184 #endif // ifndef __pinocchio_algorithm_constraint_dynamics_hxx__
1185