Tetrapod Project
kinematics.cpp
Go to the documentation of this file.
1 /*******************************************************************/
2 /* AUTHOR: Paal Arthur S. Thorseth */
3 /* ORGN: Dept of Eng Cybernetics, NTNU Trondheim */
4 /* FILE: kinematics.cpp */
5 /* DATE: Mar 1, 2021 */
6 /* */
7 /* Copyright (C) 2021 Paal Arthur S. Thorseth, */
8 /* Adrian B. Ghansah */
9 /* */
10 /* This program is free software: you can redistribute it */
11 /* and/or modify it under the terms of the GNU General */
12 /* Public License as published by the Free Software Foundation, */
13 /* either version 3 of the License, or (at your option) any */
14 /* later version. */
15 /* */
16 /* This program is distributed in the hope that it will be useful, */
17 /* but WITHOUT ANY WARRANTY; without even the implied warranty */
18 /* of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. */
19 /* See the GNU General Public License for more details. */
20 /* */
21 /* You should have received a copy of the GNU General Public */
22 /* License along with this program. If not, see */
23 /* <https://www.gnu.org/licenses/>. */
24 /* */
25 /*******************************************************************/
26 
27 #include <kinematics/kinematics.h>
28 
29 
30 // Constructor
31 Kinematics::Kinematics()
32 {
33  // Set min/max angles
34  this->min_angles(0) = math_utils::degToRad(0); // fl, hy
35  this->min_angles(1) = math_utils::degToRad(-95); // fl, hp
36  this->min_angles(2) = math_utils::degToRad(-115); // fl, kp
37  this->min_angles(3) = math_utils::degToRad(-170); // fr, hy
38  this->min_angles(4) = math_utils::degToRad(-90); // fr, hp
39  this->min_angles(5) = math_utils::degToRad(-115); // fr, kp
40  this->min_angles(6) = math_utils::degToRad(10); // rl, hy
41  this->min_angles(7) = math_utils::degToRad(-90); // rl, hp
42  this->min_angles(8) = math_utils::degToRad(-115); // rl, kp
43  this->min_angles(9) = math_utils::degToRad(-180); // rr, hy
44  this->min_angles(10) = math_utils::degToRad(-90); // rr, hp
45  this->min_angles(11) = math_utils::degToRad(-115); // rr, kp
46 
47  this->max_angles(0) = math_utils::degToRad(170); // fl, hy
48  this->max_angles(1) = math_utils::degToRad(95); // fl, hp
49  this->max_angles(2) = math_utils::degToRad(115); // fl, kp
50  this->max_angles(3) = math_utils::degToRad(0); // fr, hy
51  this->max_angles(4) = math_utils::degToRad(90); // fr, hp
52  this->max_angles(5) = math_utils::degToRad(115); // fr, kp
53  this->max_angles(6) = math_utils::degToRad(180); // rl, hy
54  this->max_angles(7) = math_utils::degToRad(90); // rl, hp
55  this->max_angles(8) = math_utils::degToRad(115); // rl, kp
56  this->max_angles(9) = math_utils::degToRad(-10); // rr, hy
57  this->max_angles(10) = math_utils::degToRad(90); // rr, hp
58  this->max_angles(11) = math_utils::degToRad(115); // rr, kp
59 
60  // Set link lenghts
61  this->L1 = 0.130;
62  this->L2 = 0.220;
63  this->L3 = 0.279;
64 
65  // On the physical robot
66  this->L3 = 0.290;
67  //this->L3 = 0.207; // Temporary while we are using the short lower legs
68 
69  this->LC1 = 0.1030;
70  this->LC2 = 0.0257;
71  this->LC3 = 0.1069;
72 
73  // Set position vectors
74  this->positionBaseToFrontLeftInB << 0.151, 0.151, 0;
75  this->positionBaseToFrontRightInB << 0.151, -0.151, 0;
76  this->positionBaseToRearLeftInB << -0.151, 0.151, 0;
77  this->positionBaseToRearRightInB << -0.151, -0.151, 0;
78 
79  // Set offsets
80  this->flOffset = false;
81  this->frOffset = true;
82  this->rlOffset = true;
83  this->rrOffset = false;
84 
85  // Set masses
86  this->M0 = 10.1;
87  this->M1 = 1.179;
88  this->M2 = 1.394;
89  this->M3 = 0.283;
90 
91  // Set inertia matrices
92 
93  this->I0 = GetInertiaMatrix(0.141, 0.143, 0.254, 0.0, 0.0, 0.0);
94  this->I1_fl_rr = GetInertiaMatrix(0.002988, 0.003332, 0.004664, -0.001845, -0.000064, 0.000037);
95  this->I1_fr_rl = GetInertiaMatrix(0.002988, 0.003332, 0.004664, 0.001845, -0.000064, -0.000037);
96  this->I2 = GetInertiaMatrix(0.00174, 0.00660, 0.00644, 0.0, 0.00108, 0.0);
97  this->I3 = GetInertiaMatrix(0.00006651, 0.00317406, 0.00318967, 0.00004913, 0.00000614, 0.0);
98 }
99 
100 // Destructor
101 Kinematics::~Kinematics() {}
102 
103 // Solve inverse kinematics
104 bool Kinematics::SolveInverseKinematics(const Twist &_q_b, const FootstepPositions &_f_pos, JointSpaceVector &_q_r)
105 {
106  // Base world position
107  Eigen::Vector3d basePositionInW(_q_b.block<3, 1>(0, 0));
108 
109  // Base orientation
110  Eigen::Vector3d baseOrientation(_q_b.block<3, 1>(3, 0));
111 
112  Eigen::Vector3d positionFrontLeftFootInW(_f_pos(0));
113  Eigen::Vector3d positionFrontRightFootInW(_f_pos(1));
114  Eigen::Vector3d positionRearLeftFootInW(_f_pos(2));
115  Eigen::Vector3d positionRearRightFootInW(_f_pos(3));
116 
117  // Transformation matrix from Body to World frame (R_b^w)
118  kindr::RotationMatrixD transformationBToW(kindr::EulerAnglesZyxD(baseOrientation(2),
119  baseOrientation(1),
120  baseOrientation(0))
121  );
122 
123  // Transformation matrix from World to Body frame (R_w^b)
124  kindr::RotationMatrixD transformationWToB(transformationBToW.transpose());
125 
126  // The vector from the desired base position to the current foot position in the desired base frame
127  Vector3d positionFrontLeftFootInB(transformationWToB.matrix()*(positionFrontLeftFootInW - basePositionInW));
128  Vector3d positionFrontRightFootInB(transformationWToB.matrix()*(positionFrontRightFootInW - basePositionInW));
129  Vector3d positionRearLeftFootInB(transformationWToB.matrix()*(positionRearLeftFootInW - basePositionInW));
130  Vector3d positionRearRightFootInB(transformationWToB.matrix()*(positionRearRightFootInW - basePositionInW));
131 
132  // A vector of hip positions given in the desired base frame
133  Eigen::Matrix<Eigen::Vector3d, 4, 1> positionHipsInB;
134  positionHipsInB(0) = positionBaseToFrontLeftInB;
135  positionHipsInB(1) = positionBaseToFrontRightInB;
136  positionHipsInB(2) = positionBaseToRearLeftInB;
137  positionHipsInB(3) = positionBaseToRearRightInB;
138 
139  // Joint angles
140  //_q_r.block<3, 1>(0, 0) = this->SolveSingleLegInverseKinematics(this->flOffset, positionHipsInB(0), positionFrontLeftFootInB);
141  //_q_r.block<3, 1>(3, 0) = this->SolveSingleLegInverseKinematics(this->frOffset, positionHipsInB(1), positionFrontRightFootInB);
142  //_q_r.block<3, 1>(6, 0) = this->SolveSingleLegInverseKinematics(this->rlOffset, positionHipsInB(2), positionRearLeftFootInB);
143  //_q_r.block<3, 1>(9, 0) = this->SolveSingleLegInverseKinematics(this->rrOffset, positionHipsInB(3), positionRearRightFootInB);
144 
145  // Joint angles
146  Eigen::Vector3d q_r_fl;
147  Eigen::Vector3d q_r_fr;
148  Eigen::Vector3d q_r_rl;
149  Eigen::Vector3d q_r_rr;
150 
151  this->SolveSingleLegInverseKinematics(this->flOffset, positionHipsInB(0), positionFrontLeftFootInB, q_r_fl);
152  this->SolveSingleLegInverseKinematics(this->frOffset, positionHipsInB(1), positionFrontRightFootInB, q_r_fr);
153  this->SolveSingleLegInverseKinematics(this->rlOffset, positionHipsInB(2), positionRearLeftFootInB, q_r_rl);
154  this->SolveSingleLegInverseKinematics(this->rrOffset, positionHipsInB(3), positionRearRightFootInB, q_r_rr);
155 
156  _q_r.block<3, 1>(0, 0) = q_r_fl;
157  _q_r.block<3, 1>(3, 0) = q_r_fr;
158  _q_r.block<3, 1>(6, 0) = q_r_rl;
159  _q_r.block<3, 1>(9, 0) = q_r_rr;
160 
161  return this->ValidateSolution(_q_r);
162 }
163 
164 // Solve forward kinematics
165 bool Kinematics::SolveForwardKinematics(const GeneralizedCoordinates &_q,
166  FootstepPositions &_f_pos)
167 {
168 
169  // Base Pose
170  kindr::Position3D base_position(_q.block(0,0,2,0));
171  kindr::Position3D base_orientation(_q.block(3,0,5,0));
172 
173  // Rotation from World to Body frame (transform from Body to World)
174  kindr::RotationMatrixD rotationWToB(kindr::EulerAnglesZyxD(base_orientation(2),
175  base_orientation(1),
176  base_orientation(0))
177  );
178 
179  // Base to hip positions in World Frame.
180  kindr::Position3D positionBaseToFrontLeftInW(rotationWToB.matrix()*positionBaseToFrontLeftInB);
181  kindr::Position3D positionBaseToFrontRightInW(rotationWToB.matrix()*positionBaseToFrontRightInB);
182  kindr::Position3D positionBaseToRearLeftInW(rotationWToB.matrix()*positionBaseToRearLeftInB);
183  kindr::Position3D positionBaseToRearRightInW(rotationWToB.matrix()*positionBaseToRearRightInB);
184 
185  // Base to hip transformations
186  kindr::HomTransformMatrixD transformBaseToFrontLeftHip(positionBaseToFrontLeftInW, rotationWToB);
187  kindr::HomTransformMatrixD transformBaseToFrontRightHip(positionBaseToFrontRightInW, rotationWToB);
188  kindr::HomTransformMatrixD transformBaseToRearLeftHip(positionBaseToRearLeftInW, rotationWToB);
189  kindr::HomTransformMatrixD transformBaseToRearRightHip(positionBaseToRearRightInW, rotationWToB);
190 
191  // Hip to foot transformations
192  kindr::HomTransformMatrixD transformFrontLeftHipToFoot = this->GetHipToBodyTransform(LegType::frontLeft,
193  BodyType::foot,
194  _q(6),
195  _q(7),
196  _q(8));
197  kindr::HomTransformMatrixD transformFrontRightHipToFoot = this->GetHipToBodyTransform(LegType::frontRight,
198  BodyType::foot,
199  _q(9),
200  _q(10),
201  _q(11));
202  kindr::HomTransformMatrixD transformRearLeftHipToFoot = this->GetHipToBodyTransform(LegType::rearLeft,
203  BodyType::foot,
204  _q(12),
205  _q(13),
206  _q(14));
207  kindr::HomTransformMatrixD transformRearRightHipToFoot = this->GetHipToBodyTransform(LegType::rearRight,
208  BodyType::foot,
209  _q(15),
210  _q(16),
211  _q(17));
212 
213  // Base to foot transformations
214  kindr::HomTransformMatrixD transformBaseToFrontLeftFoot = transformBaseToFrontLeftHip*transformFrontLeftHipToFoot;
215  kindr::HomTransformMatrixD transformBaseToFrontRightFoot = transformBaseToFrontRightHip*transformFrontRightHipToFoot;
216  kindr::HomTransformMatrixD transformBaseToRearLeftFoot = transformBaseToRearLeftHip*transformRearLeftHipToFoot;
217  kindr::HomTransformMatrixD transformBaseToRearRightFoot = transformBaseToRearRightHip*transformRearRightHipToFoot;
218 
219  // Foot positions
220  _f_pos(0) = base_position.vector() + transformBaseToFrontLeftFoot.transform(kindr::Position3D(0,0,0)).vector();
221  _f_pos(1) = base_position.vector() + transformBaseToFrontRightFoot.transform(kindr::Position3D(0,0,0)).vector();
222  _f_pos(2) = base_position.vector() + transformBaseToRearLeftFoot.transform(kindr::Position3D(0,0,0)).vector();
223  _f_pos(3) = base_position.vector() + transformBaseToRearRightFoot.transform(kindr::Position3D(0,0,0)).vector();
224 
225  return true; //TODO do some criteria evaluation here
226 }
227 
228 // Solve single leg forward kinematics
229 Vector3d Kinematics::SolveSingleLegForwardKinematics(const Vector3d &_h_pos,
230  const double &_theta_hy,
231  const double &_theta_hp,
232  const double &_theta_kp)
233 {
234  // Complete D-H transformation
235  kindr::HomTransformMatrixD transformation = this->GetHipToBodyTransform(LegType::frontLeft,
236  BodyType::foot,
237  _theta_hy,
238  _theta_hp,
239  _theta_kp);
240 
241  // End-effector position
242  Eigen::Vector3d pos = _h_pos + transformation.transform(kindr::Position3D(0,0,0)).vector();
243 
244  return pos;
245 }
246 
247 // Solve single leg forward kinematics from hip to foot
248 Vector3d Kinematics::SolveSingleLegForwardKinematics(const LegType &_leg_type, const Vector3d &_joint_angles)
249 {
250  // Complete D-H transformation
251  kindr::HomTransformMatrixD transformation = this->GetHipToBodyTransform(_leg_type,
252  BodyType::foot,
253  _joint_angles(0),
254  _joint_angles(1),
255  _joint_angles(2));
256 
257  Eigen::Vector3d pos = transformation.transform(kindr::Position3D(0, 0, 0)).vector();
258 
259  return pos;
260 }
261 
262 Eigen::Matrix<double, 3, 1> Kinematics::SolveSingleLegHipToFootForwardKinematics(const LegType &_leg_type, const Eigen::Matrix<double, 3, 1> &_joint_angles)
263 {
264  Eigen::Matrix<double, 3, 1> pos;
265 
266  double hy = _joint_angles(0);
267  double hp = _joint_angles(1);
268  double kp = _joint_angles(2);
269 
270  pos(0) = std::cos(hy) * (this->L1 + this->L2 * std::cos(hp) + this->L3 * std::cos(hp + kp));
271  pos(1) = std::sin(hy) * (this->L1 + this->L2 * std::cos(hp) + this->L3 * std::cos(hp + kp));
272  pos(2) = - this->L2 * std::sin(hp) - this->L3 * sin(hp + kp);
273 
274  if((_leg_type == LegType::frontRight) || (_leg_type == LegType::rearLeft))
275  {
276  pos(2) = - pos(2);
277  }
278 
279  /*
280  if((_leg_type == LegType::frontLeft) || (_leg_type == LegType::rearRight))
281  {
282  pos(2) = - pos(2);
283  }
284  */
285 
286  return pos;
287 }
288 
289 Vector3d Kinematics::SolveSingleLegForwardKinematics(const Vector3d &_joint_angles)
290 {
291  Eigen::Matrix<double, 3, 1> zero = Eigen::Matrix<double, 3, 1>::Zero();
292 
293  return SolveSingleLegForwardKinematics(zero, _joint_angles(0), _joint_angles(1), _joint_angles(2));
294 }
295 
296 bool Kinematics::SolveSingleLegInverseKinematics(const bool &_offset, const Vector3d &_h_pos, const Vector3d &_f_pos, Vector3d &_joint_angles)
297 {
298  // Change of variables to center hip position.
299  double x_e = _f_pos(0) - _h_pos(0);
300  double y_e = _f_pos(1) - _h_pos(1);
301  double z_e = _f_pos(2) - _h_pos(2);
302 
303  // Distance from hip to foot in the X-Y plane
304  double normHipToFootInXY = std::sqrt( std::pow(x_e, 2) + std::pow(y_e, 2) );
305 
306  // Distance between hip pitch joint and foot
307  double normHipPitchToFoot = std::sqrt( std::pow(normHipToFootInXY - this->L1, 2) + std::pow(z_e, 2) );
308 
309  // Angle from x-axis to the line going through the
310  // foot point and hip pitch joint
311  double zeta = std::atan2(-z_e, normHipToFootInXY - this->L1);
312 
313  // Angle between foot-knee-hip pitch joints
314  double alpha = std::acos( ( std::pow(this->L2, 2) + std::pow(this->L3, 2) - std::pow(normHipPitchToFoot, 2) ) / ( 2 * this->L2 * this->L3 ) );
315 
316  if (std::isnan(alpha))
317  {
318  ROS_INFO_STREAM("IK alpha returned nan, nominator was: " << ( std::pow(this->L2, 2) + std::pow(this->L3, 2) - std::pow(normHipPitchToFoot, 2) ) / ( 2 * this->L2 * this->L3 )
319  << " and normHipPitchToFoot was: " << normHipPitchToFoot );
320 
321  return false;
322  }
323 
324  // Angle between foot-hip pitch-knee joints
325  double beta = std::acos( ( std::pow(normHipPitchToFoot, 2) + std::pow(this->L2, 2) - std::pow(this->L3, 2) ) / ( 2 * normHipPitchToFoot * this->L2 ) );
326 
327  if (std::isnan(beta))
328  {
329  ROS_INFO_STREAM("IK beta returned nan, nominator was: " << ( std::pow(normHipPitchToFoot, 2) + std::pow(this->L2, 2) - std::pow(this->L3, 2) ) / ( 2 * normHipPitchToFoot * this->L2 )
330  << " and normHipPitchToFoot was: " << normHipPitchToFoot );
331 
332  return false;
333  }
334 
335  // Check offset and set angles accordingly
336  if (_offset)
337  {
338  // Calculate theta_hy
339  _joint_angles(0) = std::atan2(y_e, x_e);
340 
341  // Calculate theta_hp
342  _joint_angles(1) = beta - zeta;
343 
344  // Calculate theta_kp
345  _joint_angles(2) = alpha - math_utils::PI;
346  }
347  else
348  {
349  // Calculate theta_hy
350  _joint_angles(0) = std::atan2(y_e, x_e);
351 
352  // Calculate theta_hp
353  _joint_angles(1) = - beta + zeta;
354 
355  // Calculate theta_kp
356  _joint_angles(2) = math_utils::PI - alpha;
357  }
358 
359  return true;
360 }
361 
362 bool Kinematics::SolveSingleLegInverseKinematics(const bool &_offset, const Vector3d &_f_pos, Vector3d &_joint_angles)
363 {
364  Eigen::Matrix<double, 3, 1> zero = Eigen::Matrix<double, 3, 1>::Zero();
365  return SolveSingleLegInverseKinematics(_offset, zero, _f_pos, _joint_angles);
366 }
367 
368 // Denavit-Hartenberg Transformation
369 TransMatrix Kinematics::GetDhTransform(const double &_a,
370  const double &_alpha,
371  const double &_d,
372  const double &_theta)
373 {
374  Eigen::Vector3d unit_z(0,0,1);
375  Eigen::Vector3d unit_x(1,0,0);
376 
377  kindr::RotationMatrixD R_z(kindr::AngleAxisD(_theta, unit_z));
378  kindr::RotationMatrixD R_x(kindr::AngleAxisD(_alpha, unit_x));
379 
380  kindr::Position3D position(_a*R_z.matrix()*unit_x + _d*unit_z);
381  kindr::RotationMatrixD rotation = R_z*R_x;
382 
383  kindr::HomTransformMatrixD transformationAToB(position, rotation);
384 
385  return transformationAToB;
386 }
387 
388 // Hip to body transform
389 TransMatrix Kinematics::GetHipToBodyTransform(const LegType &_leg,
390  const BodyType &_body,
391  const double &_theta_hy,
392  const double &_theta_hp,
393  const double &_theta_kp)
394 {
395  // Declare possible transforms
396  kindr::HomTransformMatrixD t1;
397  kindr::HomTransformMatrixD t2;
398  kindr::HomTransformMatrixD t3;
399 
400  // Check offset for D-H transformation
401  double roll_offset = 0;
402 
403  switch (_leg)
404  {
405  case frontLeft:
406  {
407  roll_offset = - math_utils::HALF_PI;
408 
409  break;
410  }
411  case frontRight:
412  {
413  roll_offset = math_utils::HALF_PI;
414 
415  break;
416  }
417  case rearLeft:
418  {
419  roll_offset = math_utils::HALF_PI;
420 
421  break;
422  }
423  case rearRight:
424  {
425  roll_offset = - math_utils::HALF_PI;
426 
427  break;
428  }
429  default:
430  {
431  ROS_ERROR_STREAM("[Kinematics::GetHipToBodyTransform] Could not determine leg type!");
432 
433 
434  std::abort();
435  std::abort();
436 
437  break;
438  }
439 
440  }
441 
442 
443  switch (_body)
444  {
445  case hip:
446  {
447  // First D-H transformation
448  t1 = this->GetDhTransform(this->LC1,
449  roll_offset,
450  0,
451  _theta_hy);
452 
453  // Second D-H transformation (not applicable for hip)
454  t2.setIdentity();
455 
456  // Third D-H transformation (not applicable for hip)
457  t3.setIdentity();
458 
459  break;
460  }
461  case thigh:
462  {
463  // First D-H transformation
464  t1 = this->GetDhTransform(this->L1,
465  roll_offset,
466  0,
467  _theta_hy);
468 
469  // Second D-H transformation
470  t2 = this->GetDhTransform(this->LC2,
471  0,
472  0,
473  _theta_hp);
474 
475  // Third D-H transformation (not applicable for thigh)
476  t3.setIdentity();
477 
478  break;
479  }
480  case leg:
481  {
482  // First D-H transformation
483  t1 = this->GetDhTransform(this->L1,
484  roll_offset,
485  0,
486  _theta_hy);
487 
488  // Second D-H transformation
489  t2 = this->GetDhTransform(this->L2,
490  0,
491  0,
492  _theta_hp);
493 
494  // Third D-H transformation
495  t3 = this->GetDhTransform(this->LC3,
496  0,
497  0,
498  _theta_kp);
499 
500  break;
501  }
502  case foot:
503  {
504  // First D-H transformation
505  t1 = this->GetDhTransform(this->L1,
506  roll_offset,
507  0,
508  _theta_hy);
509 
510  // Second D-H transformation
511  t2 = this->GetDhTransform(this->L2,
512  0,
513  0,
514  _theta_hp);
515 
516  // Third D-H transformation
517  t3 = this->GetDhTransform(this->L3,
518  0,
519  0,
520  _theta_kp);
521 
522  break;
523  }
524  default:
525  {
526  ROS_ERROR_STREAM("[Kinematics::GetHipToBodyTransform] Could not determine body type!");
527 
528  break;
529  }
530  }
531 
532  // Complete D-H transformation
533  kindr::HomTransformMatrixD transformation = t1*t2*t3;
534 
535  return transformation;
536 }
537 
538 // Rotation matrix from world to body (transform from body to world)
539 Eigen::Matrix<double, 3, 3> Kinematics::GetRotationMatrixWToB(const double &_roll,
540  const double &_pitch,
541  const double &_yaw)
542 {
543  kindr::RotationMatrixD rotationWToB(kindr::EulerAnglesZyxD(_yaw,
544  _pitch,
545  _roll)
546  );
547 
548  return rotationWToB.matrix();
549 }
550 
551 // Time derivative of the rotation matrix from world to body
552 Eigen::Matrix<double, 3, 3> Kinematics::GetRotationMatrixWToBDiff(const double &_roll,
553  const double &_pitch,
554  const double &_yaw,
555  const double &_roll_rate,
556  const double &_pitch_rate,
557  const double &_yaw_rate)
558 {
559  kindr::RotationMatrixD rotationWToB(kindr::EulerAnglesZyxD(_yaw,
560  _pitch,
561  _roll)
562  );
563 
564  kindr::LocalAngularVelocityD angularVelInB(_roll_rate,
565  _pitch_rate,
566  _yaw_rate);
567 
568  kindr::RotationMatrixDiffD rotationWToBDiff(rotationWToB, angularVelInB);
569 
570  return rotationWToBDiff.matrix();
571 }
572 
573 // Rotation matrix from world to control (transform from control to world)
574 Eigen::Matrix<double, 3, 3> Kinematics::GetRotationMatrixWToC(const double &_roll,
575  const double &_pitch,
576  const double &_yaw)
577 {
578  kindr::RotationMatrixD rotationWToC(kindr::EulerAnglesZyxD(_yaw,
579  _pitch,
580  _roll)
581  );
582 
583  return rotationWToC.matrix();
584 }
585 
586 // Time derivative of the rotation matrix from world to control
587 Eigen::Matrix<double, 3, 3> Kinematics::GetRotationMatrixWToCDiff(const double &_roll,
588  const double &_pitch,
589  const double &_yaw,
590  const double &_roll_rate,
591  const double &_pitch_rate,
592  const double &_yaw_rate)
593 {
594  kindr::RotationMatrixD rotationWToC(kindr::EulerAnglesZyxD(_yaw,
595  _pitch,
596  _roll)
597  );
598 
599  kindr::LocalAngularVelocityD angularVelInC(_roll_rate,
600  _pitch_rate,
601  _yaw_rate);
602 
603  kindr::RotationMatrixDiffD rotationWToCDiff(rotationWToC, angularVelInC);
604 
605  return rotationWToCDiff.matrix();
606 }
607 
608 // Rotation matrix from control to body (transform from body to control)
609 Eigen::Matrix<double, 3, 3> Kinematics::GetRotationMatrixCToB(const double &_roll,
610  const double &_pitch,
611  const double &_yaw)
612 {
613  kindr::RotationMatrixD rotationCToB(kindr::EulerAnglesZyxD(_yaw,
614  _pitch,
615  _roll)
616  );
617 
618  return rotationCToB.matrix();
619 }
620 
621 // Time derivative of the rotation matrix from control to body
622 Eigen::Matrix<double, 3, 3> Kinematics::GetRotationMatrixCToBDiff(const double &_roll,
623  const double &_pitch,
624  const double &_yaw,
625  const double &_roll_rate,
626  const double &_pitch_rate,
627  const double &_yaw_rate)
628 {
629  kindr::RotationMatrixD rotationCToB(kindr::EulerAnglesZyxD(_yaw,
630  _pitch,
631  _roll)
632  );
633 
634  kindr::LocalAngularVelocityD angularVelInB(_roll_rate,
635  _pitch_rate,
636  _yaw_rate);
637 
638  kindr::RotationMatrixDiffD rotationCToBDiff(rotationCToB, angularVelInB);
639 
640  return rotationCToBDiff.matrix();
641 }
642 
643 // Position vector from base to body in body(base)-frame
644 Eigen::Matrix<double, 3, 1> Kinematics::GetPositionBaseToBodyInB(const LegType &_leg,
645  const BodyType &_body,
646  const Eigen::Matrix<double, 18, 1> &_q)
647 {
648  Eigen::Matrix<double, 3, 1> positionBaseToBodyInB;
649 
650  Eigen::Matrix<double, 3, 1> positionBaseToHipInB;
651 
652  double theta_hy;
653  double theta_hp;
654  double theta_kp;
655 
656  kindr::HomTransformMatrixD transformHipToBody;
657 
658  switch (_leg)
659  {
660  case frontLeft:
661  {
662  positionBaseToHipInB = this->positionBaseToFrontLeftInB;
663 
664  theta_hy = _q(6);
665  theta_hp = _q(7);
666  theta_kp = _q(8);
667 
668  break;
669  }
670  case frontRight:
671  {
672  positionBaseToHipInB = this->positionBaseToFrontRightInB;
673 
674  theta_hy = _q(9);
675  theta_hp = _q(10);
676  theta_kp = _q(11);
677 
678  break;
679  }
680  case rearLeft:
681  {
682  positionBaseToHipInB = this->positionBaseToRearLeftInB;
683 
684  theta_hy = _q(12);
685  theta_hp = _q(13);
686  theta_kp = _q(14);
687 
688  break;
689  }
690  case rearRight:
691  {
692  positionBaseToHipInB = this->positionBaseToRearRightInB;
693 
694  theta_hy = _q(15);
695  theta_hp = _q(16);
696  theta_kp = _q(17);
697 
698  break;
699  }
700  default:
701  {
702  ROS_ERROR_STREAM("[Kinematics::GetPositionBaseToBodyInB] Could not determine leg type!");
703 
704  std::abort();
705 
706  break;
707  }
708  }
709 
710  transformHipToBody = this->GetHipToBodyTransform(_leg,
711  _body,
712  theta_hy,
713  theta_hp,
714  theta_kp);
715 
716  positionBaseToBodyInB = positionBaseToHipInB + transformHipToBody.transform(kindr::Position3D(0,0,0)).vector();
717 
718 
719  return positionBaseToBodyInB;
720 }
721 
722 // 3x3 Translation Jacobian in body frame
723 Eigen::Matrix<double, 3, 3> Kinematics::GetTranslationJacobianInB(const LegType &_leg,
724  const BodyType &_body,
725  const double &_theta_hy,
726  const double &_theta_hp,
727  const double &_theta_kp)
728 {
729  Eigen::Matrix<double, 3, 3> J; // 3x3 Translation Jacobian
730 
731  double l1; // Length of link 1, if any, zero elsewise
732  double l2; // Length of link 2, if any, zero elsewise
733  double l3; // Length of link 3, if any, zero elsewise
734 
735  double c1 = std::cos(_theta_hy); // cos(theta1)
736  double s1 = std::sin(_theta_hy); // sin(theta1)
737 
738  double c2 = std::cos(_theta_hp); // cos(theta2)
739  double s2 = std::sin(_theta_hp); // sin(theta2)
740 
741  double c23 = std::cos(_theta_hp + _theta_kp); // cos(theta2 + theta3)
742  double s23 = std::sin(_theta_hp + _theta_kp); // sin(theta2 + theta3)
743 
744  switch (_body)
745  {
746  case hip:
747  {
748  l1 = this->LC1;
749  l2 = 0;
750  l3 = 0;
751 
752  break;
753  }
754  case thigh:
755  {
756  l1 = this->L1;
757  l2 = this->LC2;
758  l3 = 0;
759 
760  break;
761  }
762  case leg:
763  {
764  l1 = this->L1;
765  l2 = this->L2;
766  l3 = this->LC3;
767 
768  break;
769  }
770  case foot:
771  {
772  l1 = this->L1;
773  l2 = this->L2;
774  l3 = this->L3;
775 
776  break;
777  }
778  default:
779  {
780  ROS_ERROR_STREAM("[Kinematics::GetTranslationJacobianInB] Could not determine body type!");
781 
782  std::abort();
783 
784  break;
785  }
786  }
787 
788  switch (_leg)
789  {
790  // Equivalent of case frontLeft || rearRight:
791  case frontLeft:
792  case rearRight:
793  {
794  J(0, 0) = (l1 + l2 * c2 + l3 * c23) * (-s1);
795  J(1, 0) = (l1 + l2 * c2 + l3 * c23) * ( c1);
796  J(2, 0) = 0.0;
797 
798  J(0, 1) = (-l2 * s2 - l3 * s23) * c1;
799  J(1, 1) = (-l2 * s2 - l3 * s23) * s1;
800  J(2, 1) = -l2 * c2 - l3 * c23;
801 
802  J(0, 2) = -l3 * s23 * c1;
803  J(1, 2) = -l3 * s23 * s1;
804  J(2, 2) = -l3 * c23;
805 
806  break;
807  }
808  // Equivalent of case frontRight || rearLeft:
809  case frontRight:
810  case rearLeft:
811  {
812  J(0, 0) = (l1 + l2 * c2 + l3 * c23) * (-s1);
813  J(1, 0) = (l1 + l2 * c2 + l3 * c23) * ( c1);
814  J(2, 0) = 0.0;
815 
816  J(0, 1) = (-l2 * s2 - l3 * s23) * c1;
817  J(1, 1) = (-l2 * s2 - l3 * s23) * s1;
818  J(2, 1) = l2 * c2 + l3 * c23;
819 
820  J(0, 2) = -l3 * s23 * c1;
821  J(1, 2) = -l3 * s23 * s1;
822  J(2, 2) = l3 * c23;
823 
824  break;
825  }
826  default:
827  {
828  ROS_ERROR_STREAM("[Kinematics::GetTranslationJacobianInB] Could not determine leg type!");
829 
830  std::abort();
831 
832  break;
833  }
834  }
835 
836  return J;
837 }
838 
839 // 3x12 Translation Jacobian in body frame
840 Eigen::Matrix<double, 3, 12> Kinematics::GetTranslationJacobianInB(const LegType &_leg,
841  const BodyType &_body,
842  const Eigen::Matrix<double, 12, 1> &_q_r)
843 {
844  Eigen::Matrix<double, 3, 12> J = Eigen::Matrix<double, 3, 12>::Constant(0); // 3x12 Translation Jacobian
845 
846  int col = 0; // Jacobian placement column index
847  double theta_hy; // Hip yaw angle
848  double theta_hp; // Hip pitch angle
849  double theta_kp; // Knee pitch angle
850 
851  switch (_leg)
852  {
853  case frontLeft:
854  {
855  col = 0;
856  theta_hy = _q_r(0);
857  theta_hp = _q_r(1);
858  theta_kp = _q_r(2);
859 
860  break;
861  }
862  case frontRight:
863  {
864  col = 3;
865  theta_hy = _q_r(3);
866  theta_hp = _q_r(4);
867  theta_kp = _q_r(5);
868 
869  break;
870  }
871  case rearLeft:
872  {
873  col = 6;
874  theta_hy = _q_r(6);
875  theta_hp = _q_r(7);
876  theta_kp = _q_r(8);
877 
878  break;
879  }
880  case rearRight:
881  {
882  col = 9;
883  theta_hy = _q_r(9);
884  theta_hp = _q_r(10);
885  theta_kp = _q_r(11);
886 
887  break;
888  }
889  default:
890  {
891  ROS_ERROR_STREAM("[Kinematics::GetTranslationJacobianInB] Could not determine leg type!");
892 
893  std::abort();
894 
895  break;
896  }
897 
898  }
899 
900  // Place 3x3 Jacobian at desired columns
901  J.block<3,3>(0, col) = this->GetTranslationJacobianInB(_leg,
902  _body,
903  theta_hy,
904  theta_hp,
905  theta_kp);
906 
907  return J;
908 }
909 
910 // 3x18 Translation Jacobian in world frame
911 Eigen::Matrix<double, 3, 18> Kinematics::GetTranslationJacobianInW(const LegType &_leg,
912  const BodyType &_body,
913  const Eigen::Matrix<double, 18, 1> &_q)
914 {
915 
916  Eigen::Matrix<double, 3, 18> J; // 3x18 Translation Jacobian in World frame
917  Eigen::Matrix<double, 3, 12> translationJacobianInB; // 3x12 Translation Jacobian in Body frame
918  Eigen::Matrix<double, 3, 1> positionBaseToBodyInB; // Position vector from base to Body in Body(Base)-frame
919  Eigen::Matrix<double, 3, 3> rotationWToB; // Rotation from World to Body frame (transform from Body to World)
920 
921  rotationWToB = this->GetRotationMatrixWToB(_q(3),
922  _q(4),
923  _q(5));
924 
925  switch (_body)
926  {
927  case base:
928  {
929  translationJacobianInB.setZero();
930  positionBaseToBodyInB.setZero();
931 
932  break;
933  }
934  // Equivalent of case hip || thigh || leg || foot:
935  case hip:
936  case thigh:
937  case leg:
938  case foot:
939  {
940  positionBaseToBodyInB = this->GetPositionBaseToBodyInB(_leg,
941  _body,
942  _q);
943 
944  translationJacobianInB = this->GetTranslationJacobianInB(_leg,
945  _body,
946  _q.bottomRows(12));
947 
948  break;
949  }
950  default:
951  {
952  ROS_ERROR_STREAM("[Kinematics::GetTranslationJacobianInW] Could not determine body type!");
953 
954  std::abort();
955 
956  break;
957  }
958  }
959 
960 
961  J.block<3, 3>(0,0).setIdentity();
962  J.block<3, 3>(0,3) = - rotationWToB * kindr::getSkewMatrixFromVector(positionBaseToBodyInB);
963  J.block<3, 12>(0,6) = rotationWToB * translationJacobianInB;
964 
965  return J;
966 }
967 
968 // 3x18 Translation Jacobian in control frame
969 Eigen::Matrix<double, 3, 18> Kinematics::GetTranslationJacobianInC(const LegType &_leg,
970  const BodyType &_body,
971  const Eigen::Matrix<double, 18, 1> &_q)
972 {
973 
974  Eigen::Matrix<double, 3, 18> J; // 3x18 Translation Jacobian in World frame
975  Eigen::Matrix<double, 3, 12> translationJacobianInB; // 3x12 Translation Jacobian in Body frame
976  Eigen::Matrix<double, 3, 1> positionBaseToBodyInB; // Position vector from base to Body in Body(Base)-frame
977  Eigen::Matrix<double, 3, 3> rotationCToB; // Rotation from World to Control frame (transform from Body to Control)
978 
979  rotationCToB = this->GetRotationMatrixCToB(_q(3),
980  _q(4),
981  0);
982 
983  switch (_body)
984  {
985  case base:
986  {
987  translationJacobianInB.setZero();
988  positionBaseToBodyInB.setZero();
989 
990  break;
991  }
992  // Equivalent of case hip || thigh || leg || foot:
993  case hip:
994  case thigh:
995  case leg:
996  case foot:
997  {
998  positionBaseToBodyInB = this->GetPositionBaseToBodyInB(_leg,
999  _body,
1000  _q);
1001 
1002  translationJacobianInB = this->GetTranslationJacobianInB(_leg,
1003  _body,
1004  _q.bottomRows(12));
1005 
1006  break;
1007  }
1008  default:
1009  {
1010  ROS_ERROR_STREAM("[Kinematics::GetTranslationJacobianInW] Could not determine body type!");
1011 
1012  std::abort();
1013 
1014  break;
1015  }
1016  }
1017 
1018 
1019  J.block<3, 3>(0,0).setIdentity();
1020  J.block<3, 3>(0,3) = - rotationCToB * kindr::getSkewMatrixFromVector(positionBaseToBodyInB);
1021  J.block<3, 12>(0,6) = rotationCToB * translationJacobianInB;
1022 
1023  return J;
1024 }
1025 
1026 // 3x3 Time derivative of the translation Jacobian in body frame
1027 Eigen::Matrix<double, 3, 3> Kinematics::GetTranslationJacobianInBDiff(const LegType &_leg,
1028  const BodyType &_body,
1029  const double &_theta_hy,
1030  const double &_theta_hp,
1031  const double &_theta_kp,
1032  const double &_dot_theta_hy,
1033  const double &_dot_theta_hp,
1034  const double &_dot_theta_kp)
1035 {
1036  Eigen::Matrix<double, 3, 3> dot_J; // 3x3 Time derivative of the translation Jacobian
1037 
1038  double l1; // Length of link 1, if any, zero elsewise
1039  double l2; // Length of link 2, if any, zero elsewise
1040  double l3; // Length of link 3, if any, zero elsewise
1041 
1042  double c1 = std::cos(_theta_hy); // cos(theta1)
1043  double s1 = std::sin(_theta_hy); // sin(theta1)
1044 
1045  double c2 = std::cos(_theta_hp); // cos(theta2)
1046  double s2 = std::sin(_theta_hp); // sin(theta2)
1047 
1048  double c23 = std::cos(_theta_hp + _theta_kp); // cos(theta2 + theta3)
1049  double s23 = std::sin(_theta_hp + _theta_kp); // sin(theta2 + theta3)
1050 
1051  switch (_body)
1052  {
1053  case hip:
1054  {
1055  l1 = this->LC1;
1056  l2 = 0;
1057  l3 = 0;
1058 
1059  break;
1060  }
1061  case thigh:
1062  {
1063  l1 = this->L1;
1064  l2 = this->LC2;
1065  l3 = 0;
1066 
1067  break;
1068  }
1069  case leg:
1070  {
1071  l1 = this->L1;
1072  l2 = this->L2;
1073  l3 = this->LC3;
1074 
1075  break;
1076  }
1077  case foot:
1078  {
1079  l1 = this->L1;
1080  l2 = this->L2;
1081  l3 = this->L3;
1082 
1083  break;
1084  }
1085  default:
1086  {
1087  ROS_ERROR_STREAM("[Kinematics::GetTranslationJacobianInBDiff] Could not determine body type!");
1088 
1089  std::abort();
1090 
1091  break;
1092  }
1093  }
1094 
1095  switch (_leg)
1096  {
1097  // Equivalent of case frontLeft || rearRight:
1098  case frontLeft:
1099  case rearRight:
1100  {
1101  dot_J(0, 0) = ( l2 * s2 * _dot_theta_hp + l3 * s23 * (_dot_theta_hp + _dot_theta_kp) ) * (-s1)
1102  + ( l1 + l2 * c2 + l3 * c23 ) * (-c1 * _dot_theta_hy);
1103  dot_J(1, 0) = ( l2 * s2 * _dot_theta_hp + l3 * s23 * (_dot_theta_hp + _dot_theta_kp) ) * (c1)
1104  + ( l1 + l2 * c2 + l3 * c23 ) * (-s1 * _dot_theta_hy);
1105  dot_J(2, 0) = 0.0;
1106 
1107  dot_J(0, 1) = ( -l2 * c2 * _dot_theta_hp - l3 * c23 * (_dot_theta_hp + _dot_theta_kp) ) * (c1)
1108  + ( -l2 * s2 - l3 * s23 ) * (-s1 * _dot_theta_hy);
1109  dot_J(1, 1) = ( -l2 * c2 * _dot_theta_hp - l3 * c23 * (_dot_theta_hp + _dot_theta_kp) ) * (s1)
1110  + ( -l2 * s2 - l3 * s23 ) * (c1 * _dot_theta_hy);
1111  dot_J(2, 1) = l2 * s2 * _dot_theta_hp + l3 * s23 * (_dot_theta_hp + _dot_theta_kp);
1112 
1113  dot_J(0, 2) = ( -l3 * c23 * (_dot_theta_hp + _dot_theta_kp) ) * (c1)
1114  + ( -l3 * s23 ) * (-s1 * _dot_theta_hy);
1115  dot_J(1, 2) = ( -l3 * c23 * (_dot_theta_hp + _dot_theta_kp) ) * (s1)
1116  + ( -l3 * s23 ) * (c1 * _dot_theta_hy);
1117  dot_J(2, 2) = l3 * s23 * (_dot_theta_hp + _dot_theta_kp);
1118 
1119  break;
1120  }
1121  // Equivalent of case frontRight || rearLeft:
1122  case frontRight:
1123  case rearLeft:
1124  {
1125  dot_J(0, 0) = ( l2 * s2 * _dot_theta_hp + l3 * s23 * (_dot_theta_hp + _dot_theta_kp) ) * (-s1)
1126  + ( l1 + l2 * c2 + l3 * c23 ) * (-c1 * _dot_theta_hy);
1127  dot_J(1, 0) = ( l2 * s2 * _dot_theta_hp + l3 * s23 * (_dot_theta_hp + _dot_theta_kp) ) * (c1)
1128  + ( l1 + l2 * c2 + l3 * c23 ) * (-s1 * _dot_theta_hy);
1129  dot_J(2, 0) = 0.0;
1130 
1131  dot_J(0, 1) = ( -l2 * c2 * _dot_theta_hp - l3 * c23 * (_dot_theta_hp + _dot_theta_kp) ) * (c1)
1132  + ( -l2 * s2 - l3 * s23 ) * (-s1 * _dot_theta_hy);
1133  dot_J(1, 1) = ( -l2 * c2 * _dot_theta_hp - l3 * c23 * (_dot_theta_hp + _dot_theta_kp) ) * (s1)
1134  + ( -l2 * s2 - l3 * s23 ) * (c1 * _dot_theta_hy);
1135  dot_J(2, 1) = - l2 * s2 * _dot_theta_hp - l3 * s23 * (_dot_theta_hp + _dot_theta_kp);
1136 
1137  dot_J(0, 2) = ( -l3 * c23 * (_dot_theta_hp + _dot_theta_kp) ) * (c1)
1138  + ( -l3 * s23 ) * (-s1 * _dot_theta_hy);
1139  dot_J(1, 2) = ( -l3 * c23 * (_dot_theta_hp + _dot_theta_kp) ) * (s1)
1140  + ( -l3 * s23 ) * (c1 * _dot_theta_hy);
1141  dot_J(2, 2) = - l3 * s23 * (_dot_theta_hp + _dot_theta_kp);
1142 
1143  break;
1144  }
1145  default:
1146  {
1147  ROS_ERROR_STREAM("[Kinematics::GetTranslationJacobianInBDiff] Could not determine leg type!");
1148 
1149  std::abort();
1150 
1151  break;
1152  }
1153  }
1154 
1155  return dot_J;
1156 }
1157 
1158 // 3x12 Time derivative of the translation Jacobian in body frame
1159 Eigen::Matrix<double, 3, 12> Kinematics::GetTranslationJacobianInBDiff(const LegType &_leg,
1160  const BodyType &_body,
1161  const Eigen::Matrix<double, 12, 1> &_q_r,
1162  const Eigen::Matrix<double, 12, 1> &_dot_q_r)
1163 {
1164  Eigen::Matrix<double, 3, 12> dot_J = Eigen::Matrix<double, 3, 12>::Constant(0); // 3x12 Time derivative of the translation Jacobian
1165 
1166  int col = 0; // Jacobian placement column index
1167 
1168  double theta_hy; // Hip yaw angle
1169  double theta_hp; // Hip pitch angle
1170  double theta_kp; // Knee pitch angle
1171  double dot_theta_hy; // Hip yaw angle
1172  double dot_theta_hp; // Hip pitch angle
1173  double dot_theta_kp; // Knee pitch angle
1174 
1175  switch (_leg)
1176  {
1177  case frontLeft:
1178  {
1179  col = 0;
1180  theta_hy = _q_r(0);
1181  theta_hp = _q_r(1);
1182  theta_kp = _q_r(2);
1183 
1184  dot_theta_hy = _dot_q_r(0);
1185  dot_theta_hp = _dot_q_r(1);
1186  dot_theta_kp = _dot_q_r(2);
1187 
1188  break;
1189  }
1190  case frontRight:
1191  {
1192  col = 3;
1193  theta_hy = _q_r(3);
1194  theta_hp = _q_r(4);
1195  theta_kp = _q_r(5);
1196 
1197  dot_theta_hy = _dot_q_r(3);
1198  dot_theta_hp = _dot_q_r(4);
1199  dot_theta_kp = _dot_q_r(5);
1200 
1201  break;
1202  }
1203  case rearLeft:
1204  {
1205  col = 6;
1206  theta_hy = _q_r(6);
1207  theta_hp = _q_r(7);
1208  theta_kp = _q_r(8);
1209 
1210  dot_theta_hy = _dot_q_r(6);
1211  dot_theta_hp = _dot_q_r(7);
1212  dot_theta_kp = _dot_q_r(8);
1213 
1214  break;
1215  }
1216  case rearRight:
1217  {
1218  col = 9;
1219  theta_hy = _q_r(9);
1220  theta_hp = _q_r(10);
1221  theta_kp = _q_r(11);
1222 
1223  dot_theta_hy = _dot_q_r(9);
1224  dot_theta_hp = _dot_q_r(10);
1225  dot_theta_kp = _dot_q_r(11);
1226 
1227  break;
1228  }
1229  default:
1230  {
1231  ROS_ERROR_STREAM("[Kinematics::GetTranslationJacobianInBDiff] Could not determine leg type!");
1232 
1233  std::abort();
1234 
1235  break;
1236  }
1237 
1238  }
1239 
1240  // Place 3x3 Jacobian at desired columns
1241  dot_J.block<3,3>(0, col) = this->GetTranslationJacobianInBDiff(_leg,
1242  _body,
1243  theta_hy,
1244  theta_hp,
1245  theta_kp,
1246  dot_theta_hy,
1247  dot_theta_hp,
1248  dot_theta_kp);
1249 
1250  return dot_J;
1251 }
1252 
1253 // 3x18 Time derivative of the translation Jacobian in world frame
1254 Eigen::Matrix<double, 3, 18> Kinematics::GetTranslationJacobianInWDiff(const LegType &_leg,
1255  const BodyType &_body,
1256  const Eigen::Matrix<double, 18, 1> &_q,
1257  const Eigen::Matrix<double, 18, 1> &_u)
1258 {
1259 
1260  Eigen::Matrix<double, 3, 18> dot_J; // 3x18 Time derivative of the translation Jacobian in World frame
1261  Eigen::Matrix<double, 3, 12> translationJacobianInB; // 3x12 Translation Jacobian in Body frame
1262  Eigen::Matrix<double, 3, 12> translationJacobianInBDiff; // 3x12 Time derivative of the translation Jacobian in Body frame
1263  Eigen::Matrix<double, 3, 1> positionBaseToBodyInB; // Position vector from base to Body in Body(Base)-frame
1264  Eigen::Matrix<double, 3, 1> positionBaseToBodyInBDiff; // Time derivative of the position vector from base to Body in Body(Base)-frame
1265  Eigen::Matrix<double, 3, 3> rotationWToB; // Rotation from World to Body frame (transform from Body to World)
1266  Eigen::Matrix<double, 3, 3> rotationWToBDiff; // Time derivative of the rotation from World to Body frame
1267 
1268 
1269  switch (_body)
1270  {
1271  case base:
1272  {
1273  translationJacobianInB.setZero();
1274  positionBaseToBodyInB.setZero();
1275  rotationWToB.setZero();
1276  rotationWToBDiff.setZero();
1277 
1278  break;
1279  }
1280  // Equivalent of case hip || thigh || leg || foot:
1281  case hip:
1282  case thigh:
1283  case leg:
1284  case foot:
1285  {
1286  translationJacobianInB = this->GetTranslationJacobianInB(_leg,
1287  _body,
1288  _q.bottomRows(12));
1289 
1290  translationJacobianInBDiff = this->GetTranslationJacobianInBDiff(_leg,
1291  _body,
1292  _q.bottomRows(12),
1293  _u.bottomRows(12));
1294 
1295  positionBaseToBodyInB = this->GetPositionBaseToBodyInB(_leg,
1296  _body,
1297  _q);
1298 
1299  positionBaseToBodyInBDiff = translationJacobianInB * _q.bottomRows(12);
1300 
1301  rotationWToB = this->GetRotationMatrixWToB(_q(3),
1302  _q(4),
1303  _q(5));
1304 
1305  rotationWToBDiff = this->GetRotationMatrixWToBDiff(_q(3),
1306  _q(4),
1307  _q(5),
1308  _u(3),
1309  _u(4),
1310  _u(5));
1311 
1312  break;
1313  }
1314  default:
1315  {
1316  ROS_ERROR_STREAM("[Kinematics::GetTranslationJacobianInWDiff] Could not determine body type!");
1317 
1318  std::abort();
1319 
1320  break;
1321  }
1322  }
1323 
1324 
1325  dot_J.block<3, 3>(0,0).setZero();
1326 
1327  dot_J.block<3, 3>(0,3) = - rotationWToBDiff * kindr::getSkewMatrixFromVector(positionBaseToBodyInB)
1328  - rotationWToB * kindr::getSkewMatrixFromVector(positionBaseToBodyInBDiff);
1329 
1330  dot_J.block<3, 12>(0,6) = rotationWToBDiff * translationJacobianInB
1331  + rotationWToB * translationJacobianInBDiff;
1332 
1333  return dot_J;
1334 }
1335 
1336 // 3x18 Time derivative of the translation Jacobian in control frame
1337 Eigen::Matrix<double, 3, 18> Kinematics::GetTranslationJacobianInCDiff(const LegType &_leg,
1338  const BodyType &_body,
1339  const Eigen::Matrix<double, 18, 1> &_q,
1340  const Eigen::Matrix<double, 18, 1> &_u)
1341 {
1342 
1343  Eigen::Matrix<double, 3, 18> dot_J; // 3x18 Time derivative of the translation Jacobian in Control frame
1344  Eigen::Matrix<double, 3, 12> translationJacobianInB; // 3x12 Translation Jacobian in Body frame
1345  Eigen::Matrix<double, 3, 12> translationJacobianInBDiff; // 3x12 Time derivative of the translation Jacobian in Body frame
1346  Eigen::Matrix<double, 3, 1> positionBaseToBodyInB; // Position vector from base to Body in Body(Base)-frame
1347  Eigen::Matrix<double, 3, 1> positionBaseToBodyInBDiff; // Time derivative of the position vector from base to Body in Body(Base)-frame
1348  Eigen::Matrix<double, 3, 3> rotationCToB; // Rotation from Control to Body frame (transform from Body to Control)
1349  Eigen::Matrix<double, 3, 3> rotationCToBDiff; // Time derivative of the rotation from Control to Body frame
1350 
1351 
1352  switch (_body)
1353  {
1354  case base:
1355  {
1356  translationJacobianInB.setZero();
1357  positionBaseToBodyInB.setZero();
1358  rotationCToB.setZero();
1359  rotationCToBDiff.setZero();
1360 
1361  break;
1362  }
1363  // Equivalent of case hip || thigh || leg || foot:
1364  case hip:
1365  case thigh:
1366  case leg:
1367  case foot:
1368  {
1369  translationJacobianInB = this->GetTranslationJacobianInB(_leg,
1370  _body,
1371  _q.bottomRows(12));
1372 
1373  translationJacobianInBDiff = this->GetTranslationJacobianInBDiff(_leg,
1374  _body,
1375  _q.bottomRows(12),
1376  _u.bottomRows(12));
1377 
1378  positionBaseToBodyInB = this->GetPositionBaseToBodyInB(_leg,
1379  _body,
1380  _q);
1381 
1382  positionBaseToBodyInBDiff = translationJacobianInB * _q.bottomRows(12);
1383 
1384  rotationCToB = this->GetRotationMatrixCToB(_q(3),
1385  _q(4),
1386  0);
1387 
1388  rotationCToBDiff = this->GetRotationMatrixCToBDiff(_q(3),
1389  _q(4),
1390  0,
1391  _u(3),
1392  _u(4),
1393  0);
1394 
1395  break;
1396  }
1397  default:
1398  {
1399  ROS_ERROR_STREAM("[Kinematics::GetTranslationJacobianInCDiff] Could not determine body type!");
1400 
1401  std::abort();
1402 
1403  break;
1404  }
1405  }
1406 
1407 
1408  dot_J.block<3, 3>(0,0).setZero();
1409 
1410  dot_J.block<3, 3>(0,3) = - rotationCToBDiff * kindr::getSkewMatrixFromVector(positionBaseToBodyInB)
1411  - rotationCToB * kindr::getSkewMatrixFromVector(positionBaseToBodyInBDiff);
1412 
1413  dot_J.block<3, 12>(0,6) = rotationCToBDiff * translationJacobianInB
1414  + rotationCToB * translationJacobianInBDiff;
1415 
1416  return dot_J;
1417 }
1418 
1419 // 3x3 Rotational Jacobian in body frame
1420 Eigen::Matrix<double, 3, 3> Kinematics::GetRotationJacobianInB(const LegType &_leg,
1421  const BodyType &_body,
1422  const double &_theta_hy,
1423  const double &_theta_hp,
1424  const double &_theta_kp)
1425 {
1426  Eigen::Matrix<double, 3, 3> J = Eigen::Matrix<double, 3, 3>::Constant(0); // 3x3 Rotational Jacobian
1427 
1428  double c1 = std::cos(_theta_hy); // cos(theta1)
1429  double s1 = std::sin(_theta_hy); // sin(theta1)
1430 
1431  switch (_leg)
1432  {
1433  // Equivalent of case frontLeft || rearRight:
1434  case frontLeft:
1435  case rearRight:
1436  {
1437  switch (_body)
1438  {
1439  case hip:
1440  {
1441  J(2,0) = 1;
1442 
1443  break;
1444  }
1445  case thigh:
1446  {
1447  J(2,0) = 1;
1448 
1449  J(0,1) = - s1;
1450  J(1,1) = c1;
1451 
1452  break;
1453  }
1454  // Equivalent of case leg || foot:
1455  case leg:
1456  case foot:
1457  {
1458  J(2,0) = 1;
1459 
1460  J(0,1) = - s1;
1461  J(1,1) = c1;
1462 
1463  J(0,2) = - s1;
1464  J(1,2) = c1;
1465 
1466  break;
1467  }
1468  default:
1469  {
1470  ROS_ERROR_STREAM("[Kinematics::GetRotationJacobianInB] Could not determine body type!");
1471 
1472  std::abort();
1473 
1474  break;
1475  }
1476  }
1477 
1478  break;
1479  }
1480  // Equivalent of case frontRight || rearLeft:
1481  case frontRight:
1482  case rearLeft:
1483  {
1484  switch (_body)
1485  {
1486  case hip:
1487  {
1488  J(2,0) = 1;
1489 
1490  break;
1491  }
1492  case thigh:
1493  {
1494  J(2,0) = 1;
1495 
1496  J(0,1) = s1;
1497  J(1,1) = - c1;
1498 
1499  break;
1500  }
1501  // Equivalent of case leg || foot:
1502  case leg:
1503  case foot:
1504  {
1505  J(2,0) = 1;
1506 
1507  J(0,1) = s1;
1508  J(1,1) = - c1;
1509 
1510  J(0,2) = s1;
1511  J(1,2) = - c1;
1512 
1513  break;
1514  }
1515  default:
1516  {
1517  ROS_ERROR_STREAM("[Kinematics::GetRotationJacobianInB] Could not determine body type!");
1518 
1519  std::abort();
1520 
1521  break;
1522  }
1523  }
1524  break;
1525  }
1526  default:
1527  {
1528  ROS_ERROR_STREAM("[Kinematics::GetRotationJacobianInB] Could not determine leg type!");
1529 
1530  std::abort();
1531 
1532  break;
1533  }
1534  }
1535 
1536  return J;
1537 }
1538 
1539 // 3x12 Rotational Jacobian in body frame
1540 Eigen::Matrix<double, 3, 12> Kinematics::GetRotationJacobianInB(const LegType &_leg,
1541  const BodyType &_body,
1542  const Eigen::Matrix<double, 12, 1> &_q_r)
1543 {
1544  Eigen::Matrix<double, 3, 12> J = Eigen::Matrix<double, 3, 12>::Constant(0); // 3x12 Rotation Jacobian
1545 
1546  int col = 0; // Jacobian placement column index
1547  double theta_hy; // Hip yaw angle
1548  double theta_hp; // Hip pitch angle
1549  double theta_kp; // Knee pitch angle
1550 
1551  switch (_leg)
1552  {
1553  case frontLeft:
1554  {
1555  col = 0;
1556  theta_hy = _q_r(0);
1557  theta_hp = _q_r(1);
1558  theta_kp = _q_r(2);
1559 
1560  break;
1561  }
1562  case frontRight:
1563  {
1564  col = 3;
1565  theta_hy = _q_r(3);
1566  theta_hp = _q_r(4);
1567  theta_kp = _q_r(5);
1568 
1569  break;
1570  }
1571  case rearLeft:
1572  {
1573  col = 6;
1574  theta_hy = _q_r(6);
1575  theta_hp = _q_r(7);
1576  theta_kp = _q_r(8);
1577 
1578  break;
1579  }
1580  case rearRight:
1581  {
1582  col = 9;
1583  theta_hy = _q_r(9);
1584  theta_hp = _q_r(10);
1585  theta_kp = _q_r(11);
1586 
1587  break;
1588  }
1589  default:
1590  {
1591  ROS_ERROR_STREAM("[Kinematics::GetRotationJacobianInB] Could not determine leg type!");
1592 
1593  std::abort();
1594 
1595  break;
1596  }
1597 
1598  }
1599 
1600  // Place 3x3 Jacobian at desired columns
1601  J.block<3,3>(0, col) = this->GetRotationJacobianInB(_leg,
1602  _body,
1603  theta_hy,
1604  theta_hp,
1605  theta_kp);
1606 
1607  return J;
1608 }
1609 
1610 // 3x18 Rotational Jacobian in world frame
1611 Eigen::Matrix<double, 3, 18> Kinematics::GetRotationJacobianInW(const LegType &_leg,
1612  const BodyType &_body,
1613  const Eigen::Matrix<double, 18, 1> &_q)
1614 {
1615  Eigen::Matrix<double, 3, 18> J; // 3x18 Rotation Jacobian in World frame
1616  Eigen::Matrix<double, 3, 12> rotationJacobianInB; // 3x12 Rotation Jacobian in Body frame
1617  Eigen::Matrix<double, 3, 3> rotationWToB; // Rotation from World to Body frame (transform from Body to World)
1618 
1619  rotationWToB = this->GetRotationMatrixWToB(_q(3),
1620  _q(4),
1621  _q(5));
1622 
1623  switch (_body)
1624  {
1625  case base:
1626  {
1627  rotationJacobianInB.setZero();
1628 
1629  break;
1630  }
1631  // Equivalent of case hip || thigh || leg || foot:
1632  case hip:
1633  case thigh:
1634  case leg:
1635  case foot:
1636  {
1637  rotationJacobianInB = this->GetRotationJacobianInB(_leg,
1638  _body,
1639  _q.bottomRows(12));
1640 
1641  break;
1642  }
1643  default:
1644  {
1645  ROS_ERROR_STREAM("[Kinematics::GetRotationJacobianInW] Could not determine body type!");
1646 
1647  std::abort();
1648 
1649  break;
1650  }
1651  }
1652 
1653  J.block<3, 3>(0,0).setZero();
1654  J.block<3, 3>(0,3) = rotationWToB;
1655  J.block<3, 12>(0,6) = rotationWToB * rotationJacobianInB;
1656 
1657  return J;
1658 }
1659 
1660 // 3x18 Rotational Jacobian in control frame
1661 Eigen::Matrix<double, 3, 18> Kinematics::GetRotationJacobianInC(const LegType &_leg,
1662  const BodyType &_body,
1663  const Eigen::Matrix<double, 18, 1> &_q)
1664 {
1665  Eigen::Matrix<double, 3, 18> J; // 3x18 Rotation Jacobian in World frame
1666  Eigen::Matrix<double, 3, 12> rotationJacobianInB; // 3x12 Rotation Jacobian in Body frame
1667  Eigen::Matrix<double, 3, 3> rotationCToB; // Rotation from Control to Body frame (transform from Body to Control)
1668 
1669  rotationCToB = this->GetRotationMatrixWToB(_q(3),
1670  _q(4),
1671  0);
1672 
1673  switch (_body)
1674  {
1675  case base:
1676  {
1677  rotationJacobianInB.setZero();
1678 
1679  break;
1680  }
1681  // Equivalent of case hip || thigh || leg || foot:
1682  case hip:
1683  case thigh:
1684  case leg:
1685  case foot:
1686  {
1687  rotationJacobianInB = this->GetRotationJacobianInB(_leg,
1688  _body,
1689  _q.bottomRows(12));
1690 
1691  break;
1692  }
1693  default:
1694  {
1695  ROS_ERROR_STREAM("[Kinematics::GetRotationJacobianInW] Could not determine body type!");
1696 
1697  std::abort();
1698 
1699  break;
1700  }
1701  }
1702 
1703  J.block<3, 3>(0,0).setZero();
1704  J.block<3, 3>(0,3) = rotationCToB;
1705  J.block<3, 12>(0,6) = rotationCToB * rotationJacobianInB;
1706 
1707  return J;
1708 }
1709 
1710 // 3x3 Time derivative of the rotational Jacobian in body frame
1711 Eigen::Matrix<double, 3, 3> Kinematics::GetRotationJacobianInBDiff(const LegType &_leg,
1712  const BodyType &_body,
1713  const double &_theta_hy,
1714  const double &_theta_hp,
1715  const double &_theta_kp,
1716  const double &_dot_theta_hy,
1717  const double &_dot_theta_hp,
1718  const double &_dot_theta_kp)
1719 {
1720  Eigen::Matrix<double, 3, 3> dot_J = Eigen::Matrix<double, 3, 3>::Constant(0); // 3x3 Time derivative of the rotational Jacobian
1721 
1722  double c1 = std::cos(_theta_hy); // cos(theta1)
1723  double s1 = std::sin(_theta_hy); // sin(theta1)
1724 
1725  switch (_leg)
1726  {
1727  // Equivalent of case frontLeft || rearRight:
1728  case frontLeft:
1729  case rearRight:
1730  {
1731  switch (_body)
1732  {
1733  case hip:
1734  {
1735  break;
1736  }
1737  case thigh:
1738  {
1739  dot_J(0,1) = - c1 * _dot_theta_hy;
1740  dot_J(1,1) = - s1 * _dot_theta_hy;
1741 
1742  break;
1743  }
1744  // Equivalent of case leg || foot:
1745  case leg:
1746  case foot:
1747  {
1748  dot_J(0,1) = - c1 * _dot_theta_hy;
1749  dot_J(1,1) = - s1 * _dot_theta_hy;
1750 
1751  dot_J(0,2) = - c1 * _dot_theta_hy;
1752  dot_J(1,2) = - s1 * _dot_theta_hy;
1753 
1754  break;
1755  }
1756  default:
1757  {
1758  ROS_ERROR_STREAM("[Kinematics::GetRotationJacobianInBDiff] Could not determine body type!");
1759 
1760  std::abort();
1761 
1762  break;
1763  }
1764  }
1765 
1766  break;
1767  }
1768  // Equivalent of case frontRight || rearLeft:
1769  case frontRight:
1770  case rearLeft:
1771  {
1772  switch (_body)
1773  {
1774  case hip:
1775  {
1776  break;
1777  }
1778  case thigh:
1779  {
1780  dot_J(0,1) = c1 * _dot_theta_hy;
1781  dot_J(1,1) = s1 * _dot_theta_hy;
1782 
1783  break;
1784  }
1785  // Equivalent of case leg || foot:
1786  case leg:
1787  case foot:
1788  {
1789  dot_J(0,1) = c1 * _dot_theta_hy;
1790  dot_J(1,1) = s1 * _dot_theta_hy;
1791 
1792  dot_J(0,2) = c1 * _dot_theta_hy;
1793  dot_J(1,2) = s1 * _dot_theta_hy;
1794 
1795  break;
1796  }
1797  default:
1798  {
1799  ROS_ERROR_STREAM("[Kinematics::GetRotationJacobianInBDiff] Could not determine body type!");
1800 
1801  std::abort();
1802 
1803  break;
1804  }
1805  }
1806  break;
1807  }
1808  default:
1809  {
1810  ROS_ERROR_STREAM("[Kinematics::GetRotationJacobianInBDiff] Could not determine leg type!");
1811 
1812  std::abort();
1813 
1814  break;
1815  }
1816  }
1817 
1818  return dot_J;
1819 }
1820 
1821 // 3x12 Time derivative of the rotation Jacobian in body frame
1822 Eigen::Matrix<double, 3, 12> Kinematics::GetRotationJacobianInBDiff(const LegType &_leg,
1823  const BodyType &_body,
1824  const Eigen::Matrix<double, 12, 1> &_q_r,
1825  const Eigen::Matrix<double, 12, 1> &_dot_q_r)
1826 {
1827  Eigen::Matrix<double, 3, 12> dot_J = Eigen::Matrix<double, 3, 12>::Constant(0); // 3x12 Time derivative of the rotation Jacobian
1828 
1829  int col = 0; // Jacobian placement column index
1830 
1831  double theta_hy; // Hip yaw angle
1832  double theta_hp; // Hip pitch angle
1833  double theta_kp; // Knee pitch angle
1834  double dot_theta_hy; // Hip yaw angle
1835  double dot_theta_hp; // Hip pitch angle
1836  double dot_theta_kp; // Knee pitch angle
1837 
1838  switch (_leg)
1839  {
1840  case frontLeft:
1841  {
1842  col = 0;
1843  theta_hy = _q_r(0);
1844  theta_hp = _q_r(1);
1845  theta_kp = _q_r(2);
1846 
1847  dot_theta_hy = _dot_q_r(0);
1848  dot_theta_hp = _dot_q_r(1);
1849  dot_theta_kp = _dot_q_r(2);
1850 
1851  break;
1852  }
1853  case frontRight:
1854  {
1855  col = 3;
1856  theta_hy = _q_r(3);
1857  theta_hp = _q_r(4);
1858  theta_kp = _q_r(5);
1859 
1860  dot_theta_hy = _dot_q_r(3);
1861  dot_theta_hp = _dot_q_r(4);
1862  dot_theta_kp = _dot_q_r(5);
1863 
1864  break;
1865  }
1866  case rearLeft:
1867  {
1868  col = 6;
1869  theta_hy = _q_r(6);
1870  theta_hp = _q_r(7);
1871  theta_kp = _q_r(8);
1872 
1873  dot_theta_hy = _dot_q_r(6);
1874  dot_theta_hp = _dot_q_r(7);
1875  dot_theta_kp = _dot_q_r(8);
1876 
1877  break;
1878  }
1879  case rearRight:
1880  {
1881  col = 9;
1882  theta_hy = _q_r(9);
1883  theta_hp = _q_r(10);
1884  theta_kp = _q_r(11);
1885 
1886  dot_theta_hy = _dot_q_r(9);
1887  dot_theta_hp = _dot_q_r(10);
1888  dot_theta_kp = _dot_q_r(11);
1889 
1890  break;
1891  }
1892  default:
1893  {
1894  ROS_ERROR_STREAM("[Kinematics::GetRotationJacobianInBDiff] Could not determine leg type!");
1895 
1896  std::abort();
1897 
1898  break;
1899  }
1900 
1901  }
1902 
1903  // Place 3x3 Jacobian at desired columns
1904  dot_J.block<3,3>(0, col) = this->GetRotationJacobianInBDiff(_leg,
1905  _body,
1906  theta_hy,
1907  theta_hp,
1908  theta_kp,
1909  dot_theta_hy,
1910  dot_theta_hp,
1911  dot_theta_kp);
1912 
1913  return dot_J;
1914 }
1915 
1916 // 3x18 Time derivative of the rotational Jacobian in world frame
1917 Eigen::Matrix<double, 3, 18> Kinematics::GetRotationJacobianInWDiff(const LegType &_leg,
1918  const BodyType &_body,
1919  const Eigen::Matrix<double, 18, 1> &_q,
1920  const Eigen::Matrix<double, 18, 1> &_u)
1921 {
1922  Eigen::Matrix<double, 3, 18> dot_J; // 3x18 Time derivative of the rotation Jacobian in World frame
1923  Eigen::Matrix<double, 3, 12> rotationJacobianInB; // 3x12 Rotation Jacobian in Body frame
1924  Eigen::Matrix<double, 3, 12> rotationJacobianInBDiff; // 3x12 Time derivative of the rotation Jacobian in Body frame
1925  Eigen::Matrix<double, 3, 3> rotationWToB; // Rotation from World to Body frame (transform from Body to World)
1926  Eigen::Matrix<double, 3, 3> rotationWToBDiff; // Time derivative of the rotation from World to Body frame
1927 
1928  switch (_body)
1929  {
1930  case base:
1931  {
1932  rotationJacobianInB.setZero();
1933  rotationJacobianInBDiff.setZero();
1934  rotationWToB.setZero();
1935 
1936  rotationWToBDiff = this->GetRotationMatrixWToBDiff(_q(3),
1937  _q(4),
1938  _q(5),
1939  _u(3),
1940  _u(4),
1941  _u(5));
1942 
1943  break;
1944  }
1945  // Equivalent of case hip || thigh || leg || foot:
1946  case hip:
1947  case thigh:
1948  case leg:
1949  case foot:
1950  {
1951  rotationJacobianInB = this->GetRotationJacobianInB(_leg,
1952  _body,
1953  _q.block<12, 1>(6,0));
1954 
1955  rotationJacobianInBDiff = this->GetRotationJacobianInBDiff(_leg,
1956  _body,
1957  _q.block<12, 1>(6,0),
1958  _u.block<12, 1>(6,0));
1959 
1960  rotationWToB = this->GetRotationMatrixWToB(_q(3),
1961  _q(4),
1962  _q(5));
1963 
1964  rotationWToBDiff = this->GetRotationMatrixWToBDiff(_q(3),
1965  _q(4),
1966  _q(5),
1967  _u(3),
1968  _u(4),
1969  _u(5));
1970 
1971  break;
1972  }
1973  default:
1974  {
1975  ROS_ERROR_STREAM("[Kinematics::GetRotationJacobianInWDiff] Could not determine body type!");
1976 
1977  std::abort();
1978 
1979  break;
1980  }
1981  }
1982 
1983  dot_J.block<3, 3>(0,0).setZero();
1984  dot_J.block<3, 3>(0,3) = rotationWToBDiff;
1985  dot_J.block<3, 12>(0,6) = rotationWToBDiff * rotationJacobianInB + rotationWToB * rotationJacobianInBDiff;
1986 
1987  return dot_J;
1988 }
1989 
1990 // 3x18 Time derivative of the rotational Jacobian in control frame
1991 Eigen::Matrix<double, 3, 18> Kinematics::GetRotationJacobianInCDiff(const LegType &_leg,
1992  const BodyType &_body,
1993  const Eigen::Matrix<double, 18, 1> &_q,
1994  const Eigen::Matrix<double, 18, 1> &_u)
1995 {
1996  Eigen::Matrix<double, 3, 18> dot_J; // 3x18 Time derivative of the rotation Jacobian in Control frame
1997  Eigen::Matrix<double, 3, 12> rotationJacobianInB; // 3x12 Rotation Jacobian in Body frame
1998  Eigen::Matrix<double, 3, 12> rotationJacobianInBDiff; // 3x12 Time derivative of the rotation Jacobian in Body frame
1999  Eigen::Matrix<double, 3, 3> rotationCToB; // Rotation from Control to Body frame (transform from Body to Control)
2000  Eigen::Matrix<double, 3, 3> rotationCToBDiff; // Time derivative of the rotation from Control to Body frame
2001 
2002  switch (_body)
2003  {
2004  case base:
2005  {
2006  rotationJacobianInB.setZero();
2007  rotationJacobianInBDiff.setZero();
2008  rotationCToB.setZero();
2009 
2010  rotationCToBDiff = this->GetRotationMatrixWToBDiff(_q(3),
2011  _q(4),
2012  0,
2013  _u(3),
2014  _u(4),
2015  0);
2016 
2017  break;
2018  }
2019  // Equivalent of case hip || thigh || leg || foot:
2020  case hip:
2021  case thigh:
2022  case leg:
2023  case foot:
2024  {
2025  rotationJacobianInB = this->GetRotationJacobianInB(_leg,
2026  _body,
2027  _q.block<12, 1>(6,0));
2028 
2029  rotationJacobianInBDiff = this->GetRotationJacobianInBDiff(_leg,
2030  _body,
2031  _q.block<12, 1>(6,0),
2032  _u.block<12, 1>(6,0));
2033 
2034  rotationCToB = this->GetRotationMatrixCToB(_q(3),
2035  _q(4),
2036  0);
2037 
2038  rotationCToBDiff = this->GetRotationMatrixCToBDiff(_q(3),
2039  _q(4),
2040  0,
2041  _u(3),
2042  _u(4),
2043  0);
2044 
2045  break;
2046  }
2047  default:
2048  {
2049  ROS_ERROR_STREAM("[Kinematics::GetRotationJacobianInCDiff] Could not determine body type!");
2050 
2051  std::abort();
2052 
2053  break;
2054  }
2055  }
2056 
2057  dot_J.block<3, 3>(0,0).setZero();
2058  dot_J.block<3, 3>(0,3) = rotationCToBDiff;
2059  dot_J.block<3, 12>(0,6) = rotationCToBDiff * rotationJacobianInB + rotationCToB * rotationJacobianInBDiff;
2060 
2061  return dot_J;
2062 }
2063 
2064 // 6x18 Spatial Jacobian in world frame
2065 Eigen::Matrix<double, 6, 18> Kinematics::GetJacobianInW(const LegType &_leg,
2066  const BodyType &_body,
2067  const Eigen::Matrix<double, 18, 1> &_q)
2068 {
2069  Eigen::Matrix<double, 6, 18> J; // 6x18 Spatial Jacobian in World frame
2070 
2071  J.block<3, 18>(0,0) = this->GetTranslationJacobianInW(_leg,
2072  _body,
2073  _q);
2074 
2075  J.block<3, 18>(3,0) = this->GetRotationJacobianInW(_leg,
2076  _body,
2077  _q);
2078 
2079  return J;
2080 }
2081 
2082 // 6x18 Time derivative of the spatial Jacobian in world frame
2083 Eigen::Matrix<double, 6, 18> Kinematics::GetJacobianInWDiff(const LegType &_leg,
2084  const BodyType &_body,
2085  const Eigen::Matrix<double, 18, 1> &_q,
2086  const Eigen::Matrix<double, 18, 1> &_u)
2087 {
2088  Eigen::Matrix<double, 6, 18> dot_J; // 6x18 Spatial Jacobian in World frame
2089 
2090  dot_J.block<3, 18>(0,0) = this->GetTranslationJacobianInWDiff(_leg,
2091  _body,
2092  _q,
2093  _u);
2094 
2095  dot_J.block<3, 18>(3,0) = this->GetRotationJacobianInWDiff(_leg,
2096  _body,
2097  _q,
2098  _u);
2099 
2100  return dot_J;
2101 }
2102 
2103 // (3*n_c)x18 Contact Support Jacobian in world frame
2104 Eigen::Matrix<double, Eigen::Dynamic, 18> Kinematics::GetContactJacobianInW(std::vector<LegType> &_legs,
2105  const Eigen::Matrix<double, 18, 1> &_q)
2106 {
2107  Eigen::MatrixXd J; // Contact Support Jacobian
2108 
2109  const unsigned int n_c = _legs.size(); // Number of contact points
2110 
2111  // Validate number of contact points
2112  if (n_c == 0)
2113  {
2114  ROS_ERROR_STREAM("[Kinematics::GetContactJacobianInW] Zero contact points were given!");
2115 
2116  J.setZero();
2117 
2118  return J;
2119 
2120  std::abort();
2121  }
2122  else if (n_c > 4)
2123  {
2124 
2125  ROS_ERROR_STREAM("[Kinematics::GetContactJacobianInW] More than four contact points were given!");
2126 
2127  std::abort();
2128  }
2129 
2130  // Set Jacobian dimensions
2131  J.resize(3*n_c, 18);
2132 
2133  // Default Jacobian to zero
2134  J.setZero();
2135 
2136  // Sort vector of legs to ensure correct filling of partial Jacobians
2137  std::sort(_legs.begin(), _legs.end());
2138 
2139  // Index to track which index the partial Jacobian
2140  // should be filled into in the complete Jacobian.
2141  int row = 0;
2142 
2143  for (LegType leg : _legs)
2144  {
2145  // Fill Jacobian
2146  J.block<3,18>(row,0) = this->GetTranslationJacobianInW(leg, BodyType::foot, _q);
2147 
2148  // Increment
2149  row += 3;
2150  }
2151 
2152  return J;
2153 }
2154 
2155 // (3*n_c)x18 Time derivative of the Contact Support Jacobian in world frame
2156 Eigen::Matrix<double, Eigen::Dynamic, 18> Kinematics::GetContactJacobianInWDiff(std::vector<LegType> &_legs,
2157  const Eigen::Matrix<double, 18, 1> &_q,
2158  const Eigen::Matrix<double, 18, 1> &_u)
2159 {
2160  Eigen::MatrixXd dot_J; // Time derivative of the Contact Support Jacobian
2161 
2162  const unsigned int n_c = _legs.size(); // Number of contact points
2163 
2164  // Validate number of contact points
2165  if (n_c == 0)
2166  {
2167  ROS_ERROR_STREAM("[Kinematics::GetContactJacobianInWDiff] Zero contact points were given!");
2168 
2169  dot_J.setZero();
2170 
2171  return dot_J;
2172 
2173  std::abort();
2174  }
2175  else if (n_c > 4)
2176  {
2177 
2178  ROS_ERROR_STREAM("[Kinematics::GetContactJacobianInWDiff] More than four contact points were given!");
2179 
2180  std::abort();
2181  }
2182 
2183  // Set Jacobian dimensions
2184  dot_J.resize(3*n_c, 18);
2185 
2186  // Default Jacobian to zero
2187  dot_J.setZero();
2188 
2189  // Sort vector of legs to ensure correct filling of partial Jacobians
2190  std::sort(_legs.begin(), _legs.end());
2191 
2192  // Index to track which index the partial Jacobian
2193  // should be filled into in the complete Jacobian.
2194  int row = 0;
2195 
2196  for (LegType leg : _legs)
2197  {
2198  // Fill Jacobian
2199  dot_J.block<3,18>(row,0) = this->GetTranslationJacobianInWDiff(leg, BodyType::foot, _q, _u);
2200 
2201  // Increment
2202  row += 3;
2203  }
2204 
2205 
2206  return dot_J;
2207 }
2208 
2209 // (3*n_s)x18 Swing Support Jacobian in world frame
2210 Eigen::Matrix<double, Eigen::Dynamic, 18> Kinematics::GetSwingJacobianInW(std::vector<LegType> &_legs,
2211  const Eigen::Matrix<double, 18, 1> &_q)
2212 {
2213  Eigen::MatrixXd J; // Swing Support Jacobian
2214 
2215  const unsigned int n_s = _legs.size(); // Number of swing legs
2216 
2217  // Validate number of swing legs
2218  if (n_s == 0)
2219  {
2220  ROS_ERROR_STREAM("[Kinematics::GetSwingJacobianInW] Zero swing legs were given!");
2221 
2222  std::abort();
2223  }
2224  else if (n_s > 4)
2225  {
2226 
2227  ROS_ERROR_STREAM("[Kinematics::GetSwingJacobianInW] More than four swing legs were given!");
2228 
2229  std::abort();
2230  }
2231 
2232  // Set Jacobian dimensions
2233  J.resize(3*n_s, 18);
2234 
2235  // Default Jacobian to zero
2236  J.setZero();
2237 
2238  // Sort vector of legs to ensure correct filling of partial Jacobians
2239  std::sort(_legs.begin(), _legs.end());
2240 
2241  // Index to track which index the partial Jacobian
2242  // should be filled into in the complete Jacobian.
2243  int row = 0;
2244 
2245  for (LegType leg : _legs)
2246  {
2247  // Fill Jacobian
2248  J.block<3,18>(row,0) = this->GetTranslationJacobianInW(leg, BodyType::foot, _q);
2249 
2250  // Increment
2251  row += 3;
2252  }
2253 
2254 
2255  return J;
2256 }
2257 
2258 // (3*n_s)x18 Time derivative of the Swing Support Jacobian in world frame
2259 Eigen::Matrix<double, Eigen::Dynamic, 18> Kinematics::GetSwingJacobianInWDiff(std::vector<LegType> &_legs,
2260  const Eigen::Matrix<double, 18, 1> &_q,
2261  const Eigen::Matrix<double, 18, 1> &_u)
2262 {
2263  Eigen::MatrixXd dot_J; // Swing Support Jacobian
2264 
2265  const unsigned int n_s = _legs.size(); // Number of swing legs
2266 
2267  // Validate number of swing legs
2268  if (n_s == 0)
2269  {
2270  ROS_ERROR_STREAM("[Kinematics::GetSwingJacobianInW] Zero swing legs were given!");
2271 
2272  std::abort();
2273  }
2274  else if (n_s > 4)
2275  {
2276 
2277  ROS_ERROR_STREAM("[Kinematics::GetSwingJacobianInW] More than four swing legs were given!");
2278 
2279  std::abort();
2280  }
2281 
2282  // Set Jacobian dimensions
2283  dot_J.resize(3*n_s, 18);
2284 
2285  // Default Jacobian to zero
2286  dot_J.setZero();
2287 
2288  // Sort vector of legs to ensure correct filling of partial Jacobians
2289  std::sort(_legs.begin(), _legs.end());
2290 
2291  // Index to track which index the partial Jacobian
2292  // should be filled into in the complete Jacobian.
2293  int row = 0;
2294 
2295  for (LegType leg : _legs)
2296  {
2297  // Fill Jacobian
2298  dot_J.block<3,18>(row,0) = this->GetTranslationJacobianInWDiff(leg, BodyType::foot, _q, _u);
2299 
2300  // Increment
2301  row += 3;
2302  }
2303 
2304 
2305  return dot_J;
2306 }
2307 
2308 // Single body mass matrix
2309 Eigen::Matrix<double, 18, 18> Kinematics::GetSingleBodyMassMatrix(const LegType &_leg,
2310  const BodyType &_body,
2311  const Eigen::Matrix<double, 18, 1> &_q)
2312 {
2313  Eigen::Matrix<double, 3, 18> J_COM; // Translation Jacobian about COM of the body
2314  Eigen::Matrix<double, 3, 18> J_R; // Rotation Jacobian for the body
2315 
2316  Eigen::Matrix<double, 3, 3> I_COM; // Inertia matrix for the COM of the body
2317 
2318  double m; // Mass of the body
2319 
2320  J_COM = this->GetTranslationJacobianInW(_leg, _body, _q);
2321  J_R = this->GetRotationJacobianInW(_leg, _body, _q);
2322 
2323  I_COM = this->GetBodyInertia(_body, _leg);
2324  m = this->GetBodyMass(_body);
2325 
2326  return m * J_COM.transpose() * J_COM + J_R.transpose() * I_COM * J_R;
2327 }
2328 
2329 // Mass matrix
2330 Eigen::Matrix<double, 18, 18> Kinematics::GetMassMatrix(const Eigen::Matrix<double, 18, 1> &_q)
2331 {
2332  Eigen::Matrix<double, 18, 18> M = Eigen::Matrix<double, 18, 18>::Constant(0); // Mass matrix
2333 
2334 
2335  static const std::vector<BodyType> bodies{ BodyType::base,
2336  BodyType::hip,
2337  BodyType::thigh,
2338  BodyType::leg };
2339 
2340  static const std::vector<LegType> legs{ LegType::frontLeft,
2341  LegType::frontRight,
2342  LegType::rearLeft,
2343  LegType::rearRight };
2344 
2345  for (const auto body : bodies)
2346  {
2347  if (body == BodyType::base)
2348  {
2349  M += this->GetSingleBodyMassMatrix(LegType::NONE, body, _q);
2350  }
2351  else
2352  {
2353  for (const auto leg : legs)
2354  {
2355  M += this->GetSingleBodyMassMatrix(leg, body, _q);
2356  }
2357  }
2358  }
2359 
2360  return M;
2361 }
2362 
2363 Eigen::Matrix<double, 3, 3> Kinematics::GetSingleLegMassMatrix(const Eigen::Matrix<double, 3, 1> &_q)
2364 {
2365  Eigen::Matrix<double, 3, 3> M = Eigen::Matrix<double, 3, 3>::Zero();
2366 
2367  static const std::vector<BodyType> bodies{ BodyType::hip,
2368  BodyType::thigh,
2369  BodyType::leg };
2370 
2371  Eigen::Matrix<double, 3, 3> J_s;
2372 
2373  Eigen::Matrix<double, 3, 3> J_r;
2374 
2375  for (const auto body : bodies)
2376  {
2377  J_s = GetTranslationJacobianInB(LegType::frontLeft, body, _q(0), _q(1), _q(2));
2378  J_r = GetRotationJacobianInB(LegType::frontLeft, body, _q(0), _q(1), _q(2));
2379  M += J_s.transpose()*GetBodyMass(body)*J_s + J_r.transpose()*GetBodyInertia(body)*J_r;
2380  }
2381 
2382  return M;
2383 }
2384 
2385 // Single body coriolis and centrifugal terms
2386 Eigen::Matrix<double, 18, 1> Kinematics::GetSingleBodyCoriolisAndCentrifugalTerms(const LegType &_leg,
2387  const BodyType &_body,
2388  const Eigen::Matrix<double, 18, 1> &_q,
2389  const Eigen::Matrix<double, 18, 1> &_u)
2390 {
2391  Eigen::Matrix<double, 3, 18> J_COM; // Translation Jacobian about COM of the body
2392  Eigen::Matrix<double, 3, 18> J_R; // Rotation Jacobian for the body
2393 
2394  Eigen::Matrix<double, 3, 18> dot_J_COM; // Time derivative of translation Jacobian about COM of the body
2395  Eigen::Matrix<double, 3, 18> dot_J_R; // Time derivative of rotation Jacobian for the body
2396 
2397  Eigen::Matrix<double, 3, 1> Omega; // Absolute angular velocities of the body wrt to the world (inertial) frame
2398 
2399  Eigen::Matrix<double, 3, 3> I_COM; // Inertia matrix for the COM of the body
2400 
2401 
2402  double m; // Mass of the body
2403 
2404  J_COM = this->GetTranslationJacobianInW(_leg, _body, _q);
2405  J_R = this->GetRotationJacobianInW(_leg, _body, _q);
2406 
2407  dot_J_COM = this->GetTranslationJacobianInWDiff(_leg, _body, _q, _u);
2408  dot_J_R = this->GetRotationJacobianInWDiff(_leg, _body, _q, _u);
2409 
2410  Omega = J_R * _u;
2411 
2412  I_COM = GetBodyInertia(_body, _leg);
2413  m = GetBodyMass(_body);
2414 
2415  return m * J_COM.transpose() * dot_J_COM * _u + J_R.transpose() * ( I_COM * dot_J_R * _u + kindr::getSkewMatrixFromVector(Omega) * I_COM * Omega);
2416 }
2417 
2418 // Coriolis and centrifugal terms
2419 Eigen::Matrix<double, 18, 1> Kinematics::GetCoriolisAndCentrifugalTerms(const Eigen::Matrix<double, 18, 1> &_q,
2420  const Eigen::Matrix<double, 18, 1> &_u)
2421 {
2422  Eigen::Matrix<double, 18, 1> b = Eigen::Matrix<double, 18, 1>::Constant(0); // Coriolis and centrifugal terms.
2423 
2424 
2425  static const std::vector<BodyType> bodies{ BodyType::base,
2426  BodyType::hip,
2427  BodyType::thigh,
2428  BodyType::leg };
2429 
2430  static const std::vector<LegType> legs{ LegType::frontLeft,
2431  LegType::frontRight,
2432  LegType::rearLeft,
2433  LegType::rearRight };
2434 
2435  for (const auto body : bodies)
2436  {
2437  if (body == BodyType::base)
2438  {
2439  b += this->GetSingleBodyCoriolisAndCentrifugalTerms(LegType::NONE, body, _q, _u);
2440  }
2441  else
2442  {
2443  for (const auto leg : legs)
2444  {
2445  b += this->GetSingleBodyCoriolisAndCentrifugalTerms(leg, body, _q, _u);
2446  }
2447  }
2448  }
2449 
2450  return b;
2451 }
2452 
2453 Eigen::Matrix<double, 3, 1> Kinematics::GetSingleLegCoriolisAndCentrifugalTerms(const Eigen::Matrix<double, 3, 1> &_q,
2454  const Eigen::Matrix<double, 3, 1> &_u)
2455 {
2456  Eigen::Matrix<double, 3, 1> b = Eigen::Matrix<double, 3, 1>::Zero();
2457 
2458  static const std::vector<BodyType> bodies{ BodyType::hip,
2459  BodyType::thigh,
2460  BodyType::leg };
2461 
2462  double m;
2463  Eigen::Matrix<double, 3, 3> I;
2464  Eigen::Matrix<double, 3, 3> J_s;
2465  Eigen::Matrix<double, 3, 3> J_s_d;
2466  Eigen::Matrix<double, 3, 3> J_r;
2467  Eigen::Matrix<double, 3, 3> J_r_d;
2468  Eigen::Matrix<double, 3, 1> omega;
2469 
2470 
2471 
2472  for (const auto body : bodies)
2473  {
2474  m = GetBodyMass(body);
2475  I = GetBodyInertia(body);
2476  J_s = GetTranslationJacobianInB(LegType::frontLeft, body, _q(0), _q(1), _q(2));
2477  J_s_d = GetTranslationJacobianInBDiff(LegType::frontLeft, body, _q(0), _q(1), _q(2), _u(0), _u(1), _u(2));
2478  J_r = GetRotationJacobianInB(LegType::frontLeft, body, _q(0), _q(1), _q(2));
2479  J_r_d = GetRotationJacobianInBDiff(LegType::frontLeft, body, _q(0), _q(1), _q(2), _u(0), _u(1), _u(2));
2480  omega = J_r*_u;
2481  b += J_s.transpose()*m*J_s_d*_u + J_r.transpose()*(I*J_r_d*_u + kindr::getSkewMatrixFromVector(omega)*I*omega);
2482  }
2483 
2484  return b;
2485 }
2486 
2487 // Single body gravitational terms
2488 Eigen::Matrix<double, 18, 1> Kinematics::GetSingleBodyGravitationalTerms(const LegType &_leg,
2489  const BodyType &_body,
2490  const Eigen::Matrix<double, 18, 1> &_q)
2491 {
2492  Eigen::Matrix<double, 3, 18> J_COM; // Translation Jacobian about COM of the body
2493 
2494  Eigen::Matrix<double, 3, 1> F_g; // Gravitational force
2495 
2496  double m; // Mass of the body
2497  double g = 9.81; // Gravitational constant
2498 
2499  J_COM = this->GetTranslationJacobianInW(_leg, _body, _q);
2500 
2501  switch (_body)
2502  {
2503  case base:
2504  {
2505  m = this->M0;
2506 
2507  break;
2508  }
2509  case hip:
2510  {
2511  m = this->M1;
2512 
2513  break;
2514  }
2515  case thigh:
2516  {
2517  m = this->M2;
2518 
2519  break;
2520  }
2521  case leg:
2522  {
2523  m = this->M3;
2524 
2525  break;
2526  }
2527  default:
2528  {
2529  ROS_ERROR_STREAM("[Kinematics::GetSingleBodyGravitationalTerms] Could not determine body type!");
2530 
2531  std::abort();
2532 
2533  break;
2534  }
2535  }
2536 
2537  F_g = m * g * Eigen::Matrix<double, 3, 1>(0,0,-1);
2538 
2539  return - J_COM.transpose() * F_g;
2540 }
2541 
2542 // Gravitational terms
2543 Eigen::Matrix<double, 18, 1> Kinematics::GetGravitationalTerms(const Eigen::Matrix<double, 18, 1> &_q)
2544 {
2545  Eigen::Matrix<double, 18, 1> g = Eigen::Matrix<double, 18, 1>::Constant(0); // Gravitational terms
2546 
2547 
2548  static const std::vector<BodyType> bodies{ BodyType::base,
2549  BodyType::hip,
2550  BodyType::thigh,
2551  BodyType::leg };
2552 
2553  static const std::vector<LegType> legs{ LegType::frontLeft,
2554  LegType::frontRight,
2555  LegType::rearLeft,
2556  LegType::rearRight };
2557 
2558  for (const auto body : bodies)
2559  {
2560  if (body == BodyType::base)
2561  {
2562  g += this->GetSingleBodyGravitationalTerms(LegType::NONE, body, _q);
2563  }
2564  else
2565  {
2566  for (const auto leg : legs)
2567  {
2568  g += this->GetSingleBodyGravitationalTerms(leg, body, _q);
2569  }
2570  }
2571  }
2572 
2573  //g(2) = - g(2); // TODO WHAT THE F*** DO I DO THIS FOR
2574 
2575  return g;
2576 }
2577 
2578 Eigen::Matrix<double, 3, 1> Kinematics::GetSingleLegGravitationalTerms(const Eigen::Matrix<double, 3, 1> &_q)
2579 {
2580  Eigen::Matrix<double, 3, 1> G = Eigen::Matrix<double, 3, 1>::Zero();
2581 
2582  double g = 9.81;
2583 
2584  static const std::vector<BodyType> bodies{ BodyType::hip,
2585  BodyType::thigh,
2586  BodyType::leg };
2587 
2588  Eigen::Matrix<double, 3, 3> J_s;
2589 
2590  Eigen::Matrix<double, 3, 1> F;
2591 
2592  for (const auto body : bodies)
2593  {
2594  J_s = GetTranslationJacobianInB(LegType::frontLeft, body, _q(0), _q(1), _q(2));
2595  F = GetBodyMass(body) * g * Eigen::Matrix<double, 3, 1>(0.0, 0.0, 1.0);
2596  G += J_s.transpose()*F;
2597  }
2598 
2599  return G;
2600 }
2601 
2602 // Validate IK solution
2603 bool Kinematics::ValidateSolution(const Eigen::Matrix<double, 12, 1> &_q_r)
2604 {
2605  if (_q_r.hasNaN())
2606  {
2607  return false;
2608  }
2609 
2610  for (size_t i = 0; i < _q_r.size(); i++)
2611  {
2612  if (_q_r(i) < this->min_angles(i) || _q_r(i) > this->max_angles(i))
2613  {
2614  return false;
2615  }
2616  }
2617 
2618  return true;
2619 }
2620 
2621 // Validate IK solution for a single leg
2622 bool Kinematics::ValidateSolution(const Eigen::Matrix<double, 3, 1> &_q_r)
2623 {
2624  if (_q_r.hasNaN())
2625  {
2626  return false;
2627  }
2628 
2629  // Only the first three joint angles related to the front left leg are considered
2630  for (size_t i = 0; i < 3; i++)
2631  {
2632  if (_q_r(i) < this->min_angles(i) || _q_r(i) > this->max_angles(i))
2633  {
2634  return false;
2635  }
2636  }
2637 
2638  return true;
2639 }
2640 
2641 // Inertia matrix
2642 Eigen::Matrix<double, 3, 3> Kinematics::GetInertiaMatrix(const double &_I_XX,
2643  const double &_I_YY,
2644  const double &_I_ZZ,
2645  const double &_I_XY,
2646  const double &_I_XZ,
2647  const double &_I_YZ)
2648 {
2649  Eigen::Matrix<double, 3, 3> I;
2650  I(0, 0) = _I_XX;
2651  I(1, 1) = _I_YY;
2652  I(2, 2) = _I_ZZ;
2653  I(0, 1) = _I_XY;
2654  I(1, 0) = _I_XY;
2655  I(0, 2) = _I_XZ;
2656  I(2, 0) = _I_XZ;
2657  I(1, 2) = _I_YZ;
2658  I(2, 1) = _I_YZ;
2659 
2660  return I;
2661 }
2662 
2663 double Kinematics::GetBodyMass(BodyType _body)
2664 {
2665  double m;
2666 
2667  switch (_body)
2668  {
2669  case base:
2670  {
2671  m = this->M0;
2672 
2673  break;
2674  }
2675 
2676  case hip:
2677  {
2678  m = this->M1;
2679 
2680  break;
2681  }
2682  case thigh:
2683  {
2684  m = this->M2;
2685 
2686  break;
2687  }
2688  case leg:
2689  {
2690  m = this->M3;
2691 
2692  break;
2693  }
2694  default:
2695  ROS_ERROR_STREAM("[Kinematics::GetBodyMass] Could not determine body type");
2696 
2697  std::abort();
2698 
2699  break;
2700  }
2701 
2702  return m;
2703 }
2704 
2705 Eigen::Matrix<double, 3, 3> Kinematics::GetBodyInertia(BodyType _body)
2706 {
2707  Eigen::Matrix<double, 3, 3> I;
2708 
2709  switch (_body)
2710  {
2711  case base:
2712  {
2713  I = this->I0;
2714 
2715  break;
2716  }
2717 
2718  case hip:
2719  {
2720  I = this->I1_fl_rr;
2721 
2722  break;
2723  }
2724  case thigh:
2725  {
2726  I = this->I2;
2727 
2728  break;
2729  }
2730  case leg:
2731  {
2732  I = this->I3;
2733 
2734  break;
2735  }
2736  default:
2737  ROS_ERROR_STREAM("[Kinematics::GetBodyInertia] Could not determine body type");
2738 
2739  std::abort();
2740 
2741  break;
2742  }
2743 
2744  return I;
2745 }
2746 
2747 Eigen::Matrix<double, 3, 3> Kinematics::GetBodyInertia(BodyType _body, LegType _leg)
2748 {
2749  if(_body == BodyType::hip)
2750  {
2751  switch (_leg)
2752  {
2753  case LegType::frontLeft:
2754  case LegType::rearRight:
2755  return I1_fl_rr;
2756  break;
2757  case LegType::frontRight:
2758  case LegType::rearLeft:
2759  return I1_fr_rl;
2760  break;
2761  default:
2762  ROS_ERROR_STREAM("[Kinematics::GetBodyInertia] Could not determine leg type");
2763 
2764  std::abort();
2765 
2766  break;
2767  }
2768  }
2769  else
2770  {
2771  return GetBodyInertia(_body);
2772  }
2773 }
Kinematics::GetBodyMass
double GetBodyMass(BodyType _body)
Definition: kinematics.cpp:2663
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::L3
double L3
Leg (Fibula & Tibia) link length.
Definition: kinematics.h:709
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::GetRotationMatrixWToB
Eigen::Matrix< double, 3, 3 > GetRotationMatrixWToB(const double &_roll, const double &_pitch, const double &_yaw)
The GetRotationMatrixWToB function returns the Euler Angles ZYX rotation matrix from World to Body fr...
Definition: kinematics.cpp:539
Kinematics::GetSingleBodyCoriolisAndCentrifugalTerms
Eigen::Matrix< double, 18, 1 > GetSingleBodyCoriolisAndCentrifugalTerms(const LegType &_leg, const BodyType &_body, const Eigen::Matrix< double, 18, 1 > &_q, const Eigen::Matrix< double, 18, 1 > &_u)
The GetSingleBodyCoriolisAndCentrifugalTerms function returns the coriolis and centrifugal terms for ...
Definition: kinematics.cpp:2386
Kinematics::frOffset
bool frOffset
Front Right offset.
Definition: kinematics.h:724
Kinematics::ValidateSolution
bool ValidateSolution(const JointSpaceVector &_q_r)
The ValidateSolution function evaluates whether a set of joint angles is within joint limits.
Definition: kinematics.cpp:2603
Kinematics::GetSingleLegMassMatrix
Eigen::Matrix< double, 3, 3 > GetSingleLegMassMatrix(const Eigen::Matrix< double, 3, 1 > &_q)
Definition: kinematics.cpp:2363
Kinematics::SolveSingleLegHipToFootForwardKinematics
Vector3d SolveSingleLegHipToFootForwardKinematics(const LegType &_leg_type, const Vector3d &_joint_angles)
The SolveSingleLegHipToFootForwardKinematics function calculates the foot position in the hip frame.
Definition: kinematics.cpp:262
Kinematics::GetTranslationJacobianInCDiff
Eigen::Matrix< double, 3, 18 > GetTranslationJacobianInCDiff(const LegType &_leg, const BodyType &_body, const Eigen::Matrix< double, 18, 1 > &_q, const Eigen::Matrix< double, 18, 1 > &_u)
The GetTranslationJacobianInCDiff function returns the time derivative of the 3x18 spatial translatio...
Definition: kinematics.cpp:1337
Kinematics::GetRotationJacobianInWDiff
Eigen::Matrix< double, 3, 18 > GetRotationJacobianInWDiff(const LegType &_leg, const BodyType &_body, const Eigen::Matrix< double, 18, 1 > &_q, const Eigen::Matrix< double, 18, 1 > &_u)
The GetRotationJacobianInWDiff function returns the time derivative of the 3x18 spatial Rotation Jaco...
Definition: kinematics.cpp:1917
Kinematics::BodyType
BodyType
Body type enumerator.
Definition: kinematics.h:59
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::positionBaseToFrontLeftInB
Vector3d positionBaseToFrontLeftInB
Position vector relating the base coordinates to the front left hip.
Definition: kinematics.h:688
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::rrOffset
bool rrOffset
Rear Right offset.
Definition: kinematics.h:730
Kinematics::positionBaseToRearLeftInB
Vector3d positionBaseToRearLeftInB
Position vector relating the base coordinates to the rear left hip.
Definition: kinematics.h:696
Kinematics::I3
Eigen::Matrix< double, 3, 3 > I3
Leg link inertia matrix.
Definition: kinematics.h:757
Kinematics::GetSwingJacobianInW
Eigen::Matrix< double, Eigen::Dynamic, 18 > GetSwingJacobianInW(std::vector< LegType > &_legs, const Eigen::Matrix< double, 18, 1 > &_q)
The GetSwingJacobianInW function returns the (3*n_s)x18 support Jacobian mapping swing foot velocitie...
Definition: kinematics.cpp:2210
Kinematics::LC2
double LC2
Hip pitch joint to thigh CoM length.
Definition: kinematics.h:715
Kinematics::GetRotationJacobianInBDiff
Eigen::Matrix< double, 3, 3 > GetRotationJacobianInBDiff(const LegType &_leg, const BodyType &_body, const double &_theta_hy, const double &_theta_hp, const double &_theta_kp, const double &_dot_theta_hy, const double &_dot_theta_hp, const double &_dot_theta_kp)
The GetRotationJacobianInBDiff function returns the time derivative of the 3x3 Rotation Jacobian matr...
Definition: kinematics.cpp:1711
Kinematics::~Kinematics
virtual ~Kinematics()
Destructor.
Definition: kinematics.cpp:101
Kinematics::rlOffset
bool rlOffset
Rear Left offset.
Definition: kinematics.h:727
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::GetInertiaMatrix
Eigen::Matrix< double, 3, 3 > GetInertiaMatrix(const double &_I_XX, const double &_I_YY, const double &_I_ZZ, const double &_I_XY, const double &_I_XZ, const double &_I_YZ)
The GetInertiaMatrix function returns the inertia matrix from the six inertial parameters.
Definition: kinematics.cpp:2642
Kinematics::max_angles
JointSpaceVector max_angles
Maximum joint limits.
Definition: kinematics.h:684
Kinematics::GetJacobianInW
Eigen::Matrix< double, 6, 18 > GetJacobianInW(const LegType &_leg, const BodyType &_body, const Eigen::Matrix< double, 18, 1 > &_q)
The GetJacobianInW function returns the 6x18 spatial Jacobian mapping generalized velocities to opera...
Definition: kinematics.cpp:2065
Kinematics::GetRotationJacobianInB
Eigen::Matrix< double, 3, 3 > GetRotationJacobianInB(const LegType &_leg, const BodyType &_body, const double &_theta_hy, const double &_theta_hp, const double &_theta_kp)
The GetRotationJacobianInB function returns the 3x3 Jacobian matrix for body angular velocities in th...
Definition: kinematics.cpp:1420
Kinematics::GetSingleLegCoriolisAndCentrifugalTerms
Eigen::Matrix< double, 3, 1 > GetSingleLegCoriolisAndCentrifugalTerms(const Eigen::Matrix< double, 3, 1 > &_q, const Eigen::Matrix< double, 3, 1 > &_u)
Definition: kinematics.cpp:2453
Kinematics::LC3
double LC3
Knee pitch joint to leg CoM length.
Definition: kinematics.h:718
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::positionBaseToRearRightInB
Vector3d positionBaseToRearRightInB
Position vector relating the base coordinates to the rear right hip.
Definition: kinematics.h:700
Kinematics::I0
Eigen::Matrix< double, 3, 3 > I0
Body link inertia matrix.
Definition: kinematics.h:745
Kinematics::GetTranslationJacobianInBDiff
Eigen::Matrix< double, 3, 3 > GetTranslationJacobianInBDiff(const LegType &_leg, const BodyType &_body, const double &_theta_hy, const double &_theta_hp, const double &_theta_kp, const double &_dot_theta_hy, const double &_dot_theta_hp, const double &_dot_theta_kp)
The GetTranslationJacobianDerivativeInB function returns the time derivative of the 3x3 translation J...
Definition: kinematics.cpp:1027
Kinematics::I1_fr_rl
Eigen::Matrix< double, 3, 3 > I1_fr_rl
Front Right & Rear Left Hip link inertia matrix.
Definition: kinematics.h:751
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::GetRotationMatrixCToB
Eigen::Matrix< double, 3, 3 > GetRotationMatrixCToB(const double &_roll, const double &_pitch, const double &_yaw)
The GetRotationMatrixCToB function returns the Euler Angles ZYX rotation matrix from Control to Body ...
Definition: kinematics.cpp:609
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::M3
double M3
Leg link mass.
Definition: kinematics.h:742
Kinematics::GetContactJacobianInWDiff
Eigen::Matrix< double, Eigen::Dynamic, 18 > GetContactJacobianInWDiff(std::vector< LegType > &_legs, const Eigen::Matrix< double, 18, 1 > &_q, const Eigen::Matrix< double, 18, 1 > &_u)
The GetContactJacobianInWDiff function returns the time derivative of the (3*n_c)x18 support Jacobian...
Definition: kinematics.cpp:2156
Kinematics::M1
double M1
Hip link mass.
Definition: kinematics.h:736
Kinematics::L2
double L2
Thigh (Femur) link length.
Definition: kinematics.h:706
Kinematics::M2
double M2
Thigh link mass.
Definition: kinematics.h:739
Kinematics::M0
double M0
Body link mass.
Definition: kinematics.h:733
Kinematics::GetSingleBodyMassMatrix
Eigen::Matrix< double, 18, 18 > GetSingleBodyMassMatrix(const LegType &_leg, const BodyType &_body, const Eigen::Matrix< double, 18, 1 > &_q)
The GetSingleBodyMassMatrix function returns the mass matrix for a single body in the system.
Definition: kinematics.cpp:2309
Kinematics::GetBodyInertia
Eigen::Matrix< double, 3, 3 > GetBodyInertia(BodyType _body)
Definition: kinematics.cpp:2705
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::GetRotationJacobianInC
Eigen::Matrix< double, 3, 18 > GetRotationJacobianInC(const LegType &_leg, const BodyType &_body, const Eigen::Matrix< double, 18, 1 > &_q)
The GetRotationJacobianInC function returns the 3x18 spatial rotation Jacobian mapping generalized ve...
Definition: kinematics.cpp:1661
Kinematics::positionBaseToFrontRightInB
Vector3d positionBaseToFrontRightInB
Position vector relating the base coordinates to the front right hip.
Definition: kinematics.h:692
Kinematics::flOffset
bool flOffset
Front Left offset.
Definition: kinematics.h:721
Kinematics::min_angles
JointSpaceVector min_angles
Minimum joint limits.
Definition: kinematics.h:681
Kinematics::I1_fl_rr
Eigen::Matrix< double, 3, 3 > I1_fl_rr
Front Left & Rear Right Hip link inertia matrix.
Definition: kinematics.h:748
Kinematics::I2
Eigen::Matrix< double, 3, 3 > I2
Thigh link inertia matrix.
Definition: kinematics.h:754
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::GetRotationMatrixCToBDiff
Eigen::Matrix< double, 3, 3 > GetRotationMatrixCToBDiff(const double &_roll, const double &_pitch, const double &_yaw, const double &_roll_rate, const double &_pitch_rate, const double &_yaw_rate)
The GetRotationMatrixCToBDiff function returns the time derivative of the Euler Angles ZYX rotation m...
Definition: kinematics.cpp:622
Kinematics::GetRotationMatrixWToCDiff
Eigen::Matrix< double, 3, 3 > GetRotationMatrixWToCDiff(const double &_roll, const double &_pitch, const double &_yaw, const double &_roll_rate, const double &_pitch_rate, const double &_yaw_rate)
The GetRotationMatrixWToCDiff function returns the time derivative of the Euler Angles ZYX rotation m...
Definition: kinematics.cpp:587
Kinematics::GetRotationJacobianInCDiff
Eigen::Matrix< double, 3, 18 > GetRotationJacobianInCDiff(const LegType &_leg, const BodyType &_body, const Eigen::Matrix< double, 18, 1 > &_q, const Eigen::Matrix< double, 18, 1 > &_u)
The GetRotationJacobianInCDiff function returns the time derivative of the 3x18 control frame Rotatio...
Definition: kinematics.cpp:1991
Kinematics::GetRotationMatrixWToBDiff
Eigen::Matrix< double, 3, 3 > GetRotationMatrixWToBDiff(const double &_roll, const double &_pitch, const double &_yaw, const double &_roll_rate, const double &_pitch_rate, const double &_yaw_rate)
The GetRotationMatrixWToBDiff function returns the time derivative of the Euler Angles ZYX rotation m...
Definition: kinematics.cpp:552
Kinematics::GetTranslationJacobianInWDiff
Eigen::Matrix< double, 3, 18 > GetTranslationJacobianInWDiff(const LegType &_leg, const BodyType &_body, const Eigen::Matrix< double, 18, 1 > &_q, const Eigen::Matrix< double, 18, 1 > &_u)
The GetTranslationJacobianInWDiff function returns the time derivative of the 3x18 spatial translatio...
Definition: kinematics.cpp:1254
Kinematics::GetSwingJacobianInWDiff
Eigen::Matrix< double, Eigen::Dynamic, 18 > GetSwingJacobianInWDiff(std::vector< LegType > &_legs, const Eigen::Matrix< double, 18, 1 > &_q, const Eigen::Matrix< double, 18, 1 > &_u)
The GetContactJacobianInWDiff function returns the time derivative of the (3*n_s)x18 support Jacobian...
Definition: kinematics.cpp:2259
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::Kinematics
Kinematics()
Constructor.
Definition: kinematics.cpp:31
Kinematics::LegType
LegType
Leg type enumerator.
Definition: kinematics.h:56
Kinematics::L1
double L1
Hip link length.
Definition: kinematics.h:703
Kinematics::GetSingleLegGravitationalTerms
Eigen::Matrix< double, 3, 1 > GetSingleLegGravitationalTerms(const Eigen::Matrix< double, 3, 1 > &_q)
Definition: kinematics.cpp:2578
Kinematics::LC1
double LC1
Hip yaw joint to hip CoM length.
Definition: kinematics.h:712
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
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
TransMatrix
kindr::HomTransformMatrixD TransMatrix
Definition: kinematics.h:50
Twist
Eigen::Matrix< double, 6, 1 > Twist
Definition: kinematics.h:48
calculate_polynomials.b
tuple b
Definition: calculate_polynomials.py:27
calculate_polynomials.t3
t3
Definition: calculate_polynomials.py:34
calculate_polynomials.t2
t2
Definition: calculate_polynomials.py:33
calculate_polynomials.t1
t1
Definition: calculate_polynomials.py:32
math_utils::HALF_PI
static constexpr double HALF_PI
Define PI/2.
Definition: angle_utils.h:53
math_utils::degToRad
double degToRad(const double &_deg)
The degToRad function converts degrees to radians.
Definition: angle_utils.cpp:33
math_utils::PI
static constexpr double PI
Define PI.
Definition: angle_utils.h:44