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