| GCC Code Coverage Report | |||||||||||||||||||||
|
|||||||||||||||||||||
| Line | Branch | Exec | Source |
1 |
// |
||
2 |
// Copyright (c) 2015-2022 CNRS INRIA |
||
3 |
// |
||
4 |
|||
5 |
#ifndef __pinocchio_algorithm_rnea_hxx__ |
||
6 |
#define __pinocchio_algorithm_rnea_hxx__ |
||
7 |
|||
8 |
/// @cond DEV |
||
9 |
|||
10 |
#include "pinocchio/multibody/visitor.hpp" |
||
11 |
#include "pinocchio/algorithm/check.hpp" |
||
12 |
|||
13 |
namespace pinocchio |
||
14 |
{ |
||
15 |
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType1, typename TangentVectorType2> |
||
16 |
struct RneaForwardStep |
||
17 |
: public fusion::JointUnaryVisitorBase< RneaForwardStep<Scalar,Options,JointCollectionTpl,ConfigVectorType,TangentVectorType1,TangentVectorType2> > |
||
18 |
{ |
||
19 |
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model; |
||
20 |
typedef DataTpl<Scalar,Options,JointCollectionTpl> Data; |
||
21 |
|||
22 |
typedef boost::fusion::vector<const Model &, |
||
23 |
Data &, |
||
24 |
const ConfigVectorType &, |
||
25 |
const TangentVectorType1 &, |
||
26 |
const TangentVectorType2 & |
||
27 |
> ArgsType; |
||
28 |
|||
29 |
template<typename JointModel> |
||
30 |
19406 |
static void algo(const JointModelBase<JointModel> & jmodel, |
|
31 |
JointDataBase<typename JointModel::JointDataDerived> & jdata, |
||
32 |
const Model & model, |
||
33 |
Data & data, |
||
34 |
const Eigen::MatrixBase<ConfigVectorType> & q, |
||
35 |
const Eigen::MatrixBase<TangentVectorType1> & v, |
||
36 |
const Eigen::MatrixBase<TangentVectorType2> & a) |
||
37 |
{ |
||
38 |
typedef typename Model::JointIndex JointIndex; |
||
39 |
|||
40 |
19406 |
const JointIndex i = jmodel.id(); |
|
41 |
19406 |
const JointIndex parent = model.parents[i]; |
|
42 |
|||
43 |
19406 |
jmodel.calc(jdata.derived(),q.derived(),v.derived()); |
|
44 |
|||
45 |
✓✗ | 19406 |
data.liMi[i] = model.jointPlacements[i]*jdata.M(); |
46 |
// data.liMi[i].setIdentity(); |
||
47 |
|||
48 |
19406 |
data.v[i] = jdata.v(); |
|
49 |
✓✓ | 19406 |
if(parent>0) |
50 |
✓✗ | 18642 |
data.v[i] += data.liMi[i].actInv(data.v[parent]); |
51 |
|||
52 |
✓✗✓✗ ✓✗ |
19406 |
data.a_gf[i] = jdata.c() + (data.v[i] ^ jdata.v()); |
53 |
✓✗✓✗ ✓✗ |
19406 |
data.a_gf[i] += jdata.S() * jmodel.jointVelocitySelector(a); |
54 |
✓✗ | 19406 |
data.a_gf[i] += data.liMi[i].actInv(data.a_gf[parent]); |
55 |
// |
||
56 |
// data.f[i] = model.inertias[i]*data.a_gf[i];// + model.inertias[i].vxiv(data.v[i]); // -f_ext |
||
57 |
// data.h[i] = model.inertias[i]*data.v[i]; |
||
58 |
19406 |
model.inertias[i].__mult__(data.v[i],data.h[i]); |
|
59 |
19406 |
model.inertias[i].__mult__(data.a_gf[i],data.f[i]); |
|
60 |
✓✗ | 19406 |
data.f[i] += data.v[i].cross(data.h[i]); |
61 |
// data.h[i].motionAction(data.v[i],data.f[i]); |
||
62 |
// data.f[i] = model.inertias[i].vxiv(data.v[i]); |
||
63 |
// data.f[i].setZero(); |
||
64 |
} |
||
65 |
|||
66 |
}; |
||
67 |
|||
68 |
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl> |
||
69 |
struct RneaBackwardStep |
||
70 |
: public fusion::JointUnaryVisitorBase< RneaBackwardStep<Scalar,Options,JointCollectionTpl> > |
||
71 |
{ |
||
72 |
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model; |
||
73 |
typedef DataTpl<Scalar,Options,JointCollectionTpl> Data; |
||
74 |
|||
75 |
typedef boost::fusion::vector<const Model &, |
||
76 |
Data & |
||
77 |
> ArgsType; |
||
78 |
|||
79 |
template<typename JointModel> |
||
80 |
19406 |
static void algo(const JointModelBase<JointModel> & jmodel, |
|
81 |
JointDataBase<typename JointModel::JointDataDerived> & jdata, |
||
82 |
const Model & model, |
||
83 |
Data & data) |
||
84 |
{ |
||
85 |
typedef typename Model::JointIndex JointIndex; |
||
86 |
|||
87 |
19406 |
const JointIndex i = jmodel.id(); |
|
88 |
19406 |
const JointIndex parent = model.parents[i]; |
|
89 |
|||
90 |
✓✗✓✗ ✓✗ |
19406 |
jmodel.jointVelocitySelector(data.tau) = jdata.S().transpose()*data.f[i]; |
91 |
✓✓✓✗ |
19406 |
if(parent>0) data.f[parent] += data.liMi[i].act(data.f[i]); |
92 |
} |
||
93 |
}; |
||
94 |
|||
95 |
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType1, typename TangentVectorType2> |
||
96 |
inline const typename DataTpl<Scalar,Options,JointCollectionTpl>::TangentVectorType & |
||
97 |
484 |
rnea(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
|
98 |
DataTpl<Scalar,Options,JointCollectionTpl> & data, |
||
99 |
const Eigen::MatrixBase<ConfigVectorType> & q, |
||
100 |
const Eigen::MatrixBase<TangentVectorType1> & v, |
||
101 |
const Eigen::MatrixBase<TangentVectorType2> & a) |
||
102 |
{ |
||
103 |
✓✗✓✗ |
484 |
assert(model.check(data) && "data is not consistent with model."); |
104 |
✓✗✗✓ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ |
484 |
PINOCCHIO_CHECK_ARGUMENT_SIZE(q.size(), model.nq, "The configuration vector is not of right size"); |
105 |
✓✗✗✓ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ |
484 |
PINOCCHIO_CHECK_ARGUMENT_SIZE(v.size(), model.nv, "The velocity vector is not of right size"); |
106 |
✓✗✗✓ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ |
484 |
PINOCCHIO_CHECK_ARGUMENT_SIZE(a.size(), model.nv, "The acceleration vector is not of right size"); |
107 |
|||
108 |
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model; |
||
109 |
typedef typename Model::JointIndex JointIndex; |
||
110 |
|||
111 |
✓✗ | 484 |
data.v[0].setZero(); |
112 |
✓✗ | 484 |
data.a_gf[0] = -model.gravity; |
113 |
|||
114 |
typedef RneaForwardStep<Scalar,Options,JointCollectionTpl,ConfigVectorType,TangentVectorType1,TangentVectorType2> Pass1; |
||
115 |
✓✗ | 484 |
typename Pass1::ArgsType arg1(model,data,q.derived(),v.derived(),a.derived()); |
116 |
✓✓ | 12955 |
for(JointIndex i=1; i<(JointIndex)model.njoints; ++i) |
117 |
{ |
||
118 |
✓✗✓✗ |
12471 |
Pass1::run(model.joints[i],data.joints[i], |
119 |
arg1); |
||
120 |
} |
||
121 |
|||
122 |
typedef RneaBackwardStep<Scalar,Options,JointCollectionTpl> Pass2; |
||
123 |
✓✗ | 484 |
typename Pass2::ArgsType arg2(model,data); |
124 |
✓✓ | 12955 |
for(JointIndex i=(JointIndex)model.njoints-1; i>0; --i) |
125 |
{ |
||
126 |
✓✗✓✗ |
12471 |
Pass2::run(model.joints[i],data.joints[i], |
127 |
arg2); |
||
128 |
} |
||
129 |
|||
130 |
484 |
return data.tau; |
|
131 |
} |
||
132 |
|||
133 |
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType1, typename TangentVectorType2, typename ForceDerived> |
||
134 |
inline const typename DataTpl<Scalar,Options,JointCollectionTpl>::TangentVectorType & |
||
135 |
105 |
rnea(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
|
136 |
DataTpl<Scalar,Options,JointCollectionTpl> & data, |
||
137 |
const Eigen::MatrixBase<ConfigVectorType> & q, |
||
138 |
const Eigen::MatrixBase<TangentVectorType1> & v, |
||
139 |
const Eigen::MatrixBase<TangentVectorType2> & a, |
||
140 |
const container::aligned_vector<ForceDerived> & fext) |
||
141 |
{ |
||
142 |
✗✓✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ |
105 |
PINOCCHIO_CHECK_ARGUMENT_SIZE(fext.size(), model.joints.size()); |
143 |
✓✗ | 105 |
assert(model.check(data) && "data is not consistent with model."); |
144 |
✗✓✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗ |
105 |
PINOCCHIO_CHECK_ARGUMENT_SIZE(q.size(), model.nq, "The configuration vector is not of right size"); |
145 |
✗✓✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗ |
105 |
PINOCCHIO_CHECK_ARGUMENT_SIZE(v.size(), model.nv, "The velocity vector is not of right size"); |
146 |
✗✓✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗ |
105 |
PINOCCHIO_CHECK_ARGUMENT_SIZE(a.size(), model.nv, "The acceleration vector is not of right size"); |
147 |
|||
148 |
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model; |
||
149 |
typedef typename Model::JointIndex JointIndex; |
||
150 |
|||
151 |
105 |
data.v[0].setZero(); |
|
152 |
105 |
data.a_gf[0] = -model.gravity; |
|
153 |
|||
154 |
typedef RneaForwardStep<Scalar,Options,JointCollectionTpl,ConfigVectorType,TangentVectorType1,TangentVectorType2> Pass1; |
||
155 |
✓✓ | 2926 |
for(JointIndex i=1; i<(JointIndex)model.njoints; ++i) |
156 |
{ |
||
157 |
✓✗ | 2821 |
Pass1::run(model.joints[i],data.joints[i], |
158 |
typename Pass1::ArgsType(model,data,q.derived(),v.derived(),a.derived())); |
||
159 |
2821 |
data.f[i] -= fext[i]; |
|
160 |
} |
||
161 |
|||
162 |
typedef RneaBackwardStep<Scalar,Options,JointCollectionTpl> Pass2; |
||
163 |
✓✓ | 2926 |
for(JointIndex i=(JointIndex)model.njoints-1; i>0; --i) |
164 |
{ |
||
165 |
✓✗ | 2821 |
Pass2::run(model.joints[i],data.joints[i], |
166 |
typename Pass2::ArgsType(model,data)); |
||
167 |
} |
||
168 |
|||
169 |
|||
170 |
105 |
return data.tau; |
|
171 |
} |
||
172 |
|||
173 |
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType> |
||
174 |
struct NLEForwardStep |
||
175 |
: public fusion::JointUnaryVisitorBase< NLEForwardStep<Scalar,Options,JointCollectionTpl,ConfigVectorType,TangentVectorType> > |
||
176 |
{ |
||
177 |
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model; |
||
178 |
typedef DataTpl<Scalar,Options,JointCollectionTpl> Data; |
||
179 |
|||
180 |
typedef boost::fusion::vector<const Model &, |
||
181 |
Data &, |
||
182 |
const ConfigVectorType &, |
||
183 |
const TangentVectorType & |
||
184 |
> ArgsType; |
||
185 |
|||
186 |
template<typename JointModel> |
||
187 |
594 |
static void algo(const pinocchio::JointModelBase<JointModel> & jmodel, |
|
188 |
pinocchio::JointDataBase<typename JointModel::JointDataDerived> & jdata, |
||
189 |
const Model & model, |
||
190 |
Data & data, |
||
191 |
const Eigen::MatrixBase<ConfigVectorType> & q, |
||
192 |
const Eigen::MatrixBase<TangentVectorType> & v) |
||
193 |
{ |
||
194 |
typedef typename Model::JointIndex JointIndex; |
||
195 |
|||
196 |
594 |
const JointIndex & i = jmodel.id(); |
|
197 |
594 |
const JointIndex & parent = model.parents[i]; |
|
198 |
|||
199 |
594 |
jmodel.calc(jdata.derived(),q.derived(),v.derived()); |
|
200 |
|||
201 |
✓✗ | 594 |
data.liMi[i] = model.jointPlacements[i]*jdata.M(); |
202 |
|||
203 |
594 |
data.v[i] = jdata.v(); |
|
204 |
✓✓✓✗ |
594 |
if(parent>0) data.v[i] += data.liMi[i].actInv(data.v[parent]); |
205 |
|||
206 |
✓✗✗✗ ✓✗ |
594 |
data.a_gf[i] = jdata.c() + (data.v[i] ^ jdata.v()); |
207 |
✓✗ | 594 |
data.a_gf[i] += data.liMi[i].actInv(data.a_gf[parent]); |
208 |
|||
209 |
✓✗✓✗ ✓✗ |
594 |
data.f[i] = model.inertias[i]*data.a_gf[i] + model.inertias[i].vxiv(data.v[i]); // -f_ext |
210 |
} |
||
211 |
|||
212 |
}; |
||
213 |
|||
214 |
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl> |
||
215 |
struct NLEBackwardStep |
||
216 |
: public fusion::JointUnaryVisitorBase< NLEBackwardStep<Scalar,Options,JointCollectionTpl> > |
||
217 |
{ |
||
218 |
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model; |
||
219 |
typedef DataTpl<Scalar,Options,JointCollectionTpl> Data; |
||
220 |
|||
221 |
typedef boost::fusion::vector<const Model &, |
||
222 |
Data & |
||
223 |
> ArgsType; |
||
224 |
|||
225 |
template<typename JointModel> |
||
226 |
594 |
static void algo(const JointModelBase<JointModel> & jmodel, |
|
227 |
JointDataBase<typename JointModel::JointDataDerived> & jdata, |
||
228 |
const Model & model, |
||
229 |
Data & data) |
||
230 |
{ |
||
231 |
typedef typename Model::JointIndex JointIndex; |
||
232 |
|||
233 |
594 |
const JointIndex & i = jmodel.id(); |
|
234 |
594 |
const JointIndex & parent = model.parents[i]; |
|
235 |
|||
236 |
✓✗✓✗ ✓✗ |
594 |
jmodel.jointVelocitySelector(data.nle) = jdata.S().transpose()*data.f[i]; |
237 |
✓✓✓✗ |
594 |
if(parent>0) data.f[parent] += data.liMi[i].act(data.f[i]); |
238 |
} |
||
239 |
}; |
||
240 |
|||
241 |
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType> |
||
242 |
inline const typename DataTpl<Scalar,Options,JointCollectionTpl>::TangentVectorType & |
||
243 |
11 |
nonLinearEffects(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
|
244 |
DataTpl<Scalar,Options,JointCollectionTpl> & data, |
||
245 |
const Eigen::MatrixBase<ConfigVectorType> & q, |
||
246 |
const Eigen::MatrixBase<TangentVectorType> & v) |
||
247 |
{ |
||
248 |
✓✗ | 11 |
assert(model.check(data) && "data is not consistent with model."); |
249 |
✗✓✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗ |
11 |
PINOCCHIO_CHECK_ARGUMENT_SIZE(q.size(), model.nq, "The configuration vector is not of right size"); |
250 |
✗✓✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗ |
11 |
PINOCCHIO_CHECK_ARGUMENT_SIZE(v.size(), model.nv, "The velocity vector is not of right size"); |
251 |
|||
252 |
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model; |
||
253 |
typedef typename Model::JointIndex JointIndex; |
||
254 |
|||
255 |
11 |
data.v[0].setZero (); |
|
256 |
11 |
data.a_gf[0] = -model.gravity; |
|
257 |
|||
258 |
typedef NLEForwardStep<Scalar,Options,JointCollectionTpl,ConfigVectorType,TangentVectorType> Pass1; |
||
259 |
✓✓ | 308 |
for(JointIndex i=1; i<(JointIndex)model.njoints; ++i) |
260 |
{ |
||
261 |
✓✗ | 297 |
Pass1::run(model.joints[i],data.joints[i], |
262 |
typename Pass1::ArgsType(model,data,q.derived(),v.derived())); |
||
263 |
} |
||
264 |
|||
265 |
typedef NLEBackwardStep<Scalar,Options,JointCollectionTpl> Pass2; |
||
266 |
✓✓ | 308 |
for(JointIndex i=(JointIndex)(model.njoints-1); i>0; --i) |
267 |
{ |
||
268 |
✓✗ | 297 |
Pass2::run(model.joints[i],data.joints[i], |
269 |
typename Pass2::ArgsType(model,data)); |
||
270 |
} |
||
271 |
|||
272 |
11 |
return data.nle; |
|
273 |
} |
||
274 |
|||
275 |
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType> |
||
276 |
struct ComputeGeneralizedGravityForwardStep |
||
277 |
: public fusion::JointUnaryVisitorBase< ComputeGeneralizedGravityForwardStep<Scalar,Options,JointCollectionTpl,ConfigVectorType> > |
||
278 |
{ |
||
279 |
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model; |
||
280 |
typedef DataTpl<Scalar,Options,JointCollectionTpl> Data; |
||
281 |
|||
282 |
typedef boost::fusion::vector<const Model &, |
||
283 |
Data &, |
||
284 |
const ConfigVectorType & |
||
285 |
> ArgsType; |
||
286 |
|||
287 |
template<typename JointModel> |
||
288 |
3888 |
static void algo(const pinocchio::JointModelBase<JointModel> & jmodel, |
|
289 |
pinocchio::JointDataBase<typename JointModel::JointDataDerived> & jdata, |
||
290 |
const Model & model, |
||
291 |
Data & data, |
||
292 |
const Eigen::MatrixBase<ConfigVectorType> & q) |
||
293 |
{ |
||
294 |
typedef typename Model::JointIndex JointIndex; |
||
295 |
|||
296 |
3888 |
const JointIndex & i = jmodel.id(); |
|
297 |
3888 |
const JointIndex & parent = model.parents[i]; |
|
298 |
|||
299 |
3888 |
jmodel.calc(jdata.derived(),q.derived()); |
|
300 |
|||
301 |
✓✗ | 3888 |
data.liMi[i] = model.jointPlacements[i]*jdata.M(); |
302 |
|||
303 |
3888 |
data.a_gf[i] = data.liMi[i].actInv(data.a_gf[(size_t) parent]); |
|
304 |
✓✗ | 3888 |
data.f[i] = model.inertias[i]*data.a_gf[i]; |
305 |
} |
||
306 |
|||
307 |
}; |
||
308 |
|||
309 |
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl> |
||
310 |
struct ComputeGeneralizedGravityBackwardStep |
||
311 |
: public fusion::JointUnaryVisitorBase< ComputeGeneralizedGravityBackwardStep<Scalar,Options,JointCollectionTpl> > |
||
312 |
{ |
||
313 |
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model; |
||
314 |
typedef DataTpl<Scalar,Options,JointCollectionTpl> Data; |
||
315 |
|||
316 |
typedef boost::fusion::vector<const Model &, |
||
317 |
Data &, |
||
318 |
typename Data::VectorXs & |
||
319 |
> ArgsType; |
||
320 |
|||
321 |
template<typename JointModel> |
||
322 |
3888 |
static void algo(const JointModelBase<JointModel> & jmodel, |
|
323 |
JointDataBase<typename JointModel::JointDataDerived> & jdata, |
||
324 |
const Model & model, |
||
325 |
Data & data, |
||
326 |
typename Data::VectorXs & g) |
||
327 |
{ |
||
328 |
typedef typename Model::JointIndex JointIndex; |
||
329 |
|||
330 |
3888 |
const JointIndex & i = jmodel.id(); |
|
331 |
3888 |
const JointIndex & parent = model.parents[i]; |
|
332 |
|||
333 |
✓✗✓✗ ✓✗ |
3888 |
jmodel.jointVelocitySelector(g) = jdata.S().transpose()*data.f[i]; |
334 |
✓✓✓✗ |
3888 |
if(parent>0) data.f[(size_t) parent] += data.liMi[i].act(data.f[i]); |
335 |
} |
||
336 |
}; |
||
337 |
|||
338 |
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType> |
||
339 |
inline const typename DataTpl<Scalar,Options,JointCollectionTpl>::TangentVectorType & |
||
340 |
38 |
computeGeneralizedGravity(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
|
341 |
DataTpl<Scalar,Options,JointCollectionTpl> & data, |
||
342 |
const Eigen::MatrixBase<ConfigVectorType> & q) |
||
343 |
{ |
||
344 |
✓✗ | 38 |
assert(model.check(data) && "data is not consistent with model."); |
345 |
✗✓✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗ |
38 |
PINOCCHIO_CHECK_ARGUMENT_SIZE(q.size(), model.nq, "The configuration vector is not of right size"); |
346 |
|||
347 |
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model; |
||
348 |
typedef typename Model::JointIndex JointIndex; |
||
349 |
|||
350 |
38 |
data.a_gf[0] = -model.gravity; |
|
351 |
|||
352 |
typedef ComputeGeneralizedGravityForwardStep<Scalar,Options,JointCollectionTpl,ConfigVectorType> Pass1; |
||
353 |
✓✓ | 1064 |
for(JointIndex i=1; i<(JointIndex)model.njoints; ++i) |
354 |
{ |
||
355 |
✓✗ | 1026 |
Pass1::run(model.joints[i],data.joints[i], |
356 |
typename Pass1::ArgsType(model,data,q.derived())); |
||
357 |
} |
||
358 |
|||
359 |
typedef ComputeGeneralizedGravityBackwardStep<Scalar,Options,JointCollectionTpl> Pass2; |
||
360 |
✓✓ | 1064 |
for(JointIndex i=(JointIndex)(model.njoints-1); i>0; --i) |
361 |
{ |
||
362 |
✓✗ | 1026 |
Pass2::run(model.joints[i],data.joints[i], |
363 |
1026 |
typename Pass2::ArgsType(model,data,data.g)); |
|
364 |
} |
||
365 |
|||
366 |
38 |
return data.g; |
|
367 |
} |
||
368 |
|||
369 |
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType> |
||
370 |
inline const typename DataTpl<Scalar,Options,JointCollectionTpl>::TangentVectorType & |
||
371 |
34 |
computeStaticTorque(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
|
372 |
DataTpl<Scalar,Options,JointCollectionTpl> & data, |
||
373 |
const Eigen::MatrixBase<ConfigVectorType> & q, |
||
374 |
const container::aligned_vector< ForceTpl<Scalar,Options> > & fext) |
||
375 |
{ |
||
376 |
✓✗ | 34 |
assert(model.check(data) && "data is not consistent with model."); |
377 |
✗✓✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗ |
34 |
PINOCCHIO_CHECK_ARGUMENT_SIZE(q.size(), model.nq, "The configuration vector is not of right size"); |
378 |
✗✓✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ |
34 |
PINOCCHIO_CHECK_ARGUMENT_SIZE(fext.size(), (size_t)model.njoints, "The size of the external forces is not of right size"); |
379 |
|||
380 |
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model; |
||
381 |
typedef typename Model::JointIndex JointIndex; |
||
382 |
|||
383 |
34 |
data.a_gf[0] = -model.gravity; |
|
384 |
|||
385 |
typedef ComputeGeneralizedGravityForwardStep<Scalar,Options,JointCollectionTpl,ConfigVectorType> Pass1; |
||
386 |
✓✓ | 952 |
for(JointIndex i=1; i<(JointIndex)model.njoints; ++i) |
387 |
{ |
||
388 |
✓✗ | 918 |
Pass1::run(model.joints[i],data.joints[i], |
389 |
typename Pass1::ArgsType(model,data,q.derived())); |
||
390 |
918 |
data.f[i] -= fext[i]; |
|
391 |
} |
||
392 |
|||
393 |
typedef ComputeGeneralizedGravityBackwardStep<Scalar,Options,JointCollectionTpl> Pass2; |
||
394 |
✓✓ | 952 |
for(JointIndex i=(JointIndex)(model.njoints-1); i>0; --i) |
395 |
{ |
||
396 |
✓✗ | 918 |
Pass2::run(model.joints[i],data.joints[i], |
397 |
918 |
typename Pass2::ArgsType(model,data,data.tau)); |
|
398 |
} |
||
399 |
|||
400 |
34 |
return data.tau; |
|
401 |
} |
||
402 |
|||
403 |
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType> |
||
404 |
struct CoriolisMatrixForwardStep |
||
405 |
: public fusion::JointUnaryVisitorBase< CoriolisMatrixForwardStep<Scalar,Options,JointCollectionTpl,ConfigVectorType,TangentVectorType> > |
||
406 |
{ |
||
407 |
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model; |
||
408 |
typedef DataTpl<Scalar,Options,JointCollectionTpl> Data; |
||
409 |
|||
410 |
typedef boost::fusion::vector<const Model &, |
||
411 |
Data &, |
||
412 |
const ConfigVectorType &, |
||
413 |
const TangentVectorType & |
||
414 |
> ArgsType; |
||
415 |
|||
416 |
template<typename JointModel> |
||
417 |
162 |
static void algo(const pinocchio::JointModelBase<JointModel> & jmodel, |
|
418 |
pinocchio::JointDataBase<typename JointModel::JointDataDerived> & jdata, |
||
419 |
const Model & model, |
||
420 |
Data & data, |
||
421 |
const Eigen::MatrixBase<ConfigVectorType> & q, |
||
422 |
const Eigen::MatrixBase<TangentVectorType> & v) |
||
423 |
{ |
||
424 |
typedef typename Model::JointIndex JointIndex; |
||
425 |
|||
426 |
✓✗ | 162 |
const JointIndex & i = jmodel.id(); |
427 |
162 |
const JointIndex & parent = model.parents[i]; |
|
428 |
|||
429 |
✓✗ | 162 |
jmodel.calc(jdata.derived(),q.derived(),v.derived()); |
430 |
|||
431 |
✓✗✓✓ ✗✓✗ |
162 |
data.liMi[i] = model.jointPlacements[i]*jdata.M(); |
432 |
✓✓✓✗ |
162 |
if(parent>0) data.oMi[i] = data.oMi[parent] * data.liMi[i]; |
433 |
✓✗ | 6 |
else data.oMi[i] = data.liMi[i]; |
434 |
|||
435 |
// express quantities in the world frame |
||
436 |
✓✗✓✗ |
162 |
data.oYcrb[i] = data.oMi[i].act(model.inertias[i]); |
437 |
|||
438 |
✓✗✓✗ |
162 |
data.v[i] = jdata.v(); |
439 |
✓✓✓✗ ✓✗ |
162 |
if(parent>0) data.v[i] += data.liMi[i].actInv(data.v[parent]); |
440 |
✓✗ | 162 |
data.ov[i] = data.oMi[i].act(data.v[i]); |
441 |
✓✗✓✗ |
162 |
data.oh[i] = data.oYcrb[i] * data.ov[i]; |
442 |
|||
443 |
// computes S expressed at the world frame |
||
444 |
typedef typename SizeDepType<JointModel::NV>::template ColsReturn<typename Data::Matrix6x>::Type ColsBlock; |
||
445 |
✓✗ | 162 |
ColsBlock Jcols = jmodel.jointCols(data.J); |
446 |
✓✗✓✗ ✓✗ |
162 |
Jcols = data.oMi[i].act(jdata.S()); // collection of S expressed at the world frame |
447 |
|||
448 |
// computes vxS expressed at the world frame |
||
449 |
✓✗ | 162 |
ColsBlock dJcols = jmodel.jointCols(data.dJ); |
450 |
✓✗ | 162 |
motionSet::motionAction(data.ov[i],Jcols,dJcols); |
451 |
|||
452 |
✓✗✓✗ |
162 |
data.B[i] = data.oYcrb[i].variation(0.5*data.ov[i]); |
453 |
✓✗✓✗ |
162 |
addForceCrossMatrix(0.5*data.oh[i],data.B[i]); |
454 |
} |
||
455 |
|||
456 |
template<typename ForceDerived, typename M6> |
||
457 |
162 |
static void addForceCrossMatrix(const ForceDense<ForceDerived> & f, |
|
458 |
const Eigen::MatrixBase<M6> & mout) |
||
459 |
{ |
||
460 |
162 |
M6 & mout_ = PINOCCHIO_EIGEN_CONST_CAST(M6,mout); |
|
461 |
✓✗✓✗ ✓✗ |
162 |
addSkew(-f.linear(),mout_.template block<3,3>(ForceDerived::LINEAR,ForceDerived::ANGULAR)); |
462 |
✓✗✓✗ ✓✗ |
162 |
addSkew(-f.linear(),mout_.template block<3,3>(ForceDerived::ANGULAR,ForceDerived::LINEAR)); |
463 |
✓✗✓✗ ✓✗ |
162 |
addSkew(-f.angular(),mout_.template block<3,3>(ForceDerived::ANGULAR,ForceDerived::ANGULAR)); |
464 |
162 |
} |
|
465 |
|||
466 |
}; |
||
467 |
|||
468 |
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl> |
||
469 |
struct CoriolisMatrixBackwardStep |
||
470 |
: public fusion::JointUnaryVisitorBase< CoriolisMatrixBackwardStep<Scalar,Options,JointCollectionTpl> > |
||
471 |
{ |
||
472 |
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model; |
||
473 |
typedef DataTpl<Scalar,Options,JointCollectionTpl> Data; |
||
474 |
|||
475 |
typedef boost::fusion::vector<const Model &, |
||
476 |
Data & |
||
477 |
> ArgsType; |
||
478 |
|||
479 |
template<typename JointModel> |
||
480 |
162 |
static void algo(const JointModelBase<JointModel> & jmodel, |
|
481 |
const Model & model, |
||
482 |
Data & data) |
||
483 |
{ |
||
484 |
typedef typename Model::JointIndex JointIndex; |
||
485 |
|||
486 |
✓✗ | 162 |
const JointIndex i = jmodel.id(); |
487 |
162 |
const JointIndex parent = model.parents[i]; |
|
488 |
162 |
typename Data::RowMatrix6 & M6tmpR = data.M6tmpR; |
|
489 |
|||
490 |
typedef typename SizeDepType<JointModel::NV>::template ColsReturn<typename Data::Matrix6x>::Type ColsBlock; |
||
491 |
✓✗ | 162 |
ColsBlock dJcols = jmodel.jointCols(data.dJ); |
492 |
✓✗ | 162 |
ColsBlock Jcols = jmodel.jointCols(data.J); |
493 |
|||
494 |
✓✗✓✗ |
162 |
motionSet::inertiaAction(data.oYcrb[i],dJcols,jmodel.jointCols(data.dFdv)); |
495 |
✓✗✓✗ ✓✗ |
162 |
jmodel.jointCols(data.dFdv) += data.B[i] * Jcols; |
496 |
|||
497 |
✓✗✓✗ ✓✗✓✗ |
162 |
data.C.block(jmodel.idx_v(),jmodel.idx_v(),jmodel.nv(),data.nvSubtree[i]).noalias() |
498 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
324 |
= Jcols.transpose()*data.dFdv.middleCols(jmodel.idx_v(),data.nvSubtree[i]); |
499 |
|||
500 |
✓✗✓✗ ✓✗✓✗ |
162 |
lhsInertiaMult(data.oYcrb[i],Jcols.transpose(),M6tmpR.topRows(jmodel.nv())); |
501 |
✓✗✓✓ |
1608 |
for(int j = data.parents_fromRow[(JointIndex)jmodel.idx_v()];j >= 0; j = data.parents_fromRow[(JointIndex)j]) |
502 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
1446 |
data.C.middleRows(jmodel.idx_v(),jmodel.nv()).col(j).noalias() = M6tmpR.topRows(jmodel.nv()) * data.dJ.col(j); |
503 |
|||
504 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
162 |
M6tmpR.topRows(jmodel.nv()).noalias() = Jcols.transpose() * data.B[i]; |
505 |
✓✗✓✓ |
1608 |
for(int j = data.parents_fromRow[(JointIndex)jmodel.idx_v()];j >= 0; j = data.parents_fromRow[(JointIndex)j]) |
506 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
1446 |
data.C.middleRows(jmodel.idx_v(),jmodel.nv()).col(j).noalias() += M6tmpR.topRows(jmodel.nv()) * data.J.col(j); |
507 |
|||
508 |
✓✓ | 162 |
if(parent>0) |
509 |
{ |
||
510 |
✓✗ | 156 |
data.oYcrb[parent] += data.oYcrb[i]; |
511 |
✓✗ | 156 |
data.B[parent] += data.B[i]; |
512 |
} |
||
513 |
|||
514 |
} |
||
515 |
|||
516 |
template<typename Min, typename Mout> |
||
517 |
216 |
static void lhsInertiaMult(const typename Data::Inertia & Y, |
|
518 |
const Eigen::MatrixBase<Min> & J, |
||
519 |
const Eigen::MatrixBase<Mout> & F) |
||
520 |
{ |
||
521 |
216 |
Mout & F_ = PINOCCHIO_EIGEN_CONST_CAST(Mout,F); |
|
522 |
✓✗✓✗ |
216 |
motionSet::inertiaAction(Y,J.derived().transpose(),F_.transpose()); |
523 |
} |
||
524 |
}; |
||
525 |
|||
526 |
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType> |
||
527 |
inline const typename DataTpl<Scalar,Options,JointCollectionTpl>::MatrixXs & |
||
528 |
3 |
computeCoriolisMatrix(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
|
529 |
DataTpl<Scalar,Options,JointCollectionTpl> & data, |
||
530 |
const Eigen::MatrixBase<ConfigVectorType> & q, |
||
531 |
const Eigen::MatrixBase<TangentVectorType> & v) |
||
532 |
{ |
||
533 |
✓✗ | 3 |
assert(model.check(data) && "data is not consistent with model."); |
534 |
✗✓✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗ |
3 |
PINOCCHIO_CHECK_ARGUMENT_SIZE(q.size(), model.nq); |
535 |
✗✓✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗ |
3 |
PINOCCHIO_CHECK_ARGUMENT_SIZE(v.size(), model.nv); |
536 |
|||
537 |
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model; |
||
538 |
typedef typename Model::JointIndex JointIndex; |
||
539 |
|||
540 |
typedef CoriolisMatrixForwardStep<Scalar,Options,JointCollectionTpl,ConfigVectorType,TangentVectorType> Pass1; |
||
541 |
✓✓ | 84 |
for(JointIndex i=1; i<(JointIndex)model.njoints; ++i) |
542 |
{ |
||
543 |
✓✗ | 81 |
Pass1::run(model.joints[i],data.joints[i], |
544 |
typename Pass1::ArgsType(model,data,q.derived(),v.derived())); |
||
545 |
} |
||
546 |
|||
547 |
typedef CoriolisMatrixBackwardStep<Scalar,Options,JointCollectionTpl> Pass2; |
||
548 |
✓✓ | 84 |
for(JointIndex i=(JointIndex)(model.njoints-1); i>0; --i) |
549 |
{ |
||
550 |
✓✗ | 81 |
Pass2::run(model.joints[i], |
551 |
typename Pass2::ArgsType(model,data)); |
||
552 |
} |
||
553 |
|||
554 |
3 |
return data.C; |
|
555 |
} |
||
556 |
|||
557 |
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl> |
||
558 |
struct GetCoriolisMatrixBackwardStep |
||
559 |
: public fusion::JointUnaryVisitorBase< GetCoriolisMatrixBackwardStep<Scalar,Options,JointCollectionTpl> > |
||
560 |
{ |
||
561 |
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model; |
||
562 |
typedef DataTpl<Scalar,Options,JointCollectionTpl> Data; |
||
563 |
|||
564 |
typedef boost::fusion::vector<const Model &, |
||
565 |
Data & |
||
566 |
> ArgsType; |
||
567 |
|||
568 |
template<typename JointModel> |
||
569 |
54 |
static void algo(const JointModelBase<JointModel> & jmodel, |
|
570 |
const Model & model, |
||
571 |
Data & data) |
||
572 |
{ |
||
573 |
typedef typename Model::JointIndex JointIndex; |
||
574 |
typedef CoriolisMatrixBackwardStep<Scalar,Options,JointCollectionTpl> EquivalentPass; |
||
575 |
|||
576 |
✓✗ | 54 |
const JointIndex i = jmodel.id(); |
577 |
54 |
const JointIndex parent = model.parents[i]; |
|
578 |
54 |
typename Data::RowMatrix6 & M6tmpR = data.M6tmpR; |
|
579 |
|||
580 |
typedef typename SizeDepType<JointModel::NV>::template ColsReturn<typename Data::Matrix6x>::Type ColsBlock; |
||
581 |
✓✗ | 54 |
ColsBlock dJ_cols = jmodel.jointCols(data.dJ); |
582 |
✓✗ | 54 |
ColsBlock J_cols = jmodel.jointCols(data.J); |
583 |
54 |
typename Data::Matrix6x & dFdv = data.Fcrb[0]; |
|
584 |
✓✗ | 54 |
ColsBlock dFdv_cols = jmodel.jointCols(dFdv); |
585 |
|||
586 |
✓✗ | 54 |
motionSet::inertiaAction(data.oYcrb[i],dJ_cols,dFdv_cols); |
587 |
✓✗✓✗ ✓✗ |
54 |
dFdv_cols.noalias() += data.B[i] * J_cols; |
588 |
|||
589 |
✓✗✓✗ ✓✗✓✗ |
54 |
data.C.block(jmodel.idx_v(),jmodel.idx_v(),jmodel.nv(),data.nvSubtree[i]).noalias() |
590 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
108 |
= J_cols.transpose() * dFdv.middleCols(jmodel.idx_v(),data.nvSubtree[i]); |
591 |
|||
592 |
✓✗✓✗ ✓✗✓✗ |
54 |
EquivalentPass::lhsInertiaMult(data.oYcrb[i],J_cols.transpose(),M6tmpR.topRows(jmodel.nv())); |
593 |
✓✗✓✓ |
536 |
for(int j = data.parents_fromRow[(JointIndex)jmodel.idx_v()];j >= 0; j = data.parents_fromRow[(JointIndex)j]) |
594 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
482 |
data.C.middleRows(jmodel.idx_v(),jmodel.nv()).col(j).noalias() = M6tmpR.topRows(jmodel.nv()) * data.dJ.col(j); |
595 |
|||
596 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
54 |
M6tmpR.topRows(jmodel.nv()).noalias() = J_cols.transpose() * data.B[i]; |
597 |
✓✗✓✓ |
536 |
for(int j = data.parents_fromRow[(JointIndex)jmodel.idx_v()];j >= 0; j = data.parents_fromRow[(JointIndex)j]) |
598 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
482 |
data.C.middleRows(jmodel.idx_v(),jmodel.nv()).col(j).noalias() += M6tmpR.topRows(jmodel.nv()) * data.J.col(j); |
599 |
|||
600 |
✓✓ | 54 |
if(parent>0) |
601 |
✓✗ | 52 |
data.B[parent] += data.B[i]; |
602 |
|||
603 |
} |
||
604 |
}; |
||
605 |
|||
606 |
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl> |
||
607 |
inline const typename DataTpl<Scalar,Options,JointCollectionTpl>::MatrixXs & |
||
608 |
1 |
getCoriolisMatrix(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
|
609 |
DataTpl<Scalar,Options,JointCollectionTpl> & data) |
||
610 |
{ |
||
611 |
✓✗ | 1 |
assert(model.check(data) && "data is not consistent with model."); |
612 |
|||
613 |
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model; |
||
614 |
typedef typename Model::JointIndex JointIndex; |
||
615 |
typedef typename Model::ConfigVectorType ConfigVectorType; |
||
616 |
typedef typename Model::TangentVectorType TangentVectorType; |
||
617 |
|||
618 |
typedef CoriolisMatrixForwardStep<Scalar,Options,JointCollectionTpl,ConfigVectorType,TangentVectorType> Pass1; |
||
619 |
✓✓ | 28 |
for(JointIndex i=1; i<(JointIndex)model.njoints; ++i) |
620 |
{ |
||
621 |
typedef typename Data::Inertia Inertia; |
||
622 |
✓✗ | 27 |
const Inertia oY = data.oMi[i].act(model.inertias[i]); |
623 |
✓✗✓✗ |
27 |
data.B[i] = oY.variation(0.5*data.ov[i]); |
624 |
✓✗✓✗ |
27 |
Pass1::addForceCrossMatrix(0.5*data.oh[i],data.B[i]); |
625 |
} |
||
626 |
|||
627 |
typedef GetCoriolisMatrixBackwardStep<Scalar,Options,JointCollectionTpl> Pass2; |
||
628 |
✓✓ | 28 |
for(JointIndex i=(JointIndex)(model.njoints-1); i>0; --i) |
629 |
{ |
||
630 |
✓✗ | 27 |
Pass2::run(model.joints[i],typename Pass2::ArgsType(model,data)); |
631 |
} |
||
632 |
|||
633 |
1 |
return data.C; |
|
634 |
} |
||
635 |
|||
636 |
} // namespace pinocchio |
||
637 |
|||
638 |
/// @endcond |
||
639 |
|||
640 |
#endif // ifndef __pinocchio_algorithm_rnea_hxx__ |
| Generated by: GCOVR (Version 4.2) |