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