Joint.h

Go to the documentation of this file.

/*
 * RBDL - Rigid Body Dynamics Library
 * Copyright (c) 2011-2018 Martin Felis <martin@fysx.org>
 *
 * Licensed under the zlib license. See LICENSE for more details.
 */
 
#ifndef RBDL_JOINT_H
#define RBDL_JOINT_H
 
#include "rbdl/rbdl_math.h"
#include <assert.h>
#include <iostream>
#include "rbdl/Logging.h"
#include "rbdl/rbdl_errors.h"
 
namespace RigidBodyDynamics
{
 
struct Model;
 
enum JointType {
  JointTypeUndefined = 0,
  JointTypeRevolute,
  JointTypePrismatic,
  JointTypeRevoluteX,
  JointTypeRevoluteY,
  JointTypeRevoluteZ,
  JointTypeSpherical, 
  JointTypeEulerZYX, 
  JointTypeEulerXYZ, 
  JointTypeEulerYXZ, 
  JointTypeEulerZXY, 
  JointTypeTranslationXYZ,
  JointTypeFloatingBase, 
  JointTypeFixed, 
  JointTypeHelical, //1 DoF joint with both rotational and translational motion
  JointType1DoF,
  JointType2DoF, 
  JointType3DoF, 
  JointType4DoF, 
  JointType5DoF, 
  JointType6DoF, 
  JointTypeCustom, 
};
 
struct JointMapEntry{
  JointType id;
  const char* name;
  const char* abbr;
};
 
static const JointMapEntry JointMap[] = {
  { JointTypeUndefined        , "Undefined"        , "UNDEFINED"},
  { JointTypeRevolute         , "Revolute"         , ""        },
  { JointTypePrismatic        , "Prismatic"        , ""        },
  { JointTypeRevoluteX        , "RevoluteX"        , ""        },
  { JointTypeRevoluteY        , "RevoluteY"        , ""        },
  { JointTypeRevoluteZ        , "RevoluteZ"        , ""        },
  { JointTypeSpherical        , "Spherical"        , "Sphere"  },
  { JointTypeEulerZYX         , "EulerZYX"         , "EaZYX"   },
  { JointTypeEulerXYZ         , "EulerXYZ"         , "EaXYZ"   },
  { JointTypeEulerYXZ         , "EulerYXZ"         , "EaYXZ"   },
  { JointTypeEulerZXY         , "EulerZXY"         , "EaZXY"   },
  { JointTypeTranslationXYZ   , "TranslationXYZ"   , "TrXYZ"   },
  { JointTypeFloatingBase     , "FloatingBase"     , "FloBas"  },
  { JointTypeFixed            , "Fixed"            , "Fixed"   },
  { JointTypeHelical          , "Helical"          , "Helic"   },
  { JointType1DoF             , "1DoF"             , ""        },
  { JointType2DoF             , "2DoF"             , ""        },
  { JointType3DoF             , "3DoF"             , ""        },
  { JointType4DoF             , "4DoF"             , ""        },
  { JointType5DoF             , "5DoF"             , ""        },
  { JointType6DoF             , "6DoF"             , ""        },
  { JointTypeCustom           , "Custom"           , "Cus"     }
};
 
struct AxisMapEntry{
  unsigned int id;
  const char* name;
};
 
static const AxisMapEntry AxisMap[]={
  {0, "Rx"},
  {1, "Ry"},
  {2, "Rz"},
  {3, "Tx"},
  {4, "Ty"},
  {5, "Tz"}
};
 
struct RBDL_DLLAPI Joint {
  Joint() :
    mJointAxes (NULL),
    mJointType (JointTypeUndefined),
    mDoFCount (0),
    q_index (0),
    custom_joint_index(-1) {};
  Joint (JointType type) :
    mJointAxes (NULL),
    mJointType (type),
    mDoFCount (0),
    q_index (0),
    custom_joint_index(-1)
  {
    if (type == JointTypeRevoluteX) {
      mDoFCount = 1;
      mJointAxes = new Math::SpatialVector[mDoFCount];
      mJointAxes[0] = Math::SpatialVector (1., 0., 0., 0., 0., 0.);
    } else if (type == JointTypeRevoluteY) {
      mDoFCount = 1;
      mJointAxes = new Math::SpatialVector[mDoFCount];
      mJointAxes[0] = Math::SpatialVector (0., 1., 0., 0., 0., 0.);
    } else if (type == JointTypeRevoluteZ) {
      mDoFCount = 1;
      mJointAxes = new Math::SpatialVector[mDoFCount];
      mJointAxes[0] = Math::SpatialVector (0., 0., 1., 0., 0., 0.);
    } else if (type == JointTypeSpherical) {
      mDoFCount = 3;
 
      mJointAxes = new Math::SpatialVector[mDoFCount];
 
      mJointAxes[0] = Math::SpatialVector (0., 0., 1., 0., 0., 0.);
      mJointAxes[1] = Math::SpatialVector (0., 1., 0., 0., 0., 0.);
      mJointAxes[2] = Math::SpatialVector (1., 0., 0., 0., 0., 0.);
    } else if (type == JointTypeEulerZYX) {
      mDoFCount = 3;
 
      mJointAxes = new Math::SpatialVector[mDoFCount];
 
      mJointAxes[0] = Math::SpatialVector (0., 0., 1., 0., 0., 0.);
      mJointAxes[1] = Math::SpatialVector (0., 1., 0., 0., 0., 0.);
      mJointAxes[2] = Math::SpatialVector (1., 0., 0., 0., 0., 0.);
    } else if (type == JointTypeEulerXYZ) {
      mDoFCount = 3;
 
      mJointAxes = new Math::SpatialVector[mDoFCount];
 
      mJointAxes[0] = Math::SpatialVector (1., 0., 0., 0., 0., 0.);
      mJointAxes[1] = Math::SpatialVector (0., 1., 0., 0., 0., 0.);
      mJointAxes[2] = Math::SpatialVector (0., 0., 1., 0., 0., 0.);
    } else if (type == JointTypeEulerYXZ) {
      mDoFCount = 3;
 
      mJointAxes = new Math::SpatialVector[mDoFCount];
 
      mJointAxes[0] = Math::SpatialVector (0., 1., 0., 0., 0., 0.);
      mJointAxes[1] = Math::SpatialVector (1., 0., 0., 0., 0., 0.);
      mJointAxes[2] = Math::SpatialVector (0., 0., 1., 0., 0., 0.);
    } else if (type == JointTypeEulerZXY) {
      mDoFCount = 3;
 
      mJointAxes = new Math::SpatialVector[mDoFCount];
 
      mJointAxes[0] = Math::SpatialVector (0., 0., 1., 0., 0., 0.);
      mJointAxes[1] = Math::SpatialVector (1., 0., 0., 0., 0., 0.);
      mJointAxes[2] = Math::SpatialVector (0., 1., 0., 0., 0., 0.);
    } else if (type == JointTypeTranslationXYZ) {
      mDoFCount = 3;
 
      mJointAxes = new Math::SpatialVector[mDoFCount];
 
      mJointAxes[0] = Math::SpatialVector (0., 0., 0., 1., 0., 0.);
      mJointAxes[1] = Math::SpatialVector (0., 0., 0., 0., 1., 0.);
      mJointAxes[2] = Math::SpatialVector (0., 0., 0., 0., 0., 1.);
    } else if (type >= JointType1DoF && type <= JointType6DoF) {
      // create a joint and allocate memory for it.
      // Warning: the memory does not get initialized by this function!
      mDoFCount = type - JointType1DoF + 1;
      mJointAxes = new Math::SpatialVector[mDoFCount];
      std::cerr << "Warning: uninitalized vector" << std::endl;
    } else if (type == JointTypeCustom) {
      //This constructor cannot be used for a JointTypeCustom because
      //we have no idea what mDoFCount is.
      std::ostringstream errormsg;
      errormsg  << "Error: Invalid use of Joint constructor Joint(JointType"
                << " type). Only allowed when type != JointTypeCustom"
                << std::endl;
      throw Errors::RBDLError(errormsg.str());
    } else if (type != JointTypeFixed && type != JointTypeFloatingBase) {
      std::ostringstream errormsg;
      errormsg <<
               "Error: Invalid use of Joint constructor Joint(JointType type). Only allowed when type == JointTypeFixed or JointTypeSpherical."
               << std::endl;
      throw Errors::RBDLError(errormsg.str());
    }
  }
  Joint (JointType type, int degreesOfFreedom) :
    mJointAxes (NULL),
    mJointType (type),
    mDoFCount (0),
    q_index (0),
    custom_joint_index(-1)
  {
    if (type == JointTypeCustom) {
      mDoFCount   = degreesOfFreedom;
      mJointAxes  = new Math::SpatialVector[mDoFCount];
      for(unsigned int i=0; i<mDoFCount; ++i) {
        mJointAxes[i] = Math::SpatialVector (0., 0., 0., 0., 0., 0.);
      }
    } else {
      std::ostringstream errormsg;
      errormsg  << "Error: Invalid use of Joint constructor Joint(JointType"
                << " type, int degreesOfFreedom). Only allowed when "
                << "type  == JointTypeCustom."
                << std::endl;
      throw Errors::RBDLError(errormsg.str());
    }
  }
  Joint (const Joint &joint) :
    mJointType (joint.mJointType),
    mDoFCount (joint.mDoFCount),
    q_index (joint.q_index),
    custom_joint_index(joint.custom_joint_index)
  {
    mJointAxes = new Math::SpatialVector[mDoFCount];
 
    for (unsigned int i = 0; i < mDoFCount; i++) {
      mJointAxes[i] = joint.mJointAxes[i];
    }
  };
  Joint& operator= (const Joint &joint)
  {
    if (this != &joint) {
      if (mDoFCount > 0) {
        assert (mJointAxes);
        delete[] mJointAxes;
      }
      mJointType = joint.mJointType;
      mDoFCount = joint.mDoFCount;
      custom_joint_index = joint.custom_joint_index;
      mJointAxes = new Math::SpatialVector[mDoFCount];
 
      for (unsigned int i = 0; i < mDoFCount; i++) {
        mJointAxes[i] = joint.mJointAxes[i];
      }
 
      q_index = joint.q_index;
    }
    return *this;
  }
  ~Joint()
  {
    if (mJointAxes) {
      assert (mJointAxes);
      delete[] mJointAxes;
      mJointAxes = NULL;
      mDoFCount = 0;
      custom_joint_index = -1;
    }
  }
 
  Joint (
    const JointType joint_type,
    const Math::Vector3d &joint_axis
  )
  {
    mDoFCount = 1;
    mJointAxes = new Math::SpatialVector[mDoFCount];
 
    // Some assertions, as we concentrate on simple cases
 
    // Only rotation around the Z-axis
    assert ( joint_type == JointTypeRevolute || joint_type == JointTypePrismatic );
 
    mJointType = joint_type;
 
    if (joint_type == JointTypeRevolute) {
      // make sure we have a unit axis
      mJointAxes[0].set (
        joint_axis[0],
        joint_axis[1],
        joint_axis[2],
        0., 0., 0.
      );
 
    } else if (joint_type == JointTypePrismatic) {
      // make sure we have a unit axis
#ifndef RBDL_USE_CASADI_MATH
      assert (joint_axis.squaredNorm() == 1.);
#endif
 
      mJointAxes[0].set (
        0., 0., 0.,
        joint_axis[0],
        joint_axis[1],
        joint_axis[2]
      );
    }
  }
 
  Joint (
    const Math::SpatialVector &axis_0
  )
  {
    mDoFCount = 1;
    mJointAxes = new Math::SpatialVector[mDoFCount];
    mJointAxes[0] = Math::SpatialVector (axis_0);
    
    // TODO this has to be properly determined AND test case. Try Matt's dot product idea
#ifdef RBDL_USE_CASADI_MATH
    if (!axis_0[0].is_zero()) {
#else
    if (axis_0 == Math::SpatialVector(1., 0., 0., 0., 0., 0.)) {
#endif
      mJointType = JointTypeRevoluteX;
#ifdef RBDL_USE_CASADI_MATH
    } else if (!axis_0[1].is_zero()) {
#else
    } else if (axis_0 == Math::SpatialVector(0., 1., 0., 0., 0., 0.)) {
#endif
      mJointType = JointTypeRevoluteY;
#ifdef RBDL_USE_CASADI_MATH
    } else if (!axis_0[2].is_zero()) {
#else
    } else if (axis_0 == Math::SpatialVector(0., 0., 1., 0., 0., 0.)) {
#endif
      mJointType = JointTypeRevoluteZ;
#ifdef RBDL_USE_CASADI_MATH
    } else if (axis_0[0].is_zero() && axis_0[1].is_zero() && axis_0[2].is_zero()) {
#else
    } else if (axis_0[0] == 0. && axis_0[1] == 0. && axis_0[2] == 0.) {
#endif
      mJointType = JointTypePrismatic;
    } else {
      mJointType = JointTypeHelical;
    }
    validate_spatial_axis (mJointAxes[0]);
  }
 
  Joint (
    const Math::SpatialVector &axis_0,
    const Math::SpatialVector &axis_1
  )
  {
    mJointType = JointType2DoF;
    mDoFCount = 2;
 
    mJointAxes = new Math::SpatialVector[mDoFCount];
    mJointAxes[0] = axis_0;
    mJointAxes[1] = axis_1;
 
    validate_spatial_axis (mJointAxes[0]);
    validate_spatial_axis (mJointAxes[1]);
  }
 
  Joint (
    const Math::SpatialVector &axis_0,
    const Math::SpatialVector &axis_1,
    const Math::SpatialVector &axis_2
  )
  {
    mJointType = JointType3DoF;
    mDoFCount = 3;
 
    mJointAxes = new Math::SpatialVector[mDoFCount];
 
    mJointAxes[0] = axis_0;
    mJointAxes[1] = axis_1;
    mJointAxes[2] = axis_2;
 
    validate_spatial_axis (mJointAxes[0]);
    validate_spatial_axis (mJointAxes[1]);
    validate_spatial_axis (mJointAxes[2]);
  }
 
  Joint (
    const Math::SpatialVector &axis_0,
    const Math::SpatialVector &axis_1,
    const Math::SpatialVector &axis_2,
    const Math::SpatialVector &axis_3
  )
  {
    mJointType = JointType4DoF;
    mDoFCount = 4;
 
    mJointAxes = new Math::SpatialVector[mDoFCount];
 
    mJointAxes[0] = axis_0;
    mJointAxes[1] = axis_1;
    mJointAxes[2] = axis_2;
    mJointAxes[3] = axis_3;
 
    validate_spatial_axis (mJointAxes[0]);
    validate_spatial_axis (mJointAxes[1]);
    validate_spatial_axis (mJointAxes[2]);
    validate_spatial_axis (mJointAxes[3]);
  }
 
  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
  )
  {
    mJointType = JointType5DoF;
    mDoFCount = 5;
 
    mJointAxes = new Math::SpatialVector[mDoFCount];
 
    mJointAxes[0] = axis_0;
    mJointAxes[1] = axis_1;
    mJointAxes[2] = axis_2;
    mJointAxes[3] = axis_3;
    mJointAxes[4] = axis_4;
 
    validate_spatial_axis (mJointAxes[0]);
    validate_spatial_axis (mJointAxes[1]);
    validate_spatial_axis (mJointAxes[2]);
    validate_spatial_axis (mJointAxes[3]);
    validate_spatial_axis (mJointAxes[4]);
  }
 
  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
  )
  {
    mJointType = JointType6DoF;
    mDoFCount = 6;
 
    mJointAxes = new Math::SpatialVector[mDoFCount];
 
    mJointAxes[0] = axis_0;
    mJointAxes[1] = axis_1;
    mJointAxes[2] = axis_2;
    mJointAxes[3] = axis_3;
    mJointAxes[4] = axis_4;
    mJointAxes[5] = axis_5;
 
    validate_spatial_axis (mJointAxes[0]);
    validate_spatial_axis (mJointAxes[1]);
    validate_spatial_axis (mJointAxes[2]);
    validate_spatial_axis (mJointAxes[3]);
    validate_spatial_axis (mJointAxes[4]);
    validate_spatial_axis (mJointAxes[5]);
  }
 
  bool validate_spatial_axis (Math::SpatialVector &axis)
  {
#ifdef RBDL_USE_CASADI_MATH
      // If using casadi, the axes won't be validated
      return true;
#else
    bool axis_rotational = false;
    bool axis_translational = false;
 
    Math::Vector3d rotation (axis[0], axis[1], axis[2]);
    Math::Vector3d translation (axis[3], axis[4], axis[5]);
 
    if (fabs(rotation.norm()) > 1.0e-8) {
      axis_rotational = true;
    }
 
    if (fabs(translation.norm()) > 1.0e-8) {
      axis_translational = true;
    }
 
    if(axis_rotational && rotation.norm() - 1.0 > 1.0e-8) {
      std::cerr << "Warning: joint rotation axis is not unit!" << std::endl;
    }
 
    if(axis_translational && translation.norm() - 1.0 > 1.0e-8) {
      std::cerr << "Warning: joint translation axis is not unit!" << std::endl;
    }
 
    return axis_rotational != axis_translational;
#endif
  }
 
  Math::SpatialVector* mJointAxes;
  JointType mJointType;
  // have here a value of 0 and their actual numbers of degrees of freedom
  // can be obtained using the CustomJoint structure.
  unsigned int mDoFCount;
  unsigned int q_index;
  unsigned int custom_joint_index;
};
 
RBDL_DLLAPI
void jcalc (
  Model &model,
  unsigned int joint_id,
  const Math::VectorNd &q,
  const Math::VectorNd &qdot
);
 
RBDL_DLLAPI
Math::SpatialTransform jcalc_XJ (
  Model &model,
  unsigned int joint_id,
  const Math::VectorNd &q);
 
RBDL_DLLAPI
void jcalc_X_lambda_S (
  Model &model,
  unsigned int joint_id,
  const Math::VectorNd &q
);
 
struct RBDL_DLLAPI CustomJoint {
  CustomJoint()
  { }
  virtual ~CustomJoint() {};
 
  virtual void jcalc (Model &model,
                      unsigned int joint_id,
                      const Math::VectorNd &q,
                      const Math::VectorNd &qdot
                     ) = 0;
  virtual void jcalc_X_lambda_S (Model &model,
                                 unsigned int joint_id,
                                 const Math::VectorNd &q
                                ) = 0;
 
  unsigned int mDoFCount;
  Math::SpatialTransform XJ;
  Math::MatrixNd S;
  Math::MatrixNd U;
  Math::MatrixNd Dinv;
  Math::VectorNd u;
  Math::VectorNd d_u;
};
 
}
 
/* RBDL_JOINT_H */
#endif