Directory: | ./ |
---|---|
File: | include/pinocchio/algorithm/constrained-dynamics-derivatives.hxx |
Date: | 2024-08-27 18:20:05 |
Exec | Total | Coverage | |
---|---|---|---|
Lines: | 472 | 496 | 95.2% |
Branches: | 781 | 1467 | 53.2% |
Line | Branch | Exec | Source |
---|---|---|---|
1 | // | ||
2 | // Copyright (c) 2020-2022 CNRS INRIA | ||
3 | // | ||
4 | |||
5 | #ifndef __pinocchio_algorithm_constraint_dynamics_derivatives_hxx__ | ||
6 | #define __pinocchio_algorithm_constraint_dynamics_derivatives_hxx__ | ||
7 | |||
8 | #include "pinocchio/algorithm/check.hpp" | ||
9 | #include "pinocchio/algorithm/rnea-derivatives.hpp" | ||
10 | #include "pinocchio/algorithm/kinematics-derivatives.hpp" | ||
11 | #include "pinocchio/algorithm/kinematics.hpp" | ||
12 | #include "pinocchio/algorithm/frames.hpp" | ||
13 | #include "pinocchio/algorithm/frames-derivatives.hpp" | ||
14 | #include "pinocchio/algorithm/contact-cholesky.hpp" | ||
15 | #include "pinocchio/algorithm/utils/motion.hpp" | ||
16 | |||
17 | namespace pinocchio | ||
18 | { | ||
19 | |||
20 | template< | ||
21 | typename Scalar, | ||
22 | int Options, | ||
23 | template<typename, int> | ||
24 | class JointCollectionTpl, | ||
25 | bool ContactMode> | ||
26 | struct ComputeConstraintDynamicsDerivativesForwardStep | ||
27 | : public fusion::JointUnaryVisitorBase<ComputeConstraintDynamicsDerivativesForwardStep< | ||
28 | Scalar, | ||
29 | Options, | ||
30 | JointCollectionTpl, | ||
31 | ContactMode>> | ||
32 | { | ||
33 | typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model; | ||
34 | typedef DataTpl<Scalar, Options, JointCollectionTpl> Data; | ||
35 | |||
36 | typedef boost::fusion::vector<const Model &, Data &> ArgsType; | ||
37 | |||
38 | template<typename JointModel> | ||
39 | 1404 | static void algo( | |
40 | const JointModelBase<JointModel> & jmodel, | ||
41 | JointDataBase<typename JointModel::JointDataDerived> & jdata, | ||
42 | const Model & model, | ||
43 | Data & data) | ||
44 | { | ||
45 | typedef typename Model::JointIndex JointIndex; | ||
46 | typedef typename Data::Motion Motion; | ||
47 | |||
48 |
1/2✓ Branch 1 taken 702 times.
✗ Branch 2 not taken.
|
1404 | const JointIndex i = jmodel.id(); |
49 | 1404 | const JointIndex parent = model.parents[i]; | |
50 | |||
51 | typedef | ||
52 | typename SizeDepType<JointModel::NV>::template ColsReturn<typename Data::Matrix6x>::Type | ||
53 | ColsBlock; | ||
54 |
1/2✓ Branch 1 taken 702 times.
✗ Branch 2 not taken.
|
1404 | ColsBlock J_cols = jmodel.jointCols(data.J); |
55 |
1/2✓ Branch 1 taken 702 times.
✗ Branch 2 not taken.
|
1404 | ColsBlock dAdq_cols = jmodel.jointCols(data.dAdq); |
56 | |||
57 | if (ContactMode) | ||
58 | { | ||
59 | 1188 | const Motion & ov = data.ov[i]; | |
60 |
1/2✓ Branch 1 taken 594 times.
✗ Branch 2 not taken.
|
1188 | ColsBlock dJ_cols = jmodel.jointCols(data.dJ); |
61 |
1/2✓ Branch 1 taken 594 times.
✗ Branch 2 not taken.
|
1188 | ColsBlock dVdq_cols = jmodel.jointCols(data.dVdq); |
62 | |||
63 |
1/2✓ Branch 1 taken 594 times.
✗ Branch 2 not taken.
|
1188 | motionSet::motionAction(ov, J_cols, dJ_cols); |
64 | // TODO: make more efficient | ||
65 |
2/4✓ Branch 3 taken 594 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 594 times.
✗ Branch 8 not taken.
|
1188 | data.v[i] = data.oMi[i].actInv(data.ov[i]); |
66 | |||
67 |
2/2✓ Branch 0 taken 572 times.
✓ Branch 1 taken 22 times.
|
1188 | if (parent > 0) |
68 | { | ||
69 |
1/2✓ Branch 2 taken 572 times.
✗ Branch 3 not taken.
|
1144 | motionSet::motionAction(data.ov[parent], J_cols, dVdq_cols); |
70 | } | ||
71 | else | ||
72 |
1/2✓ Branch 1 taken 22 times.
✗ Branch 2 not taken.
|
44 | dVdq_cols.setZero(); |
73 | |||
74 | // computes variation of inertias | ||
75 |
1/2✓ Branch 2 taken 594 times.
✗ Branch 3 not taken.
|
1188 | data.doYcrb[i] = data.oinertias[i].variation(ov); |
76 | typedef impl::ComputeRNEADerivativesForwardStep< | ||
77 | Scalar, Options, JointCollectionTpl, typename Data::ConfigVectorType, | ||
78 | typename Data::TangentVectorType, typename Data::TangentVectorType> | ||
79 | RNEAForwardStepType; | ||
80 |
1/2✓ Branch 3 taken 594 times.
✗ Branch 4 not taken.
|
1188 | RNEAForwardStepType::addForceCrossMatrix(data.oh[i], data.doYcrb[i]); |
81 | 1188 | Motion & oa = data.oa[i]; | |
82 | 1188 | Motion & oa_gf = data.oa_gf[i]; | |
83 |
1/2✓ Branch 1 taken 594 times.
✗ Branch 2 not taken.
|
1188 | ColsBlock dAdv_cols = jmodel.jointCols(data.dAdv); |
84 | 1188 | const typename Data::TangentVectorType & a = data.ddq; | |
85 |
5/9✓ Branch 1 taken 594 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 572 times.
✓ Branch 5 taken 22 times.
✗ Branch 6 not taken.
✓ Branch 7 taken 572 times.
✗ Branch 8 not taken.
✓ Branch 11 taken 572 times.
✗ Branch 12 not taken.
|
2376 | data.a[i] = |
86 |
7/14✓ Branch 1 taken 594 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 594 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 594 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 594 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 594 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 22 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 22 times.
✗ Branch 21 not taken.
|
2376 | jdata.S() * jmodel.jointVelocitySelector(a) + jdata.c() + (data.v[i] ^ jdata.v()); |
87 |
2/2✓ Branch 0 taken 572 times.
✓ Branch 1 taken 22 times.
|
1188 | if (parent > 0) |
88 |
2/4✓ Branch 3 taken 572 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 572 times.
✗ Branch 8 not taken.
|
1144 | data.a[i] += data.liMi[i].actInv(data.a[parent]); |
89 |
2/4✓ Branch 3 taken 594 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 594 times.
✗ Branch 7 not taken.
|
1188 | oa = data.oMi[i].act(data.a[i]); |
90 |
2/4✓ Branch 1 taken 594 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 594 times.
✗ Branch 5 not taken.
|
1188 | oa_gf = oa - model.gravity; // add gravity contribution |
91 |
4/8✓ Branch 2 taken 594 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 594 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 594 times.
✗ Branch 10 not taken.
✓ Branch 13 taken 594 times.
✗ Branch 14 not taken.
|
1188 | data.of[i] = data.oinertias[i] * oa_gf + ov.cross(data.oh[i]); |
92 |
1/2✓ Branch 2 taken 594 times.
✗ Branch 3 not taken.
|
1188 | motionSet::motionAction(data.oa_gf[parent], J_cols, dAdq_cols); |
93 |
1/2✓ Branch 1 taken 594 times.
✗ Branch 2 not taken.
|
1188 | dAdv_cols = dJ_cols; |
94 |
2/2✓ Branch 0 taken 572 times.
✓ Branch 1 taken 22 times.
|
1188 | if (parent > 0) |
95 | { | ||
96 |
1/2✓ Branch 2 taken 572 times.
✗ Branch 3 not taken.
|
1144 | motionSet::motionAction<ADDTO>(data.ov[parent], dVdq_cols, dAdq_cols); |
97 |
2/4✓ Branch 1 taken 572 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 572 times.
✗ Branch 5 not taken.
|
1144 | dAdv_cols.noalias() += dVdq_cols; |
98 | } | ||
99 | } | ||
100 | else | ||
101 | { | ||
102 | 216 | Motion & odv = data.oa[i]; | |
103 | 216 | Motion & odvparent = data.oa[parent]; | |
104 | 216 | const typename Data::TangentVectorType & dimpulse = data.ddq; | |
105 | // Temporary calculation of J(dq_after) | ||
106 |
3/6✓ Branch 1 taken 108 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 108 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 108 times.
✗ Branch 8 not taken.
|
216 | odv = J_cols * jmodel.jointVelocitySelector(dimpulse); |
107 |
2/2✓ Branch 0 taken 104 times.
✓ Branch 1 taken 4 times.
|
216 | if (parent > 0) |
108 |
1/2✓ Branch 1 taken 104 times.
✗ Branch 2 not taken.
|
208 | odv += odvparent; |
109 |
1/2✓ Branch 1 taken 108 times.
✗ Branch 2 not taken.
|
216 | motionSet::motionAction(odvparent, J_cols, dAdq_cols); |
110 |
2/4✓ Branch 2 taken 108 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 108 times.
✗ Branch 7 not taken.
|
216 | data.of[i] = data.oinertias[i] * odv; |
111 | } | ||
112 | } | ||
113 | }; | ||
114 | |||
115 | template< | ||
116 | typename Scalar, | ||
117 | int Options, | ||
118 | template<typename, int> | ||
119 | class JointCollectionTpl, | ||
120 | bool ContactMode> | ||
121 | struct ComputeContactDynamicDerivativesBackwardStep | ||
122 | : public fusion::JointUnaryVisitorBase<ComputeContactDynamicDerivativesBackwardStep< | ||
123 | Scalar, | ||
124 | Options, | ||
125 | JointCollectionTpl, | ||
126 | ContactMode>> | ||
127 | { | ||
128 | typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model; | ||
129 | typedef DataTpl<Scalar, Options, JointCollectionTpl> Data; | ||
130 | |||
131 | typedef boost::fusion::vector<const Model &, Data &> ArgsType; | ||
132 | |||
133 | template<typename JointModel> | ||
134 | 1404 | static void algo(const JointModelBase<JointModel> & jmodel, const Model & model, Data & data) | |
135 | { | ||
136 | typedef typename Model::JointIndex JointIndex; | ||
137 | typedef Eigen::Matrix< | ||
138 | Scalar, JointModel::NV, 6, Options, JointModel::NV == Eigen::Dynamic ? 6 : JointModel::NV, | ||
139 | 6> | ||
140 | MatrixNV6; | ||
141 | typedef | ||
142 | typename SizeDepType<JointModel::NV>::template ColsReturn<typename Data::Matrix6x>::Type | ||
143 | ColsBlock; | ||
144 | |||
145 |
1/2✓ Branch 1 taken 702 times.
✗ Branch 2 not taken.
|
1404 | const JointIndex i = jmodel.id(); |
146 | 1404 | const JointIndex parent = model.parents[i]; | |
147 |
1/2✓ Branch 1 taken 702 times.
✗ Branch 2 not taken.
|
1404 | ColsBlock J_cols = jmodel.jointCols(data.J); |
148 |
1/2✓ Branch 1 taken 702 times.
✗ Branch 2 not taken.
|
1404 | ColsBlock dVdq_cols = jmodel.jointCols(data.dVdq); |
149 |
1/2✓ Branch 1 taken 702 times.
✗ Branch 2 not taken.
|
1404 | ColsBlock dAdq_cols = jmodel.jointCols(data.dAdq); |
150 |
1/2✓ Branch 1 taken 702 times.
✗ Branch 2 not taken.
|
1404 | ColsBlock dFdq_cols = jmodel.jointCols(data.dFdq); |
151 |
1/2✓ Branch 1 taken 702 times.
✗ Branch 2 not taken.
|
1404 | ColsBlock dFda_cols = jmodel.jointCols(data.dFda); |
152 | |||
153 | 1404 | typename Data::RowMatrixXs & dtau_dq = data.dtau_dq; | |
154 | |||
155 | // Temporary variables | ||
156 |
2/4✓ Branch 1 taken 702 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 702 times.
✗ Branch 5 not taken.
|
1404 | typename PINOCCHIO_EIGEN_PLAIN_ROW_MAJOR_TYPE(MatrixNV6) StdY(jmodel.nv(), 6); |
157 | |||
158 |
1/2✓ Branch 2 taken 702 times.
✗ Branch 3 not taken.
|
1404 | motionSet::inertiaAction(data.oYcrb[i], dAdq_cols, dFdq_cols); |
159 | // dtau/dq | ||
160 |
2/2✓ Branch 0 taken 676 times.
✓ Branch 1 taken 26 times.
|
1404 | if (parent > 0) |
161 | { | ||
162 | if (ContactMode) | ||
163 | { | ||
164 |
3/6✓ Branch 2 taken 572 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 572 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 572 times.
✗ Branch 9 not taken.
|
1144 | dFdq_cols.noalias() += data.doYcrb[i] * dVdq_cols; |
165 |
4/8✓ Branch 2 taken 572 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 572 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 572 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 572 times.
✗ Branch 12 not taken.
|
1144 | StdY.noalias() = J_cols.transpose() * data.doYcrb[i]; |
166 |
3/4✓ Branch 1 taken 572 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5302 times.
✓ Branch 5 taken 572 times.
|
11748 | for (int j = data.parents_fromRow[(typename Model::Index)jmodel.idx_v()]; j >= 0; |
167 | 10604 | j = data.parents_fromRow[(typename Model::Index)j]) | |
168 | { | ||
169 |
7/14✓ Branch 1 taken 5302 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5302 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 5302 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 5302 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 5302 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 5302 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 5302 times.
✗ Branch 20 not taken.
|
10604 | dtau_dq.middleRows(jmodel.idx_v(), jmodel.nv()).col(j).noalias() = |
170 |
5/10✓ Branch 1 taken 5302 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5302 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 5302 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 5302 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 5302 times.
✗ Branch 14 not taken.
|
21208 | dFda_cols.transpose() * data.dAdq.col(j) + StdY * data.dVdq.col(j); |
171 | } | ||
172 | } | ||
173 | else | ||
174 | { | ||
175 |
3/4✓ Branch 1 taken 104 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 964 times.
✓ Branch 5 taken 104 times.
|
2136 | for (int j = data.parents_fromRow[(typename Model::Index)jmodel.idx_v()]; j >= 0; |
176 | 1928 | j = data.parents_fromRow[(typename Model::Index)j]) | |
177 | { | ||
178 |
6/12✓ Branch 1 taken 964 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 964 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 964 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 964 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 964 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 964 times.
✗ Branch 17 not taken.
|
1928 | dtau_dq.middleRows(jmodel.idx_v(), jmodel.nv()).col(j).noalias() = |
179 |
3/6✓ Branch 1 taken 964 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 964 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 964 times.
✗ Branch 8 not taken.
|
3856 | dFda_cols.transpose() * data.dAdq.col(j); |
180 | } | ||
181 | } | ||
182 | } | ||
183 | |||
184 |
6/12✓ Branch 2 taken 702 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 702 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 702 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 702 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 702 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 702 times.
✗ Branch 18 not taken.
|
1404 | dtau_dq.block(jmodel.idx_v(), jmodel.idx_v(), jmodel.nv(), data.nvSubtree[i]).noalias() = |
185 |
4/8✓ Branch 2 taken 702 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 702 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 702 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 702 times.
✗ Branch 12 not taken.
|
1404 | J_cols.transpose() * data.dFdq.middleCols(jmodel.idx_v(), data.nvSubtree[i]); |
186 |
1/2✓ Branch 2 taken 702 times.
✗ Branch 3 not taken.
|
1404 | motionSet::act<ADDTO>(J_cols, data.of[i], dFdq_cols); |
187 | |||
188 | if (ContactMode) | ||
189 | { | ||
190 |
1/2✓ Branch 1 taken 594 times.
✗ Branch 2 not taken.
|
1188 | ColsBlock dAdv_cols = jmodel.jointCols(data.dAdv); |
191 |
1/2✓ Branch 1 taken 594 times.
✗ Branch 2 not taken.
|
1188 | ColsBlock dFdv_cols = jmodel.jointCols(data.dFdv); |
192 | |||
193 | 1188 | typename Data::RowMatrixXs & dtau_dv = data.dtau_dv; | |
194 |
3/6✓ Branch 2 taken 594 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 594 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 594 times.
✗ Branch 9 not taken.
|
1188 | dFdv_cols.noalias() = data.doYcrb[i] * J_cols; |
195 |
1/2✓ Branch 2 taken 594 times.
✗ Branch 3 not taken.
|
1188 | motionSet::inertiaAction<ADDTO>(data.oYcrb[i], dAdv_cols, dFdv_cols); |
196 | |||
197 |
6/12✓ Branch 2 taken 594 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 594 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 594 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 594 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 594 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 594 times.
✗ Branch 18 not taken.
|
1188 | dtau_dv.block(jmodel.idx_v(), jmodel.idx_v(), jmodel.nv(), data.nvSubtree[i]).noalias() = |
198 |
4/8✓ Branch 2 taken 594 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 594 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 594 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 594 times.
✗ Branch 12 not taken.
|
1188 | J_cols.transpose() * data.dFdv.middleCols(jmodel.idx_v(), data.nvSubtree[i]); |
199 |
2/2✓ Branch 0 taken 572 times.
✓ Branch 1 taken 22 times.
|
1188 | if (parent > 0) |
200 | { | ||
201 |
3/4✓ Branch 1 taken 572 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5302 times.
✓ Branch 5 taken 572 times.
|
11748 | for (int j = data.parents_fromRow[(typename Model::Index)jmodel.idx_v()]; j >= 0; |
202 | 10604 | j = data.parents_fromRow[(typename Model::Index)j]) | |
203 | { | ||
204 |
7/14✓ Branch 1 taken 5302 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5302 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 5302 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 5302 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 5302 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 5302 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 5302 times.
✗ Branch 20 not taken.
|
10604 | dtau_dv.middleRows(jmodel.idx_v(), jmodel.nv()).col(j).noalias() = |
205 |
5/10✓ Branch 1 taken 5302 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5302 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 5302 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 5302 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 5302 times.
✗ Branch 14 not taken.
|
21208 | dFda_cols.transpose() * data.dAdv.col(j) + StdY * data.J.col(j); |
206 | } | ||
207 |
1/2✓ Branch 3 taken 572 times.
✗ Branch 4 not taken.
|
1144 | data.doYcrb[parent] += data.doYcrb[i]; |
208 | } | ||
209 | // Restore the status of dAdq_cols (remove gravity) | ||
210 |
3/4✓ Branch 1 taken 1298 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 704 times.
✓ Branch 4 taken 594 times.
|
2596 | for (Eigen::DenseIndex k = 0; k < jmodel.nv(); ++k) |
211 | { | ||
212 | typedef typename ColsBlock::ColXpr ColType; | ||
213 |
2/4✓ Branch 1 taken 704 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 704 times.
✗ Branch 5 not taken.
|
1408 | MotionRef<ColType> min(J_cols.col(k)); |
214 |
2/4✓ Branch 1 taken 704 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 704 times.
✗ Branch 5 not taken.
|
1408 | MotionRef<ColType> mout(dAdq_cols.col(k)); |
215 |
5/10✓ Branch 1 taken 704 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 704 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 704 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 704 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 704 times.
✗ Branch 14 not taken.
|
1408 | mout.linear() += model.gravity.linear().cross(min.angular()); |
216 | } | ||
217 | } | ||
218 | |||
219 |
2/2✓ Branch 0 taken 676 times.
✓ Branch 1 taken 26 times.
|
1404 | if (parent > 0) |
220 |
1/2✓ Branch 3 taken 676 times.
✗ Branch 4 not taken.
|
1352 | data.of[parent] += data.of[i]; |
221 | } | ||
222 | }; | ||
223 | |||
224 | namespace internal | ||
225 | { | ||
226 | |||
227 | template<typename Scalar> | ||
228 | struct ContactForceContribution | ||
229 | { | ||
230 | |||
231 | template< | ||
232 | int Options, | ||
233 | template<typename, int> | ||
234 | class JointCollectionTpl, | ||
235 | class ConstraintModelAllocator, | ||
236 | class ConstraintDataAllocator> | ||
237 | 26 | static void run( | |
238 | const std::vector<RigidConstraintModelTpl<Scalar, Options>, ConstraintModelAllocator> & | ||
239 | contact_models, | ||
240 | DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
241 | std::vector<RigidConstraintDataTpl<Scalar, Options>, ConstraintDataAllocator> & | ||
242 | contact_data) | ||
243 | { | ||
244 | typedef RigidConstraintModelTpl<Scalar, Options> RigidConstraintModel; | ||
245 | typedef RigidConstraintDataTpl<Scalar, Options> RigidConstraintData; | ||
246 | typedef SE3Tpl<Scalar, Options> SE3; | ||
247 | typedef ForceTpl<Scalar, Options> Force; | ||
248 | |||
249 |
1/2✓ Branch 1 taken 26 times.
✗ Branch 2 not taken.
|
26 | Force of_tmp; |
250 | |||
251 | // Add the contribution of the external forces. | ||
252 |
2/2✓ Branch 1 taken 35 times.
✓ Branch 2 taken 26 times.
|
61 | for (size_t k = 0; k < contact_models.size(); ++k) |
253 | { | ||
254 | 35 | const RigidConstraintModel & cmodel = contact_models[k]; | |
255 | 35 | const RigidConstraintData & cdata = contact_data[k]; | |
256 | |||
257 | // TODO: Temporary variable | ||
258 | 35 | const SE3 & oMc1 = cdata.oMc1; | |
259 | 35 | Force & of1 = data.of[cmodel.joint1_id]; | |
260 | 35 | const SE3 & oMc2 = cdata.oMc2; | |
261 | 35 | Force & of2 = data.of[cmodel.joint2_id]; | |
262 | |||
263 |
2/3✓ Branch 0 taken 27 times.
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
|
35 | switch (cmodel.reference_frame) |
264 | { | ||
265 | 27 | case LOCAL: { | |
266 |
2/3✓ Branch 0 taken 12 times.
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
|
27 | switch (cmodel.type) |
267 | { | ||
268 | 12 | case CONTACT_6D: { | |
269 |
2/2✓ Branch 0 taken 11 times.
✓ Branch 1 taken 1 times.
|
12 | if (cmodel.joint1_id > 0) |
270 | { | ||
271 |
2/4✓ Branch 1 taken 11 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 11 times.
✗ Branch 5 not taken.
|
11 | of1 -= oMc1.act(cdata.contact_force); |
272 | } | ||
273 |
2/2✓ Branch 0 taken 3 times.
✓ Branch 1 taken 9 times.
|
12 | if (cmodel.joint2_id > 0) |
274 | { | ||
275 |
2/4✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
|
3 | of_tmp = oMc1.act(cdata.contact_force); |
276 |
1/2✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
|
3 | of2 += of_tmp; |
277 | } | ||
278 | 12 | break; | |
279 | } | ||
280 | 15 | case CONTACT_3D: { | |
281 |
6/12✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 15 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 15 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 15 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 15 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 15 times.
✗ Branch 17 not taken.
|
15 | of_tmp.linear().noalias() = oMc1.rotation() * cdata.contact_force.linear(); |
282 | |||
283 |
2/2✓ Branch 0 taken 14 times.
✓ Branch 1 taken 1 times.
|
15 | if (cmodel.joint1_id > 0) |
284 | { | ||
285 |
4/8✓ Branch 1 taken 14 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 14 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 14 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 14 times.
✗ Branch 11 not taken.
|
14 | of1.linear().noalias() -= of_tmp.linear(); |
286 |
6/12✓ Branch 1 taken 14 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 14 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 14 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 14 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 14 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 14 times.
✗ Branch 17 not taken.
|
14 | of1.angular().noalias() -= oMc1.translation().cross(of_tmp.linear()); |
287 | } | ||
288 |
2/2✓ Branch 0 taken 4 times.
✓ Branch 1 taken 11 times.
|
15 | if (cmodel.joint2_id > 0) |
289 | { | ||
290 |
3/6✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4 times.
✗ Branch 8 not taken.
|
4 | of2.linear() += of_tmp.linear(); |
291 |
6/12✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 4 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 4 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 4 times.
✗ Branch 17 not taken.
|
4 | of2.angular().noalias() += oMc2.translation().cross(of_tmp.linear()); |
292 | } | ||
293 | 15 | break; | |
294 | } | ||
295 | ✗ | default: { | |
296 | ✗ | assert(false && "must never happen"); | |
297 | break; | ||
298 | } | ||
299 | } | ||
300 | 27 | break; | |
301 | } | ||
302 | 8 | case LOCAL_WORLD_ALIGNED: { | |
303 |
2/3✓ Branch 0 taken 4 times.
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
|
8 | switch (cmodel.type) |
304 | { | ||
305 | 4 | case CONTACT_6D: { | |
306 |
1/2✓ Branch 0 taken 4 times.
✗ Branch 1 not taken.
|
4 | if (cmodel.joint1_id > 0) |
307 | { | ||
308 |
1/2✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
|
4 | of1 -= cdata.contact_force; |
309 |
6/12✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 4 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 4 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 4 times.
✗ Branch 17 not taken.
|
4 | of1.angular().noalias() -= oMc1.translation().cross(cdata.contact_force.linear()); |
310 | } | ||
311 |
2/2✓ Branch 0 taken 1 times.
✓ Branch 1 taken 3 times.
|
4 | if (cmodel.joint2_id > 0) |
312 | { | ||
313 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | of2 += cdata.contact_force; |
314 |
6/12✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
|
1 | of2.angular().noalias() += oMc1.translation().cross(cdata.contact_force.linear()); |
315 | } | ||
316 | 4 | break; | |
317 | } | ||
318 | 4 | case CONTACT_3D: { | |
319 |
1/2✓ Branch 0 taken 4 times.
✗ Branch 1 not taken.
|
4 | if (cmodel.joint1_id > 0) |
320 | { | ||
321 |
3/6✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4 times.
✗ Branch 8 not taken.
|
4 | of1.linear() -= cdata.contact_force.linear(); |
322 |
6/12✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 4 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 4 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 4 times.
✗ Branch 17 not taken.
|
4 | of1.angular().noalias() -= oMc1.translation().cross(cdata.contact_force.linear()); |
323 | } | ||
324 |
2/2✓ Branch 0 taken 1 times.
✓ Branch 1 taken 3 times.
|
4 | if (cmodel.joint2_id > 0) |
325 | { | ||
326 |
3/6✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
|
1 | of2.linear() += cdata.contact_force.linear(); |
327 |
6/12✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
|
1 | of2.angular().noalias() += oMc2.translation().cross(cdata.contact_force.linear()); |
328 | } | ||
329 | 4 | break; | |
330 | } | ||
331 | ✗ | default: { | |
332 | ✗ | assert(false && "must never happen"); | |
333 | break; | ||
334 | } | ||
335 | } | ||
336 | 8 | break; | |
337 | } | ||
338 | ✗ | default: | |
339 | ✗ | assert(false && "Should never happen"); | |
340 | break; | ||
341 | } | ||
342 | } | ||
343 | 26 | } | |
344 | }; | ||
345 | } // namespace internal | ||
346 | |||
347 | template< | ||
348 | typename Scalar, | ||
349 | int Options, | ||
350 | template<typename, int> | ||
351 | class JointCollectionTpl, | ||
352 | class ConstraintModelAllocator, | ||
353 | class ConstraintDataAllocator, | ||
354 | typename MatrixType1, | ||
355 | typename MatrixType2, | ||
356 | typename MatrixType3, | ||
357 | typename MatrixType4, | ||
358 | typename MatrixType5, | ||
359 | typename MatrixType6> | ||
360 | 44 | inline void computeConstraintDynamicsDerivatives( | |
361 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
362 | DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
363 | const std::vector<RigidConstraintModelTpl<Scalar, Options>, ConstraintModelAllocator> & | ||
364 | contact_models, | ||
365 | std::vector<RigidConstraintDataTpl<Scalar, Options>, ConstraintDataAllocator> & contact_data, | ||
366 | const ProximalSettingsTpl<Scalar> & settings, | ||
367 | const Eigen::MatrixBase<MatrixType1> & ddq_partial_dq, | ||
368 | const Eigen::MatrixBase<MatrixType2> & ddq_partial_dv, | ||
369 | const Eigen::MatrixBase<MatrixType3> & ddq_partial_dtau, | ||
370 | const Eigen::MatrixBase<MatrixType4> & lambda_partial_dq, | ||
371 | const Eigen::MatrixBase<MatrixType5> & lambda_partial_dv, | ||
372 | const Eigen::MatrixBase<MatrixType6> & lambda_partial_dtau) | ||
373 | { | ||
374 |
1/2✓ Branch 1 taken 22 times.
✗ Branch 2 not taken.
|
44 | const Eigen::DenseIndex & nc = data.contact_chol.constraintDim(); |
375 | |||
376 |
1/4✗ Branch 2 not taken.
✓ Branch 3 taken 22 times.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
|
44 | PINOCCHIO_CHECK_INPUT_ARGUMENT( |
377 | contact_data.size() == contact_models.size(), | ||
378 | "contact_data and contact_models do not have the same size"); | ||
379 | |||
380 |
1/4✗ Branch 1 not taken.
✓ Branch 2 taken 22 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
|
44 | PINOCCHIO_CHECK_INPUT_ARGUMENT(ddq_partial_dq.cols() == model.nv); |
381 |
1/4✗ Branch 1 not taken.
✓ Branch 2 taken 22 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
|
44 | PINOCCHIO_CHECK_INPUT_ARGUMENT(ddq_partial_dq.rows() == model.nv); |
382 | |||
383 |
1/4✗ Branch 1 not taken.
✓ Branch 2 taken 22 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
|
44 | PINOCCHIO_CHECK_INPUT_ARGUMENT(ddq_partial_dv.cols() == model.nv); |
384 |
1/4✗ Branch 1 not taken.
✓ Branch 2 taken 22 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
|
44 | PINOCCHIO_CHECK_INPUT_ARGUMENT(ddq_partial_dv.rows() == model.nv); |
385 | |||
386 |
1/4✗ Branch 1 not taken.
✓ Branch 2 taken 22 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
|
44 | PINOCCHIO_CHECK_INPUT_ARGUMENT(ddq_partial_dtau.cols() == model.nv); |
387 |
1/4✗ Branch 1 not taken.
✓ Branch 2 taken 22 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
|
44 | PINOCCHIO_CHECK_INPUT_ARGUMENT(ddq_partial_dtau.rows() == model.nv); |
388 | |||
389 |
1/4✗ Branch 1 not taken.
✓ Branch 2 taken 22 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
|
44 | PINOCCHIO_CHECK_INPUT_ARGUMENT(lambda_partial_dq.cols() == model.nv); |
390 |
1/4✗ Branch 1 not taken.
✓ Branch 2 taken 22 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
|
44 | PINOCCHIO_CHECK_INPUT_ARGUMENT(lambda_partial_dq.rows() == nc); |
391 | |||
392 |
1/4✗ Branch 1 not taken.
✓ Branch 2 taken 22 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
|
44 | PINOCCHIO_CHECK_INPUT_ARGUMENT(lambda_partial_dv.cols() == model.nv); |
393 |
1/4✗ Branch 1 not taken.
✓ Branch 2 taken 22 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
|
44 | PINOCCHIO_CHECK_INPUT_ARGUMENT(lambda_partial_dv.rows() == nc); |
394 | |||
395 |
1/4✗ Branch 1 not taken.
✓ Branch 2 taken 22 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
|
44 | PINOCCHIO_CHECK_INPUT_ARGUMENT(lambda_partial_dtau.cols() == model.nv); |
396 |
1/4✗ Branch 1 not taken.
✓ Branch 2 taken 22 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
|
44 | PINOCCHIO_CHECK_INPUT_ARGUMENT(lambda_partial_dtau.rows() == nc); |
397 | |||
398 |
11/24✓ Branch 1 taken 22 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 22 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 22 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 22 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 22 times.
✗ Branch 13 not taken.
✓ Branch 14 taken 22 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 22 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 22 times.
✗ Branch 21 not taken.
✓ Branch 22 taken 22 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 22 times.
✗ Branch 26 not taken.
✗ Branch 27 not taken.
✓ Branch 28 taken 22 times.
✗ Branch 31 not taken.
✗ Branch 32 not taken.
|
44 | PINOCCHIO_CHECK_INPUT_ARGUMENT( |
399 | check_expression_if_real<Scalar>( | ||
400 | model.gravity.angular()[0] == Scalar(0) && model.gravity.angular()[1] == Scalar(0) | ||
401 | && model.gravity.angular()[2] == Scalar(0)), | ||
402 | "The gravity must be a pure force vector, no angular part"); | ||
403 | |||
404 |
2/4✓ Branch 1 taken 22 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 22 times.
✗ Branch 4 not taken.
|
44 | assert(model.check(data) && "data is not consistent with model."); |
405 | |||
406 | // TODO: User should make sure the internal quantities are reset. | ||
407 |
1/2✓ Branch 1 taken 22 times.
✗ Branch 2 not taken.
|
44 | data.dtau_dq.setZero(); |
408 |
1/2✓ Branch 1 taken 22 times.
✗ Branch 2 not taken.
|
44 | data.dtau_dv.setZero(); |
409 |
1/2✓ Branch 1 taken 22 times.
✗ Branch 2 not taken.
|
44 | data.dac_dq.setZero(); |
410 |
1/2✓ Branch 1 taken 22 times.
✗ Branch 2 not taken.
|
44 | data.dac_dv.setZero(); |
411 | |||
412 | typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model; | ||
413 | typedef DataTpl<Scalar, Options, JointCollectionTpl> Data; | ||
414 | |||
415 | typedef RigidConstraintModelTpl<Scalar, Options> RigidConstraintModel; | ||
416 | typedef RigidConstraintDataTpl<Scalar, Options> RigidConstraintData; | ||
417 | |||
418 | typedef typename ModelTpl<Scalar, Options, JointCollectionTpl>::JointIndex JointIndex; | ||
419 | typedef typename Data::SE3 SE3; | ||
420 | typedef typename Data::Motion Motion; | ||
421 | typedef typename Data::Force Force; | ||
422 |
2/4✓ Branch 1 taken 22 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 22 times.
✗ Branch 6 not taken.
|
44 | data.oa_gf[0] = -model.gravity; |
423 | |||
424 | // TODO: Temp variable | ||
425 |
1/2✓ Branch 1 taken 22 times.
✗ Branch 2 not taken.
|
44 | Motion a_tmp; |
426 | |||
427 | typedef ComputeConstraintDynamicsDerivativesForwardStep< | ||
428 | Scalar, Options, JointCollectionTpl, true> | ||
429 | Pass1; | ||
430 |
2/2✓ Branch 0 taken 594 times.
✓ Branch 1 taken 22 times.
|
1232 | for (JointIndex i = 1; i < (JointIndex)model.njoints; ++i) |
431 | { | ||
432 |
2/4✓ Branch 1 taken 594 times.
✗ Branch 2 not taken.
✓ Branch 6 taken 594 times.
✗ Branch 7 not taken.
|
1188 | Pass1::run(model.joints[i], data.joints[i], typename Pass1::ArgsType(model, data)); |
433 | } | ||
434 | |||
435 |
1/2✓ Branch 1 taken 22 times.
✗ Branch 2 not taken.
|
44 | internal::ContactForceContribution<Scalar>::run(contact_models, data, contact_data); |
436 | |||
437 | // Backward Pass | ||
438 | typedef ComputeContactDynamicDerivativesBackwardStep<Scalar, Options, JointCollectionTpl, true> | ||
439 | Pass2; | ||
440 |
2/2✓ Branch 0 taken 594 times.
✓ Branch 1 taken 22 times.
|
1232 | for (JointIndex i = (JointIndex)(model.njoints - 1); i > 0; --i) |
441 | { | ||
442 |
2/4✓ Branch 1 taken 594 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 594 times.
✗ Branch 6 not taken.
|
1188 | Pass2::run(model.joints[i], typename Pass2::ArgsType(model, data)); |
443 | } | ||
444 | |||
445 | // Compute the contact frame partial derivatives | ||
446 |
1/2✓ Branch 1 taken 22 times.
✗ Branch 2 not taken.
|
44 | typename Data::SE3::Matrix6 Jlog; |
447 | 44 | Eigen::DenseIndex current_row_sol_id = 0; | |
448 |
2/2✓ Branch 1 taken 29 times.
✓ Branch 2 taken 22 times.
|
102 | for (size_t k = 0; k < contact_models.size(); ++k) |
449 | { | ||
450 | typedef typename RigidConstraintModel::BooleanVector BooleanVector; | ||
451 | |||
452 | 58 | const RigidConstraintModel & cmodel = contact_models[k]; | |
453 | 58 | RigidConstraintData & cdata = contact_data[k]; | |
454 | // const BooleanVector & joint1_indexes = cmodel.colwise_joint1_sparsity; | ||
455 | 58 | const BooleanVector & joint2_indexes = cmodel.colwise_joint2_sparsity; | |
456 | |||
457 |
2/3✓ Branch 0 taken 13 times.
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
|
58 | switch (cmodel.type) |
458 | { | ||
459 | 26 | case CONTACT_6D: { | |
460 | typedef | ||
461 | typename SizeDepType<6>::template RowsReturn<typename Data::MatrixXs>::Type RowsBlock; | ||
462 |
1/2✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
|
26 | RowsBlock contact_dvc_dq = SizeDepType<6>::middleRows(data.dvc_dq, current_row_sol_id); |
463 |
1/2✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
|
26 | RowsBlock contact_dac_dq = SizeDepType<6>::middleRows(data.dac_dq, current_row_sol_id); |
464 |
1/2✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
|
26 | RowsBlock contact_dac_dv = SizeDepType<6>::middleRows(data.dac_dv, current_row_sol_id); |
465 |
1/2✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
|
26 | RowsBlock contact_dac_da = SizeDepType<6>::middleRows(data.dac_da, current_row_sol_id); |
466 | |||
467 |
2/2✓ Branch 0 taken 12 times.
✓ Branch 1 taken 1 times.
|
26 | if (cmodel.joint1_id > 0) |
468 | { | ||
469 |
1/2✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
|
24 | cdata.dv1_dq.setZero(); |
470 |
1/2✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
|
24 | cdata.da1_dq.setZero(); |
471 |
1/2✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
|
24 | cdata.da1_dv.setZero(); |
472 |
1/2✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
|
24 | cdata.da1_da.setZero(); |
473 | |||
474 | 24 | getFrameAccelerationDerivatives( | |
475 | 24 | model, data, cmodel.joint1_id, cmodel.joint1_placement, cmodel.reference_frame, | |
476 |
1/2✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
|
24 | cdata.dv1_dq, cdata.da1_dq, cdata.da1_dv, cdata.da1_da); |
477 | |||
478 |
1/2✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
|
24 | contact_dvc_dq = cdata.dv1_dq; |
479 |
1/2✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
|
24 | contact_dac_dq = cdata.da1_dq; |
480 |
1/2✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
|
24 | contact_dac_dv = cdata.da1_dv; |
481 |
1/2✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
|
24 | contact_dac_da = cdata.da1_da; |
482 | } | ||
483 | |||
484 |
2/2✓ Branch 0 taken 4 times.
✓ Branch 1 taken 9 times.
|
26 | if (cmodel.joint2_id > 0) |
485 | { | ||
486 |
1/2✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
|
8 | cdata.dv2_dq.setZero(); |
487 |
1/2✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
|
8 | cdata.da2_dq.setZero(); |
488 |
1/2✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
|
8 | cdata.da2_dv.setZero(); |
489 |
1/2✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
|
8 | cdata.da2_da.setZero(); |
490 |
2/4✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
|
8 | const SE3 joint2_M_c1 = cmodel.joint2_placement * cdata.c1Mc2.inverse(); |
491 | |||
492 | 8 | getFrameAccelerationDerivatives( | |
493 | 8 | model, data, cmodel.joint2_id, joint2_M_c1, | |
494 | // cmodel.joint2_placement, | ||
495 |
1/2✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
|
8 | cmodel.reference_frame, cdata.dv2_dq, cdata.da2_dq, cdata.da2_dv, cdata.da2_da); |
496 | |||
497 | // TODO: This is only in case reference_frame is LOCAL | ||
498 | // TODO(jcarpent):to do colwise | ||
499 |
1/2✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
|
8 | contact_dvc_dq -= cdata.dv2_dq; |
500 |
1/2✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
|
8 | contact_dac_dq -= cdata.da2_dq; |
501 |
1/2✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
|
8 | contact_dac_dv -= cdata.da2_dv; |
502 |
1/2✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
|
8 | contact_dac_da -= cdata.da2_da; |
503 | |||
504 |
1/2✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
|
8 | const Motion v2_in_c1 = cdata.c1Mc2.act(cdata.contact2_velocity); |
505 |
1/2✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
|
8 | const Motion a2_in_c1 = cdata.oMc1.actInv(data.oa[cmodel.joint2_id]); |
506 | |||
507 | 8 | Eigen::DenseIndex k = Eigen::DenseIndex(cmodel.colwise_span_indexes.size()) - 1; | |
508 | 8 | Eigen::DenseIndex col_id(0); | |
509 |
5/6✓ Branch 0 taken 54 times.
✓ Branch 1 taken 1 times.
✓ Branch 3 taken 54 times.
✗ Branch 4 not taken.
✓ Branch 5 taken 54 times.
✓ Branch 6 taken 1 times.
|
110 | while (cmodel.reference_frame == LOCAL && cmodel.colwise_span_indexes.size() > 0) |
510 | { | ||
511 |
2/2✓ Branch 0 taken 35 times.
✓ Branch 1 taken 19 times.
|
108 | if (k >= 0) |
512 | { | ||
513 | 70 | col_id = cmodel.colwise_span_indexes[size_t(k)]; | |
514 | 70 | k--; | |
515 | } | ||
516 | else | ||
517 | { | ||
518 | 38 | col_id = data.parents_fromRow[size_t(col_id)]; | |
519 |
2/2✓ Branch 0 taken 3 times.
✓ Branch 1 taken 16 times.
|
38 | if (col_id < 0) |
520 | 6 | break; | |
521 | } | ||
522 | |||
523 |
2/4✓ Branch 1 taken 51 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 51 times.
✗ Branch 5 not taken.
|
102 | const MotionRef<typename RowsBlock::ColXpr> dvc_dv_col(contact_dac_da.col(col_id)); |
524 |
1/2✓ Branch 1 taken 51 times.
✗ Branch 2 not taken.
|
102 | const MotionRef<typename RigidConstraintData::Matrix6x::ColXpr> da2_da_col( |
525 |
1/2✓ Branch 1 taken 51 times.
✗ Branch 2 not taken.
|
102 | cdata.da2_da.col(col_id)); |
526 |
1/2✓ Branch 1 taken 51 times.
✗ Branch 2 not taken.
|
102 | const MotionRef<typename RigidConstraintData::Matrix6x::ColXpr> dv2_dq_col( |
527 |
1/2✓ Branch 1 taken 51 times.
✗ Branch 2 not taken.
|
102 | cdata.dv2_dq.col(col_id)); |
528 | |||
529 | // dv/dq | ||
530 |
1/2✓ Branch 1 taken 51 times.
✗ Branch 2 not taken.
|
102 | const Motion v2_in_c1_cross_dvc_dv_col = v2_in_c1.cross(dvc_dv_col); |
531 |
3/6✓ Branch 1 taken 51 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 51 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 51 times.
✗ Branch 8 not taken.
|
102 | contact_dvc_dq.col(col_id) -= v2_in_c1_cross_dvc_dv_col.toVector(); |
532 | |||
533 | // da/dv | ||
534 |
3/6✓ Branch 1 taken 51 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 51 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 51 times.
✗ Branch 8 not taken.
|
102 | contact_dac_dv.col(col_id) -= v2_in_c1_cross_dvc_dv_col.toVector(); |
535 |
4/8✓ Branch 1 taken 51 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 51 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 51 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 51 times.
✗ Branch 11 not taken.
|
102 | contact_dac_dv.col(col_id) += cdata.contact_velocity_error.cross(da2_da_col).toVector(); |
536 | |||
537 | // da/dq | ||
538 |
2/4✓ Branch 1 taken 51 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 51 times.
✗ Branch 5 not taken.
|
102 | const MotionRef<typename RowsBlock::ColXpr> dvc_dq_col(contact_dvc_dq.col(col_id)); |
539 | |||
540 |
4/8✓ Branch 1 taken 51 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 51 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 51 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 51 times.
✗ Branch 11 not taken.
|
102 | contact_dac_dq.col(col_id) -= a2_in_c1.cross(dvc_dv_col).toVector(); |
541 |
4/8✓ Branch 1 taken 51 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 51 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 51 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 51 times.
✗ Branch 11 not taken.
|
102 | contact_dac_dq.col(col_id) -= v2_in_c1.cross(dvc_dq_col).toVector(); |
542 |
1/2✓ Branch 1 taken 51 times.
✗ Branch 2 not taken.
|
102 | contact_dac_dq.col(col_id) += |
543 |
4/8✓ Branch 1 taken 51 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 51 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 51 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 51 times.
✗ Branch 11 not taken.
|
204 | cdata.contact_velocity_error.cross(v2_in_c1_cross_dvc_dv_col + dv2_dq_col).toVector(); |
544 | } | ||
545 | |||
546 |
1/2✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
|
8 | cdata.dvc_dq = contact_dvc_dq; |
547 |
1/2✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
|
8 | cdata.dac_dq = contact_dac_dq; |
548 |
1/2✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
|
8 | cdata.dac_dv = contact_dac_dv; |
549 |
1/2✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
|
8 | cdata.dac_da = contact_dac_da; |
550 | } | ||
551 | |||
552 | 26 | break; | |
553 | } | ||
554 | 32 | case CONTACT_3D: { | |
555 | typedef | ||
556 | typename SizeDepType<3>::template RowsReturn<typename Data::MatrixXs>::Type RowsBlock; | ||
557 | |||
558 |
1/2✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
|
32 | RowsBlock contact_dvc_dq = SizeDepType<3>::middleRows(data.dvc_dq, current_row_sol_id); |
559 |
1/2✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
|
32 | RowsBlock contact_dac_dq = SizeDepType<3>::middleRows(data.dac_dq, current_row_sol_id); |
560 |
1/2✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
|
32 | RowsBlock contact_dac_dv = SizeDepType<3>::middleRows(data.dac_dv, current_row_sol_id); |
561 |
1/2✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
|
32 | RowsBlock contact_dac_da = SizeDepType<3>::middleRows(data.dac_da, current_row_sol_id); |
562 | |||
563 |
2/2✓ Branch 0 taken 15 times.
✓ Branch 1 taken 1 times.
|
32 | if (cmodel.joint1_id > 0) |
564 | { | ||
565 |
1/2✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
|
30 | cdata.dv1_dq.setZero(); |
566 |
1/2✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
|
30 | cdata.da1_dq.setZero(); |
567 |
1/2✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
|
30 | cdata.da1_dv.setZero(); |
568 |
1/2✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
|
30 | cdata.da1_da.setZero(); |
569 | |||
570 | 30 | getPointClassicAccelerationDerivatives( | |
571 |
1/2✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
|
30 | model, data, cmodel.joint1_id, cmodel.joint1_placement, cmodel.reference_frame, |
572 |
2/4✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 15 times.
✗ Branch 5 not taken.
|
30 | cdata.dv1_dq.template bottomRows<3>(), cdata.da1_dq.template bottomRows<3>(), |
573 |
2/4✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 15 times.
✗ Branch 5 not taken.
|
30 | cdata.da1_dv.template bottomRows<3>(), cdata.da1_da.template bottomRows<3>()); |
574 | |||
575 |
2/4✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 15 times.
✗ Branch 5 not taken.
|
30 | contact_dvc_dq = cdata.dv1_dq.template bottomRows<3>(); |
576 |
2/4✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 15 times.
✗ Branch 5 not taken.
|
30 | contact_dac_dq = cdata.da1_dq.template bottomRows<3>(); |
577 |
2/4✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 15 times.
✗ Branch 5 not taken.
|
30 | contact_dac_dv = cdata.da1_dv.template bottomRows<3>(); |
578 |
2/4✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 15 times.
✗ Branch 5 not taken.
|
30 | contact_dac_da = cdata.da1_da.template bottomRows<3>(); |
579 | } | ||
580 | |||
581 |
2/2✓ Branch 0 taken 5 times.
✓ Branch 1 taken 11 times.
|
32 | if (cmodel.joint2_id > 0) |
582 | { | ||
583 |
1/2✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
|
10 | cdata.dv2_dq.setZero(); |
584 |
1/2✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
|
10 | cdata.da2_dq.setZero(); |
585 |
1/2✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
|
10 | cdata.da2_dv.setZero(); |
586 |
1/2✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
|
10 | cdata.da2_da.setZero(); |
587 |
1/2✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
|
10 | const SE3 joint2_M_c1( |
588 |
3/6✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 5 times.
✗ Branch 8 not taken.
|
10 | cmodel.joint2_placement.rotation() * cdata.c1Mc2.rotation().transpose(), |
589 |
2/4✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
|
10 | cmodel.joint2_placement.translation()); |
590 | |||
591 | 10 | getPointClassicAccelerationDerivatives( | |
592 |
1/2✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
|
10 | model, data, cmodel.joint2_id, joint2_M_c1, cmodel.reference_frame, |
593 |
2/4✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
|
10 | cdata.dv2_dq.template bottomRows<3>(), cdata.da2_dq.template bottomRows<3>(), |
594 |
2/4✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
|
10 | cdata.da2_dv.template bottomRows<3>(), cdata.da2_da.template bottomRows<3>()); |
595 | |||
596 | // TODO: This is only in case reference_frame is LOCAL | ||
597 |
2/4✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
|
10 | contact_dvc_dq -= cdata.dv2_dq.template bottomRows<3>(); |
598 |
2/4✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
|
10 | contact_dac_dq -= cdata.da2_dq.template bottomRows<3>(); |
599 |
2/4✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
|
10 | contact_dac_dv -= cdata.da2_dv.template bottomRows<3>(); |
600 |
2/4✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
|
10 | contact_dac_da -= cdata.da2_da.template bottomRows<3>(); |
601 | } | ||
602 | 32 | break; | |
603 | } | ||
604 | ✗ | default: | |
605 | ✗ | assert(false && "must never happen"); | |
606 | break; | ||
607 | } | ||
608 | |||
609 |
1/2✓ Branch 1 taken 29 times.
✗ Branch 2 not taken.
|
58 | assert( |
610 | cmodel.loop_span_indexes.size() > 0 | ||
611 | && "Must never happened, the sparsity pattern is empty"); | ||
612 | // Derivative of closed loop kinematic tree | ||
613 |
2/2✓ Branch 0 taken 9 times.
✓ Branch 1 taken 20 times.
|
58 | if (cmodel.joint2_id > 0) |
614 | { | ||
615 |
2/3✓ Branch 0 taken 4 times.
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
|
18 | switch (cmodel.type) |
616 | { | ||
617 | 8 | case CONTACT_6D: { | |
618 | // TODO: THIS IS FOR THE LOCAL FRAME ONLY | ||
619 | 8 | const typename Model::JointIndex joint2_id = cmodel.joint2_id; | |
620 | 8 | const Eigen::DenseIndex colRef2 = | |
621 |
2/4✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 4 times.
✗ Branch 7 not taken.
|
8 | nv(model.joints[joint2_id]) + idx_v(model.joints[joint2_id]) - 1; |
622 | |||
623 |
1/2✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
|
8 | Force contact_force_in_WORLD; |
624 |
2/3✓ Branch 0 taken 3 times.
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
8 | switch (cmodel.reference_frame) |
625 | { | ||
626 | 6 | case LOCAL: { | |
627 |
2/4✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
|
6 | contact_force_in_WORLD = cdata.oMc1.act(cdata.contact_force); |
628 | 6 | break; | |
629 | } | ||
630 | 2 | case LOCAL_WORLD_ALIGNED: { | |
631 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | contact_force_in_WORLD = cdata.contact_force; |
632 |
4/8✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
|
2 | contact_force_in_WORLD.angular().noalias() += |
633 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | cdata.oMc1.translation().cross(cdata.contact_force.linear()); |
634 | 2 | break; | |
635 | } | ||
636 | ✗ | default: { | |
637 | ✗ | assert(false && "must never happen"); | |
638 | break; | ||
639 | } | ||
640 | } | ||
641 | |||
642 | // d./dq | ||
643 |
2/2✓ Branch 1 taken 45 times.
✓ Branch 2 taken 4 times.
|
98 | for (Eigen::DenseIndex k = 0; k < Eigen::DenseIndex(cmodel.colwise_span_indexes.size()); |
644 | ++k) | ||
645 | { | ||
646 | 90 | const Eigen::DenseIndex col_id = cmodel.colwise_span_indexes[size_t(k)]; | |
647 | |||
648 |
2/4✓ Branch 1 taken 45 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 45 times.
✗ Branch 5 not taken.
|
90 | const MotionRef<typename Data::Matrix6x::ColXpr> J_col(data.J.col(col_id)); |
649 |
1/2✓ Branch 1 taken 45 times.
✗ Branch 2 not taken.
|
90 | const Force J_col_cross_contact_force_in_WORLD = J_col.cross(contact_force_in_WORLD); |
650 |
2/2✓ Branch 1 taken 597 times.
✓ Branch 2 taken 45 times.
|
1284 | for (Eigen::DenseIndex j = colRef2; j >= 0; j = data.parents_fromRow[(size_t)j]) |
651 | { | ||
652 |
3/4✓ Branch 1 taken 597 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 383 times.
✓ Branch 4 taken 214 times.
|
1194 | if (joint2_indexes[col_id]) |
653 | { | ||
654 | 1532 | data.dtau_dq(j, col_id) -= | |
655 |
4/8✓ Branch 1 taken 383 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 383 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 383 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 383 times.
✗ Branch 11 not taken.
|
766 | data.J.col(j).dot(J_col_cross_contact_force_in_WORLD.toVector()); |
656 | } | ||
657 | else | ||
658 | { | ||
659 | 856 | data.dtau_dq(j, col_id) += | |
660 |
4/8✓ Branch 1 taken 214 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 214 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 214 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 214 times.
✗ Branch 11 not taken.
|
428 | data.J.col(j).dot(J_col_cross_contact_force_in_WORLD.toVector()); |
661 | } | ||
662 | } | ||
663 | } | ||
664 | 8 | break; | |
665 | } | ||
666 | 10 | case CONTACT_3D: { | |
667 | |||
668 | typedef | ||
669 | typename SizeDepType<3>::template RowsReturn<typename Data::MatrixXs>::Type RowsBlock; | ||
670 |
1/2✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
|
10 | RowsBlock contact_dac_dq = SizeDepType<3>::middleRows(data.dac_dq, current_row_sol_id); |
671 | 10 | const typename Model::JointIndex joint2_id = cmodel.joint2_id; | |
672 | 10 | const Eigen::DenseIndex colRef2 = | |
673 |
2/4✓ Branch 2 taken 5 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 5 times.
✗ Branch 7 not taken.
|
10 | nv(model.joints[joint2_id]) + idx_v(model.joints[joint2_id]) - 1; |
674 | |||
675 |
2/4✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
|
10 | Force of_tmp, of_tmp2; // temporary Force variables |
676 | |||
677 |
2/3✓ Branch 0 taken 4 times.
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
10 | switch (cmodel.reference_frame) |
678 | { | ||
679 | 8 | case LOCAL: { | |
680 |
6/12✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 4 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 4 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 4 times.
✗ Branch 17 not taken.
|
8 | of_tmp.linear().noalias() = cdata.oMc1.rotation() * cdata.contact_force.linear(); |
681 | 16 | const Motion & c2_acc_c2 = getFrameClassicalAcceleration( | |
682 |
1/2✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
|
8 | model, data, cmodel.joint2_id, cmodel.joint2_placement, cmodel.reference_frame); |
683 |
6/12✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 4 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 4 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 4 times.
✗ Branch 17 not taken.
|
8 | a_tmp.angular().noalias() = cdata.oMc2.rotation() * c2_acc_c2.linear(); |
684 | 8 | break; | |
685 | } | ||
686 | 2 | case LOCAL_WORLD_ALIGNED: { | |
687 |
3/6✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
|
2 | of_tmp.linear() = cdata.contact_force.linear(); |
688 | 2 | break; | |
689 | } | ||
690 | ✗ | default: { | |
691 | ✗ | assert(false && "must never happen"); | |
692 | break; | ||
693 | } | ||
694 | } | ||
695 | |||
696 | // d./dq | ||
697 |
2/2✓ Branch 1 taken 217 times.
✓ Branch 2 taken 5 times.
|
444 | for (Eigen::DenseIndex k = 0; k < Eigen::DenseIndex(cmodel.loop_span_indexes.size()); ++k) |
698 | { | ||
699 | 434 | const Eigen::DenseIndex col_id = cmodel.loop_span_indexes[size_t(k)]; | |
700 | |||
701 |
2/4✓ Branch 1 taken 217 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 217 times.
✗ Branch 5 not taken.
|
434 | const MotionRef<typename Data::Matrix6x::ColXpr> J_col(data.J.col(col_id)); |
702 | |||
703 |
2/3✓ Branch 0 taken 175 times.
✓ Branch 1 taken 42 times.
✗ Branch 2 not taken.
|
434 | switch (cmodel.reference_frame) |
704 | { | ||
705 | 350 | case LOCAL: { | |
706 |
6/12✓ Branch 1 taken 175 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 175 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 175 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 175 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 175 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 175 times.
✗ Branch 17 not taken.
|
350 | a_tmp.linear().noalias() = a_tmp.angular().cross(J_col.angular()); |
707 |
3/4✓ Branch 1 taken 175 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 158 times.
✓ Branch 4 taken 17 times.
|
350 | if (joint2_indexes[col_id]) |
708 | { | ||
709 |
3/6✓ Branch 1 taken 158 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 158 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 158 times.
✗ Branch 8 not taken.
|
316 | contact_dac_dq.col(col_id).noalias() += |
710 |
4/8✓ Branch 1 taken 158 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 158 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 158 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 158 times.
✗ Branch 11 not taken.
|
632 | cdata.oMc1.rotation().transpose() * a_tmp.linear(); |
711 | } | ||
712 | else | ||
713 | { | ||
714 |
3/6✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 17 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 17 times.
✗ Branch 8 not taken.
|
34 | contact_dac_dq.col(col_id).noalias() -= |
715 |
4/8✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 17 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 17 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 17 times.
✗ Branch 11 not taken.
|
68 | cdata.oMc1.rotation().transpose() * a_tmp.linear(); |
716 | } | ||
717 | 350 | break; | |
718 | } | ||
719 | 84 | case LOCAL_WORLD_ALIGNED: { | |
720 | // Do nothing | ||
721 | 84 | break; | |
722 | } | ||
723 | ✗ | default: { | |
724 | ✗ | assert(false && "must never happen"); | |
725 | break; | ||
726 | } | ||
727 | } | ||
728 | |||
729 |
6/12✓ Branch 1 taken 217 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 217 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 217 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 217 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 217 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 217 times.
✗ Branch 17 not taken.
|
434 | of_tmp2.linear().noalias() = of_tmp.linear().cross(J_col.angular()); |
730 |
2/2✓ Branch 1 taken 2733 times.
✓ Branch 2 taken 217 times.
|
5900 | for (Eigen::DenseIndex j = colRef2; j >= 0; j = data.parents_fromRow[(size_t)j]) |
731 | { | ||
732 |
2/4✓ Branch 1 taken 2733 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2733 times.
✗ Branch 5 not taken.
|
5466 | const MotionRef<typename Data::Matrix6x::ColXpr> J2_col(data.J.col(j)); |
733 | |||
734 | // Temporary assignment | ||
735 |
5/10✓ Branch 1 taken 2733 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2733 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2733 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 2733 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 2733 times.
✗ Branch 14 not taken.
|
10932 | of_tmp2.angular().noalias() = |
736 |
3/6✓ Branch 1 taken 2733 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2733 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2733 times.
✗ Branch 8 not taken.
|
10932 | J2_col.linear() - cdata.oMc2.translation().cross(J2_col.angular()); |
737 |
3/4✓ Branch 1 taken 2733 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 2459 times.
✓ Branch 4 taken 274 times.
|
5466 | if (joint2_indexes[col_id]) |
738 | { | ||
739 |
4/8✓ Branch 1 taken 2459 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2459 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2459 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 2459 times.
✗ Branch 11 not taken.
|
4918 | data.dtau_dq(j, col_id) += of_tmp2.angular().dot(of_tmp2.linear()); |
740 | } | ||
741 | else | ||
742 | { | ||
743 |
4/8✓ Branch 1 taken 274 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 274 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 274 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 274 times.
✗ Branch 11 not taken.
|
548 | data.dtau_dq(j, col_id) -= of_tmp2.angular().dot(of_tmp2.linear()); |
744 | } | ||
745 | } | ||
746 | } | ||
747 | 10 | break; | |
748 | } | ||
749 | ✗ | default: { | |
750 | ✗ | assert(false && "must never happen"); | |
751 | break; | ||
752 | } | ||
753 | } | ||
754 | } | ||
755 | |||
756 | // Add the contribution of the corrector | ||
757 | 58 | if ( | |
758 |
2/4✓ Branch 1 taken 29 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 29 times.
✗ Branch 5 not taken.
|
58 | check_expression_if_real<Scalar>(!isZero(cmodel.corrector.Kp, static_cast<Scalar>(0.))) |
759 |
8/10✓ Branch 0 taken 6 times.
✓ Branch 1 taken 23 times.
✓ Branch 3 taken 6 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 6 times.
✗ Branch 7 not taken.
✓ Branch 8 taken 2 times.
✓ Branch 9 taken 4 times.
✓ Branch 10 taken 25 times.
✓ Branch 11 taken 4 times.
|
58 | || check_expression_if_real<Scalar>(!isZero(cmodel.corrector.Kd, static_cast<Scalar>(0.)))) |
760 | { | ||
761 |
2/4✓ Branch 1 taken 25 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 25 times.
✗ Branch 5 not taken.
|
50 | Jlog6(cdata.c1Mc2.inverse(), Jlog); |
762 | |||
763 |
2/3✓ Branch 0 taken 10 times.
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
|
50 | switch (cmodel.type) |
764 | { | ||
765 | 20 | case CONTACT_6D: { | |
766 | typedef | ||
767 | typename SizeDepType<6>::template RowsReturn<typename Data::MatrixXs>::Type RowsBlock; | ||
768 | const RowsBlock contact_dvc_dq = | ||
769 |
1/2✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
|
20 | SizeDepType<6>::middleRows(data.dvc_dq, current_row_sol_id); |
770 |
1/2✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
|
20 | RowsBlock contact_dac_dq = SizeDepType<6>::middleRows(data.dac_dq, current_row_sol_id); |
771 |
1/2✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
|
20 | RowsBlock contact_dac_dv = SizeDepType<6>::middleRows(data.dac_dv, current_row_sol_id); |
772 | const RowsBlock contact_dac_da = | ||
773 |
1/2✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
|
20 | SizeDepType<6>::middleRows(data.dac_da, current_row_sol_id); |
774 |
3/6✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 10 times.
✗ Branch 8 not taken.
|
20 | contact_dac_dq += cmodel.corrector.Kd.asDiagonal() * contact_dvc_dq; |
775 |
3/6✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 10 times.
✗ Branch 8 not taken.
|
20 | contact_dac_dv += cmodel.corrector.Kd.asDiagonal() * contact_dac_da; |
776 | // d./dq | ||
777 |
2/2✓ Branch 1 taken 119 times.
✓ Branch 2 taken 10 times.
|
258 | for (Eigen::DenseIndex k = 0; k < Eigen::DenseIndex(cmodel.colwise_span_indexes.size()); |
778 | ++k) | ||
779 | { | ||
780 | 238 | const Eigen::DenseIndex row_id = cmodel.colwise_span_indexes[size_t(k)]; | |
781 | // contact_dac_dq.col(row_id) += cmodel.corrector.Kd * contact_dvc_dq.col(row_id); | ||
782 |
3/6✓ Branch 1 taken 119 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 119 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 119 times.
✗ Branch 8 not taken.
|
238 | contact_dac_dq.col(row_id).noalias() += |
783 |
4/8✓ Branch 1 taken 119 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 119 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 119 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 119 times.
✗ Branch 11 not taken.
|
476 | cmodel.corrector.Kp.asDiagonal() * Jlog * contact_dac_da.col(row_id); |
784 | } | ||
785 | 20 | break; | |
786 | } | ||
787 | 30 | case CONTACT_3D: { | |
788 | typedef | ||
789 | typename SizeDepType<3>::template RowsReturn<typename Data::MatrixXs>::Type RowsBlock; | ||
790 | const RowsBlock contact_dvc_dq = | ||
791 |
1/2✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
|
30 | SizeDepType<3>::middleRows(data.dvc_dq, current_row_sol_id); |
792 |
1/2✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
|
30 | RowsBlock contact_dac_dq = SizeDepType<3>::middleRows(data.dac_dq, current_row_sol_id); |
793 |
1/2✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
|
30 | RowsBlock contact_dac_dv = SizeDepType<3>::middleRows(data.dac_dv, current_row_sol_id); |
794 | const RowsBlock contact_dac_da = | ||
795 |
1/2✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
|
30 | SizeDepType<3>::middleRows(data.dac_da, current_row_sol_id); |
796 |
2/2✓ Branch 0 taken 12 times.
✓ Branch 1 taken 3 times.
|
30 | if (cmodel.reference_frame == LOCAL) |
797 | { | ||
798 |
5/10✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 12 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 12 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 12 times.
✗ Branch 14 not taken.
|
48 | a_tmp.linear() = cmodel.corrector.Kd.asDiagonal() * cdata.oMc2.rotation() |
799 |
2/4✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12 times.
✗ Branch 5 not taken.
|
48 | * cdata.contact2_velocity.linear(); |
800 |
2/4✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12 times.
✗ Branch 5 not taken.
|
24 | typename SE3::Matrix3 vc2_cross_in_c1, vc2_cross_in_world; |
801 |
2/4✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12 times.
✗ Branch 5 not taken.
|
24 | skew(a_tmp.linear(), vc2_cross_in_world); |
802 |
5/10✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 12 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 12 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 12 times.
✗ Branch 14 not taken.
|
24 | vc2_cross_in_c1.noalias() = cdata.oMc1.rotation().transpose() * vc2_cross_in_world; |
803 |
2/2✓ Branch 1 taken 529 times.
✓ Branch 2 taken 12 times.
|
1082 | for (Eigen::DenseIndex k = 0; k < Eigen::DenseIndex(cmodel.loop_span_indexes.size()); |
804 | ++k) | ||
805 | { | ||
806 | 1058 | const Eigen::DenseIndex row_id = cmodel.loop_span_indexes[size_t(k)]; | |
807 |
2/4✓ Branch 1 taken 529 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 529 times.
✗ Branch 5 not taken.
|
1058 | const MotionRef<typename Data::Matrix6x::ColXpr> J_col(data.J.col(row_id)); |
808 |
3/4✓ Branch 1 taken 529 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 158 times.
✓ Branch 4 taken 371 times.
|
1058 | if (joint2_indexes[row_id]) |
809 | { | ||
810 |
5/10✓ Branch 1 taken 158 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 158 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 158 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 158 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 158 times.
✗ Branch 14 not taken.
|
316 | contact_dac_dq.col(row_id).noalias() += vc2_cross_in_c1 * J_col.angular(); |
811 | } | ||
812 | else | ||
813 | { | ||
814 |
5/10✓ Branch 1 taken 371 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 371 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 371 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 371 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 371 times.
✗ Branch 14 not taken.
|
742 | contact_dac_dq.col(row_id).noalias() -= vc2_cross_in_c1 * J_col.angular(); |
815 | } | ||
816 | } | ||
817 | 24 | const int colRef = | |
818 |
2/4✓ Branch 2 taken 12 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 12 times.
✗ Branch 7 not taken.
|
24 | nv(model.joints[cmodel.joint1_id]) + idx_v(model.joints[cmodel.joint1_id]) - 1; |
819 |
2/2✓ Branch 0 taken 135 times.
✓ Branch 1 taken 12 times.
|
294 | for (Eigen::DenseIndex j = colRef; j >= 0; j = data.parents_fromRow[(size_t)j]) |
820 | { | ||
821 | typedef typename Data::Matrix6x::ColXpr ColType; | ||
822 |
2/4✓ Branch 1 taken 135 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 135 times.
✗ Branch 5 not taken.
|
270 | const MotionRef<ColType> J_col(data.J.col(j)); |
823 |
6/12✓ Branch 1 taken 135 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 135 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 135 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 135 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 135 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 135 times.
✗ Branch 17 not taken.
|
270 | a_tmp.angular() = cdata.oMc1.rotation().transpose() * J_col.angular(); |
824 |
3/6✓ Branch 1 taken 135 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 135 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 135 times.
✗ Branch 8 not taken.
|
270 | contact_dac_dq.col(j).noalias() += |
825 |
1/2✓ Branch 1 taken 135 times.
✗ Branch 2 not taken.
|
270 | cmodel.corrector.Kp.asDiagonal() |
826 |
4/8✓ Branch 1 taken 135 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 135 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 135 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 135 times.
✗ Branch 11 not taken.
|
810 | * cdata.contact_placement_error.linear().cross(a_tmp.angular()); |
827 | } | ||
828 | } | ||
829 |
4/8✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 15 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 15 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 15 times.
✗ Branch 11 not taken.
|
30 | contact_dac_dq.noalias() += cmodel.corrector.Kd.asDiagonal() * contact_dvc_dq; |
830 |
4/8✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 15 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 15 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 15 times.
✗ Branch 11 not taken.
|
30 | contact_dac_dq.noalias() += cmodel.corrector.Kp.asDiagonal() * contact_dac_da; |
831 |
4/8✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 15 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 15 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 15 times.
✗ Branch 11 not taken.
|
30 | contact_dac_dv.noalias() += cmodel.corrector.Kd.asDiagonal() * contact_dac_da; |
832 | 30 | break; | |
833 | } | ||
834 | ✗ | default: | |
835 | ✗ | assert(false && "must never happen"); | |
836 | break; | ||
837 | } | ||
838 | } | ||
839 | |||
840 | 58 | current_row_sol_id += cmodel.size(); | |
841 | } | ||
842 | |||
843 |
1/2✓ Branch 1 taken 22 times.
✗ Branch 2 not taken.
|
44 | data.contact_chol.getOperationalSpaceInertiaMatrix(data.osim); |
844 |
1/2✓ Branch 1 taken 22 times.
✗ Branch 2 not taken.
|
44 | data.contact_chol.getInverseMassMatrix(data.Minv); |
845 | |||
846 | // Temporary: dlambda_dv stores J*Minv | ||
847 | 44 | typename Data::MatrixXs & JMinv = data.dlambda_dv; | |
848 | |||
849 |
3/6✓ Branch 1 taken 22 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 22 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 22 times.
✗ Branch 8 not taken.
|
44 | JMinv.noalias() = data.dac_da * data.Minv; |
850 | 44 | MatrixType3 & ddq_partial_dtau_ = PINOCCHIO_EIGEN_CONST_CAST(MatrixType3, ddq_partial_dtau); | |
851 | MatrixType6 & lambda_partial_dtau_ = | ||
852 | 44 | PINOCCHIO_EIGEN_CONST_CAST(MatrixType6, lambda_partial_dtau); | |
853 | 44 | typename Data::MatrixXs & dlambda_dx_prox = data.dlambda_dx_prox; | |
854 | 44 | typename Data::MatrixXs & drhs_prox = data.drhs_prox; | |
855 | { | ||
856 |
4/8✓ Branch 1 taken 22 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 22 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 22 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 22 times.
✗ Branch 11 not taken.
|
44 | lambda_partial_dtau_.noalias() = -data.osim * JMinv; // OUTPUT |
857 |
2/2✓ Branch 0 taken 20 times.
✓ Branch 1 taken 22 times.
|
84 | for (int it = 1; it < settings.iter; ++it) |
858 | { | ||
859 |
1/2✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
|
40 | lambda_partial_dtau_.swap(dlambda_dx_prox); |
860 |
1/2✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
|
40 | dlambda_dx_prox *= settings.mu; |
861 |
1/2✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
|
40 | dlambda_dx_prox -= JMinv; |
862 |
3/6✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 20 times.
✗ Branch 8 not taken.
|
40 | lambda_partial_dtau_.noalias() = data.osim * dlambda_dx_prox; |
863 | } | ||
864 | |||
865 |
3/4✓ Branch 0 taken 4 times.
✓ Branch 1 taken 18 times.
✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
|
44 | if (settings.iter % 2 == 0 && settings.iter > 0) |
866 | { | ||
867 |
1/2✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
|
8 | lambda_partial_dtau_.swap(dlambda_dx_prox); // restore previous memory address |
868 |
1/2✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
|
8 | lambda_partial_dtau_ = dlambda_dx_prox; |
869 | } | ||
870 | } | ||
871 | |||
872 |
4/8✓ Branch 1 taken 22 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 22 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 22 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 22 times.
✗ Branch 11 not taken.
|
44 | ddq_partial_dtau_.noalias() = JMinv.transpose() * lambda_partial_dtau; |
873 |
1/2✓ Branch 1 taken 22 times.
✗ Branch 2 not taken.
|
44 | ddq_partial_dtau_ += data.Minv; // OUTPUT |
874 | |||
875 | 44 | MatrixType4 & lambda_partial_dq_ = PINOCCHIO_EIGEN_CONST_CAST(MatrixType4, lambda_partial_dq); | |
876 |
4/8✓ Branch 1 taken 22 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 22 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 22 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 22 times.
✗ Branch 11 not taken.
|
44 | drhs_prox.noalias() = -JMinv * data.dtau_dq; |
877 |
1/2✓ Branch 1 taken 22 times.
✗ Branch 2 not taken.
|
44 | drhs_prox += data.dac_dq; |
878 | { | ||
879 |
4/8✓ Branch 1 taken 22 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 22 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 22 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 22 times.
✗ Branch 11 not taken.
|
44 | lambda_partial_dq_.noalias() = -data.osim * data.drhs_prox; // OUTPUT |
880 |
2/2✓ Branch 0 taken 20 times.
✓ Branch 1 taken 22 times.
|
84 | for (int it = 1; it < settings.iter; ++it) |
881 | { | ||
882 |
1/2✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
|
40 | lambda_partial_dq_.swap(dlambda_dx_prox); |
883 |
1/2✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
|
40 | dlambda_dx_prox *= settings.mu; |
884 |
1/2✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
|
40 | dlambda_dx_prox -= drhs_prox; |
885 |
3/6✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 20 times.
✗ Branch 8 not taken.
|
40 | lambda_partial_dq_.noalias() = data.osim * dlambda_dx_prox; |
886 | } | ||
887 | |||
888 |
3/4✓ Branch 0 taken 4 times.
✓ Branch 1 taken 18 times.
✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
|
44 | if (settings.iter % 2 == 0 && settings.iter > 0) |
889 | { | ||
890 |
1/2✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
|
8 | lambda_partial_dq_.swap(dlambda_dx_prox); // restore previous memory address |
891 |
1/2✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
|
8 | lambda_partial_dq_ = dlambda_dx_prox; |
892 | } | ||
893 | } | ||
894 | |||
895 | 44 | MatrixType5 & lambda_partial_dv_ = PINOCCHIO_EIGEN_CONST_CAST(MatrixType5, lambda_partial_dv); | |
896 |
4/8✓ Branch 1 taken 22 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 22 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 22 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 22 times.
✗ Branch 11 not taken.
|
44 | drhs_prox.noalias() = -JMinv * data.dtau_dv; |
897 |
1/2✓ Branch 1 taken 22 times.
✗ Branch 2 not taken.
|
44 | drhs_prox += data.dac_dv; |
898 | { | ||
899 |
4/8✓ Branch 1 taken 22 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 22 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 22 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 22 times.
✗ Branch 11 not taken.
|
44 | lambda_partial_dv_.noalias() = -data.osim * data.drhs_prox; // OUTPUT |
900 |
2/2✓ Branch 0 taken 20 times.
✓ Branch 1 taken 22 times.
|
84 | for (int it = 1; it < settings.iter; ++it) |
901 | { | ||
902 |
1/2✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
|
40 | lambda_partial_dv_.swap(dlambda_dx_prox); |
903 |
1/2✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
|
40 | dlambda_dx_prox *= settings.mu; |
904 |
1/2✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
|
40 | dlambda_dx_prox -= drhs_prox; |
905 |
3/6✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 20 times.
✗ Branch 8 not taken.
|
40 | lambda_partial_dv_.noalias() = data.osim * dlambda_dx_prox; |
906 | } | ||
907 | |||
908 |
3/4✓ Branch 0 taken 4 times.
✓ Branch 1 taken 18 times.
✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
|
44 | if (settings.iter % 2 == 0 && settings.iter > 0) |
909 | { | ||
910 |
1/2✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
|
8 | lambda_partial_dv_.swap(dlambda_dx_prox); // restore previous memory address |
911 |
1/2✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
|
8 | lambda_partial_dv_ = dlambda_dx_prox; |
912 | } | ||
913 | } | ||
914 | |||
915 | 44 | current_row_sol_id = 0; | |
916 |
2/2✓ Branch 1 taken 29 times.
✓ Branch 2 taken 22 times.
|
102 | for (size_t k = 0; k < contact_models.size(); ++k) |
917 | { | ||
918 | 58 | const RigidConstraintModel & cmodel = contact_models[k]; | |
919 | |||
920 |
2/3✓ Branch 0 taken 13 times.
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
|
58 | switch (cmodel.type) |
921 | { | ||
922 | 26 | case CONTACT_6D: { | |
923 | |||
924 | typedef | ||
925 | typename SizeDepType<6>::template RowsReturn<typename Data::MatrixXs>::Type RowsBlock; | ||
926 | typedef typename SizeDepType<6>::template RowsReturn<typename Data::MatrixXs>::ConstType | ||
927 | ConstRowsBlock; | ||
928 | |||
929 | // TODO: replace with contact_model::nc | ||
930 |
1/2✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
|
26 | RowsBlock contact_dac_da = SizeDepType<6>::middleRows(data.dac_da, current_row_sol_id); |
931 | |||
932 | ConstRowsBlock contact_dlambda_dq = | ||
933 |
1/2✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
|
26 | SizeDepType<6>::middleRows(lambda_partial_dq, current_row_sol_id); |
934 | ConstRowsBlock contact_dlambda_dv = | ||
935 |
1/2✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
|
26 | SizeDepType<6>::middleRows(lambda_partial_dv, current_row_sol_id); |
936 | |||
937 | // TODO: Sparsity in dac_da with loop joints? | ||
938 | |||
939 |
4/8✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 13 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 13 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 13 times.
✗ Branch 11 not taken.
|
26 | data.dtau_dq.noalias() -= contact_dac_da.transpose() * contact_dlambda_dq; |
940 |
4/8✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 13 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 13 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 13 times.
✗ Branch 11 not taken.
|
26 | data.dtau_dv.noalias() -= contact_dac_da.transpose() * contact_dlambda_dv; |
941 | |||
942 | // END TODO | ||
943 | |||
944 | /* | ||
945 | |||
946 | for(Eigen::DenseIndex j=colRef;j>=0;j=data.parents_fromRow[(size_t)j]) | ||
947 | { | ||
948 | data.dtau_dq.row(j).noalias() -= contact_dac_da.col(j).transpose() * contact_dlambda_dq; | ||
949 | data.dtau_dv.row(j).noalias() -= contact_dac_da.col(j).transpose() * contact_dlambda_dv; | ||
950 | } | ||
951 | */ | ||
952 | 26 | break; | |
953 | } | ||
954 | 32 | case CONTACT_3D: { | |
955 | |||
956 | typedef | ||
957 | typename SizeDepType<3>::template RowsReturn<typename Data::MatrixXs>::Type RowsBlock; | ||
958 | typedef typename SizeDepType<3>::template RowsReturn<typename Data::MatrixXs>::ConstType | ||
959 | ConstRowsBlock; | ||
960 | |||
961 |
1/2✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
|
32 | RowsBlock contact_dac_da = SizeDepType<3>::middleRows(data.dac_da, current_row_sol_id); |
962 | |||
963 | ConstRowsBlock contact_dlambda_dq = | ||
964 |
1/2✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
|
32 | SizeDepType<3>::middleRows(lambda_partial_dq, current_row_sol_id); |
965 | ConstRowsBlock contact_dlambda_dv = | ||
966 |
1/2✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
|
32 | SizeDepType<3>::middleRows(lambda_partial_dv, current_row_sol_id); |
967 | |||
968 | // TODO: Sparsity in dac_da with loop joints? | ||
969 | |||
970 |
4/8✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 16 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 16 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 16 times.
✗ Branch 11 not taken.
|
32 | data.dtau_dq.noalias() -= contact_dac_da.transpose() * contact_dlambda_dq; |
971 |
4/8✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 16 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 16 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 16 times.
✗ Branch 11 not taken.
|
32 | data.dtau_dv.noalias() -= contact_dac_da.transpose() * contact_dlambda_dv; |
972 | |||
973 | // END TODO | ||
974 | /* | ||
975 | |||
976 | |||
977 | |||
978 | for(Eigen::DenseIndex j=colRef;j>=0;j=data.parents_fromRow[(size_t)j]) | ||
979 | { | ||
980 | data.dtau_dq.row(j).noalias() -= contact_dac_da.col(j).transpose() * contact_dlambda_dq; | ||
981 | data.dtau_dv.row(j).noalias() -= contact_dac_da.col(j).transpose() * contact_dlambda_dv; | ||
982 | } | ||
983 | */ | ||
984 | 32 | break; | |
985 | } | ||
986 | |||
987 | ✗ | default: | |
988 | ✗ | assert(false && "must never happen"); | |
989 | break; | ||
990 | } | ||
991 | 58 | current_row_sol_id += cmodel.size(); | |
992 | } | ||
993 | |||
994 |
3/6✓ Branch 1 taken 22 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 22 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 22 times.
✗ Branch 9 not taken.
|
44 | PINOCCHIO_EIGEN_CONST_CAST(MatrixType1, ddq_partial_dq).noalias() = |
995 |
1/2✓ Branch 1 taken 22 times.
✗ Branch 2 not taken.
|
44 | -data.Minv * data.dtau_dq; // OUTPUT |
996 |
3/6✓ Branch 1 taken 22 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 22 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 22 times.
✗ Branch 9 not taken.
|
44 | PINOCCHIO_EIGEN_CONST_CAST(MatrixType2, ddq_partial_dv).noalias() = |
997 |
1/2✓ Branch 1 taken 22 times.
✗ Branch 2 not taken.
|
44 | -data.Minv * data.dtau_dv; // OUTPUT |
998 | |||
999 | 44 | MatrixType4 & dfc_dq = PINOCCHIO_EIGEN_CONST_CAST(MatrixType4, lambda_partial_dq); | |
1000 | typedef typename SizeDepType<6>::template RowsReturn<typename Data::MatrixXs>::Type Rows6Block; | ||
1001 | typedef typename SizeDepType<3>::template RowsReturn<typename Data::MatrixXs>::Type Rows3Block; | ||
1002 | |||
1003 | 44 | current_row_sol_id = 0; | |
1004 |
2/2✓ Branch 1 taken 29 times.
✓ Branch 2 taken 22 times.
|
102 | for (size_t k = 0; k < contact_models.size(); ++k) |
1005 | { | ||
1006 | 58 | const RigidConstraintModel & cmodel = contact_models[k]; | |
1007 | 58 | const RigidConstraintData & cdata = contact_data[k]; | |
1008 | 58 | const typename Model::JointIndex joint1_id = cmodel.joint1_id; | |
1009 |
2/4✓ Branch 2 taken 29 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 29 times.
✗ Branch 7 not taken.
|
58 | const int colRef = nv(model.joints[joint1_id]) + idx_v(model.joints[joint1_id]) - 1; |
1010 | |||
1011 |
2/3✓ Branch 0 taken 23 times.
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
58 | switch (cmodel.reference_frame) |
1012 | { | ||
1013 | 46 | case LOCAL: | |
1014 | 46 | break; | |
1015 | 12 | case LOCAL_WORLD_ALIGNED: { | |
1016 | 12 | const Force & of = cdata.contact_force; | |
1017 |
2/3✓ Branch 0 taken 3 times.
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
|
12 | switch (cmodel.type) |
1018 | { | ||
1019 | 6 | case CONTACT_6D: { | |
1020 |
1/2✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
|
6 | Rows6Block contact_dfc_dq = SizeDepType<6>::middleRows(dfc_dq, current_row_sol_id); |
1021 |
2/2✓ Branch 0 taken 37 times.
✓ Branch 1 taken 3 times.
|
80 | for (Eigen::DenseIndex j = colRef; j >= 0; j = data.parents_fromRow[(size_t)j]) |
1022 | { | ||
1023 | typedef typename Data::Matrix6x::ColXpr ColType; | ||
1024 | typedef typename Rows6Block::ColXpr ColTypeOut; | ||
1025 |
2/4✓ Branch 1 taken 37 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 37 times.
✗ Branch 5 not taken.
|
74 | const MotionRef<ColType> J_col(data.J.col(j)); |
1026 |
2/4✓ Branch 1 taken 37 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 37 times.
✗ Branch 5 not taken.
|
74 | ForceRef<ColTypeOut> fout(contact_dfc_dq.col(j)); |
1027 |
6/12✓ Branch 1 taken 37 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 37 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 37 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 37 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 37 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 37 times.
✗ Branch 17 not taken.
|
74 | fout.linear().noalias() += J_col.angular().cross(of.linear()); |
1028 |
6/12✓ Branch 1 taken 37 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 37 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 37 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 37 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 37 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 37 times.
✗ Branch 17 not taken.
|
74 | fout.angular().noalias() += J_col.angular().cross(of.angular()); |
1029 | } | ||
1030 | 6 | break; | |
1031 | } | ||
1032 | 6 | case CONTACT_3D: { | |
1033 |
1/2✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
|
6 | Rows3Block contact_dfc_dq = SizeDepType<3>::middleRows(dfc_dq, current_row_sol_id); |
1034 |
2/2✓ Branch 0 taken 39 times.
✓ Branch 1 taken 3 times.
|
84 | for (Eigen::DenseIndex j = colRef; j >= 0; j = data.parents_fromRow[(size_t)j]) |
1035 | { | ||
1036 | typedef typename Data::Matrix6x::ColXpr ColType; | ||
1037 |
2/4✓ Branch 1 taken 39 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 39 times.
✗ Branch 5 not taken.
|
78 | const MotionRef<ColType> J_col(data.J.col(j)); |
1038 |
6/12✓ Branch 1 taken 39 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 39 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 39 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 39 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 39 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 39 times.
✗ Branch 17 not taken.
|
78 | contact_dfc_dq.col(j).noalias() += J_col.angular().cross(of.linear()); |
1039 | } | ||
1040 | 6 | break; | |
1041 | } | ||
1042 | ✗ | default: | |
1043 | ✗ | assert(false && "must never happen"); | |
1044 | break; | ||
1045 | } | ||
1046 | 12 | break; | |
1047 | } | ||
1048 | ✗ | default: | |
1049 | ✗ | assert(false && "must never happen"); | |
1050 | break; | ||
1051 | } | ||
1052 | 58 | current_row_sol_id += cmodel.size(); | |
1053 | } | ||
1054 | 44 | } | |
1055 | |||
1056 | } // namespace pinocchio | ||
1057 | |||
1058 | #endif // ifndef __pinocchio_algorithm_constraint_dynamics_derivatives_hxx__ | ||
1059 |