GCC Code Coverage Report | |||||||||||||||||||||
|
|||||||||||||||||||||
Line | Branch | Exec | Source |
1 |
// |
||
2 |
// Copyright (c) 2015-2020 CNRS INRIA |
||
3 |
// |
||
4 |
|||
5 |
#ifndef __pinocchio_algorithm_frames_hxx__ |
||
6 |
#define __pinocchio_algorithm_frames_hxx__ |
||
7 |
|||
8 |
#include "pinocchio/algorithm/kinematics.hpp" |
||
9 |
#include "pinocchio/algorithm/jacobian.hpp" |
||
10 |
#include "pinocchio/algorithm/check.hpp" |
||
11 |
|||
12 |
namespace pinocchio |
||
13 |
{ |
||
14 |
|||
15 |
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl> |
||
16 |
8995 |
inline void updateFramePlacements(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
|
17 |
DataTpl<Scalar,Options,JointCollectionTpl> & data) |
||
18 |
{ |
||
19 |
✓✗ | 8995 |
assert(model.check(data) && "data is not consistent with model."); |
20 |
|||
21 |
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model; |
||
22 |
typedef typename Model::Frame Frame; |
||
23 |
typedef typename Model::FrameIndex FrameIndex; |
||
24 |
typedef typename Model::JointIndex JointIndex; |
||
25 |
|||
26 |
// The following for loop starts by index 1 because the first frame is fixed |
||
27 |
// and corresponds to the universe.s |
||
28 |
✓✓ | 503702 |
for(FrameIndex i=1; i < (FrameIndex) model.nframes; ++i) |
29 |
{ |
||
30 |
494707 |
const Frame & frame = model.frames[i]; |
|
31 |
494707 |
const JointIndex & parent = frame.parent; |
|
32 |
494707 |
data.oMf[i] = data.oMi[parent]*frame.placement; |
|
33 |
} |
||
34 |
8995 |
} |
|
35 |
|||
36 |
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl> |
||
37 |
inline const typename DataTpl<Scalar,Options,JointCollectionTpl>::SE3 & |
||
38 |
4 |
updateFramePlacement(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
|
39 |
DataTpl<Scalar,Options,JointCollectionTpl> & data, |
||
40 |
const FrameIndex frame_id) |
||
41 |
{ |
||
42 |
✓✗ | 4 |
assert(model.check(data) && "data is not consistent with model."); |
43 |
|||
44 |
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model; |
||
45 |
4 |
const typename Model::Frame & frame = model.frames[frame_id]; |
|
46 |
4 |
const typename Model::JointIndex & parent = frame.parent; |
|
47 |
|||
48 |
4 |
data.oMf[frame_id] = data.oMi[parent]*frame.placement; |
|
49 |
|||
50 |
4 |
return data.oMf[frame_id]; |
|
51 |
} |
||
52 |
|||
53 |
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType> |
||
54 |
9 |
inline void framesForwardKinematics(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
|
55 |
DataTpl<Scalar,Options,JointCollectionTpl> & data, |
||
56 |
const Eigen::MatrixBase<ConfigVectorType> & q) |
||
57 |
{ |
||
58 |
✓✗ | 9 |
assert(model.check(data) && "data is not consistent with model."); |
59 |
|||
60 |
9 |
forwardKinematics(model, data, q); |
|
61 |
9 |
updateFramePlacements(model, data); |
|
62 |
9 |
} |
|
63 |
|||
64 |
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl> |
||
65 |
inline MotionTpl<Scalar, Options> |
||
66 |
216 |
getFrameVelocity(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
|
67 |
const DataTpl<Scalar,Options,JointCollectionTpl> & data, |
||
68 |
const FrameIndex frame_id, |
||
69 |
const ReferenceFrame rf) |
||
70 |
{ |
||
71 |
✓✗ | 216 |
assert(model.check(data) && "data is not consistent with model."); |
72 |
|||
73 |
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model; |
||
74 |
typedef typename Model::Motion Motion; |
||
75 |
|||
76 |
216 |
const typename Model::Frame & frame = model.frames[frame_id]; |
|
77 |
216 |
const typename Model::SE3 & oMi = data.oMi[frame.parent]; |
|
78 |
216 |
const typename Model::Motion & v = data.v[frame.parent]; |
|
79 |
✓✓✓✗ |
216 |
switch(rf) |
80 |
{ |
||
81 |
78 |
case LOCAL: |
|
82 |
78 |
return frame.placement.actInv(v); |
|
83 |
69 |
case WORLD: |
|
84 |
69 |
return oMi.act(v); |
|
85 |
69 |
case LOCAL_WORLD_ALIGNED: |
|
86 |
✓✗✓✗ |
138 |
return Motion(oMi.rotation() * (v.linear() + v.angular().cross(frame.placement.translation())), |
87 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
207 |
oMi.rotation() * v.angular()); |
88 |
default: |
||
89 |
throw std::invalid_argument("Bad reference frame."); |
||
90 |
} |
||
91 |
} |
||
92 |
|||
93 |
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl> |
||
94 |
inline MotionTpl<Scalar, Options> |
||
95 |
315 |
getFrameAcceleration(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
|
96 |
const DataTpl<Scalar,Options,JointCollectionTpl> & data, |
||
97 |
const FrameIndex frame_id, |
||
98 |
const ReferenceFrame rf) |
||
99 |
{ |
||
100 |
✓✗ | 315 |
assert(model.check(data) && "data is not consistent with model."); |
101 |
|||
102 |
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model; |
||
103 |
typedef typename Model::Motion Motion; |
||
104 |
|||
105 |
315 |
const typename Model::Frame & frame = model.frames[frame_id]; |
|
106 |
315 |
const typename Model::SE3 & oMi = data.oMi[frame.parent]; |
|
107 |
315 |
const typename Model::Motion & a = data.a[frame.parent]; |
|
108 |
✓✓✓✗ |
315 |
switch(rf) |
109 |
{ |
||
110 |
111 |
case LOCAL: |
|
111 |
111 |
return frame.placement.actInv(a); |
|
112 |
102 |
case WORLD: |
|
113 |
102 |
return oMi.act(a); |
|
114 |
102 |
case LOCAL_WORLD_ALIGNED: |
|
115 |
✓✗✓✗ |
204 |
return Motion(oMi.rotation() * (a.linear() + a.angular().cross(frame.placement.translation())), |
116 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
306 |
oMi.rotation() * a.angular()); |
117 |
default: |
||
118 |
throw std::invalid_argument("Bad reference frame."); |
||
119 |
} |
||
120 |
} |
||
121 |
|||
122 |
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl> |
||
123 |
inline MotionTpl<Scalar, Options> |
||
124 |
9 |
getFrameClassicalAcceleration(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
|
125 |
const DataTpl<Scalar,Options,JointCollectionTpl> & data, |
||
126 |
const FrameIndex frame_id, |
||
127 |
const ReferenceFrame rf) |
||
128 |
{ |
||
129 |
✓✗✓✗ |
9 |
assert(model.check(data) && "data is not consistent with model."); |
130 |
|||
131 |
typedef MotionTpl<Scalar, Options> Motion; |
||
132 |
✓✗ | 9 |
Motion vel = getFrameVelocity(model, data, frame_id, rf); |
133 |
✓✗ | 9 |
Motion acc = getFrameAcceleration(model, data, frame_id, rf); |
134 |
|||
135 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
9 |
acc.linear() += vel.angular().cross(vel.linear()); |
136 |
|||
137 |
18 |
return acc; |
|
138 |
} |
||
139 |
|||
140 |
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename Matrix6xLike> |
||
141 |
20 |
inline void getFrameJacobian(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
|
142 |
DataTpl<Scalar,Options,JointCollectionTpl> & data, |
||
143 |
const FrameIndex frame_id, |
||
144 |
const ReferenceFrame rf, |
||
145 |
const Eigen::MatrixBase<Matrix6xLike> & J) |
||
146 |
{ |
||
147 |
|||
148 |
✗✓✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗ |
20 |
PINOCCHIO_CHECK_ARGUMENT_SIZE(J.rows(), 6); |
149 |
✗✓✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗ |
20 |
PINOCCHIO_CHECK_ARGUMENT_SIZE(J.cols(), model.nv); |
150 |
✓✗ | 20 |
assert(model.check(data) && "data is not consistent with model."); |
151 |
|||
152 |
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model; |
||
153 |
typedef typename Model::Frame Frame; |
||
154 |
typedef DataTpl<Scalar,Options,JointCollectionTpl> Data; |
||
155 |
typedef typename Model::JointIndex JointIndex; |
||
156 |
|||
157 |
20 |
const Frame & frame = model.frames[frame_id]; |
|
158 |
20 |
const JointIndex & joint_id = frame.parent; |
|
159 |
|||
160 |
20 |
Matrix6xLike & J_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLike,J); |
|
161 |
20 |
typename Data::SE3 & oMframe = data.oMf[frame_id]; |
|
162 |
20 |
oMframe = data.oMi[joint_id] * frame.placement; |
|
163 |
|||
164 |
20 |
details::translateJointJacobian(model,data,joint_id,rf,oMframe,data.J,J_); |
|
165 |
20 |
} |
|
166 |
|||
167 |
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename Matrix6xLike> |
||
168 |
8 |
inline void computeFrameJacobian(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
|
169 |
DataTpl<Scalar,Options,JointCollectionTpl> & data, |
||
170 |
const Eigen::MatrixBase<ConfigVectorType> & q, |
||
171 |
const FrameIndex frameId, |
||
172 |
const ReferenceFrame reference_frame, |
||
173 |
const Eigen::MatrixBase<Matrix6xLike> & J) |
||
174 |
{ |
||
175 |
✓✗ | 8 |
assert(model.check(data) && "data is not consistent with model."); |
176 |
✗✓✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗ |
8 |
PINOCCHIO_CHECK_ARGUMENT_SIZE(q.size(), model.nq, "The configuration vector is not of right size"); |
177 |
|||
178 |
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model; |
||
179 |
typedef DataTpl<Scalar,Options,JointCollectionTpl> Data; |
||
180 |
typedef typename Model::Frame Frame; |
||
181 |
typedef typename Model::JointIndex JointIndex; |
||
182 |
typedef typename Model::IndexVector IndexVector; |
||
183 |
|||
184 |
8 |
const Frame & frame = model.frames[frameId]; |
|
185 |
8 |
const JointIndex & joint_id = frame.parent; |
|
186 |
|||
187 |
8 |
const IndexVector & joint_support = model.supports[joint_id]; |
|
188 |
|||
189 |
✓✓✗ | 8 |
switch(reference_frame) |
190 |
{ |
||
191 |
2 |
case WORLD: |
|
192 |
case LOCAL_WORLD_ALIGNED: |
||
193 |
{ |
||
194 |
typedef JointJacobiansForwardStep<Scalar,Options,JointCollectionTpl,ConfigVectorType,Matrix6xLike> Pass; |
||
195 |
✓✓ | 12 |
for(size_t k = 1; k < joint_support.size(); k++) |
196 |
{ |
||
197 |
10 |
JointIndex parent = joint_support[k]; |
|
198 |
✓✗ | 10 |
Pass::run(model.joints[parent],data.joints[parent], |
199 |
typename Pass::ArgsType(model,data,q.derived(), |
||
200 |
10 |
PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLike,J))); |
|
201 |
} |
||
202 |
|||
203 |
✓✓ | 2 |
if(reference_frame == LOCAL_WORLD_ALIGNED) |
204 |
{ |
||
205 |
1 |
typename Data::SE3 & oMframe = data.oMf[frameId]; |
|
206 |
1 |
oMframe = data.oMi[joint_id] * frame.placement; |
|
207 |
|||
208 |
1 |
Matrix6xLike & J_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLike,J); |
|
209 |
|||
210 |
1 |
const int colRef = nv(model.joints[joint_id])+idx_v(model.joints[joint_id])-1; |
|
211 |
✓✓ | 11 |
for(Eigen::DenseIndex j=colRef;j>=0;j=data.parents_fromRow[(size_t) j]) |
212 |
{ |
||
213 |
typedef typename Matrix6xLike::ColXpr ColXprOut; |
||
214 |
✓✗✓✗ |
10 |
MotionRef<ColXprOut> J_col(J_.col(j)); |
215 |
|||
216 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
10 |
J_col.linear() -= oMframe.translation().cross(J_col.angular()); |
217 |
} |
||
218 |
} |
||
219 |
2 |
break; |
|
220 |
} |
||
221 |
6 |
case LOCAL: |
|
222 |
{ |
||
223 |
6 |
data.iMf[joint_id] = frame.placement; |
|
224 |
|||
225 |
typedef JointJacobianForwardStep<Scalar,Options,JointCollectionTpl,ConfigVectorType,Matrix6xLike> Pass; |
||
226 |
✓✓ | 32 |
for(JointIndex i=joint_id; i>0; i=model.parents[i]) |
227 |
{ |
||
228 |
✓✗ | 26 |
Pass::run(model.joints[i],data.joints[i], |
229 |
typename Pass::ArgsType(model,data,q.derived(), |
||
230 |
26 |
PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLike,J))); |
|
231 |
} |
||
232 |
6 |
break; |
|
233 |
} |
||
234 |
default: |
||
235 |
{ |
||
236 |
assert(false && "must never happened"); |
||
237 |
} |
||
238 |
} |
||
239 |
8 |
} |
|
240 |
|||
241 |
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename Matrix6xLike> |
||
242 |
6 |
void getFrameJacobianTimeVariation(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
|
243 |
DataTpl<Scalar,Options,JointCollectionTpl> & data, |
||
244 |
const FrameIndex frame_id, |
||
245 |
const ReferenceFrame rf, |
||
246 |
const Eigen::MatrixBase<Matrix6xLike> & dJ) |
||
247 |
{ |
||
248 |
✓✗ | 6 |
assert(model.check(data) && "data is not consistent with model."); |
249 |
|||
250 |
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model; |
||
251 |
typedef DataTpl<Scalar,Options,JointCollectionTpl> Data; |
||
252 |
typedef typename Model::Frame Frame; |
||
253 |
|||
254 |
6 |
const Frame & frame = model.frames[frame_id]; |
|
255 |
6 |
const JointIndex & joint_id = frame.parent; |
|
256 |
|||
257 |
6 |
typename Data::SE3 & oMframe = data.oMf[frame_id]; |
|
258 |
6 |
oMframe = data.oMi[joint_id] * frame.placement; |
|
259 |
|||
260 |
6 |
details::translateJointJacobian(model,data,joint_id,rf,oMframe, |
|
261 |
6 |
data.dJ,PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLike,dJ)); |
|
262 |
6 |
} |
|
263 |
|||
264 |
|||
265 |
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl> |
||
266 |
InertiaTpl<Scalar, Options> |
||
267 |
3 |
computeSupportedInertiaByFrame(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
|
268 |
const DataTpl<Scalar,Options,JointCollectionTpl> & data, |
||
269 |
const FrameIndex frame_id, |
||
270 |
bool with_subtree) |
||
271 |
{ |
||
272 |
✓✗✓✗ |
3 |
assert(model.check(data) && "data is not consistent with model."); |
273 |
|||
274 |
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model; |
||
275 |
typedef InertiaTpl<Scalar, Options> Inertia; |
||
276 |
|||
277 |
3 |
const Frame & frame = model.frames[frame_id]; |
|
278 |
3 |
const JointIndex & joint_id = frame.parent; |
|
279 |
|||
280 |
// Add all the inertia of child frames (i.e that are part of the same joint but comes after the given frame) |
||
281 |
✓✗ | 6 |
std::vector<typename Model::JointIndex> child_frames = {frame_id}; |
282 |
✓✗ | 3 |
Inertia I = frame.placement.act(frame.inertia); // Express the inertia in the parent joint frame |
283 |
✓✓ | 183 |
for(FrameIndex i=frame_id+1; i < (FrameIndex) model.nframes; ++i) |
284 |
{ |
||
285 |
✓✓ | 180 |
if(model.frames[i].parent != joint_id) |
286 |
171 |
continue; |
|
287 |
✓✗✓✓ |
9 |
if(std::find(child_frames.begin(), child_frames.end(), model.frames[i].previousFrame) == child_frames.end()) |
288 |
6 |
continue; |
|
289 |
✓✗ | 3 |
child_frames.push_back(i); |
290 |
✓✗✓✗ |
3 |
I += model.frames[i].placement.act(model.frames[i].inertia); |
291 |
} |
||
292 |
|||
293 |
✓✓ | 3 |
if(!with_subtree) |
294 |
{ |
||
295 |
✓✗ | 2 |
return frame.placement.actInv(I); |
296 |
} |
||
297 |
|||
298 |
// Express the inertia in the origin frame for simplicity. |
||
299 |
✓✗✓✗ |
1 |
I = data.oMi[joint_id].act(I); |
300 |
|||
301 |
// Add inertia of child joints |
||
302 |
1 |
const std::vector<typename Model::JointIndex> & subtree = model.subtrees[joint_id]; |
|
303 |
✓✓ | 15 |
for(size_t k=1; k < subtree.size(); ++k) // Skip the first joint as it is the one before the frame |
304 |
{ |
||
305 |
14 |
const typename Model::JointIndex j_id = subtree[k]; |
|
306 |
✓✗✓✗ |
14 |
I += data.oMi[j_id].act(model.inertias[j_id]); |
307 |
} |
||
308 |
|||
309 |
✓✗ | 1 |
const pinocchio::SE3 oMf = data.oMi[joint_id]*frame.placement; |
310 |
✓✗ | 1 |
return oMf.actInv(I); |
311 |
} |
||
312 |
|||
313 |
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl> |
||
314 |
ForceTpl<Scalar, Options> |
||
315 |
1 |
computeSupportedForceByFrame(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
|
316 |
const DataTpl<Scalar,Options,JointCollectionTpl> & data, |
||
317 |
const FrameIndex frame_id) |
||
318 |
{ |
||
319 |
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model; |
||
320 |
typedef InertiaTpl<Scalar, Options> Inertia; |
||
321 |
typedef MotionTpl<Scalar, Options> Motion; |
||
322 |
typedef ForceTpl<Scalar, Options> Force; |
||
323 |
|||
324 |
1 |
const Frame & frame = model.frames[frame_id]; |
|
325 |
1 |
const JointIndex & joint_id = frame.parent; |
|
326 |
|||
327 |
// Compute 'in body' forces |
||
328 |
✓✗ | 1 |
const Inertia fI = computeSupportedInertiaByFrame(model, data, frame_id, false); |
329 |
✓✗ | 1 |
const pinocchio::SE3 oMf = data.oMi[joint_id]*frame.placement; |
330 |
✓✗ | 1 |
const Motion v = getFrameVelocity(model, data, frame_id, LOCAL); |
331 |
✓✗ | 1 |
const Motion a = getFrameAcceleration(model, data, frame_id, LOCAL); |
332 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
1 |
Force f = fI.vxiv(v) + fI * (a - oMf.actInv(model.gravity)); |
333 |
|||
334 |
// Add child joints forces |
||
335 |
✓✗✓✗ |
1 |
f = frame.placement.act(f); // Express force in parent joint frame |
336 |
1 |
const std::vector<typename Model::JointIndex> & subtree = model.subtrees[joint_id]; |
|
337 |
✓✓ | 15 |
for(size_t k=1; k < subtree.size(); ++k) // Skip the first joint as it is the one before the frame |
338 |
{ |
||
339 |
14 |
const typename Model::JointIndex j_id = subtree[k]; |
|
340 |
✓✓ | 14 |
if(model.parents[j_id] != joint_id) // Joint is not a direct child |
341 |
{ |
||
342 |
11 |
continue; |
|
343 |
} |
||
344 |
✓✗✓✗ |
3 |
f += data.liMi[j_id].act(data.f[j_id]); |
345 |
} |
||
346 |
|||
347 |
// Transform back to local frame |
||
348 |
✓✗ | 2 |
return frame.placement.actInv(f); |
349 |
} |
||
350 |
} // namespace pinocchio |
||
351 |
|||
352 |
#endif // ifndef __pinocchio_algorithm_frames_hxx__ |
Generated by: GCOVR (Version 4.2) |