rdl_eigenmath.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_EIGENMATH_H__
#define __RDL_EIGENMATH_H__

#include <Eigen/Dense>
#include <Eigen/StdVector>
#include <Eigen/QR>
#include <eigen3/Eigen/Eigen>

namespace RobotDynamics
{
namespace Math
{
typedef Eigen::Matrix<double, 6, 3> Matrix63;
typedef Eigen::VectorXd VectorNd;
typedef Eigen::MatrixXd MatrixNd;
typedef Eigen::AngleAxisd AxisAngle;
}  // namespace Math
}  // namespace RobotDynamics

namespace RobotDynamics
{
namespace Math
{
class SpatialTransform;

class TransformableGeometricObject
{
  public:
    virtual void transform(const RobotDynamics::Math::SpatialTransform& X) = 0;
};

class Vector3d : public Eigen::Vector3d, public TransformableGeometricObject
{
  public:
    typedef Eigen::Vector3d Base;

    template <typename OtherDerived>
    // cppcheck-suppress noExplicitConstructor
    Vector3d(const Eigen::MatrixBase<OtherDerived>& other) : Eigen::Vector3d(other)
    {
    }

    template <typename OtherDerived>
    Vector3d& operator=(const Eigen::MatrixBase<OtherDerived>& other)
    {
        this->Base::operator=(other);
        return *this;
    }

    EIGEN_STRONG_INLINE Vector3d() : Vector3d(0., 0., 0.)
    {
    }

    EIGEN_STRONG_INLINE Vector3d(const double& v0, const double& v1, const double& v2)
    {
        Base::_check_template_params();

        (*this) << v0, v1, v2;
    }

    void set(const Eigen::Vector3d& v)
    {
        Base::_check_template_params();

        set(v[0], v[1], v[2]);
    }

    void set(const double& v0, const double& v1, const double& v2)
    {
        Base::_check_template_params();

        (*this) << v0, v1, v2;
    }

    inline void transform(const RobotDynamics::Math::SpatialTransform& X);

    inline Vector3d transform_copy(const RobotDynamics::Math::SpatialTransform& X) const;
};

class Matrix3d : public Eigen::Matrix3d
{
  public:
    typedef Eigen::Matrix3d Base;

    template <typename OtherDerived>
    // cppcheck-suppress noExplicitConstructor
    Matrix3d(const Eigen::MatrixBase<OtherDerived>& other) : Eigen::Matrix3d(other)
    {
    }

    template <typename OtherDerived>
    Matrix3d& operator=(const Eigen::MatrixBase<OtherDerived>& other)
    {
        this->Base::operator=(other);
        return *this;
    }

    EIGEN_STRONG_INLINE Matrix3d()
    {
    }

    EIGEN_STRONG_INLINE Matrix3d(const double& m00, const double& m01, const double& m02, const double& m10, const double& m11, const double& m12, const double& m20,
                                 const double& m21, const double& m22)
    {
        Base::_check_template_params();

        (*this) << m00, m01, m02, m10, m11, m12, m20, m21, m22;
    }
};

class Vector4d : public Eigen::Vector4d
{
  public:
    typedef Eigen::Vector4d Base;

    template <typename OtherDerived>
    // cppcheck-suppress noExplicitConstructor
    Vector4d(const Eigen::MatrixBase<OtherDerived>& other) : Eigen::Vector4d(other)
    {
    }

    template <typename OtherDerived>
    Vector4d& operator=(const Eigen::MatrixBase<OtherDerived>& other)
    {
        this->Base::operator=(other);
        return *this;
    }

    EIGEN_STRONG_INLINE Vector4d()
    {
    }

    EIGEN_STRONG_INLINE Vector4d(const double& v0, const double& v1, const double& v2, const double& v3)
    {
        Base::_check_template_params();

        (*this) << v0, v1, v2, v3;
    }

    void set(const double& v0, const double& v1, const double& v2, const double& v3)
    {
        Base::_check_template_params();

        (*this) << v0, v1, v2, v3;
    }
};

class SpatialVector : public Eigen::Matrix<double, 6, 1>
{
  public:
    typedef Eigen::Matrix<double, 6, 1> Base;

    template <typename OtherDerived>
    // cppcheck-suppress noExplicitConstructor
    SpatialVector(const Eigen::MatrixBase<OtherDerived>& other) : Eigen::Matrix<double, 6, 1>(other)
    {
    }

    template <typename OtherDerived>
    SpatialVector& operator=(const Eigen::MatrixBase<OtherDerived>& other)
    {
        this->Base::operator=(other);
        return *this;
    }

    EIGEN_STRONG_INLINE SpatialVector()
    {
        (*this) << 0., 0., 0., 0., 0., 0.;
    }

    EIGEN_STRONG_INLINE SpatialVector(const double& v0, const double& v1, const double& v2, const double& v3, const double& v4, const double& v5)
    {
        Base::_check_template_params();

        (*this) << v0, v1, v2, v3, v4, v5;
    }

    EIGEN_STRONG_INLINE void set(const double& v0, const double& v1, const double& v2, const double& v3, const double& v4, const double& v5)
    {
        Base::_check_template_params();

        (*this) << v0, v1, v2, v3, v4, v5;
    }

    EIGEN_STRONG_INLINE Vector3d getAngularPart() const
    {
        return Vector3d(this->data()[0], this->data()[1], this->data()[2]);
    }

    EIGEN_STRONG_INLINE Vector3d getLinearPart() const
    {
        return Vector3d(this->data()[3], this->data()[4], this->data()[5]);
    }

    inline void setAngularPart(const Vector3d& v)
    {
        this->data()[0] = v(0);
        this->data()[1] = v(1);
        this->data()[2] = v(2);
    }

    inline void setLinearPart(const Vector3d& v)
    {
        this->data()[3] = v(0);
        this->data()[4] = v(1);
        this->data()[5] = v(2);
    }

    EIGEN_STRONG_INLINE void set(const Vector3d& angularPart, const Vector3d& linearPart)
    {
        Base::_check_template_params();

        (*this) << angularPart[0], angularPart[1], angularPart[2], linearPart[0], linearPart[1], linearPart[2];
    }
};

class Matrix4d : public Eigen::Matrix<double, 4, 4>
{
  public:
    typedef Eigen::Matrix<double, 4, 4> Base;

    template <typename OtherDerived>
    // cppcheck-suppress noExplicitConstructor
    Matrix4d(const Eigen::MatrixBase<OtherDerived>& other) : Eigen::Matrix<double, 4, 4>(other)
    {
    }

    template <typename OtherDerived>
    Matrix4d& operator=(const Eigen::MatrixBase<OtherDerived>& other)
    {
        this->Base::operator=(other);
        return *this;
    }

    EIGEN_STRONG_INLINE Matrix4d()
    {
    }

    EIGEN_STRONG_INLINE Matrix4d(const Scalar& m00, const Scalar& m01, const Scalar& m02, const Scalar& m03, const Scalar& m10, const Scalar& m11, const Scalar& m12,
                                 const Scalar& m13, const Scalar& m20, const Scalar& m21, const Scalar& m22, const Scalar& m23, const Scalar& m30, const Scalar& m31,
                                 const Scalar& m32, const Scalar& m33)
    {
        Base::_check_template_params();

        (*this) << m00, m01, m02, m03, m10, m11, m12, m13, m20, m21, m22, m23, m30, m31, m32, m33;
    }

    void set(const Scalar& m00, const Scalar& m01, const Scalar& m02, const Scalar& m03, const Scalar& m10, const Scalar& m11, const Scalar& m12, const Scalar& m13,
             const Scalar& m20, const Scalar& m21, const Scalar& m22, const Scalar& m23, const Scalar& m30, const Scalar& m31, const Scalar& m32, const Scalar& m33)
    {
        Base::_check_template_params();

        (*this) << m00, m01, m02, m03, m10, m11, m12, m13, m20, m21, m22, m23, m30, m31, m32, m33;
    }
};

class SpatialMatrix : public Eigen::Matrix<double, 6, 6>
{
  public:
    typedef Eigen::Matrix<double, 6, 6> Base;

    template <typename OtherDerived>
    // cppcheck-suppress noExplicitConstructor
    SpatialMatrix(const Eigen::MatrixBase<OtherDerived>& other) : Eigen::Matrix<double, 6, 6>(other)
    {
    }

    template <typename OtherDerived>
    SpatialMatrix& operator=(const Eigen::MatrixBase<OtherDerived>& other)
    {
        this->Base::operator=(other);
        return *this;
    }

    EIGEN_STRONG_INLINE SpatialMatrix()
    {
        Base::_check_template_params();

        (*this) << 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.;
    }

    EIGEN_STRONG_INLINE SpatialMatrix(const Scalar& m00, const Scalar& m01, const Scalar& m02, const Scalar& m03, const Scalar& m04, const Scalar& m05, const Scalar& m10,
                                      const Scalar& m11, const Scalar& m12, const Scalar& m13, const Scalar& m14, const Scalar& m15, const Scalar& m20, const Scalar& m21,
                                      const Scalar& m22, const Scalar& m23, const Scalar& m24, const Scalar& m25, const Scalar& m30, const Scalar& m31, const Scalar& m32,
                                      const Scalar& m33, const Scalar& m34, const Scalar& m35, const Scalar& m40, const Scalar& m41, const Scalar& m42, const Scalar& m43,
                                      const Scalar& m44, const Scalar& m45, const Scalar& m50, const Scalar& m51, const Scalar& m52, const Scalar& m53, const Scalar& m54,
                                      const Scalar& m55)
    {
        Base::_check_template_params();

        (*this) << m00, m01, m02, m03, m04, m05, m10, m11, m12, m13, m14, m15, m20, m21, m22, m23, m24, m25, m30, m31, m32, m33, m34, m35, m40, m41, m42, m43, m44, m45,
            m50, m51, m52, m53, m54, m55;
    }

    void set(const Scalar& m00, const Scalar& m01, const Scalar& m02, const Scalar& m03, const Scalar& m04, const Scalar& m05, const Scalar& m10, const Scalar& m11,
             const Scalar& m12, const Scalar& m13, const Scalar& m14, const Scalar& m15, const Scalar& m20, const Scalar& m21, const Scalar& m22, const Scalar& m23,
             const Scalar& m24, const Scalar& m25, const Scalar& m30, const Scalar& m31, const Scalar& m32, const Scalar& m33, const Scalar& m34, const Scalar& m35,
             const Scalar& m40, const Scalar& m41, const Scalar& m42, const Scalar& m43, const Scalar& m44, const Scalar& m45, const Scalar& m50, const Scalar& m51,
             const Scalar& m52, const Scalar& m53, const Scalar& m54, const Scalar& m55)
    {
        Base::_check_template_params();

        (*this) << m00, m01, m02, m03, m04, m05, m10, m11, m12, m13, m14, m15, m20, m21, m22, m23, m24, m25, m30, m31, m32, m33, m34, m35, m40, m41, m42, m43, m44, m45,
            m50, m51, m52, m53, m54, m55;
    }
};

struct SpatialTransform
{
    SpatialTransform() : E(Matrix3d::Identity(3, 3)), r(Vector3d::Zero(3, 1))
    {
    }

    SpatialTransform(const Matrix3d& rotation, const double x, const double y, const double z) : E(rotation), r(x, y, z)
    {
    }

    SpatialTransform(const Matrix3d& rotation, const Vector3d& translation) : E(rotation), r(translation)
    {
    }

    explicit SpatialTransform(const Matrix3d& rotation) : E(rotation), r(0., 0., 0.)
    {
    }

    explicit SpatialTransform(const Vector3d& translation) : E(1., 0., 0., 0., 1., 0., 0., 0., 1.), r(translation)
    {
    }

    SpatialVector apply(const SpatialVector& v_sp) const
    {
        Vector3d v_rxw(v_sp[3] - r[1] * v_sp[2] + r[2] * v_sp[1], v_sp[4] - r[2] * v_sp[0] + r[0] * v_sp[2], v_sp[5] - r[0] * v_sp[1] + r[1] * v_sp[0]);

        return SpatialVector(E(0, 0) * v_sp[0] + E(0, 1) * v_sp[1] + E(0, 2) * v_sp[2], E(1, 0) * v_sp[0] + E(1, 1) * v_sp[1] + E(1, 2) * v_sp[2],
                             E(2, 0) * v_sp[0] + E(2, 1) * v_sp[1] + E(2, 2) * v_sp[2], E(0, 0) * v_rxw[0] + E(0, 1) * v_rxw[1] + E(0, 2) * v_rxw[2],
                             E(1, 0) * v_rxw[0] + E(1, 1) * v_rxw[1] + E(1, 2) * v_rxw[2], E(2, 0) * v_rxw[0] + E(2, 1) * v_rxw[1] + E(2, 2) * v_rxw[2]);
    }

    SpatialVector applyTranspose(const SpatialVector& f_sp) const
    {
        Vector3d E_T_f(E(0, 0) * f_sp[3] + E(1, 0) * f_sp[4] + E(2, 0) * f_sp[5], E(0, 1) * f_sp[3] + E(1, 1) * f_sp[4] + E(2, 1) * f_sp[5],
                       E(0, 2) * f_sp[3] + E(1, 2) * f_sp[4] + E(2, 2) * f_sp[5]);

        return SpatialVector(E(0, 0) * f_sp[0] + E(1, 0) * f_sp[1] + E(2, 0) * f_sp[2] - r[2] * E_T_f[1] + r[1] * E_T_f[2],
                             E(0, 1) * f_sp[0] + E(1, 1) * f_sp[1] + E(2, 1) * f_sp[2] + r[2] * E_T_f[0] - r[0] * E_T_f[2],
                             E(0, 2) * f_sp[0] + E(1, 2) * f_sp[1] + E(2, 2) * f_sp[2] - r[1] * E_T_f[0] + r[0] * E_T_f[1], E_T_f[0], E_T_f[1], E_T_f[2]);
    }

    SpatialVector applyAdjoint(const SpatialVector& f_sp) const
    {
        Vector3d En_rxf = E * (Vector3d(f_sp[0], f_sp[1], f_sp[2]) - r.cross(Vector3d(f_sp[3], f_sp[4], f_sp[5])));

        return SpatialVector(En_rxf[0], En_rxf[1], En_rxf[2], E(0, 0) * f_sp[3] + E(0, 1) * f_sp[4] + E(0, 2) * f_sp[5],
                             E(1, 0) * f_sp[3] + E(1, 1) * f_sp[4] + E(1, 2) * f_sp[5], E(2, 0) * f_sp[3] + E(2, 1) * f_sp[4] + E(2, 2) * f_sp[5]);
    }

    SpatialMatrix toMatrix() const
    {
        Matrix3d _Erx = E * Matrix3d(0., -r[2], r[1], r[2], 0., -r[0], -r[1], r[0], 0.);
        SpatialMatrix result;

        result.block<3, 3>(0, 0) = E;
        result.block<3, 3>(0, 3) = Matrix3d::Zero(3, 3);
        result.block<3, 3>(3, 0) = -_Erx;
        result.block<3, 3>(3, 3) = E;

        return result;
    }

    SpatialMatrix toMatrixAdjoint() const
    {
        Matrix3d _Erx = E * Matrix3d(0., -r[2], r[1], r[2], 0., -r[0], -r[1], r[0], 0.);
        SpatialMatrix result;

        result.block<3, 3>(0, 0) = E;
        result.block<3, 3>(0, 3) = -_Erx;
        result.block<3, 3>(3, 0) = Matrix3d::Zero(3, 3);
        result.block<3, 3>(3, 3) = E;

        return result;
    }

    SpatialMatrix toMatrixTranspose() const
    {
        Matrix3d _Erx = E * Matrix3d(0., -r[2], r[1], r[2], 0., -r[0], -r[1], r[0], 0.);
        SpatialMatrix result;

        result.block<3, 3>(0, 0) = E.transpose();
        result.block<3, 3>(0, 3) = -_Erx.transpose();
        result.block<3, 3>(3, 0) = Matrix3d::Zero(3, 3);
        result.block<3, 3>(3, 3) = E.transpose();

        return result;
    }

    SpatialTransform inverse() const
    {
        return SpatialTransform(E.transpose(), -E * r);
    }

    void invert()
    {
        r = -E * r;
        E.transposeInPlace();
    }

    SpatialTransform operator*(const SpatialTransform& XT) const
    {
        return SpatialTransform(E * XT.E, XT.r + XT.E.transpose() * r);
    }

    void operator*=(const SpatialTransform& XT)
    {
        r = XT.r + XT.E.transpose() * r;
        E *= XT.E;
    }

    Matrix3d E;
    Vector3d r;
};

Vector3d Vector3d::transform_copy(const RobotDynamics::Math::SpatialTransform& X) const
{
    return X.E * (*this);
}

void Vector3d::transform(const RobotDynamics::Math::SpatialTransform& X)
{
    *this = X.E * (*this);
}
}  // namespace Math
}  // namespace RobotDynamics

EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(RobotDynamics::Math::SpatialVector)
EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(RobotDynamics::Math::SpatialMatrix)
EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(RobotDynamics::Math::Matrix63)
EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(RobotDynamics::Math::Vector4d)

/* ___RDL_EIGENMATH_H__ */
#endif  // ifndef __RDL_EIGENMATH_H__