GCC Code Coverage Report

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