Directory: | ./ |
---|---|
File: | include/pinocchio/algorithm/aba.hxx |
Date: | 2024-08-27 18:20:05 |
Exec | Total | Coverage | |
---|---|---|---|
Lines: | 192 | 315 | 61.0% |
Branches: | 307 | 1090 | 28.2% |
Line | Branch | Exec | Source |
---|---|---|---|
1 | // | ||
2 | // Copyright (c) 2016-2021 CNRS INRIA | ||
3 | // | ||
4 | |||
5 | #ifndef __pinocchio_algorithm_aba_hxx__ | ||
6 | #define __pinocchio_algorithm_aba_hxx__ | ||
7 | |||
8 | #include "pinocchio/spatial/act-on-set.hpp" | ||
9 | #include "pinocchio/multibody/visitor.hpp" | ||
10 | #include "pinocchio/algorithm/check.hpp" | ||
11 | |||
12 | /// @cond DEV | ||
13 | |||
14 | namespace pinocchio | ||
15 | { | ||
16 | namespace impl | ||
17 | { | ||
18 | namespace internal | ||
19 | { | ||
20 | |||
21 | template<typename Scalar> | ||
22 | struct SE3actOn | ||
23 | { | ||
24 | template<int Options, typename Matrix6Type> | ||
25 | static typename PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix6Type) | ||
26 | 26 | run(const SE3Tpl<Scalar, Options> & M, const Eigen::MatrixBase<Matrix6Type> & I) | |
27 | { | ||
28 | typedef SE3Tpl<Scalar, Options> SE3; | ||
29 | typedef typename SE3::Matrix3 Matrix3; | ||
30 | typedef typename SE3::Vector3 Vector3; | ||
31 | |||
32 | typedef const Eigen::Block<Matrix6Type, 3, 3> constBlock3; | ||
33 | |||
34 | typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix6Type) ReturnType; | ||
35 | typedef Eigen::Block<ReturnType, 3, 3> Block3; | ||
36 | |||
37 | 26 | Matrix6Type & I_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix6Type, I); | |
38 |
1/2✓ Branch 1 taken 26 times.
✗ Branch 2 not taken.
|
26 | const constBlock3 & Ai = I_.template block<3, 3>(Inertia::LINEAR, Inertia::LINEAR); |
39 |
1/2✓ Branch 1 taken 26 times.
✗ Branch 2 not taken.
|
26 | const constBlock3 & Bi = I_.template block<3, 3>(Inertia::LINEAR, Inertia::ANGULAR); |
40 |
1/2✓ Branch 1 taken 26 times.
✗ Branch 2 not taken.
|
26 | const constBlock3 & Di = I_.template block<3, 3>(Inertia::ANGULAR, Inertia::ANGULAR); |
41 | |||
42 |
1/2✓ Branch 1 taken 26 times.
✗ Branch 2 not taken.
|
26 | const Matrix3 & R = M.rotation(); |
43 |
1/2✓ Branch 1 taken 26 times.
✗ Branch 2 not taken.
|
26 | const Vector3 & t = M.translation(); |
44 | |||
45 |
1/2✓ Branch 1 taken 26 times.
✗ Branch 2 not taken.
|
26 | ReturnType res; |
46 |
1/2✓ Branch 1 taken 26 times.
✗ Branch 2 not taken.
|
26 | Block3 Ao = res.template block<3, 3>(Inertia::LINEAR, Inertia::LINEAR); |
47 |
1/2✓ Branch 1 taken 26 times.
✗ Branch 2 not taken.
|
26 | Block3 Bo = res.template block<3, 3>(Inertia::LINEAR, Inertia::ANGULAR); |
48 |
1/2✓ Branch 1 taken 26 times.
✗ Branch 2 not taken.
|
26 | Block3 Co = res.template block<3, 3>(Inertia::ANGULAR, Inertia::LINEAR); |
49 |
1/2✓ Branch 1 taken 26 times.
✗ Branch 2 not taken.
|
26 | Block3 Do = res.template block<3, 3>(Inertia::ANGULAR, Inertia::ANGULAR); |
50 | |||
51 |
3/6✓ Branch 1 taken 26 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 26 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 26 times.
✗ Branch 8 not taken.
|
26 | Do.noalias() = R * Ai; // tmp variable |
52 |
4/8✓ Branch 1 taken 26 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 26 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 26 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 26 times.
✗ Branch 11 not taken.
|
26 | Ao.noalias() = Do * R.transpose(); |
53 | |||
54 |
3/6✓ Branch 1 taken 26 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 26 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 26 times.
✗ Branch 8 not taken.
|
26 | Do.noalias() = R * Bi; // tmp variable |
55 |
4/8✓ Branch 1 taken 26 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 26 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 26 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 26 times.
✗ Branch 11 not taken.
|
26 | Bo.noalias() = Do * R.transpose(); |
56 | |||
57 |
3/6✓ Branch 1 taken 26 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 26 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 26 times.
✗ Branch 8 not taken.
|
26 | Co.noalias() = R * Di; // tmp variable |
58 |
4/8✓ Branch 1 taken 26 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 26 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 26 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 26 times.
✗ Branch 11 not taken.
|
26 | Do.noalias() = Co * R.transpose(); |
59 | |||
60 |
4/8✓ Branch 1 taken 26 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 26 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 26 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 26 times.
✗ Branch 11 not taken.
|
26 | Do.row(0) += t.cross(Bo.col(0)); |
61 |
4/8✓ Branch 1 taken 26 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 26 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 26 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 26 times.
✗ Branch 11 not taken.
|
26 | Do.row(1) += t.cross(Bo.col(1)); |
62 |
4/8✓ Branch 1 taken 26 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 26 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 26 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 26 times.
✗ Branch 11 not taken.
|
26 | Do.row(2) += t.cross(Bo.col(2)); |
63 | |||
64 |
4/8✓ Branch 1 taken 26 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 26 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 26 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 26 times.
✗ Branch 11 not taken.
|
26 | Co.col(0) = t.cross(Ao.col(0)); |
65 |
4/8✓ Branch 1 taken 26 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 26 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 26 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 26 times.
✗ Branch 11 not taken.
|
26 | Co.col(1) = t.cross(Ao.col(1)); |
66 |
4/8✓ Branch 1 taken 26 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 26 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 26 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 26 times.
✗ Branch 11 not taken.
|
26 | Co.col(2) = t.cross(Ao.col(2)); |
67 |
2/4✓ Branch 1 taken 26 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 26 times.
✗ Branch 5 not taken.
|
26 | Co += Bo.transpose(); |
68 | |||
69 |
2/4✓ Branch 1 taken 26 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 26 times.
✗ Branch 5 not taken.
|
26 | Bo = Co.transpose(); |
70 |
4/8✓ Branch 1 taken 26 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 26 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 26 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 26 times.
✗ Branch 11 not taken.
|
26 | Do.col(0) += t.cross(Bo.col(0)); |
71 |
4/8✓ Branch 1 taken 26 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 26 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 26 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 26 times.
✗ Branch 11 not taken.
|
26 | Do.col(1) += t.cross(Bo.col(1)); |
72 |
4/8✓ Branch 1 taken 26 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 26 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 26 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 26 times.
✗ Branch 11 not taken.
|
26 | Do.col(2) += t.cross(Bo.col(2)); |
73 | |||
74 | 52 | return res; | |
75 | } | ||
76 | }; | ||
77 | } // namespace internal | ||
78 | |||
79 | template< | ||
80 | typename Scalar, | ||
81 | int Options, | ||
82 | template<typename, int> | ||
83 | class JointCollectionTpl, | ||
84 | typename ConfigVectorType, | ||
85 | typename TangentVectorType> | ||
86 | struct AbaWorldConventionForwardStep1 | ||
87 | : public fusion::JointUnaryVisitorBase<AbaWorldConventionForwardStep1< | ||
88 | Scalar, | ||
89 | Options, | ||
90 | JointCollectionTpl, | ||
91 | ConfigVectorType, | ||
92 | TangentVectorType>> | ||
93 | { | ||
94 | typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model; | ||
95 | typedef DataTpl<Scalar, Options, JointCollectionTpl> Data; | ||
96 | |||
97 | typedef boost::fusion:: | ||
98 | vector<const Model &, Data &, const ConfigVectorType &, const TangentVectorType &> | ||
99 | ArgsType; | ||
100 | |||
101 | template<typename JointModel> | ||
102 | 108 | static void algo( | |
103 | const pinocchio::JointModelBase<JointModel> & jmodel, | ||
104 | pinocchio::JointDataBase<typename JointModel::JointDataDerived> & jdata, | ||
105 | const Model & model, | ||
106 | Data & data, | ||
107 | const Eigen::MatrixBase<ConfigVectorType> & q, | ||
108 | const Eigen::MatrixBase<TangentVectorType> & v) | ||
109 | { | ||
110 | typedef typename Model::JointIndex JointIndex; | ||
111 | typedef typename Data::Motion Motion; | ||
112 | |||
113 | 108 | const JointIndex i = jmodel.id(); | |
114 | 108 | Motion & ov = data.ov[i]; | |
115 | 108 | jmodel.calc(jdata.derived(), q.derived(), v.derived()); | |
116 | |||
117 | 108 | const JointIndex & parent = model.parents[i]; | |
118 |
3/5✓ Branch 4 taken 52 times.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 52 times.
✗ Branch 9 not taken.
|
108 | data.liMi[i] = model.jointPlacements[i] * jdata.M(); |
119 |
2/2✓ Branch 0 taken 52 times.
✓ Branch 1 taken 2 times.
|
108 | if (parent > 0) |
120 |
1/2✓ Branch 5 taken 52 times.
✗ Branch 6 not taken.
|
104 | data.oMi[i] = data.oMi[parent] * data.liMi[i]; |
121 | else | ||
122 | 4 | data.oMi[i] = data.liMi[i]; | |
123 | |||
124 |
2/4✓ Branch 4 taken 54 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 54 times.
✗ Branch 8 not taken.
|
108 | jmodel.jointCols(data.J) = data.oMi[i].act(jdata.S()); |
125 | |||
126 |
1/2✓ Branch 4 taken 54 times.
✗ Branch 5 not taken.
|
108 | ov = data.oMi[i].act(jdata.v()); |
127 |
2/2✓ Branch 0 taken 52 times.
✓ Branch 1 taken 2 times.
|
108 | if (parent > 0) |
128 | 104 | ov += data.ov[parent]; | |
129 | |||
130 |
1/2✓ Branch 5 taken 54 times.
✗ Branch 6 not taken.
|
108 | data.oa_gf[i] = data.oMi[i].act(jdata.c()); |
131 |
2/2✓ Branch 0 taken 52 times.
✓ Branch 1 taken 2 times.
|
108 | if (parent > 0) |
132 |
1/2✓ Branch 4 taken 52 times.
✗ Branch 5 not taken.
|
104 | data.oa_gf[i] += (data.ov[parent] ^ ov); |
133 | |||
134 |
2/4✓ Branch 5 taken 54 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 54 times.
✗ Branch 10 not taken.
|
108 | data.oinertias[i] = data.oYcrb[i] = data.oMi[i].act(model.inertias[i]); |
135 | 108 | data.oYaba[i] = data.oYcrb[i].matrix(); | |
136 | |||
137 |
1/2✓ Branch 4 taken 54 times.
✗ Branch 5 not taken.
|
108 | data.oh[i] = data.oYcrb[i] * ov; // necessary for ABA derivatives |
138 |
1/2✓ Branch 4 taken 54 times.
✗ Branch 5 not taken.
|
108 | data.of[i] = ov.cross(data.oh[i]); |
139 | } | ||
140 | }; | ||
141 | |||
142 | template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl> | ||
143 | struct AbaWorldConventionBackwardStep | ||
144 | : public fusion::JointUnaryVisitorBase< | ||
145 | AbaWorldConventionBackwardStep<Scalar, Options, JointCollectionTpl>> | ||
146 | { | ||
147 | typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model; | ||
148 | typedef DataTpl<Scalar, Options, JointCollectionTpl> Data; | ||
149 | |||
150 | typedef boost::fusion::vector<const Model &, Data &> ArgsType; | ||
151 | |||
152 | template<typename JointModel> | ||
153 | 108 | static void algo( | |
154 | const JointModelBase<JointModel> & jmodel, | ||
155 | JointDataBase<typename JointModel::JointDataDerived> & jdata, | ||
156 | const Model & model, | ||
157 | Data & data) | ||
158 | { | ||
159 | typedef typename Model::JointIndex JointIndex; | ||
160 | typedef typename Data::Inertia Inertia; | ||
161 | typedef typename Data::Force Force; | ||
162 | typedef typename Data::Matrix6x Matrix6x; | ||
163 | |||
164 |
1/2✓ Branch 1 taken 54 times.
✗ Branch 2 not taken.
|
108 | const JointIndex i = jmodel.id(); |
165 | 108 | const JointIndex parent = model.parents[i]; | |
166 | 108 | typename Inertia::Matrix6 & Ia = data.oYaba[i]; | |
167 | |||
168 | typedef typename SizeDepType<JointModel::NV>::template ColsReturn<Matrix6x>::Type ColBlock; | ||
169 |
1/2✓ Branch 1 taken 54 times.
✗ Branch 2 not taken.
|
108 | ColBlock Jcols = jmodel.jointCols(data.J); |
170 | |||
171 | 108 | Force & fi = data.of[i]; | |
172 | |||
173 |
6/12✓ Branch 1 taken 54 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 54 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 54 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 54 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 54 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 54 times.
✗ Branch 17 not taken.
|
108 | jmodel.jointVelocitySelector(data.u).noalias() -= Jcols.transpose() * fi.toVector(); |
174 | |||
175 |
4/8✓ Branch 1 taken 54 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 54 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 54 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 54 times.
✗ Branch 11 not taken.
|
108 | jdata.U().noalias() = Ia * Jcols; |
176 |
6/12✓ Branch 1 taken 54 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 54 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 54 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 54 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 54 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 54 times.
✗ Branch 17 not taken.
|
108 | jdata.StU().noalias() = Jcols.transpose() * jdata.U(); |
177 | |||
178 | // Account for the rotor inertia contribution | ||
179 |
4/8✓ Branch 1 taken 54 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 54 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 54 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 54 times.
✗ Branch 11 not taken.
|
108 | jdata.StU().diagonal() += jmodel.jointVelocitySelector(model.armature); |
180 | |||
181 |
3/6✓ Branch 1 taken 54 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 54 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 54 times.
✗ Branch 8 not taken.
|
108 | ::pinocchio::internal::PerformStYSInversion<Scalar>::run(jdata.StU(), jdata.Dinv()); |
182 |
6/12✓ Branch 1 taken 54 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 54 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 54 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 54 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 54 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 54 times.
✗ Branch 17 not taken.
|
108 | jdata.UDinv().noalias() = jdata.U() * jdata.Dinv(); |
183 | |||
184 |
2/2✓ Branch 0 taken 52 times.
✓ Branch 1 taken 2 times.
|
108 | if (parent > 0) |
185 | { | ||
186 |
6/12✓ Branch 1 taken 52 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 52 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 52 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 52 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 52 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 52 times.
✗ Branch 17 not taken.
|
104 | Ia.noalias() -= jdata.UDinv() * jdata.U().transpose(); |
187 | |||
188 |
4/8✓ Branch 1 taken 52 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 52 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 52 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 52 times.
✗ Branch 11 not taken.
|
104 | fi.toVector().noalias() += |
189 |
5/10✓ Branch 1 taken 52 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 52 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 52 times.
✗ Branch 8 not taken.
✓ Branch 11 taken 52 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 52 times.
✗ Branch 15 not taken.
|
104 | Ia * data.oa_gf[i].toVector() + jdata.UDinv() * jmodel.jointVelocitySelector(data.u); |
190 |
1/2✓ Branch 2 taken 52 times.
✗ Branch 3 not taken.
|
104 | data.oYaba[parent] += Ia; |
191 |
1/2✓ Branch 2 taken 52 times.
✗ Branch 3 not taken.
|
104 | data.of[parent] += fi; |
192 | } | ||
193 | } | ||
194 | }; | ||
195 | |||
196 | template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl> | ||
197 | struct AbaWorldConventionForwardStep2 | ||
198 | : public fusion::JointUnaryVisitorBase< | ||
199 | AbaWorldConventionForwardStep2<Scalar, Options, JointCollectionTpl>> | ||
200 | { | ||
201 | typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model; | ||
202 | typedef DataTpl<Scalar, Options, JointCollectionTpl> Data; | ||
203 | |||
204 | typedef boost::fusion::vector<const Model &, Data &> ArgsType; | ||
205 | |||
206 | template<typename JointModel> | ||
207 | 108 | static void algo( | |
208 | const pinocchio::JointModelBase<JointModel> & jmodel, | ||
209 | pinocchio::JointDataBase<typename JointModel::JointDataDerived> & jdata, | ||
210 | const Model & model, | ||
211 | Data & data) | ||
212 | { | ||
213 | |||
214 | typedef typename Model::JointIndex JointIndex; | ||
215 | typedef typename Data::Matrix6x Matrix6x; | ||
216 | |||
217 | typedef typename SizeDepType<JointModel::NV>::template ColsReturn<Matrix6x>::Type ColBlock; | ||
218 |
1/2✓ Branch 1 taken 54 times.
✗ Branch 2 not taken.
|
108 | ColBlock J_cols = jmodel.jointCols(data.J); |
219 | |||
220 |
1/2✓ Branch 1 taken 54 times.
✗ Branch 2 not taken.
|
108 | const JointIndex i = jmodel.id(); |
221 | 108 | const JointIndex parent = model.parents[i]; | |
222 | |||
223 |
1/2✓ Branch 3 taken 54 times.
✗ Branch 4 not taken.
|
108 | data.oa_gf[i] += data.oa_gf[parent]; // does take into account the gravity field |
224 |
4/8✓ Branch 1 taken 54 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 54 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 54 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 54 times.
✗ Branch 11 not taken.
|
108 | jmodel.jointVelocitySelector(data.ddq).noalias() = |
225 |
3/6✓ Branch 1 taken 54 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 54 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 54 times.
✗ Branch 8 not taken.
|
108 | jdata.Dinv() * jmodel.jointVelocitySelector(data.u) |
226 |
4/8✓ Branch 2 taken 54 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 54 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 54 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 54 times.
✗ Branch 12 not taken.
|
108 | - jdata.UDinv().transpose() * data.oa_gf[i].toVector(); |
227 |
5/10✓ Branch 1 taken 54 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 54 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 54 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 54 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 54 times.
✗ Branch 15 not taken.
|
108 | data.oa_gf[i].toVector().noalias() += J_cols * jmodel.jointVelocitySelector(data.ddq); |
228 | |||
229 | // Handle consistent output | ||
230 |
2/4✓ Branch 2 taken 54 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 54 times.
✗ Branch 7 not taken.
|
108 | data.oa[i] = data.oa_gf[i] + model.gravity; |
231 |
4/8✓ Branch 3 taken 54 times.
✗ Branch 4 not taken.
✓ Branch 8 taken 54 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 54 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 54 times.
✗ Branch 16 not taken.
|
108 | data.of[i] = data.oinertias[i] * data.oa_gf[i] + data.ov[i].cross(data.oh[i]); |
232 | } | ||
233 | }; | ||
234 | |||
235 | template< | ||
236 | typename Scalar, | ||
237 | int Options, | ||
238 | template<typename, int> | ||
239 | class JointCollectionTpl, | ||
240 | typename ConfigVectorType, | ||
241 | typename TangentVectorType1, | ||
242 | typename TangentVectorType2> | ||
243 | const typename DataTpl<Scalar, Options, JointCollectionTpl>::TangentVectorType & | ||
244 | 4 | abaWorldConvention( | |
245 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
246 | DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
247 | const Eigen::MatrixBase<ConfigVectorType> & q, | ||
248 | const Eigen::MatrixBase<TangentVectorType1> & v, | ||
249 | const Eigen::MatrixBase<TangentVectorType2> & tau) | ||
250 | { | ||
251 |
1/2✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
|
4 | assert(model.check(data) && "data is not consistent with model."); |
252 |
1/4✗ Branch 1 not taken.
✓ Branch 2 taken 2 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
|
4 | PINOCCHIO_CHECK_INPUT_ARGUMENT( |
253 | q.size() == model.nq, "The joint configuration vector is not of right size"); | ||
254 |
1/4✗ Branch 1 not taken.
✓ Branch 2 taken 2 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
|
4 | PINOCCHIO_CHECK_INPUT_ARGUMENT( |
255 | v.size() == model.nv, "The joint velocity vector is not of right size"); | ||
256 |
1/4✗ Branch 1 not taken.
✓ Branch 2 taken 2 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
|
4 | PINOCCHIO_CHECK_INPUT_ARGUMENT( |
257 | tau.size() == model.nv, "The joint acceleration vector is not of right size"); | ||
258 | |||
259 | typedef typename ModelTpl<Scalar, Options, JointCollectionTpl>::JointIndex JointIndex; | ||
260 | |||
261 |
1/2✓ Branch 3 taken 2 times.
✗ Branch 4 not taken.
|
4 | data.oa_gf[0] = -model.gravity; |
262 | 4 | data.of[0].setZero(); | |
263 | 4 | data.u = tau; | |
264 | |||
265 | typedef AbaWorldConventionForwardStep1< | ||
266 | Scalar, Options, JointCollectionTpl, ConfigVectorType, TangentVectorType1> | ||
267 | Pass1; | ||
268 |
2/2✓ Branch 0 taken 54 times.
✓ Branch 1 taken 2 times.
|
112 | for (JointIndex i = 1; i < (JointIndex)model.njoints; ++i) |
269 | { | ||
270 |
1/2✓ Branch 1 taken 54 times.
✗ Branch 2 not taken.
|
108 | Pass1::run( |
271 | 108 | model.joints[i], data.joints[i], | |
272 | 216 | typename Pass1::ArgsType(model, data, q.derived(), v.derived())); | |
273 | } | ||
274 | |||
275 | typedef AbaWorldConventionBackwardStep<Scalar, Options, JointCollectionTpl> Pass2; | ||
276 |
2/2✓ Branch 0 taken 54 times.
✓ Branch 1 taken 2 times.
|
112 | for (JointIndex i = (JointIndex)model.njoints - 1; i > 0; --i) |
277 | { | ||
278 |
1/2✓ Branch 4 taken 54 times.
✗ Branch 5 not taken.
|
108 | Pass2::run(model.joints[i], data.joints[i], typename Pass2::ArgsType(model, data)); |
279 | } | ||
280 | |||
281 | typedef AbaWorldConventionForwardStep2<Scalar, Options, JointCollectionTpl> Pass3; | ||
282 |
2/2✓ Branch 0 taken 54 times.
✓ Branch 1 taken 2 times.
|
112 | for (JointIndex i = 1; i < (JointIndex)model.njoints; ++i) |
283 | { | ||
284 |
1/2✓ Branch 4 taken 54 times.
✗ Branch 5 not taken.
|
108 | Pass3::run(model.joints[i], data.joints[i], typename Pass3::ArgsType(model, data)); |
285 | } | ||
286 | |||
287 |
2/2✓ Branch 0 taken 54 times.
✓ Branch 1 taken 2 times.
|
112 | for (JointIndex i = (JointIndex)model.njoints - 1; i > 0; --i) |
288 | { | ||
289 | 108 | const JointIndex parent = model.parents[i]; | |
290 | 108 | data.of[parent] += data.of[i]; | |
291 | } | ||
292 | |||
293 | 4 | return data.ddq; | |
294 | } | ||
295 | |||
296 | template< | ||
297 | typename Scalar, | ||
298 | int Options, | ||
299 | template<typename, int> | ||
300 | class JointCollectionTpl, | ||
301 | typename ConfigVectorType, | ||
302 | typename TangentVectorType1, | ||
303 | typename TangentVectorType2, | ||
304 | typename ForceDerived> | ||
305 | const typename DataTpl<Scalar, Options, JointCollectionTpl>::TangentVectorType & | ||
306 | ✗ | abaWorldConvention( | |
307 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
308 | DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
309 | const Eigen::MatrixBase<ConfigVectorType> & q, | ||
310 | const Eigen::MatrixBase<TangentVectorType1> & v, | ||
311 | const Eigen::MatrixBase<TangentVectorType2> & tau, | ||
312 | const container::aligned_vector<ForceDerived> & fext) | ||
313 | |||
314 | { | ||
315 | ✗ | assert(model.check(data) && "data is not consistent with model."); | |
316 | ✗ | PINOCCHIO_CHECK_INPUT_ARGUMENT( | |
317 | q.size() == model.nq, "The joint configuration vector is not of right size"); | ||
318 | ✗ | PINOCCHIO_CHECK_INPUT_ARGUMENT( | |
319 | v.size() == model.nv, "The joint velocity vector is not of right size"); | ||
320 | ✗ | PINOCCHIO_CHECK_INPUT_ARGUMENT( | |
321 | tau.size() == model.nv, "The joint acceleration vector is not of right size"); | ||
322 | |||
323 | typedef typename ModelTpl<Scalar, Options, JointCollectionTpl>::JointIndex JointIndex; | ||
324 | |||
325 | ✗ | data.oa_gf[0] = -model.gravity; | |
326 | ✗ | data.u = tau; | |
327 | |||
328 | typedef AbaWorldConventionForwardStep1< | ||
329 | Scalar, Options, JointCollectionTpl, ConfigVectorType, TangentVectorType1> | ||
330 | Pass1; | ||
331 | ✗ | for (JointIndex i = 1; i < (JointIndex)model.njoints; ++i) | |
332 | { | ||
333 | ✗ | Pass1::run( | |
334 | ✗ | model.joints[i], data.joints[i], | |
335 | ✗ | typename Pass1::ArgsType(model, data, q.derived(), v.derived())); | |
336 | ✗ | data.of[i] -= data.oMi[i].act(fext[i]); | |
337 | } | ||
338 | |||
339 | typedef AbaWorldConventionBackwardStep<Scalar, Options, JointCollectionTpl> Pass2; | ||
340 | ✗ | for (JointIndex i = (JointIndex)model.njoints - 1; i > 0; --i) | |
341 | { | ||
342 | ✗ | Pass2::run(model.joints[i], data.joints[i], typename Pass2::ArgsType(model, data)); | |
343 | } | ||
344 | |||
345 | typedef AbaWorldConventionForwardStep2<Scalar, Options, JointCollectionTpl> Pass3; | ||
346 | ✗ | for (JointIndex i = 1; i < (JointIndex)model.njoints; ++i) | |
347 | { | ||
348 | ✗ | Pass3::run(model.joints[i], data.joints[i], typename Pass3::ArgsType(model, data)); | |
349 | } | ||
350 | |||
351 | ✗ | return data.ddq; | |
352 | } | ||
353 | |||
354 | template< | ||
355 | typename Scalar, | ||
356 | int Options, | ||
357 | template<typename, int> | ||
358 | class JointCollectionTpl, | ||
359 | typename ConfigVectorType, | ||
360 | typename TangentVectorType> | ||
361 | struct AbaLocalConventionForwardStep1 | ||
362 | : public fusion::JointUnaryVisitorBase<AbaLocalConventionForwardStep1< | ||
363 | Scalar, | ||
364 | Options, | ||
365 | JointCollectionTpl, | ||
366 | ConfigVectorType, | ||
367 | TangentVectorType>> | ||
368 | { | ||
369 | typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model; | ||
370 | typedef DataTpl<Scalar, Options, JointCollectionTpl> Data; | ||
371 | |||
372 | typedef boost::fusion:: | ||
373 | vector<const Model &, Data &, const ConfigVectorType &, const TangentVectorType &> | ||
374 | ArgsType; | ||
375 | |||
376 | template<typename JointModel> | ||
377 | 54 | static void algo( | |
378 | const pinocchio::JointModelBase<JointModel> & jmodel, | ||
379 | pinocchio::JointDataBase<typename JointModel::JointDataDerived> & jdata, | ||
380 | const Model & model, | ||
381 | Data & data, | ||
382 | const Eigen::MatrixBase<ConfigVectorType> & q, | ||
383 | const Eigen::MatrixBase<TangentVectorType> & v) | ||
384 | { | ||
385 | typedef typename Model::JointIndex JointIndex; | ||
386 | |||
387 | 54 | const JointIndex i = jmodel.id(); | |
388 | 54 | jmodel.calc(jdata.derived(), q.derived(), v.derived()); | |
389 | |||
390 | 54 | const JointIndex & parent = model.parents[i]; | |
391 |
3/5✓ Branch 4 taken 26 times.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 26 times.
✗ Branch 9 not taken.
|
54 | data.liMi[i] = model.jointPlacements[i] * jdata.M(); |
392 | |||
393 | 54 | data.v[i] = jdata.v(); | |
394 |
2/2✓ Branch 0 taken 26 times.
✓ Branch 1 taken 1 times.
|
54 | if (parent > 0) |
395 |
1/2✓ Branch 5 taken 26 times.
✗ Branch 6 not taken.
|
52 | data.v[i] += data.liMi[i].actInv(data.v[parent]); |
396 | |||
397 |
2/8✓ Branch 4 taken 27 times.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✓ Branch 9 taken 27 times.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 12 not taken.
|
54 | data.a_gf[i] = jdata.c() + (data.v[i] ^ jdata.v()); |
398 | |||
399 | 54 | data.Yaba[i] = model.inertias[i].matrix(); | |
400 |
1/2✓ Branch 5 taken 27 times.
✗ Branch 6 not taken.
|
54 | data.h[i] = model.inertias[i] * data.v[i]; |
401 |
1/2✓ Branch 5 taken 27 times.
✗ Branch 6 not taken.
|
54 | data.f[i] = data.v[i].cross(data.h[i]); // -f_ext |
402 | } | ||
403 | }; | ||
404 | |||
405 | template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl> | ||
406 | struct AbaLocalConventionBackwardStep | ||
407 | : public fusion::JointUnaryVisitorBase< | ||
408 | AbaLocalConventionBackwardStep<Scalar, Options, JointCollectionTpl>> | ||
409 | { | ||
410 | typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model; | ||
411 | typedef DataTpl<Scalar, Options, JointCollectionTpl> Data; | ||
412 | |||
413 | typedef boost::fusion::vector<const Model &, Data &> ArgsType; | ||
414 | |||
415 | template<typename JointModel> | ||
416 | 54 | static void algo( | |
417 | const JointModelBase<JointModel> & jmodel, | ||
418 | JointDataBase<typename JointModel::JointDataDerived> & jdata, | ||
419 | const Model & model, | ||
420 | Data & data) | ||
421 | { | ||
422 | typedef typename Model::JointIndex JointIndex; | ||
423 | typedef typename Data::Inertia Inertia; | ||
424 | typedef typename Data::Force Force; | ||
425 | |||
426 | 54 | const JointIndex i = jmodel.id(); | |
427 | 54 | const JointIndex parent = model.parents[i]; | |
428 | 54 | typename Inertia::Matrix6 & Ia = data.Yaba[i]; | |
429 | |||
430 |
3/6✓ Branch 4 taken 27 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 27 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 27 times.
✗ Branch 11 not taken.
|
54 | jmodel.jointVelocitySelector(data.u) -= jdata.S().transpose() * data.f[i]; |
431 |
1/2✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
|
54 | jmodel.calc_aba( |
432 | 54 | jdata.derived(), jmodel.jointVelocitySelector(model.armature), Ia, parent > 0); | |
433 | |||
434 |
2/2✓ Branch 0 taken 26 times.
✓ Branch 1 taken 1 times.
|
54 | if (parent > 0) |
435 | { | ||
436 | 52 | Force & pa = data.f[i]; | |
437 |
4/8✓ Branch 1 taken 26 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 26 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 26 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 26 times.
✗ Branch 11 not taken.
|
52 | pa.toVector().noalias() += |
438 |
4/8✓ Branch 2 taken 26 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 26 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 26 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 26 times.
✗ Branch 13 not taken.
|
52 | Ia * data.a_gf[i].toVector() + jdata.UDinv() * jmodel.jointVelocitySelector(data.u); |
439 |
1/2✓ Branch 4 taken 26 times.
✗ Branch 5 not taken.
|
52 | data.Yaba[parent] += internal::SE3actOn<Scalar>::run(data.liMi[i], Ia); |
440 |
1/2✓ Branch 4 taken 26 times.
✗ Branch 5 not taken.
|
52 | data.f[parent] += data.liMi[i].act(pa); |
441 | } | ||
442 | } | ||
443 | }; | ||
444 | |||
445 | template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl> | ||
446 | struct AbaLocalConventionForwardStep2 | ||
447 | : public fusion::JointUnaryVisitorBase< | ||
448 | AbaLocalConventionForwardStep2<Scalar, Options, JointCollectionTpl>> | ||
449 | { | ||
450 | typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model; | ||
451 | typedef DataTpl<Scalar, Options, JointCollectionTpl> Data; | ||
452 | |||
453 | typedef boost::fusion::vector<const Model &, Data &> ArgsType; | ||
454 | |||
455 | template<typename JointModel> | ||
456 | 54 | static void algo( | |
457 | const pinocchio::JointModelBase<JointModel> & jmodel, | ||
458 | pinocchio::JointDataBase<typename JointModel::JointDataDerived> & jdata, | ||
459 | const Model & model, | ||
460 | Data & data) | ||
461 | { | ||
462 | typedef typename Model::JointIndex JointIndex; | ||
463 | |||
464 | 54 | const JointIndex i = jmodel.id(); | |
465 | 54 | const JointIndex parent = model.parents[i]; | |
466 | |||
467 |
1/2✓ Branch 5 taken 27 times.
✗ Branch 6 not taken.
|
54 | data.a_gf[i] += data.liMi[i].actInv(data.a_gf[parent]); |
468 |
4/8✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 27 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 27 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 27 times.
✗ Branch 11 not taken.
|
54 | jmodel.jointVelocitySelector(data.ddq).noalias() = |
469 |
3/6✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 27 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 27 times.
✗ Branch 8 not taken.
|
54 | jdata.Dinv() * jmodel.jointVelocitySelector(data.u) |
470 |
1/2✓ Branch 5 taken 27 times.
✗ Branch 6 not taken.
|
54 | - jdata.UDinv().transpose() * data.a_gf[i].toVector(); |
471 |
3/6✓ Branch 2 taken 27 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 27 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 27 times.
✗ Branch 10 not taken.
|
54 | data.a_gf[i] += jdata.S() * jmodel.jointVelocitySelector(data.ddq); |
472 | |||
473 | 54 | data.a[i] = data.a_gf[i]; | |
474 |
6/12✓ Branch 3 taken 27 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 27 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 27 times.
✗ Branch 10 not taken.
✓ Branch 13 taken 27 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 27 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 27 times.
✗ Branch 20 not taken.
|
54 | data.a[i].linear().noalias() += data.oMi[i].rotation().transpose() * model.gravity.linear(); |
475 |
3/6✓ Branch 6 taken 27 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 27 times.
✗ Branch 10 not taken.
✓ Branch 13 taken 27 times.
✗ Branch 14 not taken.
|
54 | data.f[i] = model.inertias[i] * data.a_gf[i] + data.v[i].cross(data.h[i]); |
476 | } | ||
477 | }; | ||
478 | |||
479 | template< | ||
480 | typename Scalar, | ||
481 | int Options, | ||
482 | template<typename, int> | ||
483 | class JointCollectionTpl, | ||
484 | typename ConfigVectorType, | ||
485 | typename TangentVectorType1, | ||
486 | typename TangentVectorType2> | ||
487 | const typename DataTpl<Scalar, Options, JointCollectionTpl>::TangentVectorType & | ||
488 | 2 | abaLocalConvention( | |
489 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
490 | DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
491 | const Eigen::MatrixBase<ConfigVectorType> & q, | ||
492 | const Eigen::MatrixBase<TangentVectorType1> & v, | ||
493 | const Eigen::MatrixBase<TangentVectorType2> & tau) | ||
494 | { | ||
495 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | assert(model.check(data) && "data is not consistent with model."); |
496 |
1/24✗ Branch 1 not taken.
✓ Branch 2 taken 1 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.
|
2 | PINOCCHIO_CHECK_ARGUMENT_SIZE( |
497 | q.size(), model.nq, "The joint configuration vector is not of right size"); | ||
498 |
1/24✗ Branch 1 not taken.
✓ Branch 2 taken 1 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.
|
2 | PINOCCHIO_CHECK_ARGUMENT_SIZE( |
499 | v.size(), model.nv, "The joint velocity vector is not of right size"); | ||
500 |
1/24✗ Branch 1 not taken.
✓ Branch 2 taken 1 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.
|
2 | PINOCCHIO_CHECK_ARGUMENT_SIZE( |
501 | tau.size(), model.nv, "The joint torque vector is not of right size"); | ||
502 | ; | ||
503 | |||
504 | typedef typename ModelTpl<Scalar, Options, JointCollectionTpl>::JointIndex JointIndex; | ||
505 | |||
506 | 2 | data.v[0].setZero(); | |
507 |
1/2✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
|
2 | data.a_gf[0] = -model.gravity; |
508 | 2 | data.f[0].setZero(); | |
509 | 2 | data.u = tau; | |
510 | |||
511 | typedef AbaLocalConventionForwardStep1< | ||
512 | Scalar, Options, JointCollectionTpl, ConfigVectorType, TangentVectorType1> | ||
513 | Pass1; | ||
514 |
2/2✓ Branch 0 taken 27 times.
✓ Branch 1 taken 1 times.
|
56 | for (JointIndex i = 1; i < (JointIndex)model.njoints; ++i) |
515 | { | ||
516 |
1/2✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
|
54 | Pass1::run( |
517 | 54 | model.joints[i], data.joints[i], | |
518 | 108 | typename Pass1::ArgsType(model, data, q.derived(), v.derived())); | |
519 | } | ||
520 | |||
521 | typedef AbaLocalConventionBackwardStep<Scalar, Options, JointCollectionTpl> Pass2; | ||
522 |
2/2✓ Branch 0 taken 27 times.
✓ Branch 1 taken 1 times.
|
56 | for (JointIndex i = (JointIndex)model.njoints - 1; i > 0; --i) |
523 | { | ||
524 |
1/2✓ Branch 4 taken 27 times.
✗ Branch 5 not taken.
|
54 | Pass2::run(model.joints[i], data.joints[i], typename Pass2::ArgsType(model, data)); |
525 | } | ||
526 | |||
527 | typedef AbaLocalConventionForwardStep2<Scalar, Options, JointCollectionTpl> Pass3; | ||
528 |
2/2✓ Branch 0 taken 27 times.
✓ Branch 1 taken 1 times.
|
56 | for (JointIndex i = 1; i < (JointIndex)model.njoints; ++i) |
529 | { | ||
530 |
1/2✓ Branch 4 taken 27 times.
✗ Branch 5 not taken.
|
54 | Pass3::run(model.joints[i], data.joints[i], typename Pass3::ArgsType(model, data)); |
531 | } | ||
532 | |||
533 |
2/2✓ Branch 0 taken 27 times.
✓ Branch 1 taken 1 times.
|
56 | for (JointIndex i = (JointIndex)model.njoints - 1; i > 0; --i) |
534 | { | ||
535 | 54 | const JointIndex parent = model.parents[i]; | |
536 |
1/2✓ Branch 5 taken 27 times.
✗ Branch 6 not taken.
|
54 | data.f[parent] += data.liMi[i].act(data.f[i]); |
537 | } | ||
538 | |||
539 | 2 | return data.ddq; | |
540 | } | ||
541 | |||
542 | template< | ||
543 | typename Scalar, | ||
544 | int Options, | ||
545 | template<typename, int> | ||
546 | class JointCollectionTpl, | ||
547 | typename ConfigVectorType, | ||
548 | typename TangentVectorType1, | ||
549 | typename TangentVectorType2, | ||
550 | typename ForceDerived> | ||
551 | const typename DataTpl<Scalar, Options, JointCollectionTpl>::TangentVectorType & | ||
552 | ✗ | abaLocalConvention( | |
553 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
554 | DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
555 | const Eigen::MatrixBase<ConfigVectorType> & q, | ||
556 | const Eigen::MatrixBase<TangentVectorType1> & v, | ||
557 | const Eigen::MatrixBase<TangentVectorType2> & tau, | ||
558 | const container::aligned_vector<ForceDerived> & fext) | ||
559 | |||
560 | { | ||
561 | ✗ | assert(model.check(data) && "data is not consistent with model."); | |
562 | ✗ | PINOCCHIO_CHECK_ARGUMENT_SIZE( | |
563 | q.size(), model.nq, "The joint configuration vector is not of right size"); | ||
564 | ✗ | PINOCCHIO_CHECK_ARGUMENT_SIZE( | |
565 | v.size(), model.nv, "The joint velocity vector is not of right size"); | ||
566 | ✗ | PINOCCHIO_CHECK_ARGUMENT_SIZE( | |
567 | tau.size(), model.nv, "The joint torque vector is not of right size"); | ||
568 | |||
569 | typedef typename ModelTpl<Scalar, Options, JointCollectionTpl>::JointIndex JointIndex; | ||
570 | |||
571 | ✗ | data.v[0].setZero(); | |
572 | ✗ | data.a_gf[0] = -model.gravity; | |
573 | ✗ | data.u = tau; | |
574 | |||
575 | typedef AbaLocalConventionForwardStep1< | ||
576 | Scalar, Options, JointCollectionTpl, ConfigVectorType, TangentVectorType1> | ||
577 | Pass1; | ||
578 | ✗ | for (JointIndex i = 1; i < (JointIndex)model.njoints; ++i) | |
579 | { | ||
580 | ✗ | Pass1::run( | |
581 | ✗ | model.joints[i], data.joints[i], | |
582 | ✗ | typename Pass1::ArgsType(model, data, q.derived(), v.derived())); | |
583 | ✗ | data.f[i] -= fext[i]; | |
584 | } | ||
585 | |||
586 | typedef AbaLocalConventionBackwardStep<Scalar, Options, JointCollectionTpl> Pass2; | ||
587 | ✗ | for (JointIndex i = (JointIndex)model.njoints - 1; i > 0; --i) | |
588 | { | ||
589 | ✗ | Pass2::run(model.joints[i], data.joints[i], typename Pass2::ArgsType(model, data)); | |
590 | } | ||
591 | |||
592 | typedef AbaLocalConventionForwardStep2<Scalar, Options, JointCollectionTpl> Pass3; | ||
593 | ✗ | for (JointIndex i = 1; i < (JointIndex)model.njoints; ++i) | |
594 | { | ||
595 | ✗ | Pass3::run(model.joints[i], data.joints[i], typename Pass3::ArgsType(model, data)); | |
596 | } | ||
597 | |||
598 | ✗ | for (JointIndex i = (JointIndex)model.njoints - 1; i > 0; --i) | |
599 | { | ||
600 | ✗ | const JointIndex parent = model.parents[i]; | |
601 | ✗ | data.f[parent] += data.liMi[i].act(data.f[i]); | |
602 | } | ||
603 | |||
604 | ✗ | return data.ddq; | |
605 | } | ||
606 | |||
607 | template< | ||
608 | typename Scalar, | ||
609 | int Options, | ||
610 | template<typename, int> | ||
611 | class JointCollectionTpl, | ||
612 | typename ConfigVectorType> | ||
613 | struct ComputeMinverseForwardStep1 | ||
614 | : public fusion::JointUnaryVisitorBase< | ||
615 | ComputeMinverseForwardStep1<Scalar, Options, JointCollectionTpl, ConfigVectorType>> | ||
616 | { | ||
617 | typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model; | ||
618 | typedef DataTpl<Scalar, Options, JointCollectionTpl> Data; | ||
619 | |||
620 | typedef boost::fusion::vector<const Model &, Data &, const ConfigVectorType &> ArgsType; | ||
621 | |||
622 | template<typename JointModel> | ||
623 | ✗ | static void algo( | |
624 | const pinocchio::JointModelBase<JointModel> & jmodel, | ||
625 | pinocchio::JointDataBase<typename JointModel::JointDataDerived> & jdata, | ||
626 | const Model & model, | ||
627 | Data & data, | ||
628 | const Eigen::MatrixBase<ConfigVectorType> & q) | ||
629 | { | ||
630 | typedef typename Model::JointIndex JointIndex; | ||
631 | |||
632 | ✗ | const JointIndex & i = jmodel.id(); | |
633 | ✗ | jmodel.calc(jdata.derived(), q.derived()); | |
634 | |||
635 | ✗ | const JointIndex & parent = model.parents[i]; | |
636 | ✗ | data.liMi[i] = model.jointPlacements[i] * jdata.M(); | |
637 | |||
638 | ✗ | if (parent > 0) | |
639 | ✗ | data.oMi[i] = data.oMi[parent] * data.liMi[i]; | |
640 | else | ||
641 | ✗ | data.oMi[i] = data.liMi[i]; | |
642 | |||
643 | typedef | ||
644 | typename SizeDepType<JointModel::NV>::template ColsReturn<typename Data::Matrix6x>::Type | ||
645 | ColsBlock; | ||
646 | ✗ | ColsBlock J_cols = jmodel.jointCols(data.J); | |
647 | ✗ | J_cols = data.oMi[i].act(jdata.S()); | |
648 | |||
649 | ✗ | data.oYcrb[i] = data.oMi[i].act(model.inertias[i]); | |
650 | ✗ | data.oYaba[i] = data.oYcrb[i].matrix(); | |
651 | } | ||
652 | }; | ||
653 | |||
654 | template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl> | ||
655 | struct ComputeMinverseBackwardStep | ||
656 | : public fusion::JointUnaryVisitorBase< | ||
657 | ComputeMinverseBackwardStep<Scalar, Options, JointCollectionTpl>> | ||
658 | { | ||
659 | typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model; | ||
660 | typedef DataTpl<Scalar, Options, JointCollectionTpl> Data; | ||
661 | |||
662 | typedef boost::fusion::vector<const Model &, Data &> ArgsType; | ||
663 | |||
664 | template<typename JointModel> | ||
665 | ✗ | static void algo( | |
666 | const JointModelBase<JointModel> & jmodel, | ||
667 | JointDataBase<typename JointModel::JointDataDerived> & jdata, | ||
668 | const Model & model, | ||
669 | Data & data) | ||
670 | { | ||
671 | typedef typename Model::JointIndex JointIndex; | ||
672 | typedef typename Data::Inertia Inertia; | ||
673 | |||
674 | ✗ | const JointIndex i = jmodel.id(); | |
675 | ✗ | const JointIndex parent = model.parents[i]; | |
676 | |||
677 | ✗ | typename Inertia::Matrix6 & Ia = data.oYaba[i]; | |
678 | ✗ | typename Data::RowMatrixXs & Minv = data.Minv; | |
679 | ✗ | typename Data::Matrix6x & Fcrb = data.Fcrb[0]; | |
680 | typedef | ||
681 | typename SizeDepType<JointModel::NV>::template ColsReturn<typename Data::Matrix6x>::Type | ||
682 | ColsBlock; | ||
683 | |||
684 | ✗ | ColsBlock J_cols = jmodel.jointCols(data.J); | |
685 | |||
686 | ✗ | jdata.U().noalias() = Ia * J_cols; | |
687 | ✗ | jdata.StU().noalias() = J_cols.transpose() * jdata.U(); | |
688 | |||
689 | // Account for the rotor inertia contribution | ||
690 | ✗ | jdata.StU().diagonal() += jmodel.jointVelocitySelector(model.armature); | |
691 | |||
692 | ✗ | ::pinocchio::internal::PerformStYSInversion<Scalar>::run(jdata.StU(), jdata.Dinv()); | |
693 | ✗ | jdata.UDinv().noalias() = jdata.U() * jdata.Dinv(); | |
694 | |||
695 | ✗ | Minv.block(jmodel.idx_v(), jmodel.idx_v(), jmodel.nv(), jmodel.nv()) = jdata.Dinv(); | |
696 | ✗ | const int nv_children = data.nvSubtree[i] - jmodel.nv(); | |
697 | ✗ | if (nv_children > 0) | |
698 | { | ||
699 | ✗ | ColsBlock SDinv_cols = jmodel.jointCols(data.SDinv); | |
700 | ✗ | SDinv_cols.noalias() = J_cols * jdata.Dinv(); | |
701 | |||
702 | ✗ | Minv.block(jmodel.idx_v(), jmodel.idx_v() + jmodel.nv(), jmodel.nv(), nv_children) | |
703 | ✗ | .noalias() = | |
704 | ✗ | -SDinv_cols.transpose() * Fcrb.middleCols(jmodel.idx_v() + jmodel.nv(), nv_children); | |
705 | |||
706 | ✗ | if (parent > 0) | |
707 | { | ||
708 | ✗ | Fcrb.middleCols(jmodel.idx_v(), data.nvSubtree[i]).noalias() += | |
709 | ✗ | jdata.U() | |
710 | ✗ | * Minv.block(jmodel.idx_v(), jmodel.idx_v(), jmodel.nv(), data.nvSubtree[i]); | |
711 | ; | ||
712 | } | ||
713 | } | ||
714 | else | ||
715 | { | ||
716 | ✗ | Fcrb.middleCols(jmodel.idx_v(), data.nvSubtree[i]).noalias() = | |
717 | ✗ | jdata.U() * Minv.block(jmodel.idx_v(), jmodel.idx_v(), jmodel.nv(), data.nvSubtree[i]); | |
718 | } | ||
719 | |||
720 | ✗ | if (parent > 0) | |
721 | { | ||
722 | ✗ | Ia.noalias() -= jdata.UDinv() * jdata.U().transpose(); | |
723 | ✗ | data.oYaba[parent] += Ia; | |
724 | } | ||
725 | } | ||
726 | }; | ||
727 | |||
728 | namespace optimized | ||
729 | { | ||
730 | template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl> | ||
731 | struct ComputeMinverseBackwardStep | ||
732 | : public fusion::JointUnaryVisitorBase< | ||
733 | ComputeMinverseBackwardStep<Scalar, Options, JointCollectionTpl>> | ||
734 | { | ||
735 | typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model; | ||
736 | typedef DataTpl<Scalar, Options, JointCollectionTpl> Data; | ||
737 | |||
738 | typedef boost::fusion::vector<const Model &, Data &> ArgsType; | ||
739 | |||
740 | template<typename JointModel> | ||
741 | 54 | static void algo( | |
742 | const JointModelBase<JointModel> & jmodel, | ||
743 | JointDataBase<typename JointModel::JointDataDerived> & jdata, | ||
744 | const Model & model, | ||
745 | Data & data) | ||
746 | { | ||
747 | typedef typename Model::JointIndex JointIndex; | ||
748 | |||
749 |
1/2✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
|
54 | const JointIndex i = jmodel.id(); |
750 | 54 | const JointIndex parent = model.parents[i]; | |
751 | |||
752 | 54 | typename Data::RowMatrixXs & Minv = data.Minv; | |
753 | 54 | typename Data::Matrix6x & Fcrb = data.Fcrb[0]; | |
754 | typedef | ||
755 | typename SizeDepType<JointModel::NV>::template ColsReturn<typename Data::Matrix6x>::Type | ||
756 | ColsBlock; | ||
757 | |||
758 |
1/2✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
|
54 | const ColsBlock J_cols = jmodel.jointCols(data.J); |
759 | |||
760 |
7/14✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 27 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 27 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 27 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 27 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 27 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 27 times.
✗ Branch 20 not taken.
|
54 | Minv.block(jmodel.idx_v(), jmodel.idx_v(), jmodel.nv(), jmodel.nv()) = jdata.Dinv(); |
761 |
1/2✓ Branch 2 taken 27 times.
✗ Branch 3 not taken.
|
54 | const int nv_children = data.nvSubtree[i] - jmodel.nv(); |
762 |
2/2✓ Branch 0 taken 23 times.
✓ Branch 1 taken 4 times.
|
54 | if (nv_children > 0) |
763 | { | ||
764 |
1/2✓ Branch 1 taken 23 times.
✗ Branch 2 not taken.
|
46 | ColsBlock SDinv_cols = jmodel.jointCols(data.SDinv); |
765 |
4/8✓ Branch 1 taken 23 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 23 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 23 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 23 times.
✗ Branch 11 not taken.
|
46 | SDinv_cols.noalias() = J_cols * jdata.Dinv(); |
766 |
5/10✓ Branch 1 taken 23 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 23 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 23 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 23 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 23 times.
✗ Branch 14 not taken.
|
46 | Minv.block(jmodel.idx_v(), jmodel.idx_v() + jmodel.nv(), jmodel.nv(), nv_children) |
767 |
4/8✓ Branch 1 taken 23 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 23 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 23 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 23 times.
✗ Branch 11 not taken.
|
92 | .noalias() = |
768 |
4/8✓ Branch 1 taken 23 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 23 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 23 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 23 times.
✗ Branch 11 not taken.
|
46 | -SDinv_cols.transpose() * Fcrb.middleCols(jmodel.idx_v() + jmodel.nv(), nv_children); |
769 | |||
770 |
2/2✓ Branch 0 taken 22 times.
✓ Branch 1 taken 1 times.
|
46 | if (parent > 0) |
771 | { | ||
772 |
4/8✓ Branch 2 taken 22 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 22 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 22 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 22 times.
✗ Branch 12 not taken.
|
44 | Fcrb.middleCols(jmodel.idx_v(), data.nvSubtree[i]).noalias() += |
773 |
1/2✓ Branch 1 taken 22 times.
✗ Branch 2 not taken.
|
44 | jdata.U() |
774 |
5/10✓ Branch 2 taken 22 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 22 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 22 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 22 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 22 times.
✗ Branch 15 not taken.
|
132 | * Minv.block(jmodel.idx_v(), jmodel.idx_v(), jmodel.nv(), data.nvSubtree[i]); |
775 | ; | ||
776 | } | ||
777 | } | ||
778 | else | ||
779 | { | ||
780 |
4/8✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 4 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 4 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 4 times.
✗ Branch 12 not taken.
|
8 | Fcrb.middleCols(jmodel.idx_v(), data.nvSubtree[i]).noalias() = |
781 |
1/2✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
|
8 | jdata.U() |
782 |
5/10✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 4 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 4 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 4 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 4 times.
✗ Branch 15 not taken.
|
24 | * Minv.block(jmodel.idx_v(), jmodel.idx_v(), jmodel.nv(), data.nvSubtree[i]); |
783 | } | ||
784 | } | ||
785 | }; | ||
786 | } // namespace optimized | ||
787 | |||
788 | template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl> | ||
789 | struct ComputeMinverseForwardStep2 | ||
790 | : public fusion::JointUnaryVisitorBase< | ||
791 | ComputeMinverseForwardStep2<Scalar, Options, JointCollectionTpl>> | ||
792 | { | ||
793 | typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model; | ||
794 | typedef DataTpl<Scalar, Options, JointCollectionTpl> Data; | ||
795 | |||
796 | typedef boost::fusion::vector<const Model &, Data &> ArgsType; | ||
797 | |||
798 | template<typename JointModel> | ||
799 | ✗ | static void algo( | |
800 | const pinocchio::JointModelBase<JointModel> & jmodel, | ||
801 | pinocchio::JointDataBase<typename JointModel::JointDataDerived> & jdata, | ||
802 | const Model & model, | ||
803 | Data & data) | ||
804 | { | ||
805 | typedef typename Model::JointIndex JointIndex; | ||
806 | |||
807 | ✗ | const JointIndex i = jmodel.id(); | |
808 | ✗ | const JointIndex parent = model.parents[i]; | |
809 | ✗ | typename Data::RowMatrixXs & Minv = data.Minv; | |
810 | |||
811 | typedef | ||
812 | typename SizeDepType<JointModel::NV>::template ColsReturn<typename Data::Matrix6x>::Type | ||
813 | ColsBlock; | ||
814 | ✗ | ColsBlock J_cols = jmodel.jointCols(data.J); | |
815 | |||
816 | ✗ | if (parent > 0) | |
817 | { | ||
818 | ✗ | Minv.middleRows(jmodel.idx_v(), jmodel.nv()) | |
819 | ✗ | .rightCols(model.nv - jmodel.idx_v()) | |
820 | ✗ | .noalias() -= | |
821 | ✗ | jdata.UDinv().transpose() * data.Fcrb[parent].rightCols(model.nv - jmodel.idx_v()); | |
822 | } | ||
823 | |||
824 | ✗ | data.Fcrb[i].rightCols(model.nv - jmodel.idx_v()).noalias() = | |
825 | J_cols | ||
826 | ✗ | * Minv.middleRows(jmodel.idx_v(), jmodel.nv()).rightCols(model.nv - jmodel.idx_v()); | |
827 | ✗ | if (parent > 0) | |
828 | ✗ | data.Fcrb[i].rightCols(model.nv - jmodel.idx_v()) += | |
829 | ✗ | data.Fcrb[parent].rightCols(model.nv - jmodel.idx_v()); | |
830 | } | ||
831 | }; | ||
832 | |||
833 | template< | ||
834 | typename Scalar, | ||
835 | int Options, | ||
836 | template<typename, int> | ||
837 | class JointCollectionTpl, | ||
838 | typename ConfigVectorType> | ||
839 | ✗ | const typename DataTpl<Scalar, Options, JointCollectionTpl>::RowMatrixXs & computeMinverse( | |
840 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
841 | DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
842 | const Eigen::MatrixBase<ConfigVectorType> & q) | ||
843 | { | ||
844 | ✗ | assert(model.check(data) && "data is not consistent with model."); | |
845 | ✗ | PINOCCHIO_CHECK_ARGUMENT_SIZE( | |
846 | q.size(), model.nq, "The joint configuration vector is not of right size"); | ||
847 | |||
848 | typedef typename ModelTpl<Scalar, Options, JointCollectionTpl>::JointIndex JointIndex; | ||
849 | ✗ | data.Minv.template triangularView<Eigen::Upper>().setZero(); | |
850 | |||
851 | typedef ComputeMinverseForwardStep1<Scalar, Options, JointCollectionTpl, ConfigVectorType> | ||
852 | Pass1; | ||
853 | ✗ | for (JointIndex i = 1; i < (JointIndex)model.njoints; ++i) | |
854 | { | ||
855 | ✗ | Pass1::run( | |
856 | ✗ | model.joints[i], data.joints[i], typename Pass1::ArgsType(model, data, q.derived())); | |
857 | } | ||
858 | |||
859 | ✗ | data.Fcrb[0].setZero(); | |
860 | typedef ComputeMinverseBackwardStep<Scalar, Options, JointCollectionTpl> Pass2; | ||
861 | ✗ | for (JointIndex i = (JointIndex)model.njoints - 1; i > 0; --i) | |
862 | { | ||
863 | ✗ | Pass2::run(model.joints[i], data.joints[i], typename Pass2::ArgsType(model, data)); | |
864 | } | ||
865 | |||
866 | typedef ComputeMinverseForwardStep2<Scalar, Options, JointCollectionTpl> Pass3; | ||
867 | ✗ | for (JointIndex i = 1; i < (JointIndex)model.njoints; ++i) | |
868 | { | ||
869 | ✗ | Pass3::run(model.joints[i], data.joints[i], typename Pass3::ArgsType(model, data)); | |
870 | } | ||
871 | |||
872 | ✗ | return data.Minv; | |
873 | } | ||
874 | } // namespace impl | ||
875 | |||
876 | template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl> | ||
877 | 1 | const typename DataTpl<Scalar, Options, JointCollectionTpl>::RowMatrixXs & computeMinverse( | |
878 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
879 | DataTpl<Scalar, Options, JointCollectionTpl> & data) | ||
880 | { | ||
881 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | assert(model.check(data) && "data is not consistent with model."); |
882 | |||
883 | typedef typename ModelTpl<Scalar, Options, JointCollectionTpl>::JointIndex JointIndex; | ||
884 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
1 | data.Minv.template triangularView<Eigen::Upper>().setZero(); |
885 | |||
886 | 1 | data.Fcrb[0].setZero(); | |
887 | typedef impl::optimized::ComputeMinverseBackwardStep<Scalar, Options, JointCollectionTpl> Pass2; | ||
888 |
2/2✓ Branch 0 taken 27 times.
✓ Branch 1 taken 1 times.
|
28 | for (JointIndex i = (JointIndex)model.njoints - 1; i > 0; --i) |
889 | { | ||
890 |
1/2✓ Branch 4 taken 27 times.
✗ Branch 5 not taken.
|
27 | Pass2::run(model.joints[i], data.joints[i], typename Pass2::ArgsType(model, data)); |
891 | } | ||
892 | |||
893 | typedef impl::ComputeMinverseForwardStep2<Scalar, Options, JointCollectionTpl> Pass3; | ||
894 |
2/2✓ Branch 0 taken 27 times.
✓ Branch 1 taken 1 times.
|
28 | for (JointIndex i = 1; i < (JointIndex)model.njoints; ++i) |
895 | { | ||
896 |
1/2✓ Branch 4 taken 27 times.
✗ Branch 5 not taken.
|
27 | Pass3::run(model.joints[i], data.joints[i], typename Pass3::ArgsType(model, data)); |
897 | } | ||
898 | |||
899 | 1 | return data.Minv; | |
900 | } | ||
901 | |||
902 | // --- CHECKER --------------------------------------------------------------- | ||
903 | // --- CHECKER --------------------------------------------------------------- | ||
904 | // --- CHECKER --------------------------------------------------------------- | ||
905 | |||
906 | // Check whether all masses are nonzero and diagonal of inertia is nonzero | ||
907 | // The second test is overconstraining. | ||
908 | template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl> | ||
909 | inline bool | ||
910 | ✗ | ABAChecker::checkModel_impl(const ModelTpl<Scalar, Options, JointCollectionTpl> & model) const | |
911 | { | ||
912 | typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model; | ||
913 | typedef typename Model::JointIndex JointIndex; | ||
914 | |||
915 | ✗ | for (JointIndex j = 1; j < (JointIndex)model.njoints; j++) | |
916 | ✗ | if ( | |
917 | ✗ | (model.inertias[j].mass() < 1e-5) || (model.inertias[j].inertia().data()[0] < 1e-5) | |
918 | ✗ | || (model.inertias[j].inertia().data()[2] < 1e-5) | |
919 | ✗ | || (model.inertias[j].inertia().data()[5] < 1e-5)) | |
920 | ✗ | return false; | |
921 | ✗ | return true; | |
922 | } | ||
923 | |||
924 | template< | ||
925 | typename Scalar, | ||
926 | int Options, | ||
927 | template<typename, int> | ||
928 | class JointCollectionTpl, | ||
929 | typename ConfigVectorType, | ||
930 | typename TangentVectorType1, | ||
931 | typename TangentVectorType2> | ||
932 | 6 | const typename DataTpl<Scalar, Options, JointCollectionTpl>::TangentVectorType & aba( | |
933 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
934 | DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
935 | const Eigen::MatrixBase<ConfigVectorType> & q, | ||
936 | const Eigen::MatrixBase<TangentVectorType1> & v, | ||
937 | const Eigen::MatrixBase<TangentVectorType2> & tau, | ||
938 | const Convention convention) | ||
939 | { | ||
940 |
2/3✓ Branch 0 taken 1 times.
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
|
6 | switch (convention) |
941 | { | ||
942 | 2 | case Convention::LOCAL: | |
943 | 2 | return impl::abaLocalConvention(model, data, q, v, tau); | |
944 | 4 | case Convention::WORLD: | |
945 | 4 | return impl::abaWorldConvention(model, data, q, v, tau); | |
946 | } | ||
947 | } | ||
948 | |||
949 | template< | ||
950 | typename Scalar, | ||
951 | int Options, | ||
952 | template<typename, int> | ||
953 | class JointCollectionTpl, | ||
954 | typename ConfigVectorType, | ||
955 | typename TangentVectorType1, | ||
956 | typename TangentVectorType2, | ||
957 | typename ForceDerived> | ||
958 | ✗ | const typename DataTpl<Scalar, Options, JointCollectionTpl>::TangentVectorType & aba( | |
959 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
960 | DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
961 | const Eigen::MatrixBase<ConfigVectorType> & q, | ||
962 | const Eigen::MatrixBase<TangentVectorType1> & v, | ||
963 | const Eigen::MatrixBase<TangentVectorType2> & tau, | ||
964 | const container::aligned_vector<ForceDerived> & fext, | ||
965 | const Convention convention) | ||
966 | { | ||
967 | ✗ | switch (convention) | |
968 | { | ||
969 | ✗ | case Convention::LOCAL: | |
970 | ✗ | return impl::abaLocalConvention(model, data, q, v, tau, fext); | |
971 | ✗ | case Convention::WORLD: | |
972 | ✗ | return impl::abaWorldConvention(model, data, q, v, tau, fext); | |
973 | } | ||
974 | } | ||
975 | |||
976 | template< | ||
977 | typename Scalar, | ||
978 | int Options, | ||
979 | template<typename, int> | ||
980 | class JointCollectionTpl, | ||
981 | typename ConfigVectorType> | ||
982 | ✗ | const typename DataTpl<Scalar, Options, JointCollectionTpl>::RowMatrixXs & computeMinverse( | |
983 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
984 | DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
985 | const Eigen::MatrixBase<ConfigVectorType> & q) | ||
986 | { | ||
987 | ✗ | return impl::computeMinverse(model, data, make_const_ref(q)); | |
988 | } | ||
989 | |||
990 | } // namespace pinocchio | ||
991 | |||
992 | /// @endcond | ||
993 | |||
994 | #endif // ifndef __pinocchio_algorithm_aba_hxx__ | ||
995 |