GCC Code Coverage Report

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