GCC Code Coverage Report | |||||||||||||||||||||
|
|||||||||||||||||||||
Line | Branch | Exec | Source |
1 |
// |
||
2 |
// Copyright (c) 2016-2019 CNRS INRIA |
||
3 |
// |
||
4 |
|||
5 |
#ifndef __pinocchio_kinematics_hxx__ |
||
6 |
#define __pinocchio_kinematics_hxx__ |
||
7 |
|||
8 |
#include "pinocchio/multibody/visitor.hpp" |
||
9 |
#include "pinocchio/algorithm/check.hpp" |
||
10 |
|||
11 |
namespace pinocchio |
||
12 |
{ |
||
13 |
|||
14 |
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl> |
||
15 |
1 |
inline void updateGlobalPlacements(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
|
16 |
DataTpl<Scalar,Options,JointCollectionTpl> & data) |
||
17 |
{ |
||
18 |
✓✗ | 1 |
assert(model.check(data) && "data is not consistent with model."); |
19 |
|||
20 |
typedef typename ModelTpl<Scalar,Options,JointCollectionTpl>::JointIndex JointIndex; |
||
21 |
|||
22 |
✓✓ | 28 |
for(JointIndex i=1; i <(JointIndex) model.njoints; ++i) |
23 |
{ |
||
24 |
27 |
const JointIndex & parent = model.parents[i]; |
|
25 |
|||
26 |
✓✓ | 27 |
if (parent>0) |
27 |
26 |
data.oMi[i] = data.oMi[parent] * data.liMi[i]; |
|
28 |
else |
||
29 |
1 |
data.oMi[i] = data.liMi[i]; |
|
30 |
} |
||
31 |
1 |
} |
|
32 |
|||
33 |
|||
34 |
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType> |
||
35 |
struct ForwardKinematicZeroStep |
||
36 |
: fusion::JointUnaryVisitorBase< ForwardKinematicZeroStep<Scalar,Options,JointCollectionTpl,ConfigVectorType> > |
||
37 |
{ |
||
38 |
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model; |
||
39 |
typedef DataTpl<Scalar,Options,JointCollectionTpl> Data; |
||
40 |
|||
41 |
typedef boost::fusion::vector<const Model &, |
||
42 |
Data &, |
||
43 |
const ConfigVectorType &> ArgsType; |
||
44 |
|||
45 |
template<typename JointModel> |
||
46 |
776742 |
static void algo(const JointModelBase<JointModel> & jmodel, |
|
47 |
JointDataBase<typename JointModel::JointDataDerived> & jdata, |
||
48 |
const Model & model, |
||
49 |
Data & data, |
||
50 |
const Eigen::MatrixBase<ConfigVectorType> & q) |
||
51 |
{ |
||
52 |
typedef typename Model::JointIndex JointIndex; |
||
53 |
|||
54 |
776742 |
const JointIndex & i = jmodel.id(); |
|
55 |
776742 |
const JointIndex & parent = model.parents[i]; |
|
56 |
|||
57 |
776742 |
jmodel.calc(jdata.derived(),q.derived()); |
|
58 |
|||
59 |
✓✗ | 776742 |
data.liMi[i] = model.jointPlacements[i] * jdata.M(); |
60 |
|||
61 |
✓✓ | 776742 |
if (parent>0) |
62 |
747662 |
data.oMi[i] = data.oMi[parent] * data.liMi[i]; |
|
63 |
else |
||
64 |
29080 |
data.oMi[i] = data.liMi[i]; |
|
65 |
} |
||
66 |
}; |
||
67 |
|||
68 |
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType> |
||
69 |
14532 |
inline void forwardKinematics(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
|
70 |
DataTpl<Scalar,Options,JointCollectionTpl> & data, |
||
71 |
const Eigen::MatrixBase<ConfigVectorType> & q) |
||
72 |
{ |
||
73 |
✗✓✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗ |
14532 |
PINOCCHIO_CHECK_ARGUMENT_SIZE(q.size(), model.nq, "The configuration vector is not of right size"); |
74 |
✓✗ | 14532 |
assert(model.check(data) && "data is not consistent with model."); |
75 |
|||
76 |
typedef typename ModelTpl<Scalar,Options,JointCollectionTpl>::JointIndex JointIndex; |
||
77 |
|||
78 |
typedef ForwardKinematicZeroStep<Scalar,Options,JointCollectionTpl,ConfigVectorType> Algo; |
||
79 |
✓✓ | 402957 |
for(JointIndex i=1; i < (JointIndex)model.njoints; ++i) |
80 |
{ |
||
81 |
✓✗ | 388425 |
Algo::run(model.joints[i], data.joints[i], |
82 |
typename Algo::ArgsType(model,data,q.derived())); |
||
83 |
} |
||
84 |
14532 |
} |
|
85 |
|||
86 |
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType> |
||
87 |
struct ForwardKinematicFirstStep |
||
88 |
: fusion::JointUnaryVisitorBase< ForwardKinematicFirstStep<Scalar,Options,JointCollectionTpl,ConfigVectorType,TangentVectorType> > |
||
89 |
{ |
||
90 |
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model; |
||
91 |
typedef DataTpl<Scalar,Options,JointCollectionTpl> Data; |
||
92 |
|||
93 |
typedef boost::fusion::vector<const Model &, |
||
94 |
Data &, |
||
95 |
const ConfigVectorType &, |
||
96 |
const TangentVectorType & |
||
97 |
> ArgsType; |
||
98 |
|||
99 |
template<typename JointModel> |
||
100 |
11238 |
static void algo(const JointModelBase<JointModel> & jmodel, |
|
101 |
JointDataBase<typename JointModel::JointDataDerived> & jdata, |
||
102 |
const Model & model, |
||
103 |
Data & data, |
||
104 |
const Eigen::MatrixBase<ConfigVectorType> & q, |
||
105 |
const Eigen::MatrixBase<TangentVectorType> & v) |
||
106 |
{ |
||
107 |
typedef typename ModelTpl<Scalar,Options,JointCollectionTpl>::JointIndex JointIndex; |
||
108 |
|||
109 |
11238 |
const JointIndex & i = jmodel.id(); |
|
110 |
11238 |
const JointIndex & parent = model.parents[i]; |
|
111 |
|||
112 |
11238 |
jmodel.calc(jdata.derived(),q.derived(),v.derived()); |
|
113 |
|||
114 |
11238 |
data.v[i] = jdata.v(); |
|
115 |
✓✗ | 11238 |
data.liMi[i] = model.jointPlacements[i]*jdata.M(); |
116 |
|||
117 |
✓✓ | 11238 |
if(parent>0) |
118 |
{ |
||
119 |
10794 |
data.oMi[i] = data.oMi[parent]*data.liMi[i]; |
|
120 |
✓✗ | 10794 |
data.v[i] += data.liMi[i].actInv(data.v[parent]); |
121 |
} |
||
122 |
else |
||
123 |
444 |
data.oMi[i] = data.liMi[i]; |
|
124 |
} |
||
125 |
|||
126 |
}; |
||
127 |
|||
128 |
|||
129 |
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType> |
||
130 |
250 |
inline void forwardKinematics(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
|
131 |
DataTpl<Scalar,Options,JointCollectionTpl> & data, |
||
132 |
const Eigen::MatrixBase<ConfigVectorType> & q, |
||
133 |
const Eigen::MatrixBase<TangentVectorType> & v) |
||
134 |
{ |
||
135 |
✗✓✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗ |
250 |
PINOCCHIO_CHECK_ARGUMENT_SIZE(q.size(), model.nq, "The configuration vector is not of right size"); |
136 |
✗✓✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗ |
250 |
PINOCCHIO_CHECK_ARGUMENT_SIZE(v.size(), model.nv, "The velocity vector is not of right size"); |
137 |
✓✗ | 250 |
assert(model.check(data) && "data is not consistent with model."); |
138 |
|||
139 |
typedef typename ModelTpl<Scalar,Options,JointCollectionTpl>::JointIndex JointIndex; |
||
140 |
|||
141 |
250 |
data.v[0].setZero(); |
|
142 |
|||
143 |
typedef ForwardKinematicFirstStep<Scalar,Options,JointCollectionTpl,ConfigVectorType,TangentVectorType> Algo; |
||
144 |
✓✓ | 6273 |
for(JointIndex i=1; i<(JointIndex) model.njoints; ++i) |
145 |
{ |
||
146 |
✓✗ | 6023 |
Algo::run(model.joints[i],data.joints[i], |
147 |
typename Algo::ArgsType(model,data,q.derived(),v.derived())); |
||
148 |
} |
||
149 |
250 |
} |
|
150 |
|||
151 |
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType1, typename TangentVectorType2> |
||
152 |
struct ForwardKinematicSecondStep : |
||
153 |
fusion::JointUnaryVisitorBase< ForwardKinematicSecondStep<Scalar,Options,JointCollectionTpl,ConfigVectorType,TangentVectorType1,TangentVectorType2> > |
||
154 |
{ |
||
155 |
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model; |
||
156 |
typedef DataTpl<Scalar,Options,JointCollectionTpl> Data; |
||
157 |
|||
158 |
typedef boost::fusion::vector<const Model &, |
||
159 |
Data &, |
||
160 |
const ConfigVectorType &, |
||
161 |
const TangentVectorType1 &, |
||
162 |
const TangentVectorType2 & |
||
163 |
> ArgsType; |
||
164 |
|||
165 |
template<typename JointModel> |
||
166 |
23092 |
static void algo(const JointModelBase<JointModel> & jmodel, |
|
167 |
JointDataBase<typename JointModel::JointDataDerived> & jdata, |
||
168 |
const Model & model, |
||
169 |
Data & data, |
||
170 |
const Eigen::MatrixBase<ConfigVectorType> & q, |
||
171 |
const Eigen::MatrixBase<TangentVectorType1> & v, |
||
172 |
const Eigen::MatrixBase<TangentVectorType2> & a) |
||
173 |
{ |
||
174 |
typedef typename Model::JointIndex JointIndex; |
||
175 |
|||
176 |
23092 |
const JointIndex & i = jmodel.id(); |
|
177 |
23092 |
const JointIndex & parent = model.parents[i]; |
|
178 |
|||
179 |
23092 |
jmodel.calc(jdata.derived(),q.derived(),v.derived()); |
|
180 |
|||
181 |
23092 |
data.v[i] = jdata.v(); |
|
182 |
✓✗ | 23092 |
data.liMi[i] = model.jointPlacements[i] * jdata.M(); |
183 |
|||
184 |
✓✓ | 23092 |
if(parent>0) |
185 |
{ |
||
186 |
22238 |
data.oMi[i] = data.oMi[parent] * data.liMi[i]; |
|
187 |
✓✗ | 22238 |
data.v[i] += data.liMi[i].actInv(data.v[parent]); |
188 |
} |
||
189 |
else |
||
190 |
854 |
data.oMi[i] = data.liMi[i]; |
|
191 |
|||
192 |
✓✗✓✗ ✓✗✓✗ ✓✓✗✓ ✗ |
23092 |
data.a[i] = jdata.S() * jmodel.jointVelocitySelector(a) + jdata.c() + (data.v[i] ^ jdata.v()) ; |
193 |
✓✗ | 23092 |
data.a[i] += data.liMi[i].actInv(data.a[parent]); |
194 |
} |
||
195 |
}; |
||
196 |
|||
197 |
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType1, typename TangentVectorType2> |
||
198 |
530 |
inline void forwardKinematics(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
|
199 |
DataTpl<Scalar,Options,JointCollectionTpl> & data, |
||
200 |
const Eigen::MatrixBase<ConfigVectorType> & q, |
||
201 |
const Eigen::MatrixBase<TangentVectorType1> & v, |
||
202 |
const Eigen::MatrixBase<TangentVectorType2> & a) |
||
203 |
{ |
||
204 |
✗✓✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗ |
530 |
PINOCCHIO_CHECK_ARGUMENT_SIZE(q.size(), model.nq, "The configuration vector is not of right size"); |
205 |
✗✓✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗ |
530 |
PINOCCHIO_CHECK_ARGUMENT_SIZE(v.size(), model.nv, "The velocity vector is not of right size"); |
206 |
✗✓✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗ |
530 |
PINOCCHIO_CHECK_ARGUMENT_SIZE(a.size(), model.nv, "The acceleration vector is not of right size"); |
207 |
✓✗ | 530 |
assert(model.check(data) && "data is not consistent with model."); |
208 |
|||
209 |
typedef typename ModelTpl<Scalar,Options,JointCollectionTpl>::JointIndex JointIndex; |
||
210 |
|||
211 |
530 |
data.v[0].setZero(); |
|
212 |
530 |
data.a[0].setZero(); |
|
213 |
|||
214 |
typedef ForwardKinematicSecondStep<Scalar,Options,JointCollectionTpl,ConfigVectorType,TangentVectorType1,TangentVectorType2> Algo; |
||
215 |
✓✓ | 14857 |
for(JointIndex i=1; i < (JointIndex)model.njoints; ++i) |
216 |
{ |
||
217 |
✓✗ | 14327 |
Algo::run(model.joints[i],data.joints[i], |
218 |
typename Algo::ArgsType(model,data,q.derived(),v.derived(),a.derived())); |
||
219 |
} |
||
220 |
530 |
} |
|
221 |
|||
222 |
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl> |
||
223 |
inline MotionTpl<Scalar, Options> |
||
224 |
224 |
getVelocity(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
|
225 |
const DataTpl<Scalar,Options,JointCollectionTpl> & data, |
||
226 |
const JointIndex jointId, |
||
227 |
const ReferenceFrame rf) |
||
228 |
{ |
||
229 |
✓✗ | 224 |
assert(model.check(data) && "data is not consistent with model."); |
230 |
PINOCCHIO_UNUSED_VARIABLE(model); |
||
231 |
✓✓✓✗ |
224 |
switch(rf) |
232 |
{ |
||
233 |
112 |
case LOCAL: |
|
234 |
112 |
return data.v[jointId]; |
|
235 |
56 |
case WORLD: |
|
236 |
56 |
return data.oMi[jointId].act(data.v[jointId]); |
|
237 |
56 |
case LOCAL_WORLD_ALIGNED: |
|
238 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
56 |
return MotionTpl<Scalar, Options>(data.oMi[jointId].rotation() * data.v[jointId].linear(), data.oMi[jointId].rotation() * data.v[jointId].angular()); |
239 |
default: |
||
240 |
throw std::invalid_argument("Bad reference frame."); |
||
241 |
} |
||
242 |
} |
||
243 |
|||
244 |
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl> |
||
245 |
inline MotionTpl<Scalar, Options> |
||
246 |
224 |
getAcceleration(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
|
247 |
const DataTpl<Scalar,Options,JointCollectionTpl> & data, |
||
248 |
const JointIndex jointId, |
||
249 |
const ReferenceFrame rf) |
||
250 |
{ |
||
251 |
✓✗ | 224 |
assert(model.check(data) && "data is not consistent with model."); |
252 |
PINOCCHIO_UNUSED_VARIABLE(model); |
||
253 |
✓✓✓✗ |
224 |
switch(rf) |
254 |
{ |
||
255 |
112 |
case LOCAL: |
|
256 |
112 |
return data.a[jointId]; |
|
257 |
56 |
case WORLD: |
|
258 |
56 |
return data.oMi[jointId].act(data.a[jointId]); |
|
259 |
56 |
case LOCAL_WORLD_ALIGNED: |
|
260 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
56 |
return MotionTpl<Scalar, Options>(data.oMi[jointId].rotation() * data.a[jointId].linear(), data.oMi[jointId].rotation() * data.a[jointId].angular()); |
261 |
default: |
||
262 |
throw std::invalid_argument("Bad reference frame."); |
||
263 |
} |
||
264 |
} |
||
265 |
|||
266 |
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl> |
||
267 |
inline MotionTpl<Scalar, Options> |
||
268 |
112 |
getClassicalAcceleration(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
|
269 |
const DataTpl<Scalar,Options,JointCollectionTpl> & data, |
||
270 |
const JointIndex jointId, |
||
271 |
const ReferenceFrame rf) |
||
272 |
{ |
||
273 |
✓✗✓✗ |
112 |
assert(model.check(data) && "data is not consistent with model."); |
274 |
|||
275 |
typedef MotionTpl<Scalar, Options> Motion; |
||
276 |
✓✗ | 112 |
Motion vel = getVelocity(model, data, jointId, rf); |
277 |
✓✗ | 112 |
Motion acc = getAcceleration(model, data, jointId, rf); |
278 |
|||
279 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
112 |
acc.linear() += vel.angular().cross(vel.linear()); |
280 |
|||
281 |
224 |
return acc; |
|
282 |
} |
||
283 |
} // namespace pinocchio |
||
284 |
|||
285 |
#endif // ifndef __pinocchio_kinematics_hxx__ |
Generated by: GCOVR (Version 4.2) |