Rigid Body Dynamics Library
Joint.h
Go to the documentation of this file.
1 /*
2  * RBDL - Rigid Body Dynamics Library
3  * Copyright (c) 2011-2018 Martin Felis <martin@fysx.org>
4  *
5  * Licensed under the zlib license. See LICENSE for more details.
6  */
7 
8 #ifndef RBDL_JOINT_H
9 #define RBDL_JOINT_H
10 
11 #include "rbdl/rbdl_math.h"
12 #include <assert.h>
13 #include <iostream>
14 #include "rbdl/Logging.h"
15 
16 namespace RigidBodyDynamics {
17 
18 struct Model;
19 
179  enum JointType {
193  JointTypeHelical, //1 DoF joint with both rotational and translational motion
201  };
202 
209 struct RBDL_DLLAPI Joint {
210  Joint() :
211  mJointAxes (NULL),
212  mJointType (JointTypeUndefined),
213  mDoFCount (0),
214  q_index (0) {};
215  Joint (JointType type) :
216  mJointAxes (NULL),
217  mJointType (type),
218  mDoFCount (0),
219  q_index (0),
220  custom_joint_index(-1) {
221  if (type == JointTypeRevoluteX) {
222  mDoFCount = 1;
223  mJointAxes = new Math::SpatialVector[mDoFCount];
224  mJointAxes[0] = Math::SpatialVector (1., 0., 0., 0., 0., 0.);
225  } else if (type == JointTypeRevoluteY) {
226  mDoFCount = 1;
227  mJointAxes = new Math::SpatialVector[mDoFCount];
228  mJointAxes[0] = Math::SpatialVector (0., 1., 0., 0., 0., 0.);
229  } else if (type == JointTypeRevoluteZ) {
230  mDoFCount = 1;
231  mJointAxes = new Math::SpatialVector[mDoFCount];
232  mJointAxes[0] = Math::SpatialVector (0., 0., 1., 0., 0., 0.);
233  } else if (type == JointTypeSpherical) {
234  mDoFCount = 3;
235 
236  mJointAxes = new Math::SpatialVector[mDoFCount];
237 
238  mJointAxes[0] = Math::SpatialVector (0., 0., 1., 0., 0., 0.);
239  mJointAxes[1] = Math::SpatialVector (0., 1., 0., 0., 0., 0.);
240  mJointAxes[2] = Math::SpatialVector (1., 0., 0., 0., 0., 0.);
241  } else if (type == JointTypeEulerZYX) {
242  mDoFCount = 3;
243 
244  mJointAxes = new Math::SpatialVector[mDoFCount];
245 
246  mJointAxes[0] = Math::SpatialVector (0., 0., 1., 0., 0., 0.);
247  mJointAxes[1] = Math::SpatialVector (0., 1., 0., 0., 0., 0.);
248  mJointAxes[2] = Math::SpatialVector (1., 0., 0., 0., 0., 0.);
249  } else if (type == JointTypeEulerXYZ) {
250  mDoFCount = 3;
251 
252  mJointAxes = new Math::SpatialVector[mDoFCount];
253 
254  mJointAxes[0] = Math::SpatialVector (1., 0., 0., 0., 0., 0.);
255  mJointAxes[1] = Math::SpatialVector (0., 1., 0., 0., 0., 0.);
256  mJointAxes[2] = Math::SpatialVector (0., 0., 1., 0., 0., 0.);
257  } else if (type == JointTypeEulerYXZ) {
258  mDoFCount = 3;
259 
260  mJointAxes = new Math::SpatialVector[mDoFCount];
261 
262  mJointAxes[0] = Math::SpatialVector (0., 1., 0., 0., 0., 0.);
263  mJointAxes[1] = Math::SpatialVector (1., 0., 0., 0., 0., 0.);
264  mJointAxes[2] = Math::SpatialVector (0., 0., 1., 0., 0., 0.);
265  } else if (type == JointTypeTranslationXYZ) {
266  mDoFCount = 3;
267 
268  mJointAxes = new Math::SpatialVector[mDoFCount];
269 
270  mJointAxes[0] = Math::SpatialVector (0., 0., 0., 1., 0., 0.);
271  mJointAxes[1] = Math::SpatialVector (0., 0., 0., 0., 1., 0.);
272  mJointAxes[2] = Math::SpatialVector (0., 0., 0., 0., 0., 1.);
273  } else if (type >= JointType1DoF && type <= JointType6DoF) {
274  // create a joint and allocate memory for it.
275  // Warning: the memory does not get initialized by this function!
276  mDoFCount = type - JointType1DoF + 1;
277  mJointAxes = new Math::SpatialVector[mDoFCount];
278  std::cerr << "Warning: uninitalized vector" << std::endl;
279  } else if (type == JointTypeCustom) {
280  //This constructor cannot be used for a JointTypeCustom because
281  //we have no idea what mDoFCount is.
282  std::cerr << "Error: Invalid use of Joint constructor Joint(JointType"
283  << " type). Only allowed when type != JointTypeCustom"
284  << std::endl;
285  assert(0);
286  abort();
287  } else if (type != JointTypeFixed && type != JointTypeFloatingBase) {
288  std::cerr << "Error: Invalid use of Joint constructor Joint(JointType type). Only allowed when type == JointTypeFixed or JointTypeSpherical." << std::endl;
289  assert (0);
290  abort();
291  }
292  }
293  Joint (JointType type, int degreesOfFreedom) :
294  mJointAxes (NULL),
295  mJointType (type),
296  mDoFCount (0),
297  q_index (0),
298  custom_joint_index(-1) {
299  if (type == JointTypeCustom) {
300  mDoFCount = degreesOfFreedom;
301  mJointAxes = new Math::SpatialVector[mDoFCount];
302  for(unsigned int i=0; i<mDoFCount;++i){
303  mJointAxes[i] = Math::SpatialVector (0., 0., 0., 0., 0., 0.);
304  }
305  } else {
306  std::cerr << "Error: Invalid use of Joint constructor Joint(JointType"
307  << " type, int degreesOfFreedom). Only allowed when "
308  << "type == JointTypeCustom."
309  << std::endl;
310  assert (0);
311  abort();
312  }
313  }
314  Joint (const Joint &joint) :
315  mJointType (joint.mJointType),
316  mDoFCount (joint.mDoFCount),
317  q_index (joint.q_index),
318  custom_joint_index(joint.custom_joint_index) {
319  mJointAxes = new Math::SpatialVector[mDoFCount];
320 
321  for (unsigned int i = 0; i < mDoFCount; i++)
322  mJointAxes[i] = joint.mJointAxes[i];
323  };
324  Joint& operator= (const Joint &joint) {
325  if (this != &joint) {
326  if (mDoFCount > 0) {
327  assert (mJointAxes);
328  delete[] mJointAxes;
329  }
330  mJointType = joint.mJointType;
331  mDoFCount = joint.mDoFCount;
332  custom_joint_index = joint.custom_joint_index;
333  mJointAxes = new Math::SpatialVector[mDoFCount];
334 
335  for (unsigned int i = 0; i < mDoFCount; i++)
336  mJointAxes[i] = joint.mJointAxes[i];
337 
338  q_index = joint.q_index;
339  }
340  return *this;
341  }
342  ~Joint() {
343  if (mJointAxes) {
344  assert (mJointAxes);
345  delete[] mJointAxes;
346  mJointAxes = NULL;
347  mDoFCount = 0;
348  custom_joint_index = -1;
349  }
350  }
351 
361  const JointType joint_type,
362  const Math::Vector3d &joint_axis
363  ) {
364  mDoFCount = 1;
365  mJointAxes = new Math::SpatialVector[mDoFCount];
366 
367  // Some assertions, as we concentrate on simple cases
368 
369  // Only rotation around the Z-axis
370  assert ( joint_type == JointTypeRevolute || joint_type == JointTypePrismatic );
371 
372  mJointType = joint_type;
373 
374  if (joint_type == JointTypeRevolute) {
375  // make sure we have a unit axis
376  mJointAxes[0].set (
377  joint_axis[0],
378  joint_axis[1],
379  joint_axis[2],
380  0., 0., 0.
381  );
382 
383  } else if (joint_type == JointTypePrismatic) {
384  // make sure we have a unit axis
385  assert (joint_axis.squaredNorm() == 1.);
386 
387  mJointAxes[0].set (
388  0., 0., 0.,
389  joint_axis[0],
390  joint_axis[1],
391  joint_axis[2]
392  );
393  }
394  }
395 
404  const Math::SpatialVector &axis_0
405  ) {
406  mDoFCount = 1;
407  mJointAxes = new Math::SpatialVector[mDoFCount];
408  mJointAxes[0] = Math::SpatialVector (axis_0);
409  if (axis_0 == Math::SpatialVector(1., 0., 0., 0., 0., 0.)) {
410  mJointType = JointTypeRevoluteX;
411  } else if (axis_0 == Math::SpatialVector(0., 1., 0., 0., 0., 0.)) {
412  mJointType = JointTypeRevoluteY;
413  } else if (axis_0 == Math::SpatialVector(0., 0., 1., 0., 0., 0.)) {
414  mJointType = JointTypeRevoluteZ;
415  } else if (axis_0[0] == 0 &&
416  axis_0[1] == 0 &&
417  axis_0[2] == 0) {
418  mJointType = JointTypePrismatic;
419  } else {
420  mJointType = JointTypeHelical;
421  }
422  validate_spatial_axis (mJointAxes[0]);
423  }
424 
436  const Math::SpatialVector &axis_0,
437  const Math::SpatialVector &axis_1
438  ) {
439  mJointType = JointType2DoF;
440  mDoFCount = 2;
441 
442  mJointAxes = new Math::SpatialVector[mDoFCount];
443  mJointAxes[0] = axis_0;
444  mJointAxes[1] = axis_1;
445 
446  validate_spatial_axis (mJointAxes[0]);
447  validate_spatial_axis (mJointAxes[1]);
448  }
449 
462  const Math::SpatialVector &axis_0,
463  const Math::SpatialVector &axis_1,
464  const Math::SpatialVector &axis_2
465  ) {
466  mJointType = JointType3DoF;
467  mDoFCount = 3;
468 
469  mJointAxes = new Math::SpatialVector[mDoFCount];
470 
471  mJointAxes[0] = axis_0;
472  mJointAxes[1] = axis_1;
473  mJointAxes[2] = axis_2;
474 
475  validate_spatial_axis (mJointAxes[0]);
476  validate_spatial_axis (mJointAxes[1]);
477  validate_spatial_axis (mJointAxes[2]);
478  }
479 
493  const Math::SpatialVector &axis_0,
494  const Math::SpatialVector &axis_1,
495  const Math::SpatialVector &axis_2,
496  const Math::SpatialVector &axis_3
497  ) {
498  mJointType = JointType4DoF;
499  mDoFCount = 4;
500 
501  mJointAxes = new Math::SpatialVector[mDoFCount];
502 
503  mJointAxes[0] = axis_0;
504  mJointAxes[1] = axis_1;
505  mJointAxes[2] = axis_2;
506  mJointAxes[3] = axis_3;
507 
508  validate_spatial_axis (mJointAxes[0]);
509  validate_spatial_axis (mJointAxes[1]);
510  validate_spatial_axis (mJointAxes[2]);
511  validate_spatial_axis (mJointAxes[3]);
512  }
513 
528  const Math::SpatialVector &axis_0,
529  const Math::SpatialVector &axis_1,
530  const Math::SpatialVector &axis_2,
531  const Math::SpatialVector &axis_3,
532  const Math::SpatialVector &axis_4
533  ) {
534  mJointType = JointType5DoF;
535  mDoFCount = 5;
536 
537  mJointAxes = new Math::SpatialVector[mDoFCount];
538 
539  mJointAxes[0] = axis_0;
540  mJointAxes[1] = axis_1;
541  mJointAxes[2] = axis_2;
542  mJointAxes[3] = axis_3;
543  mJointAxes[4] = axis_4;
544 
545  validate_spatial_axis (mJointAxes[0]);
546  validate_spatial_axis (mJointAxes[1]);
547  validate_spatial_axis (mJointAxes[2]);
548  validate_spatial_axis (mJointAxes[3]);
549  validate_spatial_axis (mJointAxes[4]);
550  }
551 
567  const Math::SpatialVector &axis_0,
568  const Math::SpatialVector &axis_1,
569  const Math::SpatialVector &axis_2,
570  const Math::SpatialVector &axis_3,
571  const Math::SpatialVector &axis_4,
572  const Math::SpatialVector &axis_5
573  ) {
574  mJointType = JointType6DoF;
575  mDoFCount = 6;
576 
577  mJointAxes = new Math::SpatialVector[mDoFCount];
578 
579  mJointAxes[0] = axis_0;
580  mJointAxes[1] = axis_1;
581  mJointAxes[2] = axis_2;
582  mJointAxes[3] = axis_3;
583  mJointAxes[4] = axis_4;
584  mJointAxes[5] = axis_5;
585 
586  validate_spatial_axis (mJointAxes[0]);
587  validate_spatial_axis (mJointAxes[1]);
588  validate_spatial_axis (mJointAxes[2]);
589  validate_spatial_axis (mJointAxes[3]);
590  validate_spatial_axis (mJointAxes[4]);
591  validate_spatial_axis (mJointAxes[5]);
592  }
593 
600 
601  bool axis_rotational = false;
602  bool axis_translational = false;
603 
604  Math::Vector3d rotation (axis[0], axis[1], axis[2]);
605  Math::Vector3d translation (axis[3], axis[4], axis[5]);
606 
607  if (fabs(rotation.norm()) > 1.0e-8)
608  axis_rotational = true;
609 
610  if (fabs(translation.norm()) > 1.0e-8)
611  axis_translational = true;
612 
613  if(axis_rotational && rotation.norm() - 1.0 > 1.0e-8) {
614  std::cerr << "Warning: joint rotation axis is not unit!" << std::endl;
615  }
616 
617  if(axis_translational && translation.norm() - 1.0 > 1.0e-8) {
618  std::cerr << "Warning: joint translation axis is not unit!" << std::endl;
619  }
620 
621  return axis_rotational != axis_translational;
622  }
623 
629  // have here a value of 0 and their actual numbers of degrees of freedom
630  // can be obtained using the CustomJoint structure.
631  unsigned int mDoFCount;
632  unsigned int q_index;
634 };
635 
646 RBDL_DLLAPI
647 void jcalc (
648  Model &model,
649  unsigned int joint_id,
650  const Math::VectorNd &q,
651  const Math::VectorNd &qdot
652  );
653 
654 RBDL_DLLAPI
656  Model &model,
657  unsigned int joint_id,
658  const Math::VectorNd &q);
659 
660 RBDL_DLLAPI
661 void jcalc_X_lambda_S (
662  Model &model,
663  unsigned int joint_id,
664  const Math::VectorNd &q
665  );
666 
667 struct RBDL_DLLAPI CustomJoint {
669  { }
670  virtual ~CustomJoint() {};
671 
672  virtual void jcalc (Model &model,
673  unsigned int joint_id,
674  const Math::VectorNd &q,
675  const Math::VectorNd &qdot
676  ) = 0;
677  virtual void jcalc_X_lambda_S (Model &model,
678  unsigned int joint_id,
679  const Math::VectorNd &q
680  ) = 0;
681 
682  unsigned int mDoFCount;
689 };
690 
691 }
692 
693 /* RBDL_JOINT_H */
694 #endif
Compact representation of spatial transformations.
Joint(const JointType joint_type, const Math::Vector3d &joint_axis)
Constructs a joint from the given cartesian parameters.
Definition: Joint.h:360
Joint(const Math::SpatialVector &axis_0, const Math::SpatialVector &axis_1, const Math::SpatialVector &axis_2)
Constructs a 3 DoF joint with the given motion subspaces.
Definition: Joint.h:461
Math::SpatialTransform XJ
Definition: Joint.h:683
Emulated 2 DoF joint.
Definition: Joint.h:195
Joint(JointType type)
Definition: Joint.h:215
RBDL_DLLAPI void jcalc_X_lambda_S(Model &model, unsigned int joint_id, const VectorNd &q)
Definition: Joint.cc:271
Joint(const Math::SpatialVector &axis_0, const Math::SpatialVector &axis_1, const Math::SpatialVector &axis_2, const Math::SpatialVector &axis_3, const Math::SpatialVector &axis_4)
Constructs a 5 DoF joint with the given motion subspaces.
Definition: Joint.h:527
Contains all information about the rigid body model.
Definition: Model.h:121
Emulated 4 DoF joint.
Definition: Joint.h:197
SpatialVector_t SpatialVector
Definition: rbdl_math.h:59
Joint(const Math::SpatialVector &axis_0, const Math::SpatialVector &axis_1)
Constructs a 2 DoF joint with the given motion subspaces.
Definition: Joint.h:435
Joint(const Math::SpatialVector &axis_0, const Math::SpatialVector &axis_1, const Math::SpatialVector &axis_2, const Math::SpatialVector &axis_3, const Math::SpatialVector &axis_4, const Math::SpatialVector &axis_5)
Constructs a 6 DoF joint with the given motion subspaces.
Definition: Joint.h:566
bool validate_spatial_axis(Math::SpatialVector &axis)
Checks whether we have pure rotational or translational axis.
Definition: Joint.h:599
Namespace for all structures of the RigidBodyDynamics library.
Definition: Constraints.cc:23
Fixed joint which causes the inertial properties to be merged with the parent body.
Definition: Joint.h:192
Joint(const Math::SpatialVector &axis_0)
Constructs a 1 DoF joint with the given motion subspaces.
Definition: Joint.h:403
Joint(const Joint &joint)
Definition: Joint.h:314
JointType
General types of joints.
Definition: Joint.h:179
3 DoF joint that uses Euler ZYX convention (faster than emulated multi DoF joints).
Definition: Joint.h:187
RBDL_DLLAPI Math::SpatialTransform jcalc_XJ(Model &model, unsigned int joint_id, const Math::VectorNd &q)
Definition: Joint.cc:222
Joint(const Math::SpatialVector &axis_0, const Math::SpatialVector &axis_1, const Math::SpatialVector &axis_2, const Math::SpatialVector &axis_3)
Constructs a 4 DoF joint with the given motion subspaces.
Definition: Joint.h:492
3 DoF joint that uses Euler XYZ convention (faster than emulated multi DoF joints).
Definition: Joint.h:188
unsigned int mDoFCount
Number of degrees of freedom of the joint. Note: CustomJoints.
Definition: Joint.h:631
RBDL_DLLAPI void jcalc(Model &model, unsigned int joint_id, const VectorNd &q, const VectorNd &qdot)
Computes all variables for a joint model.
Definition: Joint.cc:21
A 6-DoF joint for floating-base (or freeflyer) systems.
Definition: Joint.h:191
Emulated 6 DoF joint.
Definition: Joint.h:199
Emulated 3 DoF joint.
Definition: Joint.h:196
JointType mJointType
Type of joint.
Definition: Joint.h:627
3 DoF joint using Quaternions for joint positional variables and angular velocity for joint velocity ...
Definition: Joint.h:186
Describes a joint relative to the predecessor body.
Definition: Joint.h:209
unsigned int q_index
Definition: Joint.h:632
3 DoF joint that uses Euler YXZ convention (faster than emulated multi DoF joints).
Definition: Joint.h:189
Math::SpatialVector * mJointAxes
The spatial axes of the joint.
Definition: Joint.h:625
Emulated 5 DoF joint.
Definition: Joint.h:198
Joint(JointType type, int degreesOfFreedom)
Definition: Joint.h:293
User defined joints of varying size.
Definition: Joint.h:200