| GCC Code Coverage Report | |||||||||||||||||||||
|
|||||||||||||||||||||
| Line | Branch | Exec | Source |
1 |
// |
||
2 |
// Copyright (c) 2016-2020 CNRS, INRIA |
||
3 |
// |
||
4 |
|||
5 |
#ifndef __pinocchio_aba_hxx__ |
||
6 |
#define __pinocchio_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 |
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType> |
||
17 |
struct AbaForwardStep1 |
||
18 |
: public fusion::JointUnaryVisitorBase< AbaForwardStep1<Scalar,Options,JointCollectionTpl,ConfigVectorType,TangentVectorType> > |
||
19 |
{ |
||
20 |
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model; |
||
21 |
typedef DataTpl<Scalar,Options,JointCollectionTpl> Data; |
||
22 |
|||
23 |
typedef boost::fusion::vector<const Model &, |
||
24 |
Data &, |
||
25 |
const ConfigVectorType &, |
||
26 |
const TangentVectorType & |
||
27 |
> ArgsType; |
||
28 |
|||
29 |
template<typename JointModel> |
||
30 |
10936 |
static void algo(const pinocchio::JointModelBase<JointModel> & jmodel, |
|
31 |
pinocchio::JointDataBase<typename JointModel::JointDataDerived> & jdata, |
||
32 |
const Model & model, |
||
33 |
Data & data, |
||
34 |
const Eigen::MatrixBase<ConfigVectorType> & q, |
||
35 |
const Eigen::MatrixBase<TangentVectorType> & v) |
||
36 |
{ |
||
37 |
typedef typename Model::JointIndex JointIndex; |
||
38 |
|||
39 |
10936 |
const JointIndex & i = jmodel.id(); |
|
40 |
10936 |
jmodel.calc(jdata.derived(),q.derived(),v.derived()); |
|
41 |
|||
42 |
10936 |
const JointIndex & parent = model.parents[i]; |
|
43 |
✓✗ | 10936 |
data.liMi[i] = model.jointPlacements[i] * jdata.M(); |
44 |
|||
45 |
10936 |
data.v[i] = jdata.v(); |
|
46 |
✓✓ | 10936 |
if (parent>0) |
47 |
✓✗ | 10504 |
data.v[i] += data.liMi[i].actInv(data.v[parent]); |
48 |
|||
49 |
✓✗✗✗ ✓✗ |
10936 |
data.a_gf[i] = jdata.c() + (data.v[i] ^ jdata.v()); |
50 |
|||
51 |
10936 |
data.Yaba[i] = model.inertias[i].matrix(); |
|
52 |
✓✗ | 10936 |
data.f[i] = model.inertias[i].vxiv(data.v[i]); // -f_ext |
53 |
} |
||
54 |
|||
55 |
}; |
||
56 |
|||
57 |
namespace internal |
||
58 |
{ |
||
59 |
|||
60 |
template<typename Scalar> |
||
61 |
struct SE3actOn |
||
62 |
{ |
||
63 |
template<int Options, typename Matrix6Type> |
||
64 |
static typename PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix6Type) |
||
65 |
6609 |
run(const SE3Tpl<Scalar,Options> & M, |
|
66 |
const Eigen::MatrixBase<Matrix6Type> & I) |
||
67 |
{ |
||
68 |
typedef SE3Tpl<Scalar,Options> SE3; |
||
69 |
typedef typename SE3::Matrix3 Matrix3; |
||
70 |
typedef typename SE3::Vector3 Vector3; |
||
71 |
|||
72 |
typedef const Eigen::Block<Matrix6Type,3,3> constBlock3; |
||
73 |
|||
74 |
typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix6Type) ReturnType; |
||
75 |
typedef Eigen::Block<ReturnType,3,3> Block3; |
||
76 |
|||
77 |
6609 |
Matrix6Type & I_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix6Type,I); |
|
78 |
✓✗ | 6609 |
const constBlock3 & Ai = I_.template block<3,3>(Inertia::LINEAR, Inertia::LINEAR); |
79 |
✓✗ | 6609 |
const constBlock3 & Bi = I_.template block<3,3>(Inertia::LINEAR, Inertia::ANGULAR); |
80 |
✓✗ | 6609 |
const constBlock3 & Di = I_.template block<3,3>(Inertia::ANGULAR, Inertia::ANGULAR); |
81 |
|||
82 |
✓✗ | 6609 |
const Matrix3 & R = M.rotation(); |
83 |
✓✗ | 6609 |
const Vector3 & t = M.translation(); |
84 |
|||
85 |
✓✗ | 6609 |
ReturnType res; |
86 |
✓✗ | 6609 |
Block3 Ao = res.template block<3,3>(Inertia::LINEAR, Inertia::LINEAR); |
87 |
✓✗ | 6609 |
Block3 Bo = res.template block<3,3>(Inertia::LINEAR, Inertia::ANGULAR); |
88 |
✓✗ | 6609 |
Block3 Co = res.template block<3,3>(Inertia::ANGULAR, Inertia::LINEAR); |
89 |
✓✗ | 6609 |
Block3 Do = res.template block<3,3>(Inertia::ANGULAR, Inertia::ANGULAR); |
90 |
|||
91 |
✓✗✓✗ ✓✗ |
6609 |
Do.noalias() = R*Ai; // tmp variable |
92 |
✓✗✓✗ ✓✗✓✗ |
6609 |
Ao.noalias() = Do*R.transpose(); |
93 |
|||
94 |
✓✗✓✗ ✓✗ |
6609 |
Do.noalias() = R*Bi; // tmp variable |
95 |
✓✗✓✗ ✓✗✓✗ |
6609 |
Bo.noalias() = Do*R.transpose(); |
96 |
|||
97 |
✓✗✓✗ ✓✗ |
6609 |
Co.noalias() = R*Di; // tmp variable |
98 |
✓✗✓✗ ✓✗✓✗ |
6609 |
Do.noalias() = Co*R.transpose(); |
99 |
|||
100 |
✓✗✓✗ ✓✗✓✗ |
6609 |
Do.row(0) += t.cross(Bo.col(0)); |
101 |
✓✗✓✗ ✓✗✓✗ |
6609 |
Do.row(1) += t.cross(Bo.col(1)); |
102 |
✓✗✓✗ ✓✗✓✗ |
6609 |
Do.row(2) += t.cross(Bo.col(2)); |
103 |
|||
104 |
✓✗✓✗ ✓✗✓✗ |
6609 |
Co.col(0) = t.cross(Ao.col(0)); |
105 |
✓✗✓✗ ✓✗✓✗ |
6609 |
Co.col(1) = t.cross(Ao.col(1)); |
106 |
✓✗✓✗ ✓✗✓✗ |
6609 |
Co.col(2) = t.cross(Ao.col(2)); |
107 |
✓✗✓✗ |
6609 |
Co += Bo.transpose(); |
108 |
|||
109 |
✓✗✓✗ |
6609 |
Bo = Co.transpose(); |
110 |
✓✗✓✗ ✓✗✓✗ |
6609 |
Do.col(0) += t.cross(Bo.col(0)); |
111 |
✓✗✓✗ ✓✗✓✗ |
6609 |
Do.col(1) += t.cross(Bo.col(1)); |
112 |
✓✗✓✗ ✓✗✓✗ |
6609 |
Do.col(2) += t.cross(Bo.col(2)); |
113 |
|||
114 |
13218 |
return res; |
|
115 |
} |
||
116 |
}; |
||
117 |
|||
118 |
} |
||
119 |
|||
120 |
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl> |
||
121 |
struct AbaBackwardStep |
||
122 |
: public fusion::JointUnaryVisitorBase< AbaBackwardStep<Scalar,Options,JointCollectionTpl> > |
||
123 |
{ |
||
124 |
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model; |
||
125 |
typedef DataTpl<Scalar,Options,JointCollectionTpl> Data; |
||
126 |
|||
127 |
typedef boost::fusion::vector<const Model &, |
||
128 |
Data &> ArgsType; |
||
129 |
|||
130 |
template<typename JointModel> |
||
131 |
10936 |
static void algo(const JointModelBase<JointModel> & jmodel, |
|
132 |
JointDataBase<typename JointModel::JointDataDerived> & jdata, |
||
133 |
const Model & model, |
||
134 |
Data & data) |
||
135 |
{ |
||
136 |
typedef typename Model::JointIndex JointIndex; |
||
137 |
typedef typename Data::Inertia Inertia; |
||
138 |
typedef typename Data::Force Force; |
||
139 |
|||
140 |
10936 |
const JointIndex & i = jmodel.id(); |
|
141 |
10936 |
const JointIndex & parent = model.parents[i]; |
|
142 |
10936 |
typename Inertia::Matrix6 & Ia = data.Yaba[i]; |
|
143 |
|||
144 |
✓✗✓✗ ✓✗ |
10936 |
jmodel.jointVelocitySelector(data.u) -= jdata.S().transpose()*data.f[i]; |
145 |
10936 |
jmodel.calc_aba(jdata.derived(), Ia, parent > 0); |
|
146 |
|||
147 |
✓✓ | 10936 |
if (parent > 0) |
148 |
{ |
||
149 |
10504 |
Force & pa = data.f[i]; |
|
150 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
10504 |
pa.toVector() += Ia * data.a_gf[i].toVector() + jdata.UDinv() * jmodel.jointVelocitySelector(data.u); |
151 |
✓✗ | 10504 |
data.Yaba[parent] += internal::SE3actOn<Scalar>::run(data.liMi[i], Ia); |
152 |
✓✗ | 10504 |
data.f[parent] += data.liMi[i].act(pa); |
153 |
} |
||
154 |
} |
||
155 |
|||
156 |
}; |
||
157 |
|||
158 |
|||
159 |
|||
160 |
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl> |
||
161 |
struct AbaForwardStep2 |
||
162 |
: public fusion::JointUnaryVisitorBase< AbaForwardStep2<Scalar,Options,JointCollectionTpl> > |
||
163 |
{ |
||
164 |
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model; |
||
165 |
typedef DataTpl<Scalar,Options,JointCollectionTpl> Data; |
||
166 |
|||
167 |
typedef boost::fusion::vector<const Model &, |
||
168 |
Data &> ArgsType; |
||
169 |
|||
170 |
template<typename JointModel> |
||
171 |
10936 |
static void algo(const pinocchio::JointModelBase<JointModel> & jmodel, |
|
172 |
pinocchio::JointDataBase<typename JointModel::JointDataDerived> & jdata, |
||
173 |
const Model & model, |
||
174 |
Data & data) |
||
175 |
{ |
||
176 |
typedef typename Model::JointIndex JointIndex; |
||
177 |
|||
178 |
10936 |
const JointIndex & i = jmodel.id(); |
|
179 |
10936 |
const JointIndex & parent = model.parents[i]; |
|
180 |
|||
181 |
✓✗ | 10936 |
data.a_gf[i] += data.liMi[i].actInv(data.a_gf[parent]); |
182 |
✓✗✓✗ ✓✗✓✗ |
10936 |
jmodel.jointVelocitySelector(data.ddq).noalias() = |
183 |
✓✗✓✗ ✓✗✓✗ |
10936 |
jdata.Dinv() * jmodel.jointVelocitySelector(data.u) - jdata.UDinv().transpose() * data.a_gf[i].toVector(); |
184 |
✓✗✓✗ ✓✗ |
10936 |
data.a_gf[i] += jdata.S() * jmodel.jointVelocitySelector(data.ddq); |
185 |
} |
||
186 |
|||
187 |
}; |
||
188 |
|||
189 |
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType1, typename TangentVectorType2> |
||
190 |
inline const typename DataTpl<Scalar,Options,JointCollectionTpl>::TangentVectorType & |
||
191 |
117 |
aba(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
|
192 |
DataTpl<Scalar,Options,JointCollectionTpl> & data, |
||
193 |
const Eigen::MatrixBase<ConfigVectorType> & q, |
||
194 |
const Eigen::MatrixBase<TangentVectorType1> & v, |
||
195 |
const Eigen::MatrixBase<TangentVectorType2> & tau) |
||
196 |
{ |
||
197 |
✓✗ | 117 |
assert(model.check(data) && "data is not consistent with model."); |
198 |
✗✓✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗ |
117 |
PINOCCHIO_CHECK_ARGUMENT_SIZE(q.size(), model.nq, "The joint configuration vector is not of right size"); |
199 |
✗✓✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗ |
117 |
PINOCCHIO_CHECK_ARGUMENT_SIZE(v.size(), model.nv, "The joint velocity vector is not of right size"); |
200 |
✗✓✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗ |
117 |
PINOCCHIO_CHECK_ARGUMENT_SIZE(tau.size(), model.nv, "The joint torque vector is not of right size"); |
201 |
|||
202 |
typedef typename ModelTpl<Scalar,Options,JointCollectionTpl>::JointIndex JointIndex; |
||
203 |
|||
204 |
117 |
data.v[0].setZero(); |
|
205 |
117 |
data.a_gf[0] = -model.gravity; |
|
206 |
117 |
data.u = tau; |
|
207 |
|||
208 |
typedef AbaForwardStep1<Scalar,Options,JointCollectionTpl,ConfigVectorType,TangentVectorType1> Pass1; |
||
209 |
✓✓ | 2912 |
for(JointIndex i=1; i<(JointIndex)model.njoints; ++i) |
210 |
{ |
||
211 |
✓✗ | 2795 |
Pass1::run(model.joints[i],data.joints[i], |
212 |
typename Pass1::ArgsType(model,data,q.derived(),v.derived())); |
||
213 |
} |
||
214 |
|||
215 |
typedef AbaBackwardStep<Scalar,Options,JointCollectionTpl> Pass2; |
||
216 |
✓✓ | 2912 |
for(JointIndex i=(JointIndex)model.njoints-1;i>0; --i) |
217 |
{ |
||
218 |
✓✗ | 2795 |
Pass2::run(model.joints[i],data.joints[i], |
219 |
typename Pass2::ArgsType(model,data)); |
||
220 |
} |
||
221 |
|||
222 |
typedef AbaForwardStep2<Scalar,Options,JointCollectionTpl> Pass3; |
||
223 |
✓✓ | 2912 |
for(JointIndex i=1; i<(JointIndex)model.njoints; ++i) |
224 |
{ |
||
225 |
✓✗ | 2795 |
Pass3::run(model.joints[i],data.joints[i], |
226 |
typename Pass3::ArgsType(model,data)); |
||
227 |
} |
||
228 |
|||
229 |
117 |
return data.ddq; |
|
230 |
} |
||
231 |
|||
232 |
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType1, typename TangentVectorType2, typename ForceDerived> |
||
233 |
inline const typename DataTpl<Scalar,Options,JointCollectionTpl>::TangentVectorType & |
||
234 |
99 |
aba(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
|
235 |
DataTpl<Scalar,Options,JointCollectionTpl> & data, |
||
236 |
const Eigen::MatrixBase<ConfigVectorType> & q, |
||
237 |
const Eigen::MatrixBase<TangentVectorType1> & v, |
||
238 |
const Eigen::MatrixBase<TangentVectorType2> & tau, |
||
239 |
const container::aligned_vector<ForceDerived> & fext) |
||
240 |
|||
241 |
{ |
||
242 |
✓✗ | 99 |
assert(model.check(data) && "data is not consistent with model."); |
243 |
✗✓✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗ |
99 |
PINOCCHIO_CHECK_ARGUMENT_SIZE(q.size(), model.nq, "The joint configuration vector is not of right size"); |
244 |
✗✓✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗ |
99 |
PINOCCHIO_CHECK_ARGUMENT_SIZE(v.size(), model.nv, "The joint velocity vector is not of right size"); |
245 |
✗✓✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗ |
99 |
PINOCCHIO_CHECK_ARGUMENT_SIZE(tau.size(), model.nv, "The joint torque vector is not of right size"); |
246 |
|||
247 |
typedef typename ModelTpl<Scalar,Options,JointCollectionTpl>::JointIndex JointIndex; |
||
248 |
|||
249 |
99 |
data.v[0].setZero(); |
|
250 |
99 |
data.a_gf[0] = -model.gravity; |
|
251 |
99 |
data.u = tau; |
|
252 |
|||
253 |
typedef AbaForwardStep1<Scalar,Options,JointCollectionTpl,ConfigVectorType,TangentVectorType1> Pass1; |
||
254 |
✓✓ | 2772 |
for(JointIndex i=1;i<(JointIndex)model.njoints;++i) |
255 |
{ |
||
256 |
✓✗ | 2673 |
Pass1::run(model.joints[i],data.joints[i], |
257 |
typename Pass1::ArgsType(model,data,q.derived(),v.derived())); |
||
258 |
2673 |
data.f[i] -= fext[i]; |
|
259 |
} |
||
260 |
|||
261 |
typedef AbaBackwardStep<Scalar,Options,JointCollectionTpl> Pass2; |
||
262 |
✓✓ | 2772 |
for(JointIndex i=(JointIndex)model.njoints-1;i>0; --i) |
263 |
{ |
||
264 |
✓✗ | 2673 |
Pass2::run(model.joints[i],data.joints[i], |
265 |
typename Pass2::ArgsType(model,data)); |
||
266 |
} |
||
267 |
|||
268 |
typedef AbaForwardStep2<Scalar,Options,JointCollectionTpl> Pass3; |
||
269 |
✓✓ | 2772 |
for(JointIndex i=1; i<(JointIndex)model.njoints; ++i) |
270 |
{ |
||
271 |
✓✗ | 2673 |
Pass3::run(model.joints[i],data.joints[i], |
272 |
typename Pass3::ArgsType(model,data)); |
||
273 |
} |
||
274 |
|||
275 |
99 |
return data.ddq; |
|
276 |
} |
||
277 |
|||
278 |
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType> |
||
279 |
struct ComputeMinverseForwardStep1 |
||
280 |
: public fusion::JointUnaryVisitorBase< ComputeMinverseForwardStep1<Scalar,Options,JointCollectionTpl,ConfigVectorType> > |
||
281 |
{ |
||
282 |
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model; |
||
283 |
typedef DataTpl<Scalar,Options,JointCollectionTpl> Data; |
||
284 |
|||
285 |
typedef boost::fusion::vector<const Model &, |
||
286 |
Data &, |
||
287 |
const ConfigVectorType & |
||
288 |
> ArgsType; |
||
289 |
|||
290 |
template<typename JointModel> |
||
291 |
1242 |
static void algo(const pinocchio::JointModelBase<JointModel> & jmodel, |
|
292 |
pinocchio::JointDataBase<typename JointModel::JointDataDerived> & jdata, |
||
293 |
const Model & model, |
||
294 |
Data & data, |
||
295 |
const Eigen::MatrixBase<ConfigVectorType> & q) |
||
296 |
{ |
||
297 |
typedef typename Model::JointIndex JointIndex; |
||
298 |
|||
299 |
✓✗ | 1242 |
const JointIndex & i = jmodel.id(); |
300 |
✓✗ | 1242 |
jmodel.calc(jdata.derived(),q.derived()); |
301 |
|||
302 |
1242 |
const JointIndex & parent = model.parents[i]; |
|
303 |
✓✗✓✓ ✗✓✗ |
1242 |
data.liMi[i] = model.jointPlacements[i] * jdata.M(); |
304 |
|||
305 |
✓✓ | 1242 |
if (parent>0) |
306 |
✓✗ | 1196 |
data.oMi[i] = data.oMi[parent] * data.liMi[i]; |
307 |
else |
||
308 |
✓✗ | 46 |
data.oMi[i] = data.liMi[i]; |
309 |
|||
310 |
typedef typename SizeDepType<JointModel::NV>::template ColsReturn<typename Data::Matrix6x>::Type ColsBlock; |
||
311 |
✓✗ | 1242 |
ColsBlock J_cols = jmodel.jointCols(data.J); |
312 |
✓✗✓✗ ✓✗ |
1242 |
J_cols = data.oMi[i].act(jdata.S()); |
313 |
|||
314 |
✓✗ | 1242 |
data.Yaba[i] = model.inertias[i].matrix(); |
315 |
} |
||
316 |
|||
317 |
}; |
||
318 |
|||
319 |
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl> |
||
320 |
struct ComputeMinverseBackwardStep |
||
321 |
: public fusion::JointUnaryVisitorBase< ComputeMinverseBackwardStep<Scalar,Options,JointCollectionTpl> > |
||
322 |
{ |
||
323 |
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model; |
||
324 |
typedef DataTpl<Scalar,Options,JointCollectionTpl> Data; |
||
325 |
|||
326 |
typedef boost::fusion::vector<const Model &, |
||
327 |
Data &> ArgsType; |
||
328 |
|||
329 |
template<typename JointModel> |
||
330 |
1242 |
static void algo(const JointModelBase<JointModel> & jmodel, |
|
331 |
JointDataBase<typename JointModel::JointDataDerived> & jdata, |
||
332 |
const Model & model, |
||
333 |
Data & data) |
||
334 |
{ |
||
335 |
typedef typename Model::JointIndex JointIndex; |
||
336 |
typedef typename Data::Inertia Inertia; |
||
337 |
|||
338 |
✓✗ | 1242 |
const JointIndex & i = jmodel.id(); |
339 |
1242 |
const JointIndex & parent = model.parents[i]; |
|
340 |
|||
341 |
1242 |
typename Inertia::Matrix6 & Ia = data.Yaba[i]; |
|
342 |
1242 |
typename Data::RowMatrixXs & Minv = data.Minv; |
|
343 |
1242 |
typename Data::Matrix6x & Fcrb = data.Fcrb[0]; |
|
344 |
1242 |
typename Data::Matrix6x & FcrbTmp = data.Fcrb.back(); |
|
345 |
|||
346 |
✓✗ | 1242 |
jmodel.calc_aba(jdata.derived(), Ia, parent > 0); |
347 |
|||
348 |
typedef typename SizeDepType<JointModel::NV>::template ColsReturn<typename Data::Matrix6x>::Type ColsBlock; |
||
349 |
|||
350 |
✓✗ | 1242 |
ColsBlock U_cols = jmodel.jointCols(data.IS); |
351 |
✓✗✓✗ |
1242 |
forceSet::se3Action(data.oMi[i],jdata.U(),U_cols); // expressed in the world frame |
352 |
|||
353 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
1242 |
Minv.block(jmodel.idx_v(),jmodel.idx_v(),jmodel.nv(),jmodel.nv()) = jdata.Dinv(); |
354 |
✓✗ | 1242 |
const int nv_children = data.nvSubtree[i] - jmodel.nv(); |
355 |
✓✓ | 1242 |
if(nv_children > 0) |
356 |
{ |
||
357 |
✓✗ | 1058 |
ColsBlock J_cols = jmodel.jointCols(data.J); |
358 |
✓✗ | 1058 |
ColsBlock SDinv_cols = jmodel.jointCols(data.SDinv); |
359 |
✓✗✓✗ ✓✗✓✗ |
1058 |
SDinv_cols.noalias() = J_cols * jdata.Dinv(); |
360 |
|||
361 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
1058 |
Minv.block(jmodel.idx_v(),jmodel.idx_v()+jmodel.nv(),jmodel.nv(),nv_children).noalias() |
362 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
2116 |
= -SDinv_cols.transpose() * Fcrb.middleCols(jmodel.idx_v()+jmodel.nv(),nv_children); |
363 |
|||
364 |
✓✓ | 1058 |
if(parent > 0) |
365 |
{ |
||
366 |
1012 |
FcrbTmp.leftCols(data.nvSubtree[i]).noalias() |
|
367 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
2024 |
= U_cols * Minv.block(jmodel.idx_v(),jmodel.idx_v(),jmodel.nv(),data.nvSubtree[i]); |
368 |
✓✗✓✗ ✓✗✓✗ |
1012 |
Fcrb.middleCols(jmodel.idx_v(),data.nvSubtree[i]) += FcrbTmp.leftCols(data.nvSubtree[i]); |
369 |
} |
||
370 |
} |
||
371 |
else |
||
372 |
{ |
||
373 |
184 |
Fcrb.middleCols(jmodel.idx_v(),data.nvSubtree[i]).noalias() |
|
374 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
368 |
= U_cols * Minv.block(jmodel.idx_v(),jmodel.idx_v(),jmodel.nv(),data.nvSubtree[i]); |
375 |
} |
||
376 |
|||
377 |
✓✓ | 1242 |
if(parent > 0) |
378 |
✓✗✓✗ |
1196 |
data.Yaba[parent] += internal::SE3actOn<Scalar>::run(data.liMi[i], Ia); |
379 |
} |
||
380 |
}; |
||
381 |
|||
382 |
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl> |
||
383 |
struct ComputeMinverseForwardStep2 |
||
384 |
: public fusion::JointUnaryVisitorBase< ComputeMinverseForwardStep2<Scalar,Options,JointCollectionTpl> > |
||
385 |
{ |
||
386 |
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model; |
||
387 |
typedef DataTpl<Scalar,Options,JointCollectionTpl> Data; |
||
388 |
|||
389 |
typedef boost::fusion::vector<const Model &, |
||
390 |
Data &> ArgsType; |
||
391 |
|||
392 |
template<typename JointModel> |
||
393 |
1242 |
static void algo(const pinocchio::JointModelBase<JointModel> & jmodel, |
|
394 |
pinocchio::JointDataBase<typename JointModel::JointDataDerived> & jdata, |
||
395 |
const Model & model, |
||
396 |
Data & data) |
||
397 |
{ |
||
398 |
typedef typename Model::JointIndex JointIndex; |
||
399 |
|||
400 |
✓✗ | 1242 |
const JointIndex & i = jmodel.id(); |
401 |
1242 |
const JointIndex & parent = model.parents[i]; |
|
402 |
1242 |
typename Data::RowMatrixXs & Minv = data.Minv; |
|
403 |
1242 |
typename Data::Matrix6x & FcrbTmp = data.Fcrb.back(); |
|
404 |
|||
405 |
typedef typename SizeDepType<JointModel::NV>::template ColsReturn<typename Data::Matrix6x>::Type ColsBlock; |
||
406 |
✓✗ | 1242 |
ColsBlock UDinv_cols = jmodel.jointCols(data.UDinv); |
407 |
✓✗✓✗ |
1242 |
forceSet::se3Action(data.oMi[i],jdata.UDinv(),UDinv_cols); // expressed in the world frame |
408 |
✓✗ | 1242 |
ColsBlock J_cols = jmodel.jointCols(data.J); |
409 |
|||
410 |
✓✓ | 1242 |
if(parent > 0) |
411 |
{ |
||
412 |
✓✗✓✗ ✓✗✓✗ |
1196 |
FcrbTmp.topRows(jmodel.nv()).rightCols(model.nv - jmodel.idx_v()).noalias() |
413 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
2392 |
= UDinv_cols.transpose() * data.Fcrb[parent].rightCols(model.nv - jmodel.idx_v()); |
414 |
✓✗✓✗ ✓✗✓✗ |
1196 |
Minv.middleRows(jmodel.idx_v(),jmodel.nv()).rightCols(model.nv - jmodel.idx_v()) |
415 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
2392 |
-= FcrbTmp.topRows(jmodel.nv()).rightCols(model.nv - jmodel.idx_v()); |
416 |
} |
||
417 |
|||
418 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
1242 |
data.Fcrb[i].rightCols(model.nv - jmodel.idx_v()).noalias() = J_cols * Minv.middleRows(jmodel.idx_v(),jmodel.nv()).rightCols(model.nv - jmodel.idx_v()); |
419 |
✓✓ | 1242 |
if(parent > 0) |
420 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
1196 |
data.Fcrb[i].rightCols(model.nv - jmodel.idx_v()) += data.Fcrb[parent].rightCols(model.nv - jmodel.idx_v()); |
421 |
} |
||
422 |
|||
423 |
}; |
||
424 |
|||
425 |
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType> |
||
426 |
inline const typename DataTpl<Scalar,Options,JointCollectionTpl>::RowMatrixXs & |
||
427 |
23 |
computeMinverse(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
|
428 |
DataTpl<Scalar,Options,JointCollectionTpl> & data, |
||
429 |
const Eigen::MatrixBase<ConfigVectorType> & q) |
||
430 |
{ |
||
431 |
✓✗ | 23 |
assert(model.check(data) && "data is not consistent with model."); |
432 |
✗✓✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗ |
23 |
PINOCCHIO_CHECK_ARGUMENT_SIZE(q.size(), model.nq, "The joint configuration vector is not of right size"); |
433 |
|||
434 |
typedef typename ModelTpl<Scalar,Options,JointCollectionTpl>::JointIndex JointIndex; |
||
435 |
✓✗ | 23 |
data.Minv.template triangularView<Eigen::Upper>().setZero(); |
436 |
|||
437 |
typedef ComputeMinverseForwardStep1<Scalar,Options,JointCollectionTpl,ConfigVectorType> Pass1; |
||
438 |
✓✓ | 644 |
for(JointIndex i=1; i<(JointIndex)model.njoints; ++i) |
439 |
{ |
||
440 |
✓✗ | 621 |
Pass1::run(model.joints[i],data.joints[i], |
441 |
typename Pass1::ArgsType(model,data,q.derived())); |
||
442 |
} |
||
443 |
|||
444 |
23 |
data.Fcrb[0].setZero(); |
|
445 |
typedef ComputeMinverseBackwardStep<Scalar,Options,JointCollectionTpl> Pass2; |
||
446 |
✓✓ | 644 |
for(JointIndex i=(JointIndex)model.njoints-1; i>0; --i) |
447 |
{ |
||
448 |
✓✗ | 621 |
Pass2::run(model.joints[i],data.joints[i], |
449 |
typename Pass2::ArgsType(model,data)); |
||
450 |
} |
||
451 |
|||
452 |
typedef ComputeMinverseForwardStep2<Scalar,Options,JointCollectionTpl> Pass3; |
||
453 |
✓✓ | 644 |
for(JointIndex i=1; i<(JointIndex)model.njoints; ++i) |
454 |
{ |
||
455 |
✓✗ | 621 |
Pass3::run(model.joints[i],data.joints[i], |
456 |
typename Pass3::ArgsType(model,data)); |
||
457 |
} |
||
458 |
|||
459 |
23 |
return data.Minv; |
|
460 |
} |
||
461 |
|||
462 |
|||
463 |
// --- CHECKER --------------------------------------------------------------- |
||
464 |
// --- CHECKER --------------------------------------------------------------- |
||
465 |
// --- CHECKER --------------------------------------------------------------- |
||
466 |
|||
467 |
// Check whether all masses are nonzero and diagonal of inertia is nonzero |
||
468 |
// The second test is overconstraining. |
||
469 |
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl> |
||
470 |
3 |
inline bool ABAChecker::checkModel_impl(const ModelTpl<Scalar,Options,JointCollectionTpl> & model) const |
|
471 |
{ |
||
472 |
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model; |
||
473 |
typedef typename Model::JointIndex JointIndex; |
||
474 |
|||
475 |
✓✓ | 57 |
for(JointIndex j=1;j<(JointIndex)model.njoints;j++) |
476 |
55 |
if( (model.inertias[j].mass () < 1e-5) |
|
477 |
✓✓ | 55 |
|| (model.inertias[j].inertia().data()[0] < 1e-5) |
478 |
✓✗ | 54 |
|| (model.inertias[j].inertia().data()[2] < 1e-5) |
479 |
✓✗✗✓ ✓✓ |
110 |
|| (model.inertias[j].inertia().data()[5] < 1e-5) ) |
480 |
1 |
return false; |
|
481 |
2 |
return true; |
|
482 |
} |
||
483 |
|||
484 |
} // namespace pinocchio |
||
485 |
|||
486 |
/// @endcond |
||
487 |
|||
488 |
#endif // ifndef __pinocchio_aba_hxx__ |
| Generated by: GCOVR (Version 4.2) |