- 3.0.2 rigid body dynamics module.
inverse_dynamics.impl.h
Go to the documentation of this file.
1 // Initialization of static-const data
2 template <typename TRAIT>
3 const typename iit::TestHyQ::dyn::tpl::InverseDynamics<TRAIT>::ExtForces
4 iit::TestHyQ::dyn::tpl::InverseDynamics<TRAIT>::zeroExtForces(Force::Zero());
5 
6 template <typename TRAIT>
7 iit::TestHyQ::dyn::tpl::InverseDynamics<TRAIT>::InverseDynamics(IProperties& inertia, MTransforms& transforms) :
8  inertiaProps( & inertia ),
9  xm( & transforms ),
10  LF_hipassembly_I(inertiaProps->getTensor_LF_hipassembly() ),
11  LF_upperleg_I(inertiaProps->getTensor_LF_upperleg() ),
12  LF_lowerleg_I(inertiaProps->getTensor_LF_lowerleg() ),
13  RF_hipassembly_I(inertiaProps->getTensor_RF_hipassembly() ),
14  RF_upperleg_I(inertiaProps->getTensor_RF_upperleg() ),
15  RF_lowerleg_I(inertiaProps->getTensor_RF_lowerleg() ),
16  LH_hipassembly_I(inertiaProps->getTensor_LH_hipassembly() ),
17  LH_upperleg_I(inertiaProps->getTensor_LH_upperleg() ),
18  LH_lowerleg_I(inertiaProps->getTensor_LH_lowerleg() ),
19  RH_hipassembly_I(inertiaProps->getTensor_RH_hipassembly() ),
20  RH_upperleg_I(inertiaProps->getTensor_RH_upperleg() ),
21  RH_lowerleg_I(inertiaProps->getTensor_RH_lowerleg() )
22  ,
23  trunk_I( inertiaProps->getTensor_trunk() ),
24  LF_lowerleg_Ic(LF_lowerleg_I),
25  RF_lowerleg_Ic(RF_lowerleg_I),
26  LH_lowerleg_Ic(LH_lowerleg_I),
27  RH_lowerleg_Ic(RH_lowerleg_I)
28 {
29 #ifndef EIGEN_NO_DEBUG
30  std::cout << "Robot TestHyQ, InverseDynamics<TRAIT>::InverseDynamics()" << std::endl;
31  std::cout << "Compiled with Eigen debug active" << std::endl;
32 #endif
33  LF_hipassembly_v.setZero();
34  LF_upperleg_v.setZero();
35  LF_lowerleg_v.setZero();
36  RF_hipassembly_v.setZero();
37  RF_upperleg_v.setZero();
38  RF_lowerleg_v.setZero();
39  LH_hipassembly_v.setZero();
40  LH_upperleg_v.setZero();
41  LH_lowerleg_v.setZero();
42  RH_hipassembly_v.setZero();
43  RH_upperleg_v.setZero();
44  RH_lowerleg_v.setZero();
45 
46  vcross.setZero();
47 }
48 
49 template <typename TRAIT>
50 void iit::TestHyQ::dyn::tpl::InverseDynamics<TRAIT>::id(
51  JointState& jForces, Acceleration& trunk_a,
52  const Acceleration& g, const Velocity& trunk_v,
53  const JointState& qd, const JointState& qdd,
54  const ExtForces& fext)
55 {
56  trunk_Ic = trunk_I;
57  LF_hipassembly_Ic = LF_hipassembly_I;
58  LF_upperleg_Ic = LF_upperleg_I;
59  RF_hipassembly_Ic = RF_hipassembly_I;
60  RF_upperleg_Ic = RF_upperleg_I;
61  LH_hipassembly_Ic = LH_hipassembly_I;
62  LH_upperleg_Ic = LH_upperleg_I;
63  RH_hipassembly_Ic = RH_hipassembly_I;
64  RH_upperleg_Ic = RH_upperleg_I;
65 
66  // First pass, link 'LF_hipassembly'
67  LF_hipassembly_v = ((xm->fr_LF_hipassembly_X_fr_trunk) * trunk_v);
68  LF_hipassembly_v(iit::rbd::AZ) += qd(LF_HAA);
69 
70  iit::rbd::motionCrossProductMx<Scalar>(LF_hipassembly_v, vcross);
71 
72  LF_hipassembly_a = (vcross.col(iit::rbd::AZ) * qd(LF_HAA));
73  LF_hipassembly_a(iit::rbd::AZ) += qdd(LF_HAA);
74 
75  LF_hipassembly_f = LF_hipassembly_I * LF_hipassembly_a + iit::rbd::vxIv(LF_hipassembly_v, LF_hipassembly_I);
76 
77  // First pass, link 'LF_upperleg'
78  LF_upperleg_v = ((xm->fr_LF_upperleg_X_fr_LF_hipassembly) * LF_hipassembly_v);
79  LF_upperleg_v(iit::rbd::AZ) += qd(LF_HFE);
80 
81  iit::rbd::motionCrossProductMx<Scalar>(LF_upperleg_v, vcross);
82 
83  LF_upperleg_a = (xm->fr_LF_upperleg_X_fr_LF_hipassembly) * LF_hipassembly_a + vcross.col(iit::rbd::AZ) * qd(LF_HFE);
84  LF_upperleg_a(iit::rbd::AZ) += qdd(LF_HFE);
85 
86  LF_upperleg_f = LF_upperleg_I * LF_upperleg_a + iit::rbd::vxIv(LF_upperleg_v, LF_upperleg_I);
87 
88  // First pass, link 'LF_lowerleg'
89  LF_lowerleg_v = ((xm->fr_LF_lowerleg_X_fr_LF_upperleg) * LF_upperleg_v);
90  LF_lowerleg_v(iit::rbd::AZ) += qd(LF_KFE);
91 
92  iit::rbd::motionCrossProductMx<Scalar>(LF_lowerleg_v, vcross);
93 
94  LF_lowerleg_a = (xm->fr_LF_lowerleg_X_fr_LF_upperleg) * LF_upperleg_a + vcross.col(iit::rbd::AZ) * qd(LF_KFE);
95  LF_lowerleg_a(iit::rbd::AZ) += qdd(LF_KFE);
96 
97  LF_lowerleg_f = LF_lowerleg_I * LF_lowerleg_a + iit::rbd::vxIv(LF_lowerleg_v, LF_lowerleg_I);
98 
99  // First pass, link 'RF_hipassembly'
100  RF_hipassembly_v = ((xm->fr_RF_hipassembly_X_fr_trunk) * trunk_v);
101  RF_hipassembly_v(iit::rbd::AZ) += qd(RF_HAA);
102 
103  iit::rbd::motionCrossProductMx<Scalar>(RF_hipassembly_v, vcross);
104 
105  RF_hipassembly_a = (vcross.col(iit::rbd::AZ) * qd(RF_HAA));
106  RF_hipassembly_a(iit::rbd::AZ) += qdd(RF_HAA);
107 
108  RF_hipassembly_f = RF_hipassembly_I * RF_hipassembly_a + iit::rbd::vxIv(RF_hipassembly_v, RF_hipassembly_I);
109 
110  // First pass, link 'RF_upperleg'
111  RF_upperleg_v = ((xm->fr_RF_upperleg_X_fr_RF_hipassembly) * RF_hipassembly_v);
112  RF_upperleg_v(iit::rbd::AZ) += qd(RF_HFE);
113 
114  iit::rbd::motionCrossProductMx<Scalar>(RF_upperleg_v, vcross);
115 
116  RF_upperleg_a = (xm->fr_RF_upperleg_X_fr_RF_hipassembly) * RF_hipassembly_a + vcross.col(iit::rbd::AZ) * qd(RF_HFE);
117  RF_upperleg_a(iit::rbd::AZ) += qdd(RF_HFE);
118 
119  RF_upperleg_f = RF_upperleg_I * RF_upperleg_a + iit::rbd::vxIv(RF_upperleg_v, RF_upperleg_I);
120 
121  // First pass, link 'RF_lowerleg'
122  RF_lowerleg_v = ((xm->fr_RF_lowerleg_X_fr_RF_upperleg) * RF_upperleg_v);
123  RF_lowerleg_v(iit::rbd::AZ) += qd(RF_KFE);
124 
125  iit::rbd::motionCrossProductMx<Scalar>(RF_lowerleg_v, vcross);
126 
127  RF_lowerleg_a = (xm->fr_RF_lowerleg_X_fr_RF_upperleg) * RF_upperleg_a + vcross.col(iit::rbd::AZ) * qd(RF_KFE);
128  RF_lowerleg_a(iit::rbd::AZ) += qdd(RF_KFE);
129 
130  RF_lowerleg_f = RF_lowerleg_I * RF_lowerleg_a + iit::rbd::vxIv(RF_lowerleg_v, RF_lowerleg_I);
131 
132  // First pass, link 'LH_hipassembly'
133  LH_hipassembly_v = ((xm->fr_LH_hipassembly_X_fr_trunk) * trunk_v);
134  LH_hipassembly_v(iit::rbd::AZ) += qd(LH_HAA);
135 
136  iit::rbd::motionCrossProductMx<Scalar>(LH_hipassembly_v, vcross);
137 
138  LH_hipassembly_a = (vcross.col(iit::rbd::AZ) * qd(LH_HAA));
139  LH_hipassembly_a(iit::rbd::AZ) += qdd(LH_HAA);
140 
141  LH_hipassembly_f = LH_hipassembly_I * LH_hipassembly_a + iit::rbd::vxIv(LH_hipassembly_v, LH_hipassembly_I);
142 
143  // First pass, link 'LH_upperleg'
144  LH_upperleg_v = ((xm->fr_LH_upperleg_X_fr_LH_hipassembly) * LH_hipassembly_v);
145  LH_upperleg_v(iit::rbd::AZ) += qd(LH_HFE);
146 
147  iit::rbd::motionCrossProductMx<Scalar>(LH_upperleg_v, vcross);
148 
149  LH_upperleg_a = (xm->fr_LH_upperleg_X_fr_LH_hipassembly) * LH_hipassembly_a + vcross.col(iit::rbd::AZ) * qd(LH_HFE);
150  LH_upperleg_a(iit::rbd::AZ) += qdd(LH_HFE);
151 
152  LH_upperleg_f = LH_upperleg_I * LH_upperleg_a + iit::rbd::vxIv(LH_upperleg_v, LH_upperleg_I);
153 
154  // First pass, link 'LH_lowerleg'
155  LH_lowerleg_v = ((xm->fr_LH_lowerleg_X_fr_LH_upperleg) * LH_upperleg_v);
156  LH_lowerleg_v(iit::rbd::AZ) += qd(LH_KFE);
157 
158  iit::rbd::motionCrossProductMx<Scalar>(LH_lowerleg_v, vcross);
159 
160  LH_lowerleg_a = (xm->fr_LH_lowerleg_X_fr_LH_upperleg) * LH_upperleg_a + vcross.col(iit::rbd::AZ) * qd(LH_KFE);
161  LH_lowerleg_a(iit::rbd::AZ) += qdd(LH_KFE);
162 
163  LH_lowerleg_f = LH_lowerleg_I * LH_lowerleg_a + iit::rbd::vxIv(LH_lowerleg_v, LH_lowerleg_I);
164 
165  // First pass, link 'RH_hipassembly'
166  RH_hipassembly_v = ((xm->fr_RH_hipassembly_X_fr_trunk) * trunk_v);
167  RH_hipassembly_v(iit::rbd::AZ) += qd(RH_HAA);
168 
169  iit::rbd::motionCrossProductMx<Scalar>(RH_hipassembly_v, vcross);
170 
171  RH_hipassembly_a = (vcross.col(iit::rbd::AZ) * qd(RH_HAA));
172  RH_hipassembly_a(iit::rbd::AZ) += qdd(RH_HAA);
173 
174  RH_hipassembly_f = RH_hipassembly_I * RH_hipassembly_a + iit::rbd::vxIv(RH_hipassembly_v, RH_hipassembly_I);
175 
176  // First pass, link 'RH_upperleg'
177  RH_upperleg_v = ((xm->fr_RH_upperleg_X_fr_RH_hipassembly) * RH_hipassembly_v);
178  RH_upperleg_v(iit::rbd::AZ) += qd(RH_HFE);
179 
180  iit::rbd::motionCrossProductMx<Scalar>(RH_upperleg_v, vcross);
181 
182  RH_upperleg_a = (xm->fr_RH_upperleg_X_fr_RH_hipassembly) * RH_hipassembly_a + vcross.col(iit::rbd::AZ) * qd(RH_HFE);
183  RH_upperleg_a(iit::rbd::AZ) += qdd(RH_HFE);
184 
185  RH_upperleg_f = RH_upperleg_I * RH_upperleg_a + iit::rbd::vxIv(RH_upperleg_v, RH_upperleg_I);
186 
187  // First pass, link 'RH_lowerleg'
188  RH_lowerleg_v = ((xm->fr_RH_lowerleg_X_fr_RH_upperleg) * RH_upperleg_v);
189  RH_lowerleg_v(iit::rbd::AZ) += qd(RH_KFE);
190 
191  iit::rbd::motionCrossProductMx<Scalar>(RH_lowerleg_v, vcross);
192 
193  RH_lowerleg_a = (xm->fr_RH_lowerleg_X_fr_RH_upperleg) * RH_upperleg_a + vcross.col(iit::rbd::AZ) * qd(RH_KFE);
194  RH_lowerleg_a(iit::rbd::AZ) += qdd(RH_KFE);
195 
196  RH_lowerleg_f = RH_lowerleg_I * RH_lowerleg_a + iit::rbd::vxIv(RH_lowerleg_v, RH_lowerleg_I);
197 
198  // The force exerted on the floating base by the links
199  trunk_f = iit::rbd::vxIv(trunk_v, trunk_I);
200 
201 
202  // Add the external forces:
203  trunk_f -= fext[TRUNK];
204  LF_hipassembly_f -= fext[LF_HIPASSEMBLY];
205  LF_upperleg_f -= fext[LF_UPPERLEG];
206  LF_lowerleg_f -= fext[LF_LOWERLEG];
207  RF_hipassembly_f -= fext[RF_HIPASSEMBLY];
208  RF_upperleg_f -= fext[RF_UPPERLEG];
209  RF_lowerleg_f -= fext[RF_LOWERLEG];
210  LH_hipassembly_f -= fext[LH_HIPASSEMBLY];
211  LH_upperleg_f -= fext[LH_UPPERLEG];
212  LH_lowerleg_f -= fext[LH_LOWERLEG];
213  RH_hipassembly_f -= fext[RH_HIPASSEMBLY];
214  RH_upperleg_f -= fext[RH_UPPERLEG];
215  RH_lowerleg_f -= fext[RH_LOWERLEG];
216 
217  RH_upperleg_Ic = RH_upperleg_Ic + (xm->fr_RH_lowerleg_X_fr_RH_upperleg).transpose() * RH_lowerleg_Ic * (xm->fr_RH_lowerleg_X_fr_RH_upperleg);
218  RH_upperleg_f = RH_upperleg_f + (xm->fr_RH_lowerleg_X_fr_RH_upperleg).transpose() * RH_lowerleg_f;
219 
220  RH_hipassembly_Ic = RH_hipassembly_Ic + (xm->fr_RH_upperleg_X_fr_RH_hipassembly).transpose() * RH_upperleg_Ic * (xm->fr_RH_upperleg_X_fr_RH_hipassembly);
221  RH_hipassembly_f = RH_hipassembly_f + (xm->fr_RH_upperleg_X_fr_RH_hipassembly).transpose() * RH_upperleg_f;
222 
223  trunk_Ic = trunk_Ic + (xm->fr_RH_hipassembly_X_fr_trunk).transpose() * RH_hipassembly_Ic * (xm->fr_RH_hipassembly_X_fr_trunk);
224  trunk_f = trunk_f + (xm->fr_RH_hipassembly_X_fr_trunk).transpose() * RH_hipassembly_f;
225 
226  LH_upperleg_Ic = LH_upperleg_Ic + (xm->fr_LH_lowerleg_X_fr_LH_upperleg).transpose() * LH_lowerleg_Ic * (xm->fr_LH_lowerleg_X_fr_LH_upperleg);
227  LH_upperleg_f = LH_upperleg_f + (xm->fr_LH_lowerleg_X_fr_LH_upperleg).transpose() * LH_lowerleg_f;
228 
229  LH_hipassembly_Ic = LH_hipassembly_Ic + (xm->fr_LH_upperleg_X_fr_LH_hipassembly).transpose() * LH_upperleg_Ic * (xm->fr_LH_upperleg_X_fr_LH_hipassembly);
230  LH_hipassembly_f = LH_hipassembly_f + (xm->fr_LH_upperleg_X_fr_LH_hipassembly).transpose() * LH_upperleg_f;
231 
232  trunk_Ic = trunk_Ic + (xm->fr_LH_hipassembly_X_fr_trunk).transpose() * LH_hipassembly_Ic * (xm->fr_LH_hipassembly_X_fr_trunk);
233  trunk_f = trunk_f + (xm->fr_LH_hipassembly_X_fr_trunk).transpose() * LH_hipassembly_f;
234 
235  RF_upperleg_Ic = RF_upperleg_Ic + (xm->fr_RF_lowerleg_X_fr_RF_upperleg).transpose() * RF_lowerleg_Ic * (xm->fr_RF_lowerleg_X_fr_RF_upperleg);
236  RF_upperleg_f = RF_upperleg_f + (xm->fr_RF_lowerleg_X_fr_RF_upperleg).transpose() * RF_lowerleg_f;
237 
238  RF_hipassembly_Ic = RF_hipassembly_Ic + (xm->fr_RF_upperleg_X_fr_RF_hipassembly).transpose() * RF_upperleg_Ic * (xm->fr_RF_upperleg_X_fr_RF_hipassembly);
239  RF_hipassembly_f = RF_hipassembly_f + (xm->fr_RF_upperleg_X_fr_RF_hipassembly).transpose() * RF_upperleg_f;
240 
241  trunk_Ic = trunk_Ic + (xm->fr_RF_hipassembly_X_fr_trunk).transpose() * RF_hipassembly_Ic * (xm->fr_RF_hipassembly_X_fr_trunk);
242  trunk_f = trunk_f + (xm->fr_RF_hipassembly_X_fr_trunk).transpose() * RF_hipassembly_f;
243 
244  LF_upperleg_Ic = LF_upperleg_Ic + (xm->fr_LF_lowerleg_X_fr_LF_upperleg).transpose() * LF_lowerleg_Ic * (xm->fr_LF_lowerleg_X_fr_LF_upperleg);
245  LF_upperleg_f = LF_upperleg_f + (xm->fr_LF_lowerleg_X_fr_LF_upperleg).transpose() * LF_lowerleg_f;
246 
247  LF_hipassembly_Ic = LF_hipassembly_Ic + (xm->fr_LF_upperleg_X_fr_LF_hipassembly).transpose() * LF_upperleg_Ic * (xm->fr_LF_upperleg_X_fr_LF_hipassembly);
248  LF_hipassembly_f = LF_hipassembly_f + (xm->fr_LF_upperleg_X_fr_LF_hipassembly).transpose() * LF_upperleg_f;
249 
250  trunk_Ic = trunk_Ic + (xm->fr_LF_hipassembly_X_fr_trunk).transpose() * LF_hipassembly_Ic * (xm->fr_LF_hipassembly_X_fr_trunk);
251  trunk_f = trunk_f + (xm->fr_LF_hipassembly_X_fr_trunk).transpose() * LF_hipassembly_f;
252 
253 
254  // The base acceleration due to the force due to the movement of the links
255  trunk_a = - trunk_Ic.inverse() * trunk_f;
256 
257  LF_hipassembly_a = xm->fr_LF_hipassembly_X_fr_trunk * trunk_a;
258  jForces(LF_HAA) = (LF_hipassembly_Ic.row(iit::rbd::AZ) * LF_hipassembly_a)(0) + LF_hipassembly_f(iit::rbd::AZ);
259 
260  LF_upperleg_a = xm->fr_LF_upperleg_X_fr_LF_hipassembly * LF_hipassembly_a;
261  jForces(LF_HFE) = (LF_upperleg_Ic.row(iit::rbd::AZ) * LF_upperleg_a)(0) + LF_upperleg_f(iit::rbd::AZ);
262 
263  LF_lowerleg_a = xm->fr_LF_lowerleg_X_fr_LF_upperleg * LF_upperleg_a;
264  jForces(LF_KFE) = (LF_lowerleg_Ic.row(iit::rbd::AZ) * LF_lowerleg_a)(0) + LF_lowerleg_f(iit::rbd::AZ);
265 
266  RF_hipassembly_a = xm->fr_RF_hipassembly_X_fr_trunk * trunk_a;
267  jForces(RF_HAA) = (RF_hipassembly_Ic.row(iit::rbd::AZ) * RF_hipassembly_a)(0) + RF_hipassembly_f(iit::rbd::AZ);
268 
269  RF_upperleg_a = xm->fr_RF_upperleg_X_fr_RF_hipassembly * RF_hipassembly_a;
270  jForces(RF_HFE) = (RF_upperleg_Ic.row(iit::rbd::AZ) * RF_upperleg_a)(0) + RF_upperleg_f(iit::rbd::AZ);
271 
272  RF_lowerleg_a = xm->fr_RF_lowerleg_X_fr_RF_upperleg * RF_upperleg_a;
273  jForces(RF_KFE) = (RF_lowerleg_Ic.row(iit::rbd::AZ) * RF_lowerleg_a)(0) + RF_lowerleg_f(iit::rbd::AZ);
274 
275  LH_hipassembly_a = xm->fr_LH_hipassembly_X_fr_trunk * trunk_a;
276  jForces(LH_HAA) = (LH_hipassembly_Ic.row(iit::rbd::AZ) * LH_hipassembly_a)(0) + LH_hipassembly_f(iit::rbd::AZ);
277 
278  LH_upperleg_a = xm->fr_LH_upperleg_X_fr_LH_hipassembly * LH_hipassembly_a;
279  jForces(LH_HFE) = (LH_upperleg_Ic.row(iit::rbd::AZ) * LH_upperleg_a)(0) + LH_upperleg_f(iit::rbd::AZ);
280 
281  LH_lowerleg_a = xm->fr_LH_lowerleg_X_fr_LH_upperleg * LH_upperleg_a;
282  jForces(LH_KFE) = (LH_lowerleg_Ic.row(iit::rbd::AZ) * LH_lowerleg_a)(0) + LH_lowerleg_f(iit::rbd::AZ);
283 
284  RH_hipassembly_a = xm->fr_RH_hipassembly_X_fr_trunk * trunk_a;
285  jForces(RH_HAA) = (RH_hipassembly_Ic.row(iit::rbd::AZ) * RH_hipassembly_a)(0) + RH_hipassembly_f(iit::rbd::AZ);
286 
287  RH_upperleg_a = xm->fr_RH_upperleg_X_fr_RH_hipassembly * RH_hipassembly_a;
288  jForces(RH_HFE) = (RH_upperleg_Ic.row(iit::rbd::AZ) * RH_upperleg_a)(0) + RH_upperleg_f(iit::rbd::AZ);
289 
290  RH_lowerleg_a = xm->fr_RH_lowerleg_X_fr_RH_upperleg * RH_upperleg_a;
291  jForces(RH_KFE) = (RH_lowerleg_Ic.row(iit::rbd::AZ) * RH_lowerleg_a)(0) + RH_lowerleg_f(iit::rbd::AZ);
292 
293 
294  trunk_a += g;
295 }
296 
297 template <typename TRAIT>
298 void iit::TestHyQ::dyn::tpl::InverseDynamics<TRAIT>::G_terms_fully_actuated(
299  Force& baseWrench, JointState& jForces,
300  const Acceleration& g)
301 {
302  const Acceleration& trunk_a = -g;
303 
304  // Link 'LF_hipassembly'
305  LF_hipassembly_a = (xm->fr_LF_hipassembly_X_fr_trunk) * trunk_a;
306  LF_hipassembly_f = LF_hipassembly_I * LF_hipassembly_a;
307  // Link 'LF_upperleg'
308  LF_upperleg_a = (xm->fr_LF_upperleg_X_fr_LF_hipassembly) * LF_hipassembly_a;
309  LF_upperleg_f = LF_upperleg_I * LF_upperleg_a;
310  // Link 'LF_lowerleg'
311  LF_lowerleg_a = (xm->fr_LF_lowerleg_X_fr_LF_upperleg) * LF_upperleg_a;
312  LF_lowerleg_f = LF_lowerleg_I * LF_lowerleg_a;
313  // Link 'RF_hipassembly'
314  RF_hipassembly_a = (xm->fr_RF_hipassembly_X_fr_trunk) * trunk_a;
315  RF_hipassembly_f = RF_hipassembly_I * RF_hipassembly_a;
316  // Link 'RF_upperleg'
317  RF_upperleg_a = (xm->fr_RF_upperleg_X_fr_RF_hipassembly) * RF_hipassembly_a;
318  RF_upperleg_f = RF_upperleg_I * RF_upperleg_a;
319  // Link 'RF_lowerleg'
320  RF_lowerleg_a = (xm->fr_RF_lowerleg_X_fr_RF_upperleg) * RF_upperleg_a;
321  RF_lowerleg_f = RF_lowerleg_I * RF_lowerleg_a;
322  // Link 'LH_hipassembly'
323  LH_hipassembly_a = (xm->fr_LH_hipassembly_X_fr_trunk) * trunk_a;
324  LH_hipassembly_f = LH_hipassembly_I * LH_hipassembly_a;
325  // Link 'LH_upperleg'
326  LH_upperleg_a = (xm->fr_LH_upperleg_X_fr_LH_hipassembly) * LH_hipassembly_a;
327  LH_upperleg_f = LH_upperleg_I * LH_upperleg_a;
328  // Link 'LH_lowerleg'
329  LH_lowerleg_a = (xm->fr_LH_lowerleg_X_fr_LH_upperleg) * LH_upperleg_a;
330  LH_lowerleg_f = LH_lowerleg_I * LH_lowerleg_a;
331  // Link 'RH_hipassembly'
332  RH_hipassembly_a = (xm->fr_RH_hipassembly_X_fr_trunk) * trunk_a;
333  RH_hipassembly_f = RH_hipassembly_I * RH_hipassembly_a;
334  // Link 'RH_upperleg'
335  RH_upperleg_a = (xm->fr_RH_upperleg_X_fr_RH_hipassembly) * RH_hipassembly_a;
336  RH_upperleg_f = RH_upperleg_I * RH_upperleg_a;
337  // Link 'RH_lowerleg'
338  RH_lowerleg_a = (xm->fr_RH_lowerleg_X_fr_RH_upperleg) * RH_upperleg_a;
339  RH_lowerleg_f = RH_lowerleg_I * RH_lowerleg_a;
340 
341  trunk_f = trunk_I * trunk_a;
342 
343  secondPass_fullyActuated(jForces);
344 
345  baseWrench = trunk_f;
346 }
347 
348 template <typename TRAIT>
349 void iit::TestHyQ::dyn::tpl::InverseDynamics<TRAIT>::C_terms_fully_actuated(
350  Force& baseWrench, JointState& jForces,
351  const Velocity& trunk_v, const JointState& qd)
352 {
353  // Link 'LF_hipassembly'
354  LF_hipassembly_v = ((xm->fr_LF_hipassembly_X_fr_trunk) * trunk_v);
355  LF_hipassembly_v(iit::rbd::AZ) += qd(LF_HAA);
356  iit::rbd::motionCrossProductMx<Scalar>(LF_hipassembly_v, vcross);
357  LF_hipassembly_a = (vcross.col(iit::rbd::AZ) * qd(LF_HAA));
358  LF_hipassembly_f = LF_hipassembly_I * LF_hipassembly_a + iit::rbd::vxIv(LF_hipassembly_v, LF_hipassembly_I);
359 
360  // Link 'LF_upperleg'
361  LF_upperleg_v = ((xm->fr_LF_upperleg_X_fr_LF_hipassembly) * LF_hipassembly_v);
362  LF_upperleg_v(iit::rbd::AZ) += qd(LF_HFE);
363  iit::rbd::motionCrossProductMx<Scalar>(LF_upperleg_v, vcross);
364  LF_upperleg_a = (xm->fr_LF_upperleg_X_fr_LF_hipassembly) * LF_hipassembly_a + vcross.col(iit::rbd::AZ) * qd(LF_HFE);
365  LF_upperleg_f = LF_upperleg_I * LF_upperleg_a + iit::rbd::vxIv(LF_upperleg_v, LF_upperleg_I);
366 
367  // Link 'LF_lowerleg'
368  LF_lowerleg_v = ((xm->fr_LF_lowerleg_X_fr_LF_upperleg) * LF_upperleg_v);
369  LF_lowerleg_v(iit::rbd::AZ) += qd(LF_KFE);
370  iit::rbd::motionCrossProductMx<Scalar>(LF_lowerleg_v, vcross);
371  LF_lowerleg_a = (xm->fr_LF_lowerleg_X_fr_LF_upperleg) * LF_upperleg_a + vcross.col(iit::rbd::AZ) * qd(LF_KFE);
372  LF_lowerleg_f = LF_lowerleg_I * LF_lowerleg_a + iit::rbd::vxIv(LF_lowerleg_v, LF_lowerleg_I);
373 
374  // Link 'RF_hipassembly'
375  RF_hipassembly_v = ((xm->fr_RF_hipassembly_X_fr_trunk) * trunk_v);
376  RF_hipassembly_v(iit::rbd::AZ) += qd(RF_HAA);
377  iit::rbd::motionCrossProductMx<Scalar>(RF_hipassembly_v, vcross);
378  RF_hipassembly_a = (vcross.col(iit::rbd::AZ) * qd(RF_HAA));
379  RF_hipassembly_f = RF_hipassembly_I * RF_hipassembly_a + iit::rbd::vxIv(RF_hipassembly_v, RF_hipassembly_I);
380 
381  // Link 'RF_upperleg'
382  RF_upperleg_v = ((xm->fr_RF_upperleg_X_fr_RF_hipassembly) * RF_hipassembly_v);
383  RF_upperleg_v(iit::rbd::AZ) += qd(RF_HFE);
384  iit::rbd::motionCrossProductMx<Scalar>(RF_upperleg_v, vcross);
385  RF_upperleg_a = (xm->fr_RF_upperleg_X_fr_RF_hipassembly) * RF_hipassembly_a + vcross.col(iit::rbd::AZ) * qd(RF_HFE);
386  RF_upperleg_f = RF_upperleg_I * RF_upperleg_a + iit::rbd::vxIv(RF_upperleg_v, RF_upperleg_I);
387 
388  // Link 'RF_lowerleg'
389  RF_lowerleg_v = ((xm->fr_RF_lowerleg_X_fr_RF_upperleg) * RF_upperleg_v);
390  RF_lowerleg_v(iit::rbd::AZ) += qd(RF_KFE);
391  iit::rbd::motionCrossProductMx<Scalar>(RF_lowerleg_v, vcross);
392  RF_lowerleg_a = (xm->fr_RF_lowerleg_X_fr_RF_upperleg) * RF_upperleg_a + vcross.col(iit::rbd::AZ) * qd(RF_KFE);
393  RF_lowerleg_f = RF_lowerleg_I * RF_lowerleg_a + iit::rbd::vxIv(RF_lowerleg_v, RF_lowerleg_I);
394 
395  // Link 'LH_hipassembly'
396  LH_hipassembly_v = ((xm->fr_LH_hipassembly_X_fr_trunk) * trunk_v);
397  LH_hipassembly_v(iit::rbd::AZ) += qd(LH_HAA);
398  iit::rbd::motionCrossProductMx<Scalar>(LH_hipassembly_v, vcross);
399  LH_hipassembly_a = (vcross.col(iit::rbd::AZ) * qd(LH_HAA));
400  LH_hipassembly_f = LH_hipassembly_I * LH_hipassembly_a + iit::rbd::vxIv(LH_hipassembly_v, LH_hipassembly_I);
401 
402  // Link 'LH_upperleg'
403  LH_upperleg_v = ((xm->fr_LH_upperleg_X_fr_LH_hipassembly) * LH_hipassembly_v);
404  LH_upperleg_v(iit::rbd::AZ) += qd(LH_HFE);
405  iit::rbd::motionCrossProductMx<Scalar>(LH_upperleg_v, vcross);
406  LH_upperleg_a = (xm->fr_LH_upperleg_X_fr_LH_hipassembly) * LH_hipassembly_a + vcross.col(iit::rbd::AZ) * qd(LH_HFE);
407  LH_upperleg_f = LH_upperleg_I * LH_upperleg_a + iit::rbd::vxIv(LH_upperleg_v, LH_upperleg_I);
408 
409  // Link 'LH_lowerleg'
410  LH_lowerleg_v = ((xm->fr_LH_lowerleg_X_fr_LH_upperleg) * LH_upperleg_v);
411  LH_lowerleg_v(iit::rbd::AZ) += qd(LH_KFE);
412  iit::rbd::motionCrossProductMx<Scalar>(LH_lowerleg_v, vcross);
413  LH_lowerleg_a = (xm->fr_LH_lowerleg_X_fr_LH_upperleg) * LH_upperleg_a + vcross.col(iit::rbd::AZ) * qd(LH_KFE);
414  LH_lowerleg_f = LH_lowerleg_I * LH_lowerleg_a + iit::rbd::vxIv(LH_lowerleg_v, LH_lowerleg_I);
415 
416  // Link 'RH_hipassembly'
417  RH_hipassembly_v = ((xm->fr_RH_hipassembly_X_fr_trunk) * trunk_v);
418  RH_hipassembly_v(iit::rbd::AZ) += qd(RH_HAA);
419  iit::rbd::motionCrossProductMx<Scalar>(RH_hipassembly_v, vcross);
420  RH_hipassembly_a = (vcross.col(iit::rbd::AZ) * qd(RH_HAA));
421  RH_hipassembly_f = RH_hipassembly_I * RH_hipassembly_a + iit::rbd::vxIv(RH_hipassembly_v, RH_hipassembly_I);
422 
423  // Link 'RH_upperleg'
424  RH_upperleg_v = ((xm->fr_RH_upperleg_X_fr_RH_hipassembly) * RH_hipassembly_v);
425  RH_upperleg_v(iit::rbd::AZ) += qd(RH_HFE);
426  iit::rbd::motionCrossProductMx<Scalar>(RH_upperleg_v, vcross);
427  RH_upperleg_a = (xm->fr_RH_upperleg_X_fr_RH_hipassembly) * RH_hipassembly_a + vcross.col(iit::rbd::AZ) * qd(RH_HFE);
428  RH_upperleg_f = RH_upperleg_I * RH_upperleg_a + iit::rbd::vxIv(RH_upperleg_v, RH_upperleg_I);
429 
430  // Link 'RH_lowerleg'
431  RH_lowerleg_v = ((xm->fr_RH_lowerleg_X_fr_RH_upperleg) * RH_upperleg_v);
432  RH_lowerleg_v(iit::rbd::AZ) += qd(RH_KFE);
433  iit::rbd::motionCrossProductMx<Scalar>(RH_lowerleg_v, vcross);
434  RH_lowerleg_a = (xm->fr_RH_lowerleg_X_fr_RH_upperleg) * RH_upperleg_a + vcross.col(iit::rbd::AZ) * qd(RH_KFE);
435  RH_lowerleg_f = RH_lowerleg_I * RH_lowerleg_a + iit::rbd::vxIv(RH_lowerleg_v, RH_lowerleg_I);
436 
437 
438  trunk_f = iit::rbd::vxIv(trunk_v, trunk_I);
439 
440  secondPass_fullyActuated(jForces);
441 
442  baseWrench = trunk_f;
443 }
444 
445 template <typename TRAIT>
446 void iit::TestHyQ::dyn::tpl::InverseDynamics<TRAIT>::id_fully_actuated(
447  Force& baseWrench, JointState& jForces,
448  const Acceleration& g, const Velocity& trunk_v, const Acceleration& baseAccel,
449  const JointState& qd, const JointState& qdd, const ExtForces& fext)
450 {
451  Acceleration trunk_a = baseAccel -g;
452 
453  // First pass, link 'LF_hipassembly'
454  LF_hipassembly_v = ((xm->fr_LF_hipassembly_X_fr_trunk) * trunk_v);
455  LF_hipassembly_v(iit::rbd::AZ) += qd(LF_HAA);
456 
457  iit::rbd::motionCrossProductMx<Scalar>(LF_hipassembly_v, vcross);
458 
459  LF_hipassembly_a = (xm->fr_LF_hipassembly_X_fr_trunk) * trunk_a + vcross.col(iit::rbd::AZ) * qd(LF_HAA);
460  LF_hipassembly_a(iit::rbd::AZ) += qdd(LF_HAA);
461 
462  LF_hipassembly_f = LF_hipassembly_I * LF_hipassembly_a + iit::rbd::vxIv(LF_hipassembly_v, LF_hipassembly_I) - fext[LF_HIPASSEMBLY];
463 
464  // First pass, link 'LF_upperleg'
465  LF_upperleg_v = ((xm->fr_LF_upperleg_X_fr_LF_hipassembly) * LF_hipassembly_v);
466  LF_upperleg_v(iit::rbd::AZ) += qd(LF_HFE);
467 
468  iit::rbd::motionCrossProductMx<Scalar>(LF_upperleg_v, vcross);
469 
470  LF_upperleg_a = (xm->fr_LF_upperleg_X_fr_LF_hipassembly) * LF_hipassembly_a + vcross.col(iit::rbd::AZ) * qd(LF_HFE);
471  LF_upperleg_a(iit::rbd::AZ) += qdd(LF_HFE);
472 
473  LF_upperleg_f = LF_upperleg_I * LF_upperleg_a + iit::rbd::vxIv(LF_upperleg_v, LF_upperleg_I) - fext[LF_UPPERLEG];
474 
475  // First pass, link 'LF_lowerleg'
476  LF_lowerleg_v = ((xm->fr_LF_lowerleg_X_fr_LF_upperleg) * LF_upperleg_v);
477  LF_lowerleg_v(iit::rbd::AZ) += qd(LF_KFE);
478 
479  iit::rbd::motionCrossProductMx<Scalar>(LF_lowerleg_v, vcross);
480 
481  LF_lowerleg_a = (xm->fr_LF_lowerleg_X_fr_LF_upperleg) * LF_upperleg_a + vcross.col(iit::rbd::AZ) * qd(LF_KFE);
482  LF_lowerleg_a(iit::rbd::AZ) += qdd(LF_KFE);
483 
484  LF_lowerleg_f = LF_lowerleg_I * LF_lowerleg_a + iit::rbd::vxIv(LF_lowerleg_v, LF_lowerleg_I) - fext[LF_LOWERLEG];
485 
486  // First pass, link 'RF_hipassembly'
487  RF_hipassembly_v = ((xm->fr_RF_hipassembly_X_fr_trunk) * trunk_v);
488  RF_hipassembly_v(iit::rbd::AZ) += qd(RF_HAA);
489 
490  iit::rbd::motionCrossProductMx<Scalar>(RF_hipassembly_v, vcross);
491 
492  RF_hipassembly_a = (xm->fr_RF_hipassembly_X_fr_trunk) * trunk_a + vcross.col(iit::rbd::AZ) * qd(RF_HAA);
493  RF_hipassembly_a(iit::rbd::AZ) += qdd(RF_HAA);
494 
495  RF_hipassembly_f = RF_hipassembly_I * RF_hipassembly_a + iit::rbd::vxIv(RF_hipassembly_v, RF_hipassembly_I) - fext[RF_HIPASSEMBLY];
496 
497  // First pass, link 'RF_upperleg'
498  RF_upperleg_v = ((xm->fr_RF_upperleg_X_fr_RF_hipassembly) * RF_hipassembly_v);
499  RF_upperleg_v(iit::rbd::AZ) += qd(RF_HFE);
500 
501  iit::rbd::motionCrossProductMx<Scalar>(RF_upperleg_v, vcross);
502 
503  RF_upperleg_a = (xm->fr_RF_upperleg_X_fr_RF_hipassembly) * RF_hipassembly_a + vcross.col(iit::rbd::AZ) * qd(RF_HFE);
504  RF_upperleg_a(iit::rbd::AZ) += qdd(RF_HFE);
505 
506  RF_upperleg_f = RF_upperleg_I * RF_upperleg_a + iit::rbd::vxIv(RF_upperleg_v, RF_upperleg_I) - fext[RF_UPPERLEG];
507 
508  // First pass, link 'RF_lowerleg'
509  RF_lowerleg_v = ((xm->fr_RF_lowerleg_X_fr_RF_upperleg) * RF_upperleg_v);
510  RF_lowerleg_v(iit::rbd::AZ) += qd(RF_KFE);
511 
512  iit::rbd::motionCrossProductMx<Scalar>(RF_lowerleg_v, vcross);
513 
514  RF_lowerleg_a = (xm->fr_RF_lowerleg_X_fr_RF_upperleg) * RF_upperleg_a + vcross.col(iit::rbd::AZ) * qd(RF_KFE);
515  RF_lowerleg_a(iit::rbd::AZ) += qdd(RF_KFE);
516 
517  RF_lowerleg_f = RF_lowerleg_I * RF_lowerleg_a + iit::rbd::vxIv(RF_lowerleg_v, RF_lowerleg_I) - fext[RF_LOWERLEG];
518 
519  // First pass, link 'LH_hipassembly'
520  LH_hipassembly_v = ((xm->fr_LH_hipassembly_X_fr_trunk) * trunk_v);
521  LH_hipassembly_v(iit::rbd::AZ) += qd(LH_HAA);
522 
523  iit::rbd::motionCrossProductMx<Scalar>(LH_hipassembly_v, vcross);
524 
525  LH_hipassembly_a = (xm->fr_LH_hipassembly_X_fr_trunk) * trunk_a + vcross.col(iit::rbd::AZ) * qd(LH_HAA);
526  LH_hipassembly_a(iit::rbd::AZ) += qdd(LH_HAA);
527 
528  LH_hipassembly_f = LH_hipassembly_I * LH_hipassembly_a + iit::rbd::vxIv(LH_hipassembly_v, LH_hipassembly_I) - fext[LH_HIPASSEMBLY];
529 
530  // First pass, link 'LH_upperleg'
531  LH_upperleg_v = ((xm->fr_LH_upperleg_X_fr_LH_hipassembly) * LH_hipassembly_v);
532  LH_upperleg_v(iit::rbd::AZ) += qd(LH_HFE);
533 
534  iit::rbd::motionCrossProductMx<Scalar>(LH_upperleg_v, vcross);
535 
536  LH_upperleg_a = (xm->fr_LH_upperleg_X_fr_LH_hipassembly) * LH_hipassembly_a + vcross.col(iit::rbd::AZ) * qd(LH_HFE);
537  LH_upperleg_a(iit::rbd::AZ) += qdd(LH_HFE);
538 
539  LH_upperleg_f = LH_upperleg_I * LH_upperleg_a + iit::rbd::vxIv(LH_upperleg_v, LH_upperleg_I) - fext[LH_UPPERLEG];
540 
541  // First pass, link 'LH_lowerleg'
542  LH_lowerleg_v = ((xm->fr_LH_lowerleg_X_fr_LH_upperleg) * LH_upperleg_v);
543  LH_lowerleg_v(iit::rbd::AZ) += qd(LH_KFE);
544 
545  iit::rbd::motionCrossProductMx<Scalar>(LH_lowerleg_v, vcross);
546 
547  LH_lowerleg_a = (xm->fr_LH_lowerleg_X_fr_LH_upperleg) * LH_upperleg_a + vcross.col(iit::rbd::AZ) * qd(LH_KFE);
548  LH_lowerleg_a(iit::rbd::AZ) += qdd(LH_KFE);
549 
550  LH_lowerleg_f = LH_lowerleg_I * LH_lowerleg_a + iit::rbd::vxIv(LH_lowerleg_v, LH_lowerleg_I) - fext[LH_LOWERLEG];
551 
552  // First pass, link 'RH_hipassembly'
553  RH_hipassembly_v = ((xm->fr_RH_hipassembly_X_fr_trunk) * trunk_v);
554  RH_hipassembly_v(iit::rbd::AZ) += qd(RH_HAA);
555 
556  iit::rbd::motionCrossProductMx<Scalar>(RH_hipassembly_v, vcross);
557 
558  RH_hipassembly_a = (xm->fr_RH_hipassembly_X_fr_trunk) * trunk_a + vcross.col(iit::rbd::AZ) * qd(RH_HAA);
559  RH_hipassembly_a(iit::rbd::AZ) += qdd(RH_HAA);
560 
561  RH_hipassembly_f = RH_hipassembly_I * RH_hipassembly_a + iit::rbd::vxIv(RH_hipassembly_v, RH_hipassembly_I) - fext[RH_HIPASSEMBLY];
562 
563  // First pass, link 'RH_upperleg'
564  RH_upperleg_v = ((xm->fr_RH_upperleg_X_fr_RH_hipassembly) * RH_hipassembly_v);
565  RH_upperleg_v(iit::rbd::AZ) += qd(RH_HFE);
566 
567  iit::rbd::motionCrossProductMx<Scalar>(RH_upperleg_v, vcross);
568 
569  RH_upperleg_a = (xm->fr_RH_upperleg_X_fr_RH_hipassembly) * RH_hipassembly_a + vcross.col(iit::rbd::AZ) * qd(RH_HFE);
570  RH_upperleg_a(iit::rbd::AZ) += qdd(RH_HFE);
571 
572  RH_upperleg_f = RH_upperleg_I * RH_upperleg_a + iit::rbd::vxIv(RH_upperleg_v, RH_upperleg_I) - fext[RH_UPPERLEG];
573 
574  // First pass, link 'RH_lowerleg'
575  RH_lowerleg_v = ((xm->fr_RH_lowerleg_X_fr_RH_upperleg) * RH_upperleg_v);
576  RH_lowerleg_v(iit::rbd::AZ) += qd(RH_KFE);
577 
578  iit::rbd::motionCrossProductMx<Scalar>(RH_lowerleg_v, vcross);
579 
580  RH_lowerleg_a = (xm->fr_RH_lowerleg_X_fr_RH_upperleg) * RH_upperleg_a + vcross.col(iit::rbd::AZ) * qd(RH_KFE);
581  RH_lowerleg_a(iit::rbd::AZ) += qdd(RH_KFE);
582 
583  RH_lowerleg_f = RH_lowerleg_I * RH_lowerleg_a + iit::rbd::vxIv(RH_lowerleg_v, RH_lowerleg_I) - fext[RH_LOWERLEG];
584 
585 
586  // The base
587  trunk_f = trunk_I * trunk_a + iit::rbd::vxIv(trunk_v, trunk_I) - fext[TRUNK];
588 
589  secondPass_fullyActuated(jForces);
590 
591  baseWrench = trunk_f;
592 }
593 
594 template <typename TRAIT>
595 void iit::TestHyQ::dyn::tpl::InverseDynamics<TRAIT>::secondPass_fullyActuated(JointState& jForces)
596 {
597  // Link 'RH_lowerleg'
598  jForces(RH_KFE) = RH_lowerleg_f(iit::rbd::AZ);
599  RH_upperleg_f += xm->fr_RH_lowerleg_X_fr_RH_upperleg.transpose() * RH_lowerleg_f;
600  // Link 'RH_upperleg'
601  jForces(RH_HFE) = RH_upperleg_f(iit::rbd::AZ);
602  RH_hipassembly_f += xm->fr_RH_upperleg_X_fr_RH_hipassembly.transpose() * RH_upperleg_f;
603  // Link 'RH_hipassembly'
604  jForces(RH_HAA) = RH_hipassembly_f(iit::rbd::AZ);
605  trunk_f += xm->fr_RH_hipassembly_X_fr_trunk.transpose() * RH_hipassembly_f;
606  // Link 'LH_lowerleg'
607  jForces(LH_KFE) = LH_lowerleg_f(iit::rbd::AZ);
608  LH_upperleg_f += xm->fr_LH_lowerleg_X_fr_LH_upperleg.transpose() * LH_lowerleg_f;
609  // Link 'LH_upperleg'
610  jForces(LH_HFE) = LH_upperleg_f(iit::rbd::AZ);
611  LH_hipassembly_f += xm->fr_LH_upperleg_X_fr_LH_hipassembly.transpose() * LH_upperleg_f;
612  // Link 'LH_hipassembly'
613  jForces(LH_HAA) = LH_hipassembly_f(iit::rbd::AZ);
614  trunk_f += xm->fr_LH_hipassembly_X_fr_trunk.transpose() * LH_hipassembly_f;
615  // Link 'RF_lowerleg'
616  jForces(RF_KFE) = RF_lowerleg_f(iit::rbd::AZ);
617  RF_upperleg_f += xm->fr_RF_lowerleg_X_fr_RF_upperleg.transpose() * RF_lowerleg_f;
618  // Link 'RF_upperleg'
619  jForces(RF_HFE) = RF_upperleg_f(iit::rbd::AZ);
620  RF_hipassembly_f += xm->fr_RF_upperleg_X_fr_RF_hipassembly.transpose() * RF_upperleg_f;
621  // Link 'RF_hipassembly'
622  jForces(RF_HAA) = RF_hipassembly_f(iit::rbd::AZ);
623  trunk_f += xm->fr_RF_hipassembly_X_fr_trunk.transpose() * RF_hipassembly_f;
624  // Link 'LF_lowerleg'
625  jForces(LF_KFE) = LF_lowerleg_f(iit::rbd::AZ);
626  LF_upperleg_f += xm->fr_LF_lowerleg_X_fr_LF_upperleg.transpose() * LF_lowerleg_f;
627  // Link 'LF_upperleg'
628  jForces(LF_HFE) = LF_upperleg_f(iit::rbd::AZ);
629  LF_hipassembly_f += xm->fr_LF_upperleg_X_fr_LF_hipassembly.transpose() * LF_upperleg_f;
630  // Link 'LF_hipassembly'
631  jForces(LF_HAA) = LF_hipassembly_f(iit::rbd::AZ);
632  trunk_f += xm->fr_LF_hipassembly_X_fr_trunk.transpose() * LF_hipassembly_f;
633 }
634 
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::dyn::tpl::InverseDynamics::C_terms_fully_actuated
void C_terms_fully_actuated(Force &baseWrench, JointState &jForces, const Velocity &trunk_v, const JointState &q, const JointState &qd)
Definition: inverse_dynamics.h:343
iit::TestHyQ::dyn::tpl::InverseDynamics::secondPass_fullyActuated
void secondPass_fullyActuated(JointState &jForces)
Definition: inverse_dynamics.impl.h:595
iit::TestHyQ::RH_KFE
Definition: declarations.h:37
iit::TestHyQ::dyn::tpl::InverseDynamics::Velocity
CoreS::VelocityVector Velocity
Definition: inverse_dynamics.h:51
iit::TestHyQ::LF_KFE
Definition: declarations.h:28
iit::TestHyQ::dyn::tpl::InverseDynamics
Definition: inverse_dynamics.h:41
iit::TestHyQ::LH_HFE
Definition: declarations.h:33
iit::TestHyQ::LF_HIPASSEMBLY
Definition: declarations.h:42
iit::TestHyQ::LH_HAA
Definition: declarations.h:32
iit::TestHyQ::dyn::tpl::InverseDynamics::G_terms_fully_actuated
void G_terms_fully_actuated(Force &baseWrench, JointState &jForces, const Acceleration &g, const JointState &q)
Definition: inverse_dynamics.h:334
iit::TestHyQ::RF_UPPERLEG
Definition: declarations.h:46
iit::TestHyQ::LH_UPPERLEG
Definition: declarations.h:49
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::InverseDynamics::Acceleration
CoreS::VelocityVector Acceleration
Definition: inverse_dynamics.h:52
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::RF_KFE
Definition: declarations.h:31
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::dyn::tpl::InverseDynamics::JointState
iit::TestHyQ::tpl::JointState< Scalar > JointState
Definition: inverse_dynamics.h:54
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::dyn::tpl::InverseDynamics::id_fully_actuated
void id_fully_actuated(Force &baseWrench, JointState &jForces, const Acceleration &g, const Velocity &trunk_v, const Acceleration &baseAccel, const JointState &q, const JointState &qd, const JointState &qdd, const ExtForces &fext=zeroExtForces)
Definition: inverse_dynamics.h:352
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::dyn::tpl::InverseDynamics::id
void id(JointState &jForces, Acceleration &trunk_a, const Acceleration &g, const Velocity &trunk_v, const JointState &q, const JointState &qd, const JointState &qdd, const ExtForces &fext=zeroExtForces)
Definition: inverse_dynamics.h:322
iit::TestHyQ::dyn::tpl::InverseDynamics::Force
CoreS::ForceVector Force
Definition: inverse_dynamics.h:49
iit::TestHyQ::dyn::tpl::InverseDynamics::InverseDynamics
InverseDynamics(IProperties &in, MTransforms &tr)
Definition: inverse_dynamics.impl.h:7
iit::TestHyQ::dyn::tpl::InverseDynamics::MTransforms
iit::TestHyQ::tpl::MotionTransforms< TRAIT > MTransforms
Definition: inverse_dynamics.h:56
iit::TestHyQ::RH_HAA
Definition: declarations.h:35
iit::TestHyQ::LH_HIPASSEMBLY
Definition: declarations.h:48