Tetrapod Project
kinematics_node.cpp
Go to the documentation of this file.
1 //TODO remove this code
2 #include "ros/ros.h"
3 
4 #include <kindr/Core>
5 
6 #include <Eigen/Core>
7 
8 #include <math_utils/Core>
9 
10 #include <kinematics/kinematics.h>
11 
12 #include <debug_utils/debug_utils.h>
13 
14 
15 void someRandomTesting()
16 {
17  // TODO Remove these
18  double yaw = math_utils::HALF_PI;
19  double pitch = math_utils::HALF_PI;
20  double roll = 0;
21 
22  kindr::EulerAnglesZyxD R1(yaw, pitch, roll);
23 
24  ROS_INFO_STREAM("R1: " << R1);
25 
26  kindr::RotationMatrixD rotation(R1);
27 
28  ROS_INFO_STREAM("Rotation: " << rotation);
29 
30  kindr::Position3D position(0,0,0);
31 
32  ROS_INFO_STREAM("Position: " << position);
33 
34  kindr::HomTransformMatrixD t1(position, rotation);
35 
36  ROS_INFO_STREAM("t1: " << t1);
37 
38 
39  kindr::RotationMatrixD R_z(kindr::AngleAxisD(yaw, Eigen::Vector3d(0,0,1)));
40  kindr::RotationMatrixD R_y(kindr::AngleAxisD(pitch, Eigen::Vector3d(0,1,0)));
41  kindr::RotationMatrixD R_x(kindr::AngleAxisD(roll, Eigen::Vector3d(1,0,0)));
42 
43  kindr::RotationMatrixD R2 = R_z*R_y*R_x;
44 
45  kindr::HomTransformMatrixD t2(position, R_z*R_y*R_x);
46 
47  ROS_INFO_STREAM("t2: " << t2);
48 }
49 
50 void testKindr()
51 {
52  double x_angle = math_utils::HALF_PI;
53  double z_angle = math_utils::TWO_THIRDS_PI;
54 
55  Eigen::Vector3d unit_x(1,0,0);
56  Eigen::Vector3d unit_z(0,0,1);
57 
58  kindr::RotationMatrixD R_z(kindr::AngleAxisD(z_angle, unit_z));
59  kindr::RotationMatrixD R_x(kindr::AngleAxisD(x_angle, unit_x));
60 
61  kindr::RotationMatrixD R = R_z*R_x;
62 
63  ROS_INFO_STREAM("R_z: \n" << R_z.matrix());
64  ROS_INFO_STREAM("R_x: \n" << R_x.matrix());
65  ROS_INFO_STREAM("R = R_z*R_x: \n" << R.matrix());
66 }
67 
68 void testDhTransform()
69 {
70  // ---------- D-H Transform --------------
71  double alpha_1 = -math_utils::HALF_PI;
72  double theta_1 = math_utils::THIRD_PI;
73  double a_1 = 1;
74  double d_1 = 0;
75 
76  Kinematics K;
77  kindr::HomTransformMatrixD t1 = K.GetDhTransform(a_1, alpha_1, d_1, theta_1);
78  ROS_INFO_STREAM("t1: " << t1);
79 
80  kindr::Position3D pos = t1.transform(kindr::Position3D(0,0,0));
81 
82  ROS_INFO_STREAM("pos: " << pos);
83 }
84 
85 void testSingleLegKinematics()
86 {
87  Kinematics K;
88 
89  Eigen::Vector3d h_pos(0,0,0);
90 
91  double theta_hy = 0;
92  double theta_hp = math_utils::HALF_PI;
93  double theta_kp = -math_utils::HALF_PI;
94 
95  Eigen::Vector3d pos = K.SolveSingleLegForwardKinematics(h_pos,
96  theta_hy,
97  theta_hp,
98  theta_kp);
99 
100  ROS_INFO_STREAM("pos: " << pos);
101 }
102 
103 void testEigen()
104 {
105  Eigen::Matrix<double, 6, 1> twist;
106 
107  twist << 1,
108  2,
109  3,
110  4,
111  5,
112  6;
113 
114  ROS_INFO_STREAM("twist: " << twist);
115 
116  //Eigen::Matrix<double, 3, 1> pos = twist.block(0,0,2,0);
117  Eigen::Matrix<double, 3, 1> pos = twist.block(3,0,5,0);
118 
119  ROS_INFO_STREAM("pos: " << pos);
120 
121  Eigen::Vector3d pos2;
122  pos2 << 1,2,3;
123 
124  ROS_INFO_STREAM("pos2: " << pos2);
125 }
126 
127 void testHipToFootTransform()
128 {
129  Kinematics K;
130 
131  kindr::HomTransformMatrixD transformHipToFoot = K.GetHipToBodyTransform(Kinematics::LegType::frontLeft,
132  Kinematics::BodyType::foot,
133  math_utils::degToRad(135),
134  math_utils::degToRad(20),
135  math_utils::degToRad(-90));
136 
137  kindr::Position3D hip_position(-0.151,0.185,1);
138 
139  kindr::Position3D positionHipToFoot = transformHipToFoot.transform(kindr::Position3D(0,0,0));
140 
141  Eigen::Vector3d f_pos = hip_position.vector() + positionHipToFoot.vector();
142 
143  ROS_INFO_STREAM("f_pos: \n" << f_pos);
144 }
145 
146 void testForwardKinematics()
147 {
148  Kinematics K;
149 
150  GeneralizedCoordinates q = GeneralizedCoordinates::Constant(0);
151 
152  q << 0, // base_x
153  0, // base_y
154  0, // base_z
155  0, // base_roll
156  0, // base_pitch
157  0, // base_yaw
158  math_utils::degToRad(45), // FL-theta_hy
159  math_utils::degToRad(-20), // FL-theta_hp
160  math_utils::degToRad(90), // FL-theta_kp
161  math_utils::degToRad(-45), // FR-theta_hy
162  math_utils::degToRad(20), // FR-theta_hp
163  math_utils::degToRad(-90), // FR-theta_kp
164  math_utils::degToRad(135), // RL-theta_hy
165  math_utils::degToRad(20), // RL-theta_hp
166  math_utils::degToRad(-90), // RL-theta_kp
167  math_utils::degToRad(-135), // RR-theta_hy
168  math_utils::degToRad(-20), // RR-theta_hp
169  math_utils::degToRad(90); // RR-theta_kp
170 
171 
172  FootstepPositions fPos;
173 
174  bool FK = K.SolveForwardKinematics(q, fPos);
175 
176  debug_utils::printFootstepPositions(fPos);
177 
178 }
179 
180 void testSingeLegInverseKinematics()
181 {
182  Kinematics K;
183  Eigen::Vector3d h_pos(1,-1,0);
184  ROS_INFO_STREAM("h_pos: " << h_pos);
185 
186  Eigen::Vector3d f_pos(4,-1,0);
187  ROS_INFO_STREAM("f_pos: " << f_pos);
188 
189  Eigen::Vector3d joint_angles;
190 
191  K.SolveSingleLegInverseKinematics(false, h_pos, f_pos, joint_angles);
192 
193  ROS_INFO_STREAM("joint_angles: " << joint_angles);
194 }
195 
196 void testInverseKinematics()
197 {
198  Kinematics K;
199 
200  Twist q_b = Twist::Constant(0);
201 
202  q_b(0) = 0;
203  q_b(1) = 0;
204  q_b(2) = 1;
205  q_b(3) = 0;
206  q_b(4) = 0;
207  q_b(5) = 0;
208 
209  ROS_INFO_STREAM("q_b: " << q_b);
210 
211  FootstepPositions f_pos;
212 
213  f_pos(0) = Eigen::Vector3d(0.553807,0.587807,0.666803);
214  f_pos(1) = Eigen::Vector3d(0.553807, -0.587807, 0.66803);
215  f_pos(2) = Eigen::Vector3d(-0.553807,0.587807,0.66803);
216  f_pos(3) = Eigen::Vector3d(-0.553807,-0.587807,0.666803);
217 
218  JointSpaceVector q_r;
219 
220  bool IK = K.SolveInverseKinematics(q_b, f_pos, q_r);
221 
222  debug_utils::printJointState(q_r);
223 }
224 
225 void testPositionBaseToFoot()
226 {
227  Kinematics K;
228 
229  GeneralizedCoordinates q = GeneralizedCoordinates::Constant(0);
230 
231  q << 0, // base_x
232  0, // base_y
233  0, // base_z
234  0, // base_roll
235  0, // base_pitch
236  0, // base_yaw
237  math_utils::degToRad(45), // FL-theta_hy
238  math_utils::degToRad(-20), // FL-theta_hp
239  math_utils::degToRad(90), // FL-theta_kp
240  math_utils::degToRad(-45), // FR-theta_hy
241  math_utils::degToRad(20), // FR-theta_hp
242  math_utils::degToRad(-90), // FR-theta_kp
243  math_utils::degToRad(135), // RL-theta_hy
244  math_utils::degToRad(20), // RL-theta_hp
245  math_utils::degToRad(-90), // RL-theta_kp
246  math_utils::degToRad(-135), // RR-theta_hy
247  math_utils::degToRad(-20), // RR-theta_hp
248  math_utils::degToRad(90); // RR-theta_kp
249 
250  FootstepPositions f_pos;
251 
252  f_pos(0) = K.GetPositionBaseToBodyInB(Kinematics::LegType::frontLeft,
253  Kinematics::BodyType::foot,
254  q);
255  f_pos(1) = K.GetPositionBaseToBodyInB(Kinematics::LegType::frontRight,
256  Kinematics::BodyType::foot,
257  q);
258  f_pos(2) = K.GetPositionBaseToBodyInB(Kinematics::LegType::rearLeft,
259  Kinematics::BodyType::foot,
260  q);
261  f_pos(3) = K.GetPositionBaseToBodyInB(Kinematics::LegType::rearRight,
262  Kinematics::BodyType::foot,
263  q);
264 
265  debug_utils::printFootstepPositions(f_pos);
266 }
267 
268 void testTranslationJacobian()
269 {
270  Kinematics K;
271 
272  GeneralizedCoordinates q = GeneralizedCoordinates::Constant(0);
273 
274  q << 0, // base_x
275  0, // base_y
276  0, // base_z
277  0, // base_roll
278  0, // base_pitch
279  0, // base_yaw
280  0*math_utils::degToRad(45), // FL-theta_hy
281  0*math_utils::degToRad(-20), // FL-theta_hp
282  0*math_utils::degToRad(90), // FL-theta_kp
283  0*math_utils::degToRad(-45), // FR-theta_hy
284  0*math_utils::degToRad(20), // FR-theta_hp
285  0*math_utils::degToRad(-90), // FR-theta_kp
286  0*math_utils::degToRad(135), // RL-theta_hy
287  0*math_utils::degToRad(20), // RL-theta_hp
288  0*math_utils::degToRad(-90), // RL-theta_kp
289  0*math_utils::degToRad(-135), // RR-theta_hy
290  0*math_utils::degToRad(-20), // RR-theta_hp
291  0*math_utils::degToRad(90); // RR-theta_kp
292 
293  Eigen::Matrix<double, 3, 18> J_fl = K.GetTranslationJacobianInW(Kinematics::LegType::frontLeft, Kinematics::BodyType::foot, q);
294  Eigen::Matrix<double, 3, 18> J_fr = K.GetTranslationJacobianInW(Kinematics::LegType::frontRight, Kinematics::BodyType::foot, q);
295  Eigen::Matrix<double, 3, 18> J_rl = K.GetTranslationJacobianInW(Kinematics::LegType::rearLeft, Kinematics::BodyType::foot, q);
296  Eigen::Matrix<double, 3, 18> J_rr = K.GetTranslationJacobianInW(Kinematics::LegType::rearRight, Kinematics::BodyType::foot, q);
297 
298  ROS_INFO_STREAM("Translation Jacobian front left, J_P: \n" << J_fl);
299  ROS_INFO_STREAM("Translation Jacobian front right, J_P: \n" << J_fr);
300  ROS_INFO_STREAM("Translation Jacobian rear left, J_P: \n" << J_rl);
301  ROS_INFO_STREAM("Translation Jacobian rear right, J_P: \n" << J_rr);
302 
303 }
304 
305 void testRotationJacobian()
306 {
307  Kinematics K;
308 
309  GeneralizedCoordinates q = GeneralizedCoordinates::Constant(0);
310 
311  q << 0, // base_x
312  0, // base_y
313  0, // base_z
314  0, // base_roll
315  0, // base_pitch
316  0, // base_yaw
317  math_utils::degToRad(45), // FL-theta_hy
318  0*math_utils::degToRad(-20), // FL-theta_hp
319  0*math_utils::degToRad(90), // FL-theta_kp
320  0*math_utils::degToRad(-45), // FR-theta_hy
321  0*math_utils::degToRad(20), // FR-theta_hp
322  0*math_utils::degToRad(-90), // FR-theta_kp
323  0*math_utils::degToRad(135), // RL-theta_hy
324  0*math_utils::degToRad(20), // RL-theta_hp
325  0*math_utils::degToRad(-90), // RL-theta_kp
326  0*math_utils::degToRad(-135), // RR-theta_hy
327  0*math_utils::degToRad(-20), // RR-theta_hp
328  0*math_utils::degToRad(90); // RR-theta_kp
329 
330  Eigen::Matrix<double, 3, 18> J_fl = K.GetRotationJacobianInW(Kinematics::LegType::frontLeft, Kinematics::BodyType::foot, q);
331  Eigen::Matrix<double, 3, 18> J_fr = K.GetRotationJacobianInW(Kinematics::LegType::frontRight, Kinematics::BodyType::foot, q);
332  Eigen::Matrix<double, 3, 18> J_rl = K.GetRotationJacobianInW(Kinematics::LegType::rearLeft, Kinematics::BodyType::foot, q);
333  Eigen::Matrix<double, 3, 18> J_rr = K.GetRotationJacobianInW(Kinematics::LegType::rearRight, Kinematics::BodyType::foot, q);
334 
335  ROS_INFO_STREAM("Rotation Jacobian front left, J_R: \n" << J_fl);
336  ROS_INFO_STREAM("Rotation Jacobian front right, J_R: \n" << J_fr);
337  ROS_INFO_STREAM("Rotation Jacobian rear left, J_R: \n" << J_rl);
338  ROS_INFO_STREAM("Rotation Jacobian rear right, J_R: \n" << J_rr);
339 }
340 
341 void testJacobian()
342 {
343  Kinematics K;
344 
345  GeneralizedCoordinates q = GeneralizedCoordinates::Constant(0);
346 
347  q << 0, // base_x
348  0, // base_y
349  0, // base_z
350  0, // base_roll
351  0, // base_pitch
352  0*math_utils::degToRad(90), // base_yaw
353  math_utils::degToRad(45),
354  math_utils::degToRad(40),
355  math_utils::degToRad(35),
356  math_utils::degToRad(-45),
357  math_utils::degToRad(-40),
358  math_utils::degToRad(-35),
359  math_utils::degToRad(135),
360  math_utils::degToRad(-40),
361  math_utils::degToRad(-35),
362  math_utils::degToRad(-135),
363  math_utils::degToRad(40),
364  math_utils::degToRad(35);
365 
366  Eigen::Matrix<double, 3, 18> J_fb = K.GetTranslationJacobianInW(Kinematics::LegType::NONE, Kinematics::BodyType::base, q);
367  Eigen::Matrix<double, 3, 18> J_fl = K.GetTranslationJacobianInW(Kinematics::LegType::frontLeft, Kinematics::BodyType::foot, q);
368  Eigen::Matrix<double, 3, 18> J_fr = K.GetTranslationJacobianInW(Kinematics::LegType::frontRight, Kinematics::BodyType::foot, q);
369  Eigen::Matrix<double, 3, 18> J_rl = K.GetTranslationJacobianInW(Kinematics::LegType::rearLeft, Kinematics::BodyType::foot, q);
370  Eigen::Matrix<double, 3, 18> J_rr = K.GetTranslationJacobianInW(Kinematics::LegType::rearRight, Kinematics::BodyType::foot, q);
371 
372  ROS_INFO_STREAM("Jacobian floating base, J: \n" << J_fb);
373  ROS_INFO_STREAM("Jacobian front left, J: \n" << J_fl);
374  ROS_INFO_STREAM("Jacobian front right, J: \n" << J_fr);
375  ROS_INFO_STREAM("Jacobian rear left, J: \n" << J_rl);
376  ROS_INFO_STREAM("Jacobian rear right, J: \n" << J_rr);
377 }
378 
379 void testBaseJacobian()
380 {
381  Kinematics K;
382 
383  GeneralizedCoordinates q = GeneralizedCoordinates::Constant(0);
384 
385  q << 0, // base_x
386  0, // base_y
387  0, // base_z
388  0, // base_roll
389  0, // base_pitch
390  0*math_utils::degToRad(90), // base_yaw
391  0*math_utils::degToRad(45), // FL-theta_hy
392  0*math_utils::degToRad(-20), // FL-theta_hp
393  0*math_utils::degToRad(90), // FL-theta_kp
394  0*math_utils::degToRad(-45), // FR-theta_hy
395  0*math_utils::degToRad(20), // FR-theta_hp
396  0*math_utils::degToRad(-90), // FR-theta_kp
397  0*math_utils::degToRad(135), // RL-theta_hy
398  0*math_utils::degToRad(20), // RL-theta_hp
399  0*math_utils::degToRad(-90), // RL-theta_kp
400  0*math_utils::degToRad(-135), // RR-theta_hy
401  0*math_utils::degToRad(-20), // RR-theta_hp
402  0*math_utils::degToRad(90); // RR-theta_kp
403 
404 
405  Eigen::Matrix<double, 3, 18> J = K.GetTranslationJacobianInW(Kinematics::LegType::rearRight, Kinematics::BodyType::base, q);
406 
407  ROS_INFO_STREAM("Jacobian, J: \n" << J.transpose() * J);
408 }
409 
410 void testMassMatrix()
411 {
412  Kinematics K;
413 
414  GeneralizedCoordinates q = GeneralizedCoordinates::Constant(0);
415 
416  q << 0, // base_x
417  0, // base_y
418  0, // base_z
419  0, // base_roll
420  0, // base_pitch
421  0*math_utils::degToRad(90), // base_yaw
422  math_utils::degToRad(45),
423  math_utils::degToRad(40),
424  math_utils::degToRad(35),
425  math_utils::degToRad(-45),
426  math_utils::degToRad(-40),
427  math_utils::degToRad(-35),
428  math_utils::degToRad(135),
429  math_utils::degToRad(-40),
430  math_utils::degToRad(-35),
431  math_utils::degToRad(-135),
432  math_utils::degToRad(40),
433  math_utils::degToRad(35);
434 
435  //Eigen::Matrix<double, 18, 18> M = K.GetSingleBodyMassMatrix(Kinematics::LegType::rearRight, Kinematics::BodyType::leg, q);
436  Eigen::Matrix<double, 18, 18> M = K.GetMassMatrix(q);
437 
438  ROS_INFO_STREAM("Mass matrix, M: \n" << M);
439 }
440 
441 void testGravitationalTerms()
442 {
443  Kinematics K;
444 
445  GeneralizedCoordinates q = GeneralizedCoordinates::Constant(0);
446 
447  q << 0,
448  0,
449  0.42,
450  0,
451  0,
452  0,
453  math_utils::degToRad(45),
454  math_utils::degToRad(40),
455  math_utils::degToRad(35),
456  math_utils::degToRad(-45),
457  math_utils::degToRad(-40),
458  math_utils::degToRad(-35),
459  math_utils::degToRad(135),
460  math_utils::degToRad(-40),
461  math_utils::degToRad(-35),
462  math_utils::degToRad(-135),
463  math_utils::degToRad(40),
464  math_utils::degToRad(35);
465 
466  Eigen::Matrix<double, 18, 1> g = K.GetGravitationalTerms(q);
467  Eigen::Matrix<double, 18, 1> g_fl = K.GetSingleBodyGravitationalTerms(Kinematics::LegType::frontLeft, Kinematics::BodyType::leg, q);
468  Eigen::Matrix<double, 18, 1> g_fr = K.GetSingleBodyGravitationalTerms(Kinematics::LegType::frontRight, Kinematics::BodyType::leg, q);
469 
470  ROS_INFO_STREAM("Gravitational terms, g: \n" << g);
471  ROS_INFO_STREAM("Gravitational terms front left, g_fl: \n" << g_fl);
472  ROS_INFO_STREAM("Gravitational terms front_right, g_fr: \n" << g_fr);
473 }
474 
475 void testEulerDiff()
476 {
477  Kinematics K;
478 
479  GeneralizedCoordinates q = GeneralizedCoordinates::Constant(0);
480  GeneralizedCoordinates u = GeneralizedCoordinates::Constant(1);
481 
482  q << 0, // base_x
483  0, // base_y
484  0, // base_z
485  0, // base_roll
486  0, // base_pitch
487  math_utils::degToRad(90), // base_yaw
488  0*math_utils::degToRad(45), // FL-theta_hy
489  0*math_utils::degToRad(-20), // FL-theta_hp
490  0*math_utils::degToRad(90), // FL-theta_kp
491  0*math_utils::degToRad(-45), // FR-theta_hy
492  0*math_utils::degToRad(20), // FR-theta_hp
493  0*math_utils::degToRad(-90), // FR-theta_kp
494  0*math_utils::degToRad(135), // RL-theta_hy
495  0*math_utils::degToRad(20), // RL-theta_hp
496  0*math_utils::degToRad(-90), // RL-theta_kp
497  0*math_utils::degToRad(-135), // RR-theta_hy
498  0*math_utils::degToRad(-20), // RR-theta_hp
499  0*math_utils::degToRad(90); // RR-theta_kp
500 
501  u << 0, // base_x
502  0, // base_y
503  0, // base_z
504  1, // base_roll
505  2, // base_pitch
506  3, // base_yaw
507  0*math_utils::degToRad(45), // FL-theta_hy
508  0*math_utils::degToRad(-20), // FL-theta_hp
509  0*math_utils::degToRad(90), // FL-theta_kp
510  0*math_utils::degToRad(-45), // FR-theta_hy
511  0*math_utils::degToRad(20), // FR-theta_hp
512  0*math_utils::degToRad(-90), // FR-theta_kp
513  0*math_utils::degToRad(135), // RL-theta_hy
514  0*math_utils::degToRad(20), // RL-theta_hp
515  0*math_utils::degToRad(-90), // RL-theta_kp
516  0*math_utils::degToRad(-135), // RR-theta_hy
517  0*math_utils::degToRad(-20), // RR-theta_hp
518  0*math_utils::degToRad(90); // RR-theta_kp
519 
520  //Eigen::Matrix<double, 3, 3> rotationDiff = K.GetRotationMatrixWToBDiff(0,0,0,1,2,3);
521  Eigen::Matrix<double, 6, 18> rotationDiff = K.GetJacobianInWDiff(Kinematics::LegType::frontLeft,
522  Kinematics::BodyType::foot,
523  q,
524  u);
525 
526  ROS_INFO_STREAM("Rotation derivative, rotationDiff: \n" << rotationDiff);
527 }
528 
529 void testCoriolisAndCentrifugalTerms()
530 {
531  Kinematics K;
532 
533  GeneralizedCoordinates q = GeneralizedCoordinates::Constant(0);
534  GeneralizedCoordinates u = GeneralizedCoordinates::Constant(0);
535 
536  q << 0,
537  0,
538  0.42,
539  0,
540  0,
541  0,
542  math_utils::degToRad(45),
543  math_utils::degToRad(40),
544  math_utils::degToRad(35),
545  math_utils::degToRad(-45),
546  math_utils::degToRad(-40),
547  math_utils::degToRad(-35),
548  math_utils::degToRad(135),
549  math_utils::degToRad(-40),
550  math_utils::degToRad(-35),
551  math_utils::degToRad(-135),
552  math_utils::degToRad(40),
553  math_utils::degToRad(35);
554 
555  u << 1, // base_x
556  0, // base_y
557  0, // base_z
558  2, // base_roll
559  3, // base_pitch
560  3, // base_yaw
561  0*math_utils::degToRad(45), // FL-theta_hy
562  0*math_utils::degToRad(-20), // FL-theta_hp
563  0*math_utils::degToRad(90), // FL-theta_kp
564  0*math_utils::degToRad(-45), // FR-theta_hy
565  0*math_utils::degToRad(20), // FR-theta_hp
566  0*math_utils::degToRad(-90), // FR-theta_kp
567  0*math_utils::degToRad(135), // RL-theta_hy
568  0*math_utils::degToRad(20), // RL-theta_hp
569  0*math_utils::degToRad(-90), // RL-theta_kp
570  0*math_utils::degToRad(-135), // RR-theta_hy
571  0*math_utils::degToRad(-20), // RR-theta_hp
572  0*math_utils::degToRad(90); // RR-theta_kp
573 
574  Eigen::Matrix<double, 18, 1> CCTerms = K.GetCoriolisAndCentrifugalTerms(q, u);
575 
576  ROS_INFO_STREAM("Coriolis and Centrifugal terms, b: \n" << CCTerms);
577 }
578 
579 void testNullSpaceProjector()
580 {
581  Eigen::Matrix<double, 2, 3> A;
582 
583  A << 5, 1, 0,
584  0, 0, 1;
585 
586  Eigen::Matrix<double, 3, 3> N;
587 
588  Eigen::MatrixXd N_svd = math_utils::SVDNullSpaceProjector(A);
589 
590  math_utils::nullSpaceProjector(A,N);
591 
592 
593  Eigen::Matrix<double, 3, 2> pinvA;
594 
595  kindr::pseudoInverse(A, pinvA);
596 
597  Eigen::Matrix<double, 3, 3> N_test = Eigen::Matrix<double, 3, 3>::Identity() - pinvA * A;
598 
599  ROS_INFO_STREAM("Null-space projector, N: \n" << N);
600  ROS_INFO_STREAM("Null-space projector test, N: \n" << N_test);
601  ROS_INFO_STREAM("SVD Null-space projector, N_svd: \n" << N_svd << "\n\n");
602 
603  ROS_INFO_STREAM("A*N = \n" << A*N << "\n");
604  ROS_INFO_STREAM("A*N_svd = \n" << A*N_svd << "\n");
605 }
606 
607 void testSVDNullSpaceProjector()
608 {
609  Eigen::Matrix<double, 3, 3> A;
610  Eigen::Matrix<double, 6, 4> B;
611  Eigen::Matrix<double, 2, 3> C;
612  Eigen::Matrix<double, 5, 9> D;
613 
614  A << 1, 0, 0,
615  0, 1, 0,
616  0, 0, 1;
617  B << 1, 0, 0, 0,
618  0, 0, 0, 0,
619  0, 0, 0, 0,
620  0, 0, 1, 0,
621  0, 0, 0, 0,
622  0, 0, 0, 0;
623  C.setRandom();
624  D.setRandom();
625 
626  Eigen::MatrixXd N_A = math_utils::SVDNullSpaceProjector(A);
627  Eigen::MatrixXd N_B = math_utils::SVDNullSpaceProjector(B);
628  Eigen::MatrixXd N_C = math_utils::SVDNullSpaceProjector(C);
629  Eigen::MatrixXd N_D = math_utils::SVDNullSpaceProjector(D);
630 
631  ROS_INFO_STREAM("N_A = \n" << N_A << "\n\n\n");
632  ROS_INFO_STREAM("N_B = \n" << N_B << "\n\n\n");
633  ROS_INFO_STREAM("N_C = \n" << N_C << "\n\n\n");
634  ROS_INFO_STREAM("N_D = \n" << N_D << "\n\n\n");
635 
636  ROS_INFO_STREAM("A*N_A = \n" << A*N_A << "\n\n");
637  ROS_INFO_STREAM("B*N_B = \n" << B*N_B << "\n\n");
638  ROS_INFO_STREAM("C*N_C = \n" << C*N_C << "\n\n");
639  ROS_INFO_STREAM("D*N_D = \n" << D*N_D << "\n\n");
640 
641 }
642 
643 void testContactJacobian()
644 {
645  Kinematics K;
646 
647  GeneralizedCoordinates q = GeneralizedCoordinates::Constant(0);
648 
649  q << 0, // base_x
650  0, // base_y
651  0, // base_z
652  0, // base_roll
653  0, // base_pitch
654  0*math_utils::degToRad(90), // base_yaw
655  0*math_utils::degToRad(45), // FL-theta_hy
656  0*math_utils::degToRad(-20), // FL-theta_hp
657  0*math_utils::degToRad(90), // FL-theta_kp
658  0*math_utils::degToRad(-45), // FR-theta_hy
659  0*math_utils::degToRad(20), // FR-theta_hp
660  0*math_utils::degToRad(-90), // FR-theta_kp
661  0*math_utils::degToRad(135), // RL-theta_hy
662  0*math_utils::degToRad(20), // RL-theta_hp
663  0*math_utils::degToRad(-90), // RL-theta_kp
664  0*math_utils::degToRad(-135), // RR-theta_hy
665  0*math_utils::degToRad(-20), // RR-theta_hp
666  0*math_utils::degToRad(90); // RR-theta_kp
667 
668 
669  std::vector<Kinematics::LegType> legs {Kinematics::LegType::rearLeft, Kinematics::LegType::frontLeft, Kinematics::LegType::rearRight};
670 
671  Eigen::MatrixXd J = K.GetContactJacobianInW(legs, q);
672 
673  ROS_INFO_STREAM("Contact Jacobian, J_c: \n" << J);
674 }
675 
676 void testEigenStacking()
677 {
678  Eigen::Matrix<double, 3, 18> A1;
679  Eigen::Matrix<double, 4, 18> A2;
680  Eigen::Matrix<double, 3, 18> A3;
681  Eigen::Matrix<double, 2, 18> A4;
682 
683  A1.setZero();
684  A2.setIdentity();
685  A3.setConstant(100);
686  A4.setRandom();
687 
688  Eigen::Matrix<Eigen::MatrixXd, Eigen::Dynamic, 1> A;
689 
690  A.resize(4, 1);
691 
692  A(0) = A1;
693  A(1) = A2;
694  A(2) = A3;
695  A(3) = A4;
696 
697  Eigen::MatrixXd A_stacked;
698 
699  for (size_t i = 0; i < A.rows(); i++)
700  {
701  Eigen::MatrixXd A_stacked_tmp = A_stacked;
702 
703  A_stacked.resize(A_stacked_tmp.rows() + A(i).rows(), A(i).cols());
704 
705  A_stacked << A_stacked_tmp,
706  A(i);
707 
708  ROS_INFO_STREAM("A_stacked at iteration " << i << ": \n" << A_stacked << "\n\n");
709  }
710 
711  Eigen::Matrix<double, 18, 18> N;
712 
713  N.setIdentity();
714 
715  N = 10*N;
716 
717  ROS_INFO_STREAM("A_stacked*N: \n" << A_stacked*N);
718 
719  Eigen::MatrixXd E_ineq;
720 
721  int rowsA_ineq = 3;
722  int colsN = N.cols();
723 
724  E_ineq.resize(A_stacked.rows() + rowsA_ineq, colsN + rowsA_ineq);
725 
726  E_ineq.setZero();
727 
728  E_ineq.topLeftCorner(A_stacked.rows(), colsN) = A_stacked * N;
729  E_ineq.topRightCorner(rowsA_ineq, rowsA_ineq) = - Eigen::MatrixXd::Identity(rowsA_ineq, rowsA_ineq);
730 
731  E_ineq.bottomRightCorner(rowsA_ineq, rowsA_ineq) = - Eigen::MatrixXd::Identity(rowsA_ineq, rowsA_ineq);
732 
733  ROS_INFO_STREAM("E_ineq: \n" << E_ineq);
734 
735  Eigen::MatrixXd B;
736 
737  B.resize(20, 20);
738  B.setConstant(10);
739 
740  ROS_INFO_STREAM("B: \n" << B);
741 
742  B = math_utils::SVDNullSpaceProjector(Eigen::Matrix3d::Identity());
743 
744  ROS_INFO_STREAM("B: \n" << B);
745  ROS_INFO_STREAM("B is zero?: " << B.isZero());
746 
747  Eigen::Matrix<double, 18, 1> vec;
748 
749  vec.setConstant(10);
750 
751  Eigen::VectorXd somevector;
752 
753  somevector.resize(14, 1);
754  somevector.setOnes();
755 
756  vec.segment(3, 14) += somevector;
757 
758  ROS_INFO_STREAM("vec: \n" << vec);
759 
760  Eigen::MatrixXd P(18, 18);
761 
762 
763 
764 
765 
766  //A_stacked.resize(A1.rows() + A2.rows() + A3.rows(), A1.cols());
767  //A_stacked << A1,
768  // A2,
769  // A3;
770 
771  //Eigen::MatrixXd A_stacked_tmp = A_stacked;
772 
773  //A_stacked.resize(A_stacked_tmp.rows() + A4.rows(), A4.cols());
774 
775  //A_stacked << A_stacked_tmp,
776  // A3;
777 
778  //ROS_INFO_STREAM("A_stacked: \n" << A_stacked);
779 
780 }
781 
782 void testRotationWToC()
783 {
784  Kinematics kinematics;
785 
786  double yaw = math_utils::THIRD_PI;
787 
788  Eigen::Matrix3d rotationWToC = kinematics.GetRotationMatrixWToC(0, 0, yaw);
789 
790  Eigen::Vector3d h = rotationWToC * Eigen::Vector3d(1, 0, 0);
791  Eigen::Vector3d l = rotationWToC * Eigen::Vector3d(0, 1, 0);
792  Eigen::Vector3d n = Eigen::Vector3d(0, 0, 1);
793 
794  ROS_INFO_STREAM("h: \n" << h << "\n");
795  ROS_INFO_STREAM("l: \n" << l << "\n");
796  ROS_INFO_STREAM("n: \n" << n << "\n");
797 }
798 
799 void testContactForceLimitsMatrix()
800 {
801  Eigen::MatrixXd A;
802 
803  int n_c = 4;
804 
805  int state_dim = 18 + 3*n_c;
806 
807  double mu = 1;
808 
809  Eigen::Vector3d h(1,0,0);
810  Eigen::Vector3d l(0,1,0);
811  Eigen::Vector3d n(0,0,1);
812 
813  A.resize(4*n_c, state_dim);
814 
815  for (size_t i = 0; i < n_c; i++)
816  {
817  A.block(4*i, 0, 1, state_dim).setZero();
818  A.block(4*i, 18 + 3*i, 1, 3) = (h.transpose() - n.transpose() * mu);
819 
820  A.block(4*i + 1, 0, 1, state_dim).setZero();
821  A.block(4*i + 1, 18 + 3*i, 1, 3) = - (h.transpose() + n.transpose() * mu);
822 
823  A.block(4*i + 2, 0, 1, state_dim).setZero();
824  A.block(4*i + 2, 18 + 3*i, 1, 3) = (l.transpose() - n.transpose() * mu);
825 
826  A.block(4*i + 3, 0, 1, state_dim).setZero();
827  A.block(4*i + 3, 18 + 3*i, 1, 3) = - (l.transpose() + n.transpose() * mu);
828  }
829 
830  ROS_INFO_STREAM("A: \n" << A);
831 }
832 
833 void testControlFrame()
834 {
835  Kinematics K;
836 
837  GeneralizedCoordinates q = GeneralizedCoordinates::Constant(0);
838  GeneralizedCoordinates u = GeneralizedCoordinates::Constant(0);
839 
840  q << 0,
841  0,
842  0,
843  math_utils::degToRad(10),
844  math_utils::degToRad(5),
845  math_utils::degToRad(0),
846  0*math_utils::degToRad(45),
847  0*math_utils::degToRad(40),
848  0*math_utils::degToRad(35),
849  0*math_utils::degToRad(-45),
850  0*math_utils::degToRad(-40),
851  0*math_utils::degToRad(-35),
852  0*math_utils::degToRad(135),
853  0*math_utils::degToRad(-40),
854  0*math_utils::degToRad(-35),
855  0*math_utils::degToRad(-135),
856  0*math_utils::degToRad(40),
857  0*math_utils::degToRad(35);
858 
859  Eigen::Matrix3d rotWToC = K.GetRotationMatrixWToC(0, 0, q(5));
860 
861  Eigen::MatrixXd J_P_inB = K.GetTranslationJacobianInB(Kinematics::LegType::frontLeft,
862  Kinematics::BodyType::foot,
863  q.bottomRows(12));
864 
865  Eigen::MatrixXd J_P_inC = K.GetTranslationJacobianInC(Kinematics::LegType::frontLeft,
866  Kinematics::BodyType::foot,
867  q);
868 
869  Eigen::MatrixXd J_P_inW = K.GetTranslationJacobianInW(Kinematics::LegType::frontLeft,
870  Kinematics::BodyType::foot,
871  q);
872  Eigen::MatrixXd J_P_inC2 = rotWToC.inverse() * K.GetTranslationJacobianInW(Kinematics::LegType::frontLeft,
873  Kinematics::BodyType::foot,
874  q);
875 
876  ROS_INFO_STREAM("J_P_inB: \n" << J_P_inB << "\n");
877  ROS_INFO_STREAM("J_P_inC: \n" << J_P_inC << "\n");
878  ROS_INFO_STREAM("J_P_inW: \n" << J_P_inW << "\n");
879  ROS_INFO_STREAM("J_P_inC2: \n" << J_P_inC2 << "\n");
880 
881 }
882 
883 void testEigenQ()
884 {
885  double colsN = 12;
886  double rowsA_ineq = 0;
887 
888  Eigen::MatrixXd Q;
889 
890  Q.resize(colsN + rowsA_ineq, colsN + rowsA_ineq);
891 
892  Q.topLeftCorner(colsN, colsN).setConstant(10);
893  Q.topRightCorner(colsN, rowsA_ineq).setZero();
894  Q.bottomLeftCorner(rowsA_ineq, colsN).setZero();
895  Q.bottomRightCorner(rowsA_ineq, rowsA_ineq).setIdentity();
896 
897  ROS_INFO_STREAM("Q: \n " << Q);
898 }
899 
900 
901 int main(int argc, char **argv)
902 {
903  ros::init(argc, argv, "kinematics_node");
904  ros::NodeHandle nh;
905 
906  //testSingleLegKinematics();
907  //testDhTransform();
908  //testEigen();
909  //ROS_INFO_STREAM("--------------- Test Kindr --------------");
910  //testKindr();
911  //ROS_INFO_STREAM("--------------- Test Hip to Foot Transform --------------");
912  //testHipToFootTransform();
913  //ROS_INFO_STREAM("--------------- Test FK --------------");
914  //testForwardKinematics();
915  //ROS_INFO_STREAM("--------------- Test Single Leg IK --------------");
916  //testSingeLegInverseKinematics();
917  //ROS_INFO_STREAM("--------------- Test IK --------------");
918  //testInverseKinematics();
919  //ROS_INFO_STREAM("--------------- Test positionBaseToFoot --------------");
920  //testPositionBaseToFoot();
921  //ROS_INFO_STREAM("--------------- Test translationJacobian --------------");
922  //testTranslationJacobian();
923  //ROS_INFO_STREAM("--------------- Test rotationJacobian --------------");
924  //testRotationJacobian();
925  //ROS_INFO_STREAM("--------------- Test Jacobian --------------");
926  //testJacobian();
927  //ROS_INFO_STREAM("--------------- Test Mass Matrix -----------");
928  //testMassMatrix();
929  //ROS_INFO_STREAM("--------------- Test Gravitational Terms -----------");
930  //testGravitationalTerms();
931  //ROS_INFO_STREAM("--------------- Test Rotation Derivative -------");
932  //testEulerDiff();
933  //ROS_INFO_STREAM("--------------- Test Coriolis And Centrifugal terms --------------");
934  //testCoriolisAndCentrifugalTerms();
935  //ROS_INFO_STREAM("--------------- Test null-space projector ------------------------");
936  //testNullSpaceProjector();
937  //testSVDNullSpaceProjector();
938  //ROS_INFO_STREAM("--------------- Test contact Jacobian ----------------------------");
939  //testContactJacobian();
940  //ROS_INFO_STREAM("--------------- Test stacking ----------------------------");
941  //testEigenStacking();
942  //ROS_INFO_STREAM("--------------- Test rotationWToC ----------------------------");
943  //testRotationWToC();
944  //ROS_INFO_STREAM("--------------- Test control frame ----------------------------");
945  //testControlFrame();
946  //ROS_INFO_STREAM("--------------- Test Eigen Q generation ----------------------------");
947  //testEigenQ();
948  Eigen::Vector3d a(5, 2, 3);
949  Eigen::Vector3d b(-1, 5, 2);
950 
951  Eigen::Vector3d res = math_utils::boxMinus(a, b);
952 
953  Eigen::Vector3d expectedRes(std::log(5/(-1)),
954  std::log(2/5),
955  std::log(3/2));
956 
957  ROS_INFO_STREAM("res: \n" << res);
958  ROS_INFO_STREAM("expectedRes: \n" << expectedRes);
959 
960 
961 
962  ros::spin();
963  return 0;
964 }
Kinematics
A class for analytical Kinematics Solving.
Definition: kinematics.h:54
Kinematics::GetTranslationJacobianInW
Eigen::Matrix< double, 3, 18 > GetTranslationJacobianInW(const LegType &_leg, const BodyType &_body, const Eigen::Matrix< double, 18, 1 > &_q)
The GetTranslationJacobianInW function returns the 3x18 spatial translation Jacobian mapping generali...
Definition: kinematics.cpp:911
Kinematics::GetContactJacobianInW
Eigen::Matrix< double, Eigen::Dynamic, 18 > GetContactJacobianInW(std::vector< LegType > &_legs, const Eigen::Matrix< double, 18, 1 > &_q)
The GetContactJacobianInW function returns the (3*n_c)x18 support Jacobian mapping reaction forces at...
Definition: kinematics.cpp:2104
Kinematics::GetMassMatrix
Eigen::Matrix< double, 18, 18 > GetMassMatrix(const Eigen::Matrix< double, 18, 1 > &_q)
The GetMassMatrix function returns the mass matrix for the floating base system.
Definition: kinematics.cpp:2330
Kinematics::SolveInverseKinematics
bool SolveInverseKinematics(const Twist &_q_b, const FootstepPositions &_f_pos, JointSpaceVector &_q_r)
The SolveInverseKinematics function calculates the Inverse Kinematics, i.e. maps a coordinate point i...
Definition: kinematics.cpp:104
Kinematics::GetRotationMatrixWToC
Eigen::Matrix< double, 3, 3 > GetRotationMatrixWToC(const double &_roll, const double &_pitch, const double &_yaw)
The GetRotationMatrixWToC function returns the Euler Angles ZYX rotation matrix from World to Control...
Definition: kinematics.cpp:574
Kinematics::GetDhTransform
TransMatrix GetDhTransform(const double &_a, const double &_alpha, const double &_d, const double &_theta)
The GetDhTransform function returns the Denavit-Hartenberg transformation from frame A to frame B.
Definition: kinematics.cpp:369
Kinematics::GetHipToBodyTransform
TransMatrix GetHipToBodyTransform(const LegType &_leg, const BodyType &_body, const double &_theta_hy, const double &_theta_hp, const double &_theta_kp)
The GetHipToBodyTransform function returns the homogeneous transformation from the hip frame to the s...
Definition: kinematics.cpp:389
Kinematics::GetSingleBodyGravitationalTerms
Eigen::Matrix< double, 18, 1 > GetSingleBodyGravitationalTerms(const LegType &_leg, const BodyType &_body, const Eigen::Matrix< double, 18, 1 > &_q)
The GetSingleBodyGravitationalTerms function returns the gravitational terms for a single body in the...
Definition: kinematics.cpp:2488
Kinematics::GetTranslationJacobianInB
Eigen::Matrix< double, 3, 3 > GetTranslationJacobianInB(const LegType &_leg, const BodyType &_body, const double &_theta_hy, const double &_theta_hp, const double &_theta_kp)
The GetTranslationJacobianInB function returns the 3x3 Jacobian matrix for body linear velocities in ...
Definition: kinematics.cpp:723
Kinematics::SolveSingleLegForwardKinematics
Vector3d SolveSingleLegForwardKinematics(const Vector3d &_h_pos, const double &_theta_hy, const double &_theta_hp, const double &_theta_kp)
The SolveSingleLegForwardKinematics function calculates the Forward Kinematics for a single leg.
Definition: kinematics.cpp:229
Kinematics::SolveForwardKinematics
bool SolveForwardKinematics(const GeneralizedCoordinates &_q, FootstepPositions &_f_pos)
The SolveForwardKinematics function calculates the Forward Kinematics, i.e. maps joint angles in Join...
Definition: kinematics.cpp:165
Kinematics::GetGravitationalTerms
Eigen::Matrix< double, 18, 1 > GetGravitationalTerms(const Eigen::Matrix< double, 18, 1 > &_q)
The GetGravitationalTerms function returns the gravitational terms for the floating base system.
Definition: kinematics.cpp:2543
Kinematics::GetTranslationJacobianInC
Eigen::Matrix< double, 3, 18 > GetTranslationJacobianInC(const LegType &_leg, const BodyType &_body, const Eigen::Matrix< double, 18, 1 > &_q)
The GetTranslationJacobianInC function returns the 3x18 spatial translation Jacobian mapping generali...
Definition: kinematics.cpp:969
Kinematics::GetPositionBaseToBodyInB
Eigen::Matrix< double, 3, 1 > GetPositionBaseToBodyInB(const LegType &_leg, const BodyType &_body, const Eigen::Matrix< double, 18, 1 > &_q)
The GetPositionBaseToBodyInB returns the position vector from base origin to COM position in body-fra...
Definition: kinematics.cpp:644
Kinematics::SolveSingleLegInverseKinematics
bool SolveSingleLegInverseKinematics(const bool &_offset, const Vector3d &_h_pos, const Vector3d &_f_pos, Vector3d &_joint_angles)
The SolveSingeLegInverseKinematics function calculates the inverse kinematics for a single leg.
Definition: kinematics.cpp:296
Kinematics::GetRotationJacobianInW
Eigen::Matrix< double, 3, 18 > GetRotationJacobianInW(const LegType &_leg, const BodyType &_body, const Eigen::Matrix< double, 18, 1 > &_q)
The GetRotationJacobianInW function returns the 3x18 spatial rotation Jacobian mapping generalized ve...
Definition: kinematics.cpp:1611
Kinematics::GetJacobianInWDiff
Eigen::Matrix< double, 6, 18 > GetJacobianInWDiff(const LegType &_leg, const BodyType &_body, const Eigen::Matrix< double, 18, 1 > &_q, const Eigen::Matrix< double, 18, 1 > &_u)
The GetJacobianInW function returns the time derivative of the 6x18 spatial Jacobian.
Definition: kinematics.cpp:2083
Kinematics::GetCoriolisAndCentrifugalTerms
Eigen::Matrix< double, 18, 1 > GetCoriolisAndCentrifugalTerms(const Eigen::Matrix< double, 18, 1 > &_q, const Eigen::Matrix< double, 18, 1 > &_u)
The GetCoriolisAndCentrifugalTerms function returns the coriolis and centrifugal terms for the floati...
Definition: kinematics.cpp:2419
debug_utils.h
R2
@ R2
Definition: joystick_controller.h:22
R1
@ R1
Definition: joystick_controller.h:28
GeneralizedCoordinates
Eigen::Matrix< double, 18, 1 > GeneralizedCoordinates
Definition: kinematics.h:45
Vector3d
Eigen::Vector3d Vector3d
Definition: kinematics.h:49
JointSpaceVector
Eigen::Matrix< double, 12, 1 > JointSpaceVector
Definition: kinematics.h:47
FootstepPositions
Eigen::Matrix< Eigen::Vector3d, 4, 1 > FootstepPositions
Definition: kinematics.h:46
Twist
Eigen::Matrix< double, 6, 1 > Twist
Definition: kinematics.h:48
testEigenStacking
void testEigenStacking()
Definition: kinematics_node.cpp:676
testHipToFootTransform
void testHipToFootTransform()
Definition: kinematics_node.cpp:127
testControlFrame
void testControlFrame()
Definition: kinematics_node.cpp:833
testNullSpaceProjector
void testNullSpaceProjector()
Definition: kinematics_node.cpp:579
testRotationWToC
void testRotationWToC()
Definition: kinematics_node.cpp:782
testContactForceLimitsMatrix
void testContactForceLimitsMatrix()
Definition: kinematics_node.cpp:799
main
int main(int argc, char **argv)
Definition: kinematics_node.cpp:901
testInverseKinematics
void testInverseKinematics()
Definition: kinematics_node.cpp:196
testGravitationalTerms
void testGravitationalTerms()
Definition: kinematics_node.cpp:441
testBaseJacobian
void testBaseJacobian()
Definition: kinematics_node.cpp:379
testEigenQ
void testEigenQ()
Definition: kinematics_node.cpp:883
testPositionBaseToFoot
void testPositionBaseToFoot()
Definition: kinematics_node.cpp:225
testContactJacobian
void testContactJacobian()
Definition: kinematics_node.cpp:643
testKindr
void testKindr()
Definition: kinematics_node.cpp:50
testForwardKinematics
void testForwardKinematics()
Definition: kinematics_node.cpp:146
testMassMatrix
void testMassMatrix()
Definition: kinematics_node.cpp:410
testRotationJacobian
void testRotationJacobian()
Definition: kinematics_node.cpp:305
testSVDNullSpaceProjector
void testSVDNullSpaceProjector()
Definition: kinematics_node.cpp:607
testEigen
void testEigen()
Definition: kinematics_node.cpp:103
someRandomTesting
void someRandomTesting()
Definition: kinematics_node.cpp:15
testCoriolisAndCentrifugalTerms
void testCoriolisAndCentrifugalTerms()
Definition: kinematics_node.cpp:529
testJacobian
void testJacobian()
Definition: kinematics_node.cpp:341
testEulerDiff
void testEulerDiff()
Definition: kinematics_node.cpp:475
testTranslationJacobian
void testTranslationJacobian()
Definition: kinematics_node.cpp:268
testSingeLegInverseKinematics
void testSingeLegInverseKinematics()
Definition: kinematics_node.cpp:180
testSingleLegKinematics
void testSingleLegKinematics()
Definition: kinematics_node.cpp:85
testDhTransform
void testDhTransform()
Definition: kinematics_node.cpp:68
calculate_polynomials.a
int a
Definition: calculate_polynomials.py:26
calculate_polynomials.b
tuple b
Definition: calculate_polynomials.py:27
calculate_polynomials.t2
t2
Definition: calculate_polynomials.py:33
calculate_polynomials.t1
t1
Definition: calculate_polynomials.py:32
debug_utils::printJointState
void printJointState(const Eigen::Matrix< double, 12, 1 > &_joint_state)
The printJointState function prints a given joint- state to ROS using print level INFO.
Definition: debug_utils.cpp:72
debug_utils::printFootstepPositions
void printFootstepPositions(const Eigen::Matrix< Eigen::Vector3d, 4, 1 > &_f_pos)
The printFootstepPositions function prints a given set of footstep positions to ROS using print level...
Definition: debug_utils.cpp:169
math_utils::HALF_PI
static constexpr double HALF_PI
Define PI/2.
Definition: angle_utils.h:53
math_utils::boxMinus
Eigen::Matrix< typename Vector_TypeA::Scalar, Eigen::Dynamic, 1 > boxMinus(const Vector_TypeA &_a, const Vector_TypeB &_b)
The boxMinus function calculates the box-minus operation between two vectors of equal size.
Definition: linalg_utils.h:180
math_utils::THIRD_PI
static constexpr double THIRD_PI
Define PI/3.
Definition: angle_utils.h:56
math_utils::degToRad
double degToRad(const double &_deg)
The degToRad function converts degrees to radians.
Definition: angle_utils.cpp:33
math_utils::SVDNullSpaceProjector
Eigen::Matrix< typename Matrix_TypeA::Scalar, Eigen::Dynamic, Eigen::Dynamic > SVDNullSpaceProjector(const Matrix_TypeA &_A)
The SVDNullSpaceProjector function calculates the null-space projector of a given matrix....
Definition: linalg_utils.h:144
math_utils::nullSpaceProjector
static bool nullSpaceProjector(const Matrix_TypeA &_A, Matrix_TypeB &_N, const double _alpha=1)
The nullSpaceProjector function calculates the null-space projector of a given matrix....
Definition: linalg_utils.h:104
math_utils::TWO_THIRDS_PI
static constexpr double TWO_THIRDS_PI
Define 2PI/3.
Definition: angle_utils.h:50