- 3.0.2 rigid body dynamics module.
forward_dynamics.impl.h
Go to the documentation of this file.
1 
2 // Initialization of static-const data
3 template <typename TRAIT>
4 const typename iit::TestHyQ::dyn::tpl::ForwardDynamics<TRAIT>::ExtForces
5  iit::TestHyQ::dyn::tpl::ForwardDynamics<TRAIT>::zeroExtForces(Force::Zero());
6 
7 template <typename TRAIT>
8 iit::TestHyQ::dyn::tpl::ForwardDynamics<TRAIT>::ForwardDynamics(iit::TestHyQ::dyn::tpl::InertiaProperties<TRAIT>& inertia, MTransforms& transforms) :
9  inertiaProps( & inertia ),
10  motionTransforms( & transforms )
11 {
12  LF_hipassembly_v.setZero();
13  LF_hipassembly_c.setZero();
14  LF_upperleg_v.setZero();
15  LF_upperleg_c.setZero();
16  LF_lowerleg_v.setZero();
17  LF_lowerleg_c.setZero();
18  RF_hipassembly_v.setZero();
19  RF_hipassembly_c.setZero();
20  RF_upperleg_v.setZero();
21  RF_upperleg_c.setZero();
22  RF_lowerleg_v.setZero();
23  RF_lowerleg_c.setZero();
24  LH_hipassembly_v.setZero();
25  LH_hipassembly_c.setZero();
26  LH_upperleg_v.setZero();
27  LH_upperleg_c.setZero();
28  LH_lowerleg_v.setZero();
29  LH_lowerleg_c.setZero();
30  RH_hipassembly_v.setZero();
31  RH_hipassembly_c.setZero();
32  RH_upperleg_v.setZero();
33  RH_upperleg_c.setZero();
34  RH_lowerleg_v.setZero();
35  RH_lowerleg_c.setZero();
36 
37  vcross.setZero();
38  Ia_r.setZero();
39 
40 }
41 
42 template <typename TRAIT>
43 void iit::TestHyQ::dyn::tpl::ForwardDynamics<TRAIT>::fd(
44  JointState& qdd,
45  Acceleration& trunk_a,
46  const Velocity& trunk_v,
47  const Acceleration& g,
48  const JointState& qd,
49  const JointState& tau,
50  const ExtForces& fext/* = zeroExtForces */)
51 {
52 
53  trunk_AI = inertiaProps->getTensor_trunk();
54  trunk_p = - fext[TRUNK];
55  LF_hipassembly_AI = inertiaProps->getTensor_LF_hipassembly();
56  LF_hipassembly_p = - fext[LF_HIPASSEMBLY];
57  LF_upperleg_AI = inertiaProps->getTensor_LF_upperleg();
58  LF_upperleg_p = - fext[LF_UPPERLEG];
59  LF_lowerleg_AI = inertiaProps->getTensor_LF_lowerleg();
60  LF_lowerleg_p = - fext[LF_LOWERLEG];
61  RF_hipassembly_AI = inertiaProps->getTensor_RF_hipassembly();
62  RF_hipassembly_p = - fext[RF_HIPASSEMBLY];
63  RF_upperleg_AI = inertiaProps->getTensor_RF_upperleg();
64  RF_upperleg_p = - fext[RF_UPPERLEG];
65  RF_lowerleg_AI = inertiaProps->getTensor_RF_lowerleg();
66  RF_lowerleg_p = - fext[RF_LOWERLEG];
67  LH_hipassembly_AI = inertiaProps->getTensor_LH_hipassembly();
68  LH_hipassembly_p = - fext[LH_HIPASSEMBLY];
69  LH_upperleg_AI = inertiaProps->getTensor_LH_upperleg();
70  LH_upperleg_p = - fext[LH_UPPERLEG];
71  LH_lowerleg_AI = inertiaProps->getTensor_LH_lowerleg();
72  LH_lowerleg_p = - fext[LH_LOWERLEG];
73  RH_hipassembly_AI = inertiaProps->getTensor_RH_hipassembly();
74  RH_hipassembly_p = - fext[RH_HIPASSEMBLY];
75  RH_upperleg_AI = inertiaProps->getTensor_RH_upperleg();
76  RH_upperleg_p = - fext[RH_UPPERLEG];
77  RH_lowerleg_AI = inertiaProps->getTensor_RH_lowerleg();
78  RH_lowerleg_p = - fext[RH_LOWERLEG];
79  // ---------------------- FIRST PASS ---------------------- //
80  // Note that, during the first pass, the articulated inertias are really
81  // just the spatial inertia of the links (see assignments above).
82  // Afterwards things change, and articulated inertias shall not be used
83  // in functions which work specifically with spatial inertias.
84 
85  // + Link LF_hipassembly
86  // - The spatial velocity:
87  LF_hipassembly_v = (motionTransforms-> fr_LF_hipassembly_X_fr_trunk) * trunk_v;
88  LF_hipassembly_v(iit::rbd::AZ) += qd(LF_HAA);
89 
90  // - The velocity-product acceleration term:
91  iit::rbd::motionCrossProductMx<Scalar>(LF_hipassembly_v, vcross);
92  LF_hipassembly_c = vcross.col(iit::rbd::AZ) * qd(LF_HAA);
93 
94  // - The bias force term:
95  LF_hipassembly_p += iit::rbd::vxIv(LF_hipassembly_v, LF_hipassembly_AI);
96 
97  // + Link LF_upperleg
98  // - The spatial velocity:
99  LF_upperleg_v = (motionTransforms-> fr_LF_upperleg_X_fr_LF_hipassembly) * LF_hipassembly_v;
100  LF_upperleg_v(iit::rbd::AZ) += qd(LF_HFE);
101 
102  // - The velocity-product acceleration term:
103  iit::rbd::motionCrossProductMx<Scalar>(LF_upperleg_v, vcross);
104  LF_upperleg_c = vcross.col(iit::rbd::AZ) * qd(LF_HFE);
105 
106  // - The bias force term:
107  LF_upperleg_p += iit::rbd::vxIv(LF_upperleg_v, LF_upperleg_AI);
108 
109  // + Link LF_lowerleg
110  // - The spatial velocity:
111  LF_lowerleg_v = (motionTransforms-> fr_LF_lowerleg_X_fr_LF_upperleg) * LF_upperleg_v;
112  LF_lowerleg_v(iit::rbd::AZ) += qd(LF_KFE);
113 
114  // - The velocity-product acceleration term:
115  iit::rbd::motionCrossProductMx<Scalar>(LF_lowerleg_v, vcross);
116  LF_lowerleg_c = vcross.col(iit::rbd::AZ) * qd(LF_KFE);
117 
118  // - The bias force term:
119  LF_lowerleg_p += iit::rbd::vxIv(LF_lowerleg_v, LF_lowerleg_AI);
120 
121  // + Link RF_hipassembly
122  // - The spatial velocity:
123  RF_hipassembly_v = (motionTransforms-> fr_RF_hipassembly_X_fr_trunk) * trunk_v;
124  RF_hipassembly_v(iit::rbd::AZ) += qd(RF_HAA);
125 
126  // - The velocity-product acceleration term:
127  iit::rbd::motionCrossProductMx<Scalar>(RF_hipassembly_v, vcross);
128  RF_hipassembly_c = vcross.col(iit::rbd::AZ) * qd(RF_HAA);
129 
130  // - The bias force term:
131  RF_hipassembly_p += iit::rbd::vxIv(RF_hipassembly_v, RF_hipassembly_AI);
132 
133  // + Link RF_upperleg
134  // - The spatial velocity:
135  RF_upperleg_v = (motionTransforms-> fr_RF_upperleg_X_fr_RF_hipassembly) * RF_hipassembly_v;
136  RF_upperleg_v(iit::rbd::AZ) += qd(RF_HFE);
137 
138  // - The velocity-product acceleration term:
139  iit::rbd::motionCrossProductMx<Scalar>(RF_upperleg_v, vcross);
140  RF_upperleg_c = vcross.col(iit::rbd::AZ) * qd(RF_HFE);
141 
142  // - The bias force term:
143  RF_upperleg_p += iit::rbd::vxIv(RF_upperleg_v, RF_upperleg_AI);
144 
145  // + Link RF_lowerleg
146  // - The spatial velocity:
147  RF_lowerleg_v = (motionTransforms-> fr_RF_lowerleg_X_fr_RF_upperleg) * RF_upperleg_v;
148  RF_lowerleg_v(iit::rbd::AZ) += qd(RF_KFE);
149 
150  // - The velocity-product acceleration term:
151  iit::rbd::motionCrossProductMx<Scalar>(RF_lowerleg_v, vcross);
152  RF_lowerleg_c = vcross.col(iit::rbd::AZ) * qd(RF_KFE);
153 
154  // - The bias force term:
155  RF_lowerleg_p += iit::rbd::vxIv(RF_lowerleg_v, RF_lowerleg_AI);
156 
157  // + Link LH_hipassembly
158  // - The spatial velocity:
159  LH_hipassembly_v = (motionTransforms-> fr_LH_hipassembly_X_fr_trunk) * trunk_v;
160  LH_hipassembly_v(iit::rbd::AZ) += qd(LH_HAA);
161 
162  // - The velocity-product acceleration term:
163  iit::rbd::motionCrossProductMx<Scalar>(LH_hipassembly_v, vcross);
164  LH_hipassembly_c = vcross.col(iit::rbd::AZ) * qd(LH_HAA);
165 
166  // - The bias force term:
167  LH_hipassembly_p += iit::rbd::vxIv(LH_hipassembly_v, LH_hipassembly_AI);
168 
169  // + Link LH_upperleg
170  // - The spatial velocity:
171  LH_upperleg_v = (motionTransforms-> fr_LH_upperleg_X_fr_LH_hipassembly) * LH_hipassembly_v;
172  LH_upperleg_v(iit::rbd::AZ) += qd(LH_HFE);
173 
174  // - The velocity-product acceleration term:
175  iit::rbd::motionCrossProductMx<Scalar>(LH_upperleg_v, vcross);
176  LH_upperleg_c = vcross.col(iit::rbd::AZ) * qd(LH_HFE);
177 
178  // - The bias force term:
179  LH_upperleg_p += iit::rbd::vxIv(LH_upperleg_v, LH_upperleg_AI);
180 
181  // + Link LH_lowerleg
182  // - The spatial velocity:
183  LH_lowerleg_v = (motionTransforms-> fr_LH_lowerleg_X_fr_LH_upperleg) * LH_upperleg_v;
184  LH_lowerleg_v(iit::rbd::AZ) += qd(LH_KFE);
185 
186  // - The velocity-product acceleration term:
187  iit::rbd::motionCrossProductMx<Scalar>(LH_lowerleg_v, vcross);
188  LH_lowerleg_c = vcross.col(iit::rbd::AZ) * qd(LH_KFE);
189 
190  // - The bias force term:
191  LH_lowerleg_p += iit::rbd::vxIv(LH_lowerleg_v, LH_lowerleg_AI);
192 
193  // + Link RH_hipassembly
194  // - The spatial velocity:
195  RH_hipassembly_v = (motionTransforms-> fr_RH_hipassembly_X_fr_trunk) * trunk_v;
196  RH_hipassembly_v(iit::rbd::AZ) += qd(RH_HAA);
197 
198  // - The velocity-product acceleration term:
199  iit::rbd::motionCrossProductMx<Scalar>(RH_hipassembly_v, vcross);
200  RH_hipassembly_c = vcross.col(iit::rbd::AZ) * qd(RH_HAA);
201 
202  // - The bias force term:
203  RH_hipassembly_p += iit::rbd::vxIv(RH_hipassembly_v, RH_hipassembly_AI);
204 
205  // + Link RH_upperleg
206  // - The spatial velocity:
207  RH_upperleg_v = (motionTransforms-> fr_RH_upperleg_X_fr_RH_hipassembly) * RH_hipassembly_v;
208  RH_upperleg_v(iit::rbd::AZ) += qd(RH_HFE);
209 
210  // - The velocity-product acceleration term:
211  iit::rbd::motionCrossProductMx<Scalar>(RH_upperleg_v, vcross);
212  RH_upperleg_c = vcross.col(iit::rbd::AZ) * qd(RH_HFE);
213 
214  // - The bias force term:
215  RH_upperleg_p += iit::rbd::vxIv(RH_upperleg_v, RH_upperleg_AI);
216 
217  // + Link RH_lowerleg
218  // - The spatial velocity:
219  RH_lowerleg_v = (motionTransforms-> fr_RH_lowerleg_X_fr_RH_upperleg) * RH_upperleg_v;
220  RH_lowerleg_v(iit::rbd::AZ) += qd(RH_KFE);
221 
222  // - The velocity-product acceleration term:
223  iit::rbd::motionCrossProductMx<Scalar>(RH_lowerleg_v, vcross);
224  RH_lowerleg_c = vcross.col(iit::rbd::AZ) * qd(RH_KFE);
225 
226  // - The bias force term:
227  RH_lowerleg_p += iit::rbd::vxIv(RH_lowerleg_v, RH_lowerleg_AI);
228 
229  // + The floating base body
230  trunk_p += iit::rbd::vxIv(trunk_v, trunk_AI);
231 
232  // ---------------------- SECOND PASS ---------------------- //
233  Matrix66S IaB;
234  Force pa;
235 
236  // + Link RH_lowerleg
237  RH_lowerleg_u = tau(RH_KFE) - RH_lowerleg_p(iit::rbd::AZ);
238  RH_lowerleg_U = RH_lowerleg_AI.col(iit::rbd::AZ);
239  RH_lowerleg_D = RH_lowerleg_U(iit::rbd::AZ);
240 
241  iit::rbd::compute_Ia_revolute(RH_lowerleg_AI, RH_lowerleg_U, RH_lowerleg_D, Ia_r); // same as: Ia_r = RH_lowerleg_AI - RH_lowerleg_U/RH_lowerleg_D * RH_lowerleg_U.transpose();
242  pa = RH_lowerleg_p + Ia_r * RH_lowerleg_c + RH_lowerleg_U * RH_lowerleg_u/RH_lowerleg_D;
243  ctransform_Ia_revolute(Ia_r, motionTransforms-> fr_RH_lowerleg_X_fr_RH_upperleg, IaB);
244  RH_upperleg_AI += IaB;
245  RH_upperleg_p += (motionTransforms-> fr_RH_lowerleg_X_fr_RH_upperleg).transpose() * pa;
246 
247  // + Link RH_upperleg
248  RH_upperleg_u = tau(RH_HFE) - RH_upperleg_p(iit::rbd::AZ);
249  RH_upperleg_U = RH_upperleg_AI.col(iit::rbd::AZ);
250  RH_upperleg_D = RH_upperleg_U(iit::rbd::AZ);
251 
252  iit::rbd::compute_Ia_revolute(RH_upperleg_AI, RH_upperleg_U, RH_upperleg_D, Ia_r); // same as: Ia_r = RH_upperleg_AI - RH_upperleg_U/RH_upperleg_D * RH_upperleg_U.transpose();
253  pa = RH_upperleg_p + Ia_r * RH_upperleg_c + RH_upperleg_U * RH_upperleg_u/RH_upperleg_D;
254  ctransform_Ia_revolute(Ia_r, motionTransforms-> fr_RH_upperleg_X_fr_RH_hipassembly, IaB);
255  RH_hipassembly_AI += IaB;
256  RH_hipassembly_p += (motionTransforms-> fr_RH_upperleg_X_fr_RH_hipassembly).transpose() * pa;
257 
258  // + Link RH_hipassembly
259  RH_hipassembly_u = tau(RH_HAA) - RH_hipassembly_p(iit::rbd::AZ);
260  RH_hipassembly_U = RH_hipassembly_AI.col(iit::rbd::AZ);
261  RH_hipassembly_D = RH_hipassembly_U(iit::rbd::AZ);
262 
263  iit::rbd::compute_Ia_revolute(RH_hipassembly_AI, RH_hipassembly_U, RH_hipassembly_D, Ia_r); // same as: Ia_r = RH_hipassembly_AI - RH_hipassembly_U/RH_hipassembly_D * RH_hipassembly_U.transpose();
264  pa = RH_hipassembly_p + Ia_r * RH_hipassembly_c + RH_hipassembly_U * RH_hipassembly_u/RH_hipassembly_D;
265  ctransform_Ia_revolute(Ia_r, motionTransforms-> fr_RH_hipassembly_X_fr_trunk, IaB);
266  trunk_AI += IaB;
267  trunk_p += (motionTransforms-> fr_RH_hipassembly_X_fr_trunk).transpose() * pa;
268 
269  // + Link LH_lowerleg
270  LH_lowerleg_u = tau(LH_KFE) - LH_lowerleg_p(iit::rbd::AZ);
271  LH_lowerleg_U = LH_lowerleg_AI.col(iit::rbd::AZ);
272  LH_lowerleg_D = LH_lowerleg_U(iit::rbd::AZ);
273 
274  iit::rbd::compute_Ia_revolute(LH_lowerleg_AI, LH_lowerleg_U, LH_lowerleg_D, Ia_r); // same as: Ia_r = LH_lowerleg_AI - LH_lowerleg_U/LH_lowerleg_D * LH_lowerleg_U.transpose();
275  pa = LH_lowerleg_p + Ia_r * LH_lowerleg_c + LH_lowerleg_U * LH_lowerleg_u/LH_lowerleg_D;
276  ctransform_Ia_revolute(Ia_r, motionTransforms-> fr_LH_lowerleg_X_fr_LH_upperleg, IaB);
277  LH_upperleg_AI += IaB;
278  LH_upperleg_p += (motionTransforms-> fr_LH_lowerleg_X_fr_LH_upperleg).transpose() * pa;
279 
280  // + Link LH_upperleg
281  LH_upperleg_u = tau(LH_HFE) - LH_upperleg_p(iit::rbd::AZ);
282  LH_upperleg_U = LH_upperleg_AI.col(iit::rbd::AZ);
283  LH_upperleg_D = LH_upperleg_U(iit::rbd::AZ);
284 
285  iit::rbd::compute_Ia_revolute(LH_upperleg_AI, LH_upperleg_U, LH_upperleg_D, Ia_r); // same as: Ia_r = LH_upperleg_AI - LH_upperleg_U/LH_upperleg_D * LH_upperleg_U.transpose();
286  pa = LH_upperleg_p + Ia_r * LH_upperleg_c + LH_upperleg_U * LH_upperleg_u/LH_upperleg_D;
287  ctransform_Ia_revolute(Ia_r, motionTransforms-> fr_LH_upperleg_X_fr_LH_hipassembly, IaB);
288  LH_hipassembly_AI += IaB;
289  LH_hipassembly_p += (motionTransforms-> fr_LH_upperleg_X_fr_LH_hipassembly).transpose() * pa;
290 
291  // + Link LH_hipassembly
292  LH_hipassembly_u = tau(LH_HAA) - LH_hipassembly_p(iit::rbd::AZ);
293  LH_hipassembly_U = LH_hipassembly_AI.col(iit::rbd::AZ);
294  LH_hipassembly_D = LH_hipassembly_U(iit::rbd::AZ);
295 
296  iit::rbd::compute_Ia_revolute(LH_hipassembly_AI, LH_hipassembly_U, LH_hipassembly_D, Ia_r); // same as: Ia_r = LH_hipassembly_AI - LH_hipassembly_U/LH_hipassembly_D * LH_hipassembly_U.transpose();
297  pa = LH_hipassembly_p + Ia_r * LH_hipassembly_c + LH_hipassembly_U * LH_hipassembly_u/LH_hipassembly_D;
298  ctransform_Ia_revolute(Ia_r, motionTransforms-> fr_LH_hipassembly_X_fr_trunk, IaB);
299  trunk_AI += IaB;
300  trunk_p += (motionTransforms-> fr_LH_hipassembly_X_fr_trunk).transpose() * pa;
301 
302  // + Link RF_lowerleg
303  RF_lowerleg_u = tau(RF_KFE) - RF_lowerleg_p(iit::rbd::AZ);
304  RF_lowerleg_U = RF_lowerleg_AI.col(iit::rbd::AZ);
305  RF_lowerleg_D = RF_lowerleg_U(iit::rbd::AZ);
306 
307  iit::rbd::compute_Ia_revolute(RF_lowerleg_AI, RF_lowerleg_U, RF_lowerleg_D, Ia_r); // same as: Ia_r = RF_lowerleg_AI - RF_lowerleg_U/RF_lowerleg_D * RF_lowerleg_U.transpose();
308  pa = RF_lowerleg_p + Ia_r * RF_lowerleg_c + RF_lowerleg_U * RF_lowerleg_u/RF_lowerleg_D;
309  ctransform_Ia_revolute(Ia_r, motionTransforms-> fr_RF_lowerleg_X_fr_RF_upperleg, IaB);
310  RF_upperleg_AI += IaB;
311  RF_upperleg_p += (motionTransforms-> fr_RF_lowerleg_X_fr_RF_upperleg).transpose() * pa;
312 
313  // + Link RF_upperleg
314  RF_upperleg_u = tau(RF_HFE) - RF_upperleg_p(iit::rbd::AZ);
315  RF_upperleg_U = RF_upperleg_AI.col(iit::rbd::AZ);
316  RF_upperleg_D = RF_upperleg_U(iit::rbd::AZ);
317 
318  iit::rbd::compute_Ia_revolute(RF_upperleg_AI, RF_upperleg_U, RF_upperleg_D, Ia_r); // same as: Ia_r = RF_upperleg_AI - RF_upperleg_U/RF_upperleg_D * RF_upperleg_U.transpose();
319  pa = RF_upperleg_p + Ia_r * RF_upperleg_c + RF_upperleg_U * RF_upperleg_u/RF_upperleg_D;
320  ctransform_Ia_revolute(Ia_r, motionTransforms-> fr_RF_upperleg_X_fr_RF_hipassembly, IaB);
321  RF_hipassembly_AI += IaB;
322  RF_hipassembly_p += (motionTransforms-> fr_RF_upperleg_X_fr_RF_hipassembly).transpose() * pa;
323 
324  // + Link RF_hipassembly
325  RF_hipassembly_u = tau(RF_HAA) - RF_hipassembly_p(iit::rbd::AZ);
326  RF_hipassembly_U = RF_hipassembly_AI.col(iit::rbd::AZ);
327  RF_hipassembly_D = RF_hipassembly_U(iit::rbd::AZ);
328 
329  iit::rbd::compute_Ia_revolute(RF_hipassembly_AI, RF_hipassembly_U, RF_hipassembly_D, Ia_r); // same as: Ia_r = RF_hipassembly_AI - RF_hipassembly_U/RF_hipassembly_D * RF_hipassembly_U.transpose();
330  pa = RF_hipassembly_p + Ia_r * RF_hipassembly_c + RF_hipassembly_U * RF_hipassembly_u/RF_hipassembly_D;
331  ctransform_Ia_revolute(Ia_r, motionTransforms-> fr_RF_hipassembly_X_fr_trunk, IaB);
332  trunk_AI += IaB;
333  trunk_p += (motionTransforms-> fr_RF_hipassembly_X_fr_trunk).transpose() * pa;
334 
335  // + Link LF_lowerleg
336  LF_lowerleg_u = tau(LF_KFE) - LF_lowerleg_p(iit::rbd::AZ);
337  LF_lowerleg_U = LF_lowerleg_AI.col(iit::rbd::AZ);
338  LF_lowerleg_D = LF_lowerleg_U(iit::rbd::AZ);
339 
340  iit::rbd::compute_Ia_revolute(LF_lowerleg_AI, LF_lowerleg_U, LF_lowerleg_D, Ia_r); // same as: Ia_r = LF_lowerleg_AI - LF_lowerleg_U/LF_lowerleg_D * LF_lowerleg_U.transpose();
341  pa = LF_lowerleg_p + Ia_r * LF_lowerleg_c + LF_lowerleg_U * LF_lowerleg_u/LF_lowerleg_D;
342  ctransform_Ia_revolute(Ia_r, motionTransforms-> fr_LF_lowerleg_X_fr_LF_upperleg, IaB);
343  LF_upperleg_AI += IaB;
344  LF_upperleg_p += (motionTransforms-> fr_LF_lowerleg_X_fr_LF_upperleg).transpose() * pa;
345 
346  // + Link LF_upperleg
347  LF_upperleg_u = tau(LF_HFE) - LF_upperleg_p(iit::rbd::AZ);
348  LF_upperleg_U = LF_upperleg_AI.col(iit::rbd::AZ);
349  LF_upperleg_D = LF_upperleg_U(iit::rbd::AZ);
350 
351  iit::rbd::compute_Ia_revolute(LF_upperleg_AI, LF_upperleg_U, LF_upperleg_D, Ia_r); // same as: Ia_r = LF_upperleg_AI - LF_upperleg_U/LF_upperleg_D * LF_upperleg_U.transpose();
352  pa = LF_upperleg_p + Ia_r * LF_upperleg_c + LF_upperleg_U * LF_upperleg_u/LF_upperleg_D;
353  ctransform_Ia_revolute(Ia_r, motionTransforms-> fr_LF_upperleg_X_fr_LF_hipassembly, IaB);
354  LF_hipassembly_AI += IaB;
355  LF_hipassembly_p += (motionTransforms-> fr_LF_upperleg_X_fr_LF_hipassembly).transpose() * pa;
356 
357  // + Link LF_hipassembly
358  LF_hipassembly_u = tau(LF_HAA) - LF_hipassembly_p(iit::rbd::AZ);
359  LF_hipassembly_U = LF_hipassembly_AI.col(iit::rbd::AZ);
360  LF_hipassembly_D = LF_hipassembly_U(iit::rbd::AZ);
361 
362  iit::rbd::compute_Ia_revolute(LF_hipassembly_AI, LF_hipassembly_U, LF_hipassembly_D, Ia_r); // same as: Ia_r = LF_hipassembly_AI - LF_hipassembly_U/LF_hipassembly_D * LF_hipassembly_U.transpose();
363  pa = LF_hipassembly_p + Ia_r * LF_hipassembly_c + LF_hipassembly_U * LF_hipassembly_u/LF_hipassembly_D;
364  ctransform_Ia_revolute(Ia_r, motionTransforms-> fr_LF_hipassembly_X_fr_trunk, IaB);
365  trunk_AI += IaB;
366  trunk_p += (motionTransforms-> fr_LF_hipassembly_X_fr_trunk).transpose() * pa;
367 
368  // + The acceleration of the floating base trunk, without gravity
369  trunk_a = - TRAIT::solve(trunk_AI, trunk_p); // trunk_a = - IA^-1 * trunk_p
370 
371  // ---------------------- THIRD PASS ---------------------- //
372  LF_hipassembly_a = (motionTransforms-> fr_LF_hipassembly_X_fr_trunk) * trunk_a + LF_hipassembly_c;
373  qdd(LF_HAA) = (LF_hipassembly_u - LF_hipassembly_U.dot(LF_hipassembly_a)) / LF_hipassembly_D;
374  LF_hipassembly_a(iit::rbd::AZ) += qdd(LF_HAA);
375 
376  LF_upperleg_a = (motionTransforms-> fr_LF_upperleg_X_fr_LF_hipassembly) * LF_hipassembly_a + LF_upperleg_c;
377  qdd(LF_HFE) = (LF_upperleg_u - LF_upperleg_U.dot(LF_upperleg_a)) / LF_upperleg_D;
378  LF_upperleg_a(iit::rbd::AZ) += qdd(LF_HFE);
379 
380  LF_lowerleg_a = (motionTransforms-> fr_LF_lowerleg_X_fr_LF_upperleg) * LF_upperleg_a + LF_lowerleg_c;
381  qdd(LF_KFE) = (LF_lowerleg_u - LF_lowerleg_U.dot(LF_lowerleg_a)) / LF_lowerleg_D;
382  LF_lowerleg_a(iit::rbd::AZ) += qdd(LF_KFE);
383 
384  RF_hipassembly_a = (motionTransforms-> fr_RF_hipassembly_X_fr_trunk) * trunk_a + RF_hipassembly_c;
385  qdd(RF_HAA) = (RF_hipassembly_u - RF_hipassembly_U.dot(RF_hipassembly_a)) / RF_hipassembly_D;
386  RF_hipassembly_a(iit::rbd::AZ) += qdd(RF_HAA);
387 
388  RF_upperleg_a = (motionTransforms-> fr_RF_upperleg_X_fr_RF_hipassembly) * RF_hipassembly_a + RF_upperleg_c;
389  qdd(RF_HFE) = (RF_upperleg_u - RF_upperleg_U.dot(RF_upperleg_a)) / RF_upperleg_D;
390  RF_upperleg_a(iit::rbd::AZ) += qdd(RF_HFE);
391 
392  RF_lowerleg_a = (motionTransforms-> fr_RF_lowerleg_X_fr_RF_upperleg) * RF_upperleg_a + RF_lowerleg_c;
393  qdd(RF_KFE) = (RF_lowerleg_u - RF_lowerleg_U.dot(RF_lowerleg_a)) / RF_lowerleg_D;
394  RF_lowerleg_a(iit::rbd::AZ) += qdd(RF_KFE);
395 
396  LH_hipassembly_a = (motionTransforms-> fr_LH_hipassembly_X_fr_trunk) * trunk_a + LH_hipassembly_c;
397  qdd(LH_HAA) = (LH_hipassembly_u - LH_hipassembly_U.dot(LH_hipassembly_a)) / LH_hipassembly_D;
398  LH_hipassembly_a(iit::rbd::AZ) += qdd(LH_HAA);
399 
400  LH_upperleg_a = (motionTransforms-> fr_LH_upperleg_X_fr_LH_hipassembly) * LH_hipassembly_a + LH_upperleg_c;
401  qdd(LH_HFE) = (LH_upperleg_u - LH_upperleg_U.dot(LH_upperleg_a)) / LH_upperleg_D;
402  LH_upperleg_a(iit::rbd::AZ) += qdd(LH_HFE);
403 
404  LH_lowerleg_a = (motionTransforms-> fr_LH_lowerleg_X_fr_LH_upperleg) * LH_upperleg_a + LH_lowerleg_c;
405  qdd(LH_KFE) = (LH_lowerleg_u - LH_lowerleg_U.dot(LH_lowerleg_a)) / LH_lowerleg_D;
406  LH_lowerleg_a(iit::rbd::AZ) += qdd(LH_KFE);
407 
408  RH_hipassembly_a = (motionTransforms-> fr_RH_hipassembly_X_fr_trunk) * trunk_a + RH_hipassembly_c;
409  qdd(RH_HAA) = (RH_hipassembly_u - RH_hipassembly_U.dot(RH_hipassembly_a)) / RH_hipassembly_D;
410  RH_hipassembly_a(iit::rbd::AZ) += qdd(RH_HAA);
411 
412  RH_upperleg_a = (motionTransforms-> fr_RH_upperleg_X_fr_RH_hipassembly) * RH_hipassembly_a + RH_upperleg_c;
413  qdd(RH_HFE) = (RH_upperleg_u - RH_upperleg_U.dot(RH_upperleg_a)) / RH_upperleg_D;
414  RH_upperleg_a(iit::rbd::AZ) += qdd(RH_HFE);
415 
416  RH_lowerleg_a = (motionTransforms-> fr_RH_lowerleg_X_fr_RH_upperleg) * RH_upperleg_a + RH_lowerleg_c;
417  qdd(RH_KFE) = (RH_lowerleg_u - RH_lowerleg_U.dot(RH_lowerleg_a)) / RH_lowerleg_D;
418  RH_lowerleg_a(iit::rbd::AZ) += qdd(RH_KFE);
419 
420 
421  // + Add gravity to the acceleration of the floating base
422  trunk_a += g;
423 }
iit::TestHyQ::LH_LOWERLEG
Definition: declarations.h:50
iit::TestHyQ::LF_UPPERLEG
Definition: declarations.h:43
iit::TestHyQ::RH_HIPASSEMBLY
Definition: declarations.h:51
iit::TestHyQ::RH_KFE
Definition: declarations.h:37
iit::rbd::compute_Ia_revolute
void compute_Ia_revolute(const MatrixBase< D1 > &IA, const Vec6< typename D1::Scalar > &U, const typename D1::Scalar &D, const MatrixBase< D2 > &Ia_const)
Definition: robcogen_commons.h:595
iit::TestHyQ::LF_KFE
Definition: declarations.h:28
iit::TestHyQ::dyn::tpl::ForwardDynamics::fd
void fd(JointState &qdd, Acceleration &trunk_a, const Velocity &trunk_v, const Acceleration &g, const JointState &q, const JointState &qd, const JointState &tau, const ExtForces &fext=zeroExtForces)
Definition: forward_dynamics.h:239
iit::TestHyQ::LH_HFE
Definition: declarations.h:33
iit::TestHyQ::dyn::tpl::ForwardDynamics::Acceleration
CoreS::VelocityVector Acceleration
Definition: forward_dynamics.h:44
iit::TestHyQ::LF_HIPASSEMBLY
Definition: declarations.h:42
iit::TestHyQ::LH_HAA
Definition: declarations.h:32
iit::TestHyQ::dyn::tpl::ForwardDynamics::Matrix66S
CoreS::Matrix66 Matrix66S
Definition: forward_dynamics.h:47
iit::TestHyQ::RF_UPPERLEG
Definition: declarations.h:46
iit::TestHyQ::LH_UPPERLEG
Definition: declarations.h:49
iit::TestHyQ::dyn::tpl::ForwardDynamics::ForwardDynamics
ForwardDynamics(iit::TestHyQ::dyn::tpl::InertiaProperties< TRAIT > &in, MTransforms &tr)
Definition: forward_dynamics.impl.h:8
iit::TestHyQ::TRUNK
Definition: declarations.h:41
iit::rbd::vxIv
Force< DScalar > vxIv(const Velocity< DScalar > &v, const MatrixBase< Derived > &I)
Definition: robcogen_commons.h:58
iit::TestHyQ::dyn::tpl::ForwardDynamics::MTransforms
iit::TestHyQ::tpl::MotionTransforms< TRAIT > MTransforms
Definition: forward_dynamics.h:48
iit::TestHyQ::LH_KFE
Definition: declarations.h:34
iit::rbd::AZ
Definition: rbd.h:175
iit::TestHyQ::RF_LOWERLEG
Definition: declarations.h:47
iit::TestHyQ::RF_HIPASSEMBLY
Definition: declarations.h:45
iit::TestHyQ::dyn::tpl::ForwardDynamics::Force
CoreS::ForceVector Force
Definition: forward_dynamics.h:41
iit::TestHyQ::RF_KFE
Definition: declarations.h:31
iit::TestHyQ::dyn::tpl::ForwardDynamics::JointState
iit::TestHyQ::tpl::JointState< Scalar > JointState
Definition: forward_dynamics.h:46
iit::TestHyQ::LF_HFE
Definition: declarations.h:27
iit::TestHyQ::dyn::tpl::InertiaProperties
Definition: inertia_properties.h:24
iit::TestHyQ::LinkDataMap
Definition: link_data_map.h:12
iit::TestHyQ::RF_HAA
Definition: declarations.h:29
iit::TestHyQ::RH_UPPERLEG
Definition: declarations.h:52
iit::TestHyQ::LF_LOWERLEG
Definition: declarations.h:44
iit::TestHyQ::RH_LOWERLEG
Definition: declarations.h:53
iit::TestHyQ::RF_HFE
Definition: declarations.h:30
iit::TestHyQ::LF_HAA
Definition: declarations.h:26
iit::TestHyQ::RH_HFE
Definition: declarations.h:36
iit::TestHyQ::RH_HAA
Definition: declarations.h:35
iit::TestHyQ::dyn::tpl::ForwardDynamics
Definition: forward_dynamics.h:32
iit::TestHyQ::LH_HIPASSEMBLY
Definition: declarations.h:48
iit::rbd::ctransform_Ia_revolute
void ctransform_Ia_revolute(const MatrixBase< D1 > &Ia_A, const MatrixBase< D2 > &XM, const MatrixBase< D3 > &Ia_B_const)
Definition: robcogen_commons.h:315
iit::TestHyQ::dyn::tpl::ForwardDynamics::Velocity
CoreS::VelocityVector Velocity
Definition: forward_dynamics.h:43