- 3.0.2 rigid body dynamics module.
robcogenHelpers.h
Go to the documentation of this file.
1 /**********************************************************************************************************************
2 This file is part of the Control Toolbox (https://github.com/ethz-adrl/control-toolbox), copyright by ETH Zurich.
3 Licensed under the BSD-2 license (see LICENSE file in main directory)
4 **********************************************************************************************************************/
5 
6 
7 #ifdef CT_L23
8 #error You specified 23 links but the maximum supported is 22.
9 #endif
10 
11 #ifdef CT_EE23
12 #error You specified 23 end-effectors but the maximum supported is 22.
13 #endif
14 
15 #ifndef ROBCOGEN_NS
16 #error Please specify the NS of the robot in RobCoGen (ROBCOGEN_NS)
17 #endif
18 
19 #ifndef TARGET_NS
20 #error Please specify a target NS (TARGET_NS)
21 #endif
22 
23 #ifndef CT_BASE
24 #error Please specify base name (CT_BASE)
25 #endif
26 
27 #ifndef CT_N_EE
28 #error Please specify the number of endeffectors (CT_N_EE)
29 #endif
30 
31 #define CT_RBD_TRANSFORM_BASE_TO_ID(BASE, LINK_ID) transforms.BASE##_X_##LINK_ID
32 
33 #define CT_RBD_JACOBIAN_BASE_TO_ID(BASE, EE_ID) jacobians.BASE##_J_##EE_ID
34 
35 #define CT_RBD_JACOBIAN_GET_BLOCK(FIRST, LAST) jacobian.template block<6, LAST - FIRST + 1>(0, FIRST)
36 
37 // This is just a helper Macro that generates a case-statement for each link to shorten the macro below.
38 #define CT_RBD_CASE_HELPER_BASE_ID(BASE, LINK_ID, INDEX) \
39  case INDEX: \
40  return CT_RBD_TRANSFORM_BASE_TO_ID(BASE, LINK_ID); \
41  break;
42 
43 // This is just a helper Macro that generates a case-statement for each link to shorten the macro below.
44 #define CT_RBD_CASE_HELPER_JAC_UPDATE(BASE, LINK_ID) CT_RBD_JACOBIAN_BASE_TO_ID(BASE, LINK_ID).update(q);
45 
46 // This is just a helper Macro that generates a case-statement for each link to shorten the macro below.
47 #define CT_RBD_CASE_HELPER_JOINT_BEGIN_BASE_ID(BASE, LINK_ID, INDEX) \
48  case INDEX: \
49  return CT_RBD_TRANSFORM_BASE_TO_ID(BASE, LINK_ID).update(
50 
51 #define CT_RBD_TRANSFORM_ID_TO_BASE(BASE, LINK_ID) transforms.LINK_ID##_X_##BASE
52 
53 // This is just a helper Macro that generates a case-statement for each link to shorten the macro below.
54 #define CT_RBD_CASE_HELPER_ID_BASE(BASE, LINK_ID, INDEX) \
55  case INDEX: \
56  return CT_RBD_TRANSFORM_ID_TO_BASE(BASE, LINK_ID); \
57  break;
58 
59 // This is just a helper Macro that generates a case-statement for each link to shorten the macro below.
60 #define CT_RBD_CASE_HELPER_JOINT_BEGIN_ID_BASE(BASE, LINK_ID, INDEX) \
61  case INDEX: \
62  return CT_RBD_TRANSFORM_ID_TO_BASE(BASE, LINK_ID).update(
63 
64 #define CT_RBD_CASE_HELPER_JOINT_END \
65  ) ; \
66  break;
67 
68 
69 namespace ct {
70 namespace rbd {
71 namespace TARGET_NS {
72 
73 template <typename SCALAR>
74 class Utils
75 {
76 private:
77  static const int NJOINTS = iit::ROBCOGEN_NS::tpl::Traits<SCALAR>::joints_count;
78 
79 public:
80  static const int N_EE = CT_N_EE;
81 
82  // This defines a function to get the transform by ID
83  template <class TRANS>
84  static typename TRANS::MatrixType getTransformBaseLinkById(TRANS& transforms,
85  size_t link_id,
86  const Eigen::Matrix<SCALAR, NJOINTS, 1>& q)
87  {
88  switch (link_id)
89  {
90  case 0:
91  {
92  typename TRANS::MatrixType identity;
93  identity.setZero();
94  identity.template topRightCorner<3, 3>().setIdentity();
95  return identity;
96  break;
97  }
98 #ifdef CT_L0
99  CT_RBD_CASE_HELPER_JOINT_BEGIN_BASE_ID(CT_BASE, CT_L0, 0 + 1)
100  q CT_RBD_CASE_HELPER_JOINT_END
101 #endif
102 #ifdef CT_L1
103  CT_RBD_CASE_HELPER_JOINT_BEGIN_BASE_ID(CT_BASE, CT_L1, 1 + 1) q CT_RBD_CASE_HELPER_JOINT_END
104 #endif
105 #ifdef CT_L2
106  CT_RBD_CASE_HELPER_JOINT_BEGIN_BASE_ID(CT_BASE, CT_L2, 2 + 1) q CT_RBD_CASE_HELPER_JOINT_END
107 #endif
108 #ifdef CT_L3
109  CT_RBD_CASE_HELPER_JOINT_BEGIN_BASE_ID(
110  CT_BASE, CT_L3, 3 + 1) q CT_RBD_CASE_HELPER_JOINT_END
111 #endif
112 #ifdef CT_L4
113  CT_RBD_CASE_HELPER_JOINT_BEGIN_BASE_ID(
114  CT_BASE, CT_L4, 4 + 1) q CT_RBD_CASE_HELPER_JOINT_END
115 #endif
116 #ifdef CT_L5
117  CT_RBD_CASE_HELPER_JOINT_BEGIN_BASE_ID(
118  CT_BASE, CT_L5, 5 + 1) q CT_RBD_CASE_HELPER_JOINT_END
119 #endif
120 #ifdef CT_L6
121  CT_RBD_CASE_HELPER_JOINT_BEGIN_BASE_ID(
122  CT_BASE, CT_L6, 6 + 1) q CT_RBD_CASE_HELPER_JOINT_END
123 #endif
124 #ifdef CT_L7
125  CT_RBD_CASE_HELPER_JOINT_BEGIN_BASE_ID(
126  CT_BASE, CT_L7, 7 + 1) q CT_RBD_CASE_HELPER_JOINT_END
127 #endif
128 #ifdef CT_L8
129  CT_RBD_CASE_HELPER_JOINT_BEGIN_BASE_ID(
130  CT_BASE, CT_L8, 8 + 1) q CT_RBD_CASE_HELPER_JOINT_END
131 #endif
132 #ifdef CT_L9
133  CT_RBD_CASE_HELPER_JOINT_BEGIN_BASE_ID(
134  CT_BASE, CT_L9, 9 + 1) q CT_RBD_CASE_HELPER_JOINT_END
135 #endif
136 #ifdef CT_L10
137  CT_RBD_CASE_HELPER_JOINT_BEGIN_BASE_ID(
138  CT_BASE, CT_L10, 10 + 1) q CT_RBD_CASE_HELPER_JOINT_END
139 #endif
140 #ifdef CT_L11
141  CT_RBD_CASE_HELPER_JOINT_BEGIN_BASE_ID(CT_BASE, CT_L11,
142  11 + 1) q CT_RBD_CASE_HELPER_JOINT_END
143 #endif
144 #ifdef CT_L12
145  CT_RBD_CASE_HELPER_JOINT_BEGIN_BASE_ID(CT_BASE,
146  CT_L12, 12 + 1) q CT_RBD_CASE_HELPER_JOINT_END
147 #endif
148 #ifdef CT_L13
149  CT_RBD_CASE_HELPER_JOINT_BEGIN_BASE_ID(CT_BASE,
150  CT_L13,
151  13 + 1) q CT_RBD_CASE_HELPER_JOINT_END
152 #endif
153 #ifdef CT_L14
154  CT_RBD_CASE_HELPER_JOINT_BEGIN_BASE_ID(
155  CT_BASE, CT_L14,
156  14 + 1) q CT_RBD_CASE_HELPER_JOINT_END
157 #endif
158 #ifdef CT_L15
159  CT_RBD_CASE_HELPER_JOINT_BEGIN_BASE_ID(
160  CT_BASE, CT_L15,
161  15 +
162  1) q CT_RBD_CASE_HELPER_JOINT_END
163 #endif
164 #ifdef CT_L16
165  CT_RBD_CASE_HELPER_JOINT_BEGIN_BASE_ID(
166  CT_BASE, CT_L16,
167  16 +
168  1) q CT_RBD_CASE_HELPER_JOINT_END
169 #endif
170 #ifdef CT_L17
171  CT_RBD_CASE_HELPER_JOINT_BEGIN_BASE_ID(
172  CT_BASE, CT_L17,
173  17 +
174  1) q CT_RBD_CASE_HELPER_JOINT_END
175 #endif
176 #ifdef CT_L18
177  CT_RBD_CASE_HELPER_JOINT_BEGIN_BASE_ID(
178  CT_BASE, CT_L18,
179  18 +
180  1) q CT_RBD_CASE_HELPER_JOINT_END
181 #endif
182 #ifdef CT_L19
183  CT_RBD_CASE_HELPER_JOINT_BEGIN_BASE_ID(
184  CT_BASE, CT_L19,
185  19 +
186  1) q CT_RBD_CASE_HELPER_JOINT_END
187 #endif
188 #ifdef CT_L20
189  CT_RBD_CASE_HELPER_JOINT_BEGIN_BASE_ID(
190  CT_BASE, CT_L20,
191  20 +
192  1) q CT_RBD_CASE_HELPER_JOINT_END
193 #endif
194 #ifdef CT_L21
195  CT_RBD_CASE_HELPER_JOINT_BEGIN_BASE_ID(
196  CT_BASE,
197  CT_L21,
198  21 +
199  1) q CT_RBD_CASE_HELPER_JOINT_END
200 #endif
201 #ifdef CT_L22
202  CT_RBD_CASE_HELPER_JOINT_BEGIN_BASE_ID(
203  CT_BASE,
204  CT_L22,
205  22 +
206  1) q
207  CT_RBD_CASE_HELPER_JOINT_END
208 #endif
209  default : std::cout
210  << "getTransformByLinkID: requested joint does not exist, requested: "
211  << link_id
212  << std::endl;
213  throw std::runtime_error("getTransformByLinkID: requested joint does not exist");
214  break;
215  }
216  }
217 
218 
219  // This defines a function to get the transform by ID
220  template <class TRANS>
221  static typename TRANS::MatrixType getTransformLinkBaseById(TRANS& transforms,
222  size_t link_id,
223  const Eigen::Matrix<SCALAR, NJOINTS, 1>& q)
224  {
225  switch (link_id)
226  {
227  case 0:
228  {
229  typename TRANS::MatrixType identity;
230  identity.setZero();
231  identity.template topRightCorner<3, 3>().setIdentity();
232  return identity;
233  break;
234  }
235 #ifdef CT_L0
236  CT_RBD_CASE_HELPER_JOINT_BEGIN_ID_BASE(CT_BASE, CT_L0, 0 + 1)
237  q CT_RBD_CASE_HELPER_JOINT_END
238 #endif
239 #ifdef CT_L1
240  CT_RBD_CASE_HELPER_JOINT_BEGIN_ID_BASE(CT_BASE, CT_L1, 1 + 1) q CT_RBD_CASE_HELPER_JOINT_END
241 #endif
242 #ifdef CT_L2
243  CT_RBD_CASE_HELPER_JOINT_BEGIN_ID_BASE(CT_BASE, CT_L2, 2 + 1) q CT_RBD_CASE_HELPER_JOINT_END
244 #endif
245 #ifdef CT_L3
246  CT_RBD_CASE_HELPER_JOINT_BEGIN_ID_BASE(
247  CT_BASE, CT_L3, 3 + 1) q CT_RBD_CASE_HELPER_JOINT_END
248 #endif
249 #ifdef CT_L4
250  CT_RBD_CASE_HELPER_JOINT_BEGIN_ID_BASE(
251  CT_BASE, CT_L4, 4 + 1) q CT_RBD_CASE_HELPER_JOINT_END
252 #endif
253 #ifdef CT_L5
254  CT_RBD_CASE_HELPER_JOINT_BEGIN_ID_BASE(
255  CT_BASE, CT_L5, 5 + 1) q CT_RBD_CASE_HELPER_JOINT_END
256 #endif
257 #ifdef CT_L6
258  CT_RBD_CASE_HELPER_JOINT_BEGIN_ID_BASE(
259  CT_BASE, CT_L6, 6 + 1) q CT_RBD_CASE_HELPER_JOINT_END
260 #endif
261 #ifdef CT_L7
262  CT_RBD_CASE_HELPER_JOINT_BEGIN_ID_BASE(
263  CT_BASE, CT_L7, 7 + 1) q CT_RBD_CASE_HELPER_JOINT_END
264 #endif
265 #ifdef CT_L8
266  CT_RBD_CASE_HELPER_JOINT_BEGIN_ID_BASE(
267  CT_BASE, CT_L8, 8 + 1) q CT_RBD_CASE_HELPER_JOINT_END
268 #endif
269 #ifdef CT_L9
270  CT_RBD_CASE_HELPER_JOINT_BEGIN_ID_BASE(
271  CT_BASE, CT_L9, 9 + 1) q CT_RBD_CASE_HELPER_JOINT_END
272 #endif
273 #ifdef CT_L10
274  CT_RBD_CASE_HELPER_JOINT_BEGIN_ID_BASE(
275  CT_BASE, CT_L10, 10 + 1) q CT_RBD_CASE_HELPER_JOINT_END
276 #endif
277 #ifdef CT_L11
278  CT_RBD_CASE_HELPER_JOINT_BEGIN_ID_BASE(CT_BASE, CT_L11,
279  11 + 1) q CT_RBD_CASE_HELPER_JOINT_END
280 #endif
281 #ifdef CT_L12
282  CT_RBD_CASE_HELPER_JOINT_BEGIN_ID_BASE(CT_BASE,
283  CT_L12, 12 + 1) q CT_RBD_CASE_HELPER_JOINT_END
284 #endif
285 #ifdef CT_L13
286  CT_RBD_CASE_HELPER_JOINT_BEGIN_ID_BASE(CT_BASE,
287  CT_L13,
288  13 + 1) q CT_RBD_CASE_HELPER_JOINT_END
289 #endif
290 #ifdef CT_L14
291  CT_RBD_CASE_HELPER_JOINT_BEGIN_ID_BASE(
292  CT_BASE, CT_L14,
293  14 + 1) q CT_RBD_CASE_HELPER_JOINT_END
294 #endif
295 #ifdef CT_L15
296  CT_RBD_CASE_HELPER_JOINT_BEGIN_ID_BASE(
297  CT_BASE, CT_L15,
298  15 +
299  1) q CT_RBD_CASE_HELPER_JOINT_END
300 #endif
301 #ifdef CT_L16
302  CT_RBD_CASE_HELPER_JOINT_BEGIN_ID_BASE(
303  CT_BASE, CT_L16,
304  16 +
305  1) q CT_RBD_CASE_HELPER_JOINT_END
306 #endif
307 #ifdef CT_L17
308  CT_RBD_CASE_HELPER_JOINT_BEGIN_ID_BASE(
309  CT_BASE, CT_L17,
310  17 +
311  1) q CT_RBD_CASE_HELPER_JOINT_END
312 #endif
313 #ifdef CT_L18
314  CT_RBD_CASE_HELPER_JOINT_BEGIN_ID_BASE(
315  CT_BASE, CT_L18,
316  18 +
317  1) q CT_RBD_CASE_HELPER_JOINT_END
318 #endif
319 #ifdef CT_L19
320  CT_RBD_CASE_HELPER_JOINT_BEGIN_ID_BASE(
321  CT_BASE, CT_L19,
322  19 +
323  1) q CT_RBD_CASE_HELPER_JOINT_END
324 #endif
325 #ifdef CT_L20
326  CT_RBD_CASE_HELPER_JOINT_BEGIN_ID_BASE(
327  CT_BASE, CT_L20,
328  20 +
329  1) q CT_RBD_CASE_HELPER_JOINT_END
330 #endif
331 #ifdef CT_L21
332  CT_RBD_CASE_HELPER_JOINT_BEGIN_ID_BASE(
333  CT_BASE,
334  CT_L21,
335  21 +
336  1) q CT_RBD_CASE_HELPER_JOINT_END
337 #endif
338 #ifdef CT_L22
339  CT_RBD_CASE_HELPER_JOINT_BEGIN_ID_BASE(
340  CT_BASE,
341  CT_L22,
342  22 +
343  1) q
344  CT_RBD_CASE_HELPER_JOINT_END
345 #endif
346  default : std::cout
347  << "getTransformByLinkID: requested joint does not exist, requested: "
348  << link_id
349  << std::endl;
350  throw std::runtime_error("getTransformByLinkID: requested joint does not exist");
351  break;
352  }
353  }
354 
355 
356  // This defines a function to get the transform by ID
357  template <class TRANS>
358  static typename TRANS::MatrixType getTransformBaseEEById(TRANS& transforms,
359  size_t ee_id,
360  const Eigen::Matrix<SCALAR, NJOINTS, 1>& q)
361  {
362  switch (ee_id)
363  {
364 #ifdef CT_EE0
365  CT_RBD_CASE_HELPER_JOINT_BEGIN_BASE_ID(CT_BASE, CT_EE0, 0)
366  q CT_RBD_CASE_HELPER_JOINT_END
367 #endif
368 #ifdef CT_EE1
369  CT_RBD_CASE_HELPER_JOINT_BEGIN_BASE_ID(CT_BASE, CT_EE1, 1) q CT_RBD_CASE_HELPER_JOINT_END
370 #endif
371 #ifdef CT_EE2
372  CT_RBD_CASE_HELPER_JOINT_BEGIN_BASE_ID(CT_BASE, CT_EE2, 2) q CT_RBD_CASE_HELPER_JOINT_END
373 #endif
374 #ifdef CT_EE3
375  CT_RBD_CASE_HELPER_JOINT_BEGIN_BASE_ID(CT_BASE, CT_EE3, 3) q CT_RBD_CASE_HELPER_JOINT_END
376 #endif
377 #ifdef CT_EE4
378  CT_RBD_CASE_HELPER_JOINT_BEGIN_BASE_ID(
379  CT_BASE, CT_EE4, 4) q CT_RBD_CASE_HELPER_JOINT_END
380 #endif
381 #ifdef CT_EE5
382  CT_RBD_CASE_HELPER_JOINT_BEGIN_BASE_ID(
383  CT_BASE, CT_EE5, 5) q CT_RBD_CASE_HELPER_JOINT_END
384 #endif
385 #ifdef CT_EE6
386  CT_RBD_CASE_HELPER_JOINT_BEGIN_BASE_ID(
387  CT_BASE, CT_EE6, 6) q CT_RBD_CASE_HELPER_JOINT_END
388 #endif
389 #ifdef CT_EE7
390  CT_RBD_CASE_HELPER_JOINT_BEGIN_BASE_ID(
391  CT_BASE, CT_EE7, 7) q CT_RBD_CASE_HELPER_JOINT_END
392 #endif
393 #ifdef CT_EE8
394  CT_RBD_CASE_HELPER_JOINT_BEGIN_BASE_ID(
395  CT_BASE, CT_EE8, 8) q CT_RBD_CASE_HELPER_JOINT_END
396 #endif
397 #ifdef CT_EE9
398  CT_RBD_CASE_HELPER_JOINT_BEGIN_BASE_ID(
399  CT_BASE, CT_EE9, 9) q CT_RBD_CASE_HELPER_JOINT_END
400 #endif
401 #ifdef CT_EE10
402  CT_RBD_CASE_HELPER_JOINT_BEGIN_BASE_ID(
403  CT_BASE, CT_EE10, 10) q CT_RBD_CASE_HELPER_JOINT_END
404 #endif
405 #ifdef CT_EE11
406  CT_RBD_CASE_HELPER_JOINT_BEGIN_BASE_ID(
407  CT_BASE, CT_EE11, 11) q CT_RBD_CASE_HELPER_JOINT_END
408 #endif
409 #ifdef CT_EE12
410  CT_RBD_CASE_HELPER_JOINT_BEGIN_BASE_ID(
411  CT_BASE, CT_EE12, 12) q CT_RBD_CASE_HELPER_JOINT_END
412 #endif
413 #ifdef CT_EE13
414  CT_RBD_CASE_HELPER_JOINT_BEGIN_BASE_ID(CT_BASE,
415  CT_EE13, 13) q CT_RBD_CASE_HELPER_JOINT_END
416 #endif
417 #ifdef CT_EE14
418  CT_RBD_CASE_HELPER_JOINT_BEGIN_BASE_ID(CT_BASE,
419  CT_EE14, 14) q CT_RBD_CASE_HELPER_JOINT_END
420 #endif
421 #ifdef CT_EE15
422  CT_RBD_CASE_HELPER_JOINT_BEGIN_BASE_ID(
423  CT_BASE, CT_EE15,
424  15) q CT_RBD_CASE_HELPER_JOINT_END
425 #endif
426 #ifdef CT_EE16
427  CT_RBD_CASE_HELPER_JOINT_BEGIN_BASE_ID(
428  CT_BASE, CT_EE16,
429  16) q CT_RBD_CASE_HELPER_JOINT_END
430 #endif
431 #ifdef CT_EE17
432  CT_RBD_CASE_HELPER_JOINT_BEGIN_BASE_ID(
433  CT_BASE, CT_EE17,
434  17) q CT_RBD_CASE_HELPER_JOINT_END
435 #endif
436 #ifdef CT_EE18
437  CT_RBD_CASE_HELPER_JOINT_BEGIN_BASE_ID(
438  CT_BASE, CT_EE18,
439  18) q CT_RBD_CASE_HELPER_JOINT_END
440 #endif
441 #ifdef CT_EE19
442  CT_RBD_CASE_HELPER_JOINT_BEGIN_BASE_ID(
443  CT_BASE, CT_EE19,
444  19) q CT_RBD_CASE_HELPER_JOINT_END
445 #endif
446 #ifdef CT_EE20
447  CT_RBD_CASE_HELPER_JOINT_BEGIN_BASE_ID(
448  CT_BASE, CT_EE20,
449  20) q CT_RBD_CASE_HELPER_JOINT_END
450 #endif
451 #ifdef CT_EE21
452  CT_RBD_CASE_HELPER_JOINT_BEGIN_BASE_ID(
453  CT_BASE,
454  CT_EE21,
455  21) q CT_RBD_CASE_HELPER_JOINT_END
456 #endif
457 #ifdef CT_EE22
458  CT_RBD_CASE_HELPER_JOINT_BEGIN_BASE_ID(
459  CT_BASE,
460  CT_EE22,
461  22) q
462  CT_RBD_CASE_HELPER_JOINT_END
463 #endif
464  default : std::cout
465  << "getTransformByEEID: requested end-effector does not exist, requested: "
466  << ee_id
467  << std::endl;
468  throw std::runtime_error("getTransformByEEID: requested end-effector does not exist");
469  break;
470  }
471  }
472 
473  static size_t eeIdToLinkId(size_t ee_id)
474  {
475  switch (ee_id)
476  {
477 #ifdef CT_EE0
478  case 0:
479  return CT_EE0_IS_ON_LINK;
480  break;
481 #endif
482 #ifdef CT_EE1
483  case 1:
484  return CT_EE1_IS_ON_LINK;
485  break;
486 #endif
487 #ifdef CT_EE2
488  case 2:
489  return CT_EE2_IS_ON_LINK;
490  break;
491 #endif
492 #ifdef CT_EE3
493  case 3:
494  return CT_EE3_IS_ON_LINK;
495  break;
496 #endif
497 #ifdef CT_EE4
498  case 4:
499  return CT_EE4_IS_ON_LINK;
500  break;
501 #endif
502 #ifdef CT_EE5
503  case 5:
504  return CT_EE5_IS_ON_LINK;
505  break;
506 #endif
507 #ifdef CT_EE6
508  case 6:
509  return CT_EE6_IS_ON_LINK;
510  break;
511 #endif
512 #ifdef CT_EE7
513  case 7:
514  return CT_EE7_IS_ON_LINK;
515  break;
516 #endif
517 #ifdef CT_EE8
518  case 8:
519  return CT_EE8_IS_ON_LINK;
520  break;
521 #endif
522 #ifdef CT_EE9
523  case 9:
524  return CT_EE9_IS_ON_LINK;
525  break;
526 #endif
527 #ifdef CT_EE10
528  case 10:
529  return CT_EE10_IS_ON_LINK;
530  break;
531 #endif
532 #ifdef CT_EE11
533  case 11:
534  return CT_EE11_IS_ON_LINK;
535  break;
536 #endif
537 #ifdef CT_EE12
538  case 12:
539  return CT_EE12_IS_ON_LINK;
540  break;
541 #endif
542 #ifdef CT_EE13
543  case 13:
544  return CT_EE13_IS_ON_LINK;
545  break;
546 #endif
547 #ifdef CT_EE14
548  case 14:
549  return CT_EE14_IS_ON_LINK;
550  break;
551 #endif
552 #ifdef CT_EE15
553  case 15:
554  return CT_EE15_IS_ON_LINK;
555  break;
556 #endif
557 #ifdef CT_EE16
558  case 16:
559  return CT_EE16_IS_ON_LINK;
560  break;
561 #endif
562 #ifdef CT_EE17
563  case 17:
564  return CT_EE17_IS_ON_LINK;
565  break;
566 #endif
567 #ifdef CT_EE18
568  case 18:
569  return CT_EE18_IS_ON_LINK;
570  break;
571 #endif
572 #ifdef CT_EE19
573  case 19:
574  return CT_EE19_IS_ON_LINK;
575  break;
576 #endif
577 #ifdef CT_EE20
578  case 20:
579  return CT_EE20_IS_ON_LINK;
580  break;
581 #endif
582 #ifdef CT_EE21
583  case 21:
584  return CT_EE21_IS_ON_LINK;
585  break;
586 #endif
587 #ifdef CT_EE22
588  case 22:
589  return CT_EE22_IS_ON_LINK;
590  break;
591 #endif
592 
593  default:
594  std::cout << "eeIdToLinkId: requested end-effector does not exist, requested: " << ee_id << std::endl;
595  throw std::runtime_error("getTransformByEEID: requested end-effector does not exist");
596  break;
597  }
598  }
599 
600 
601  // This defines a function to get the transform by ID
602  template <class JACS>
603  static typename Eigen::Matrix<SCALAR, 6, NJOINTS> getJacobianBaseEEbyId(JACS& jacobians,
604  size_t ee_id,
605  const Eigen::Matrix<SCALAR, NJOINTS, 1>& q)
606  {
607  Eigen::Matrix<SCALAR, 6, NJOINTS> jacobian(Eigen::Matrix<SCALAR, 6, NJOINTS>::Zero());
608 
609  switch (ee_id)
610  {
611 #ifdef CT_EE0
612  case 0:
613  CT_RBD_JACOBIAN_GET_BLOCK(CT_EE0_FIRST_JOINT, CT_EE0_LAST_JOINT) =
614  CT_RBD_CASE_HELPER_JAC_UPDATE(CT_BASE, CT_EE0);
615  break;
616 #endif
617 #ifdef CT_EE1
618  case 1:
619  CT_RBD_JACOBIAN_GET_BLOCK(CT_EE1_FIRST_JOINT, CT_EE1_LAST_JOINT) =
620  CT_RBD_CASE_HELPER_JAC_UPDATE(CT_BASE, CT_EE1);
621  break;
622 #endif
623 #ifdef CT_EE2
624  case 2:
625  CT_RBD_JACOBIAN_GET_BLOCK(CT_EE2_FIRST_JOINT, CT_EE2_LAST_JOINT) =
626  CT_RBD_CASE_HELPER_JAC_UPDATE(CT_BASE, CT_EE2);
627  break;
628 #endif
629 #ifdef CT_EE3
630  case 3:
631  CT_RBD_JACOBIAN_GET_BLOCK(CT_EE3_FIRST_JOINT, CT_EE3_LAST_JOINT) =
632  CT_RBD_CASE_HELPER_JAC_UPDATE(CT_BASE, CT_EE3);
633  break;
634 #endif
635 #ifdef CT_EE4
636  case 4:
637  CT_RBD_JACOBIAN_GET_BLOCK(CT_EE4_FIRST_JOINT, CT_EE4_LAST_JOINT) =
638  CT_RBD_CASE_HELPER_JAC_UPDATE(CT_BASE, CT_EE4);
639  break;
640 #endif
641 #ifdef CT_EE5
642  case 5:
643  CT_RBD_JACOBIAN_GET_BLOCK(CT_EE5_FIRST_JOINT, CT_EE5_LAST_JOINT) =
644  CT_RBD_CASE_HELPER_JAC_UPDATE(CT_BASE, CT_EE5);
645  break;
646 #endif
647 #ifdef CT_EE6
648  case 6:
649  CT_RBD_JACOBIAN_GET_BLOCK(CT_EE6_FIRST_JOINT, CT_EE6_LAST_JOINT) =
650  CT_RBD_CASE_HELPER_JAC_UPDATE(CT_BASE, CT_EE6);
651  break;
652 #endif
653 #ifdef CT_EE7
654  case 7:
655  CT_RBD_JACOBIAN_GET_BLOCK(CT_EE7_FIRST_JOINT, CT_EE7_LAST_JOINT) =
656  CT_RBD_CASE_HELPER_JAC_UPDATE(CT_BASE, CT_EE7);
657  break;
658 #endif
659 #ifdef CT_EE8
660  case 8:
661  CT_RBD_JACOBIAN_GET_BLOCK(CT_EE8_FIRST_JOINT, CT_EE8_LAST_JOINT) =
662  CT_RBD_CASE_HELPER_JAC_UPDATE(CT_BASE, CT_EE8);
663  break;
664 #endif
665 #ifdef CT_EE9
666  case 9:
667  CT_RBD_JACOBIAN_GET_BLOCK(CT_EE9_FIRST_JOINT, CT_EE9_LAST_JOINT) =
668  CT_RBD_CASE_HELPER_JAC_UPDATE(CT_BASE, CT_EE9);
669  break;
670 #endif
671 #ifdef CT_EE10
672  case 10:
673  CT_RBD_JACOBIAN_GET_BLOCK(CT_EE10_FIRST_JOINT, CT_EE10_LAST_JOINT) =
674  CT_RBD_CASE_HELPER_JAC_UPDATE(CT_BASE, CT_EE10);
675  break;
676 #endif
677 #ifdef CT_EE11
678  case 11:
679  CT_RBD_JACOBIAN_GET_BLOCK(CT_EE11_FIRST_JOINT, CT_EE11_LAST_JOINT) =
680  CT_RBD_CASE_HELPER_JAC_UPDATE(CT_BASE, CT_EE11);
681  break;
682 #endif
683 #ifdef CT_EE12
684  case 12:
685  CT_RBD_JACOBIAN_GET_BLOCK(CT_EE12_FIRST_JOINT, CT_EE12_LAST_JOINT) =
686  CT_RBD_CASE_HELPER_JAC_UPDATE(CT_BASE, CT_EE12);
687  break;
688 #endif
689 #ifdef CT_EE13
690  case 13:
691  CT_RBD_JACOBIAN_GET_BLOCK(CT_EE13_FIRST_JOINT, CT_EE13_LAST_JOINT) =
692  CT_RBD_CASE_HELPER_JAC_UPDATE(CT_BASE, CT_EE13);
693  break;
694 #endif
695 #ifdef CT_EE14
696  case 14:
697  CT_RBD_JACOBIAN_GET_BLOCK(CT_EE14_FIRST_JOINT, CT_EE14_LAST_JOINT) =
698  CT_RBD_CASE_HELPER_JAC_UPDATE(CT_BASE, CT_EE14);
699  break;
700 #endif
701 #ifdef CT_EE15
702  case 15:
703  CT_RBD_JACOBIAN_GET_BLOCK(CT_EE15_FIRST_JOINT, CT_EE15_LAST_JOINT) =
704  CT_RBD_CASE_HELPER_JAC_UPDATE(CT_BASE, CT_EE15);
705  break;
706 #endif
707 #ifdef CT_EE16
708  case 16:
709  CT_RBD_JACOBIAN_GET_BLOCK(CT_EE16_FIRST_JOINT, CT_EE16_LAST_JOINT) =
710  CT_RBD_CASE_HELPER_JAC_UPDATE(CT_BASE, CT_EE16);
711  break;
712 #endif
713 #ifdef CT_EE17
714  case 17:
715  CT_RBD_JACOBIAN_GET_BLOCK(CT_EE17_FIRST_JOINT, CT_EE17_LAST_JOINT) =
716  CT_RBD_CASE_HELPER_JAC_UPDATE(CT_BASE, CT_EE17);
717  break;
718 #endif
719 #ifdef CT_EE18
720  case 18:
721  CT_RBD_JACOBIAN_GET_BLOCK(CT_EE18_FIRST_JOINT, CT_EE18_LAST_JOINT) =
722  CT_RBD_CASE_HELPER_JAC_UPDATE(CT_BASE, CT_EE18);
723  break;
724 #endif
725 #ifdef CT_EE19
726  case 19:
727  CT_RBD_JACOBIAN_GET_BLOCK(CT_EE19_FIRST_JOINT, CT_EE19_LAST_JOINT) =
728  CT_RBD_CASE_HELPER_JAC_UPDATE(CT_BASE, CT_EE19);
729  break;
730 #endif
731 #ifdef CT_EE20
732  case 20:
733  CT_RBD_JACOBIAN_GET_BLOCK(CT_EE20_FIRST_JOINT, CT_EE20_LAST_JOINT) =
734  CT_RBD_CASE_HELPER_JAC_UPDATE(CT_BASE, CT_EE20);
735  break;
736 #endif
737 #ifdef CT_EE21
738  case 21:
739  CT_RBD_JACOBIAN_GET_BLOCK(CT_EE21_FIRST_JOINT, CT_EE21_LAST_JOINT) =
740  CT_RBD_CASE_HELPER_JAC_UPDATE(CT_BASE, CT_EE21);
741  break;
742 #endif
743 #ifdef CT_EE22
744  case 22:
745  CT_RBD_JACOBIAN_GET_BLOCK(CT_EE22_FIRST_JOINT, CT_EE22_LAST_JOINT) =
746  CT_RBD_CASE_HELPER_JAC_UPDATE(CT_BASE, CT_EE22);
747  break;
748 #endif
749 
750  default:
751  std::cout << "getJacobianBaseByEEID: requested end-effector does not exist, requested: " << ee_id
752  << std::endl;
753  throw std::runtime_error("getJacobianBaseByEEID: requested end-effector does not exist");
754  break;
755  }
756 
757  return jacobian;
758  }
759 
760 }; // class Utils
761 
762 
763 namespace tpl {
764 
765 template <typename SCALAR>
766 using RobCoGenContainer =
767  ct::rbd::RobCoGenContainer<iit::ROBCOGEN_NS::tpl::Traits<SCALAR>, iit::ROBCOGEN_NS::LinkDataMap, Utils<SCALAR>>;
768 
769 template <typename SCALAR>
770 using Kinematics = ct::rbd::Kinematics<RobCoGenContainer<SCALAR>, Utils<SCALAR>::N_EE>;
771 
772 template <typename SCALAR>
773 using Dynamics = ct::rbd::Dynamics<RobCoGenContainer<SCALAR>, Utils<SCALAR>::N_EE>;
774 
775 
776 } // namespace tpl
777 
778 
779 typedef tpl::RobCoGenContainer<double> RobCoGenContainer;
780 typedef tpl::Kinematics<double> Kinematics;
781 typedef Dynamics<RobCoGenContainer, Kinematics::NUM_EE> Dynamics;
782 
783 } // TARGET_NS
784 
785 
786 } // namespace rbd
787 
788 } // namespace ct
789 
790 // CLEAN UP
791 
792 #undef CT_BASE
793 #undef ROBCOGEN_NS
794 #undef ROBOT_NAME
795 #undef TARGET_NS
796 #undef CT_N_EE
797 
798 #undef CT_L0
799 #undef CT_L1
800 #undef CT_L2
801 #undef CT_L3
802 #undef CT_L4
803 #undef CT_L5
804 #undef CT_L6
805 #undef CT_L7
806 #undef CT_L8
807 #undef CT_L9
808 #undef CT_L10
809 #undef CT_L11
810 #undef CT_L12
811 #undef CT_L13
812 #undef CT_L14
813 #undef CT_L15
814 #undef CT_L16
815 #undef CT_L17
816 #undef CT_L18
817 #undef CT_L19
818 #undef CT_L20
819 #undef CT_L21
820 #undef CT_L22
821 
822 #undef CT_EE0
823 #undef CT_EE1
824 #undef CT_EE2
825 #undef CT_EE3
826 #undef CT_EE4
827 #undef CT_EE5
828 #undef CT_EE6
829 #undef CT_EE7
830 #undef CT_EE8
831 #undef CT_EE9
832 #undef CT_EE10
833 #undef CT_EE11
834 #undef CT_EE12
835 #undef CT_EE13
836 #undef CT_EE14
837 #undef CT_EE15
838 #undef CT_EE16
839 #undef CT_EE17
840 #undef CT_EE18
841 #undef CT_EE19
842 #undef CT_EE20
843 #undef CT_EE21
844 #undef CT_EE22
845 
846 #undef CT_EE0_IS_ON_LINK
847 #undef CT_EE1_IS_ON_LINK
848 #undef CT_EE2_IS_ON_LINK
849 #undef CT_EE3_IS_ON_LINK
850 #undef CT_EE4_IS_ON_LINK
851 #undef CT_EE5_IS_ON_LINK
852 #undef CT_EE6_IS_ON_LINK
853 #undef CT_EE7_IS_ON_LINK
854 #undef CT_EE8_IS_ON_LINK
855 #undef CT_EE9_IS_ON_LINK
856 #undef CT_EE10_IS_ON_LINK
857 #undef CT_EE11_IS_ON_LINK
858 #undef CT_EE12_IS_ON_LINK
859 #undef CT_EE13_IS_ON_LINK
860 #undef CT_EE14_IS_ON_LINK
861 #undef CT_EE15_IS_ON_LINK
862 #undef CT_EE16_IS_ON_LINK
863 #undef CT_EE17_IS_ON_LINK
864 #undef CT_EE18_IS_ON_LINK
865 #undef CT_EE19_IS_ON_LINK
866 #undef CT_EE20_IS_ON_LINK
867 #undef CT_EE21_IS_ON_LINK
868 #undef CT_EE22_IS_ON_LINK
869 
870 #undef CT_EE0_FIRST_JOINT
871 #undef CT_EE1_FIRST_JOINT
872 #undef CT_EE2_FIRST_JOINT
873 #undef CT_EE3_FIRST_JOINT
874 #undef CT_EE4_FIRST_JOINT
875 #undef CT_EE5_FIRST_JOINT
876 #undef CT_EE6_FIRST_JOINT
877 #undef CT_EE7_FIRST_JOINT
878 #undef CT_EE8_FIRST_JOINT
879 #undef CT_EE9_FIRST_JOINT
880 #undef CT_EE10_FIRST_JOINT
881 #undef CT_EE11_FIRST_JOINT
882 #undef CT_EE12_FIRST_JOINT
883 #undef CT_EE13_FIRST_JOINT
884 #undef CT_EE14_FIRST_JOINT
885 #undef CT_EE15_FIRST_JOINT
886 #undef CT_EE16_FIRST_JOINT
887 #undef CT_EE17_FIRST_JOINT
888 #undef CT_EE18_FIRST_JOINT
889 #undef CT_EE19_FIRST_JOINT
890 #undef CT_EE20_FIRST_JOINT
891 #undef CT_EE21_FIRST_JOINT
892 #undef CT_EE22_FIRST_JOINT
893 
894 
895 #undef CT_EE0_LAST_JOINT
896 #undef CT_EE1_LAST_JOINT
897 #undef CT_EE2_LAST_JOINT
898 #undef CT_EE3_LAST_JOINT
899 #undef CT_EE4_LAST_JOINT
900 #undef CT_EE5_LAST_JOINT
901 #undef CT_EE6_LAST_JOINT
902 #undef CT_EE7_LAST_JOINT
903 #undef CT_EE8_LAST_JOINT
904 #undef CT_EE9_LAST_JOINT
905 #undef CT_EE10_LAST_JOINT
906 #undef CT_EE11_LAST_JOINT
907 #undef CT_EE12_LAST_JOINT
908 #undef CT_EE13_LAST_JOINT
909 #undef CT_EE14_LAST_JOINT
910 #undef CT_EE15_LAST_JOINT
911 #undef CT_EE16_LAST_JOINT
912 #undef CT_EE17_LAST_JOINT
913 #undef CT_EE18_LAST_JOINT
914 #undef CT_EE19_LAST_JOINT
915 #undef CT_EE20_LAST_JOINT
916 #undef CT_EE21_LAST_JOINT
917 #undef CT_EE22_LAST_JOINT
918 
919 
920 #undef CT_RBD_TRANSFORM_BASE_TO_ID
921 #undef CT_RBD_JACOBIAN_BASE_TO_ID
922 #undef CT_RBD_JACOBIAN_GET_BLOCK
923 #undef CT_RBD_CASE_HELPER_BASE_ID
924 #undef CT_RBD_CASE_HELPER_JAC_UPDATE
925 #undef CT_RBD_CASE_HELPER_JOINT_BEGIN_BASE_ID
926 #undef CT_RBD_TRANSFORM_ID_TO_BASE
927 #undef CT_RBD_CASE_HELPER_ID_BASE
928 #undef CT_RBD_CASE_HELPER_JOINT_BEGIN_ID_BASE
929 #undef CT_RBD_CASE_HELPER_JOINT_END
CT_EE3
#define CT_EE3
Definition: RobCoGenTestHyQ.h:64
CT_EE0_IS_ON_LINK
#define CT_EE0_IS_ON_LINK
Definition: RobCoGenTestHyQ.h:49
CT_BASE
#define CT_BASE
Definition: RobCoGenTestHyQ.h:30
CT_EE2_FIRST_JOINT
#define CT_EE2_FIRST_JOINT
Definition: RobCoGenTestHyQ.h:61
CT_EE1_IS_ON_LINK
#define CT_EE1_IS_ON_LINK
Definition: RobCoGenTestHyQ.h:55
CT_EE3_LAST_JOINT
#define CT_EE3_LAST_JOINT
Definition: RobCoGenTestHyQ.h:67
CT_L7
#define CT_L7
Definition: RobCoGenTestHyQ.h:38
ct::rbd::TARGET_NS::Utils::getJacobianBaseEEbyId
static Eigen::Matrix< SCALAR, 6, NJOINTS > getJacobianBaseEEbyId(JACS &jacobians, size_t ee_id, const Eigen::Matrix< SCALAR, NJOINTS, 1 > &q)
Definition: robcogenHelpers.h:603
CT_RBD_CASE_HELPER_JOINT_BEGIN_ID_BASE
#define CT_RBD_CASE_HELPER_JOINT_BEGIN_ID_BASE(BASE, LINK_ID, INDEX)
Definition: robcogenHelpers.h:60
ct::rbd::TARGET_NS::Utils::eeIdToLinkId
static size_t eeIdToLinkId(size_t ee_id)
Definition: robcogenHelpers.h:473
ct::rbd::TARGET_NS::Utils::N_EE
static const int N_EE
Definition: robcogenHelpers.h:80
ct::rbd::Dynamics
This class implements the equations of motion of a Rigid Body System.
Definition: Dynamics.h:43
CT_RBD_CASE_HELPER_JAC_UPDATE
#define CT_RBD_CASE_HELPER_JAC_UPDATE(BASE, LINK_ID)
Definition: robcogenHelpers.h:44
CT_L6
#define CT_L6
Definition: RobCoGenTestHyQ.h:37
CT_L10
#define CT_L10
Definition: RobCoGenTestHyQ.h:41
ct::rbd::TARGET_NS::Utils::getTransformLinkBaseById
static TRANS::MatrixType getTransformLinkBaseById(TRANS &transforms, size_t link_id, const Eigen::Matrix< SCALAR, NJOINTS, 1 > &q)
Definition: robcogenHelpers.h:221
ct::rbd::TARGET_NS::RobCoGenContainer
tpl::RobCoGenContainer< double > RobCoGenContainer
Definition: robcogenHelpers.h:779
CT_EE1_LAST_JOINT
#define CT_EE1_LAST_JOINT
Definition: RobCoGenTestHyQ.h:57
CT_L2
#define CT_L2
Definition: RobCoGenTestHyQ.h:33
CT_EE2_LAST_JOINT
#define CT_EE2_LAST_JOINT
Definition: RobCoGenTestHyQ.h:62
CT_EE3_IS_ON_LINK
#define CT_EE3_IS_ON_LINK
Definition: RobCoGenTestHyQ.h:65
CT_L11
#define CT_L11
Definition: RobCoGenTestHyQ.h:42
CT_EE2
#define CT_EE2
Definition: RobCoGenTestHyQ.h:59
ct::rbd::TARGET_NS::Utils::getTransformBaseEEById
static TRANS::MatrixType getTransformBaseEEById(TRANS &transforms, size_t ee_id, const Eigen::Matrix< SCALAR, NJOINTS, 1 > &q)
Definition: robcogenHelpers.h:358
CT_EE0
#define CT_EE0
Definition: RobCoGenTestHyQ.h:48
ct::rbd::TARGET_NS::Dynamics
Dynamics< RobCoGenContainer, Kinematics::NUM_EE > Dynamics
Definition: robcogenHelpers.h:781
CT_EE0_FIRST_JOINT
#define CT_EE0_FIRST_JOINT
Definition: RobCoGenTestHyQ.h:50
CT_EE1
#define CT_EE1
Definition: RobCoGenTestHyQ.h:54
CT_RBD_CASE_HELPER_JOINT_BEGIN_BASE_ID
#define CT_RBD_CASE_HELPER_JOINT_BEGIN_BASE_ID(BASE, LINK_ID, INDEX)
Definition: robcogenHelpers.h:47
CT_L0
#define CT_L0
Definition: RobCoGenTestHyQ.h:31
CT_RBD_JACOBIAN_GET_BLOCK
#define CT_RBD_JACOBIAN_GET_BLOCK(FIRST, LAST)
Definition: robcogenHelpers.h:35
CT_EE2_IS_ON_LINK
#define CT_EE2_IS_ON_LINK
Definition: RobCoGenTestHyQ.h:60
CT_L9
#define CT_L9
Definition: RobCoGenTestHyQ.h:40
ct::rbd::RobCoGenContainer
Container class containing all robcogen classes.
Definition: RobCoGenContainer.h:23
ct::rbd::TARGET_NS::Utils::getTransformBaseLinkById
static TRANS::MatrixType getTransformBaseLinkById(TRANS &transforms, size_t link_id, const Eigen::Matrix< SCALAR, NJOINTS, 1 > &q)
Definition: robcogenHelpers.h:84
CT_EE3_FIRST_JOINT
#define CT_EE3_FIRST_JOINT
Definition: RobCoGenTestHyQ.h:66
CT_RBD_CASE_HELPER_JOINT_END
#define CT_RBD_CASE_HELPER_JOINT_END
Definition: robcogenHelpers.h:64
ct::rbd::Kinematics
A general class for computing Kinematic properties.
Definition: Kinematics.h:27
CT_L5
#define CT_L5
Definition: RobCoGenTestHyQ.h:36
CT_N_EE
#define CT_N_EE
Definition: RobCoGenTestHyQ.h:45
CT_L1
#define CT_L1
Definition: RobCoGenTestHyQ.h:32
ct::rbd::TARGET_NS::Kinematics
tpl::Kinematics< double > Kinematics
Definition: robcogenHelpers.h:780
CT_L3
#define CT_L3
Definition: RobCoGenTestHyQ.h:34
CT_EE0_LAST_JOINT
#define CT_EE0_LAST_JOINT
Definition: RobCoGenTestHyQ.h:51
CT_L8
#define CT_L8
Definition: RobCoGenTestHyQ.h:39
ct::rbd::TARGET_NS::Utils
Definition: robcogenHelpers.h:74
TARGET_NS
#define TARGET_NS
Definition: RobCoGenTestHyQ.h:26
CT_EE1_FIRST_JOINT
#define CT_EE1_FIRST_JOINT
Definition: RobCoGenTestHyQ.h:56
CT_L4
#define CT_L4
Definition: RobCoGenTestHyQ.h:35