Joint.h

Go to the documentation of this file.

/*
 * Original Copyright (c) 2011-2016 Martin Felis <martin.felis@iwr.uni-heidelberg.de>
 *
 *
 * RDL - Robot Dynamics Library
 * Modifications Copyright (c) 2017 Jordan Lack <jlack1987@gmail.com>
 *
 * Licensed under the zlib license. See LICENSE for more details.
 */

#ifndef __RDL_JOINT_H__
#define __RDL_JOINT_H__

#include "rdl_dynamics/SpatialAlgebraOperators.h"
#include <assert.h>
#include <iostream>
#include "rdl_dynamics/RdlExceptions.hpp"

namespace RobotDynamics
{
struct Model;

enum JointType
{
    JointTypeUndefined = 0,
    JointTypeRevolute,
    JointTypePrismatic,
    JointTypeRevoluteX,
    JointTypeRevoluteY,
    JointTypeRevoluteZ,
    JointTypeSpherical,  
                         // angular velocity for joint velocity variables.
    JointTypeEulerZYX,   
                         // multi DoF joints).
    JointTypeEulerXYZ,   
                         // multi DoF joints).
    JointTypeEulerYXZ,   
                         // multi DoF joints).
    JointTypeTranslationXYZ,
    JointTypeFloatingBase,  
    JointTypeFixed,         
                            // the parent body.
    JointType1DoF,
    JointType2DoF,    
    JointType3DoF,    
    JointType4DoF,    
    JointType5DoF,    
    JointType6DoF,    
    JointTypeCustom,  
};

struct Joint
{
    Joint() : mJointAxes(nullptr), mJointType(JointTypeUndefined), mDoFCount(0), q_index(0), custom_joint_index(-1)
    {
    }

    explicit Joint(JointType type) : mJointAxes(nullptr), 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 == 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];
        }
        else if (type == JointTypeCustom)
        {
            // This constructor cannot be used for a JointTypeCustom because
            // we have no idea what mDoFCount is.
            throw RdlException("Error: Invalid use of Joint constructor Joint(JointType type). Only allowed when type != JointTypeCustom");
        }
        else if ((type != JointTypeFixed) && (type != JointTypeFloatingBase))
        {
            throw RdlException("Error: Invalid use of Joint constructor Joint(JointType type).");
        }
    }

    Joint(JointType type, int degreesOfFreedom) : mJointAxes(nullptr), 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
        {
            throw RdlException("Error: Invalid use of Joint constructor Joint(JointType type, int degreesOfFreedom). Only allowed when type  == JointTypeCustom.");
        }
    }

    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 = nullptr;
            mDoFCount = 0;
            custom_joint_index = -1;
        }
    }

    Joint(const JointType joint_type, const Math::Vector3d& joint_axis) : mDoFCount(1), q_index(0), custom_joint_index(-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
            assert(joint_axis.squaredNorm() == 1.);

            mJointAxes[0].set(0., 0., 0., joint_axis[0], joint_axis[1], joint_axis[2]);
        }
    }

    explicit Joint(const Math::SpatialVector& axis_0) : mDoFCount(1), q_index(0), custom_joint_index(-1)
    {
        mJointAxes = new Math::SpatialVector[mDoFCount];
        mJointAxes[0] = Math::SpatialVector(axis_0);

        if (axis_0 == Math::SpatialVector(1., 0., 0., 0., 0., 0.))
        {
            mJointType = JointTypeRevoluteX;
        }
        else if (axis_0 == Math::SpatialVector(0., 1., 0., 0., 0., 0.))
        {
            mJointType = JointTypeRevoluteY;
        }
        else if (axis_0 == Math::SpatialVector(0., 0., 1., 0., 0., 0.))
        {
            mJointType = JointTypeRevoluteZ;
        }
        else
        {
            mJointType = JointType1DoF;
        }
        validate_spatial_axis(mJointAxes[0]);
    }

    Joint(const Math::SpatialVector& axis_0, const Math::SpatialVector& axis_1) : mJointType(JointType2DoF), mDoFCount(2), q_index(0), custom_joint_index(-1)
    {
        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), q_index(0), custom_joint_index(-1)
    {
        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), q_index(0), custom_joint_index(-1)
    {
        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), q_index(0), custom_joint_index(-1)
    {
        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), q_index(0), custom_joint_index(-1)
    {
        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)
    {
        if (fabs(axis.norm() - 1.0) > 1.0e-8)
        {
            std::cerr << "Warning: joint axis is not unit!" << std::endl;
        }

        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(translation.norm()) < 1.0e-8)
        {
            axis_rotational = true;
        }

        if (fabs(rotation.norm()) < 1.0e-8)
        {
            axis_translational = true;
        }

        return axis_rotational || axis_translational;
    }

    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;
};

void jcalc(Model& model, unsigned int joint_id, const Math::VectorNd& q, const Math::VectorNd& qdot);

Math::SpatialTransform jcalc_XJ(Model& model, unsigned int joint_id, const Math::VectorNd& q);

void jcalc_X_lambda_S(Model& model, unsigned int joint_id, const Math::VectorNd& q);

struct CustomJoint
{
    CustomJoint() : mDoFCount(0)
    {
    }

    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 S_o;        
    Math::MatrixNd U;
    Math::MatrixNd Dinv;
    Math::VectorNd u;
    Math::VectorNd d_u;
    Math::VectorNd ndof0_vec; 
};
}  // namespace RobotDynamics

/* RDL_JOINT_H */
#endif  // ifndef __RDL_JOINT_H__