frames.hpp

Go to the documentation of this file.

/***************************************************************************
                        frames.hpp `-  description
                       -------------------------
    begin                : June 2006
    copyright            : (C) 2006 Erwin Aertbelien
    email                : firstname.lastname@mech.kuleuven.be

 History (only major changes)( AUTHOR-Description ) :

 ***************************************************************************
 *   This library is free software; you can redistribute it and/or         *
 *   modify it under the terms of the GNU Lesser General Public            *
 *   License as published by the Free Software Foundation; either          *
 *   version 2.1 of the License, or (at your option) any later version.    *
 *                                                                         *
 *   This library is distributed in the hope that it will be useful,       *
 *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
 *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU     *
 *   Lesser General Public License for more details.                       *
 *                                                                         *
 *   You should have received a copy of the GNU Lesser General Public      *
 *   License along with this library; if not, write to the Free Software   *
 *   Foundation, Inc., 59 Temple Place,                                    *
 *   Suite 330, Boston, MA  02111-1307  USA                                *
 *                                                                         *
 ***************************************************************************/

#ifndef KDL_FRAMES_H
#define KDL_FRAMES_H


#include "utilities/kdl-config.h"
#include "utilities/utility.h"
#include "utilities/hash_combine.h"

#include <functional>


namespace KDL {


class Vector;
class Rotation;
class Frame;
class Wrench;
class Twist;
class Vector2;
class Rotation2;
class Frame2;


// Equal is friend function, but default arguments for friends are forbidden (§8.3.6.4)
inline bool Equal(const Vector& a,const Vector& b,double eps=epsilon);
inline bool Equal(const Frame& a,const Frame& b,double eps=epsilon);
inline bool Equal(const Twist& a,const Twist& b,double eps=epsilon);
inline bool Equal(const Wrench& a,const Wrench& b,double eps=epsilon);
inline bool Equal(const Vector2& a,const Vector2& b,double eps=epsilon);
inline bool Equal(const Rotation2& a,const Rotation2& b,double eps=epsilon);
inline bool Equal(const Frame2& a,const Frame2& b,double eps=epsilon);


class Vector
{
public:
    double data[3];
     inline Vector() {data[0]=data[1]=data[2] = 0.0;}

     inline Vector(double x,double y, double z);

     inline Vector(const Vector& arg);

     inline Vector& operator = ( const Vector& arg);

     inline double operator()(int index) const;

     inline double& operator() (int index);

     double operator[] ( int index ) const
       {
     return this->operator() ( index );
       }

     double& operator[] ( int index )
       {
     return this->operator() ( index );
       }

     inline double x() const;
     inline double y() const;
     inline double z() const;
     inline void x(double);
     inline void y(double);
     inline void z(double);

     inline void ReverseSign();


     inline Vector& operator-=(const Vector& arg);


     inline Vector& operator +=(const Vector& arg);

     inline friend Vector operator*(const Vector& lhs,double rhs);
     inline friend Vector operator*(double lhs,const Vector& rhs);

     inline friend Vector operator/(const Vector& lhs,double rhs);
     inline friend Vector operator+(const Vector& lhs,const Vector& rhs);
     inline friend Vector operator-(const Vector& lhs,const Vector& rhs);
     inline friend Vector operator*(const Vector& lhs,const Vector& rhs);
     inline friend Vector operator-(const Vector& arg);
     inline friend double dot(const Vector& lhs,const Vector& rhs);

     inline friend void SetToZero(Vector& v);

     inline static Vector Zero();

     double Normalize(double eps=epsilon);

     double Norm(double eps=epsilon) const;



     inline void Set2DXY(const Vector2& v);
     inline void Set2DYZ(const Vector2& v);
     inline void Set2DZX(const Vector2& v);
     inline void Set2DPlane(const Frame& F_someframe_XY,const Vector2& v_XY);


     inline friend bool Equal(const Vector& a,const Vector& b,double eps);

     inline friend bool operator==(const Vector& a,const Vector& b);
     inline friend bool operator!=(const Vector& a,const Vector& b);

     friend class Rotation;
     friend class Frame;
};


class Rotation
{
public:
    double data[9];

    inline Rotation() {
        *this = Rotation::Identity();
    }
    inline Rotation(double Xx,double Yx,double Zx,
                double Xy,double Yy,double Zy,
                double Xz,double Yz,double Zz);
    inline Rotation(const Vector& x,const Vector& y,const Vector& z);

    inline Rotation(const Rotation& arg);

     inline Rotation& operator=(const Rotation& arg);

     inline Vector operator*(const Vector& v) const;

     inline double& operator()(int i,int j);

     inline double operator() (int i,int j) const;

     friend Rotation operator *(const Rotation& lhs,const Rotation& rhs);

     inline void SetInverse();

     inline Rotation Inverse() const;

     inline Vector Inverse(const Vector& v) const;

     inline Wrench Inverse(const Wrench& arg) const;

     inline Twist Inverse(const Twist& arg) const;

     inline static Rotation Identity();


// = Rotations
    inline static Rotation RotX(double angle);
    inline static Rotation RotY(double angle);
    inline static Rotation RotZ(double angle);
    inline void DoRotX(double angle);
    inline void DoRotY(double angle);
    inline void DoRotZ(double angle);

    // @see Rot2 if you want to handle this error in another way.
    static Rotation Rot(const Vector& rotvec,double angle);

    static Rotation Rot2(const Vector& rotvec,double angle);

    Vector GetRot() const;

    double GetRotAngle(Vector& axis,double eps=epsilon) const;


    static Rotation EulerZYZ(double Alfa,double Beta,double Gamma);

    void GetEulerZYZ(double& alpha,double& beta,double& gamma) const;

    static Rotation Quaternion(double x,double y,double z, double w);
    
    void GetQuaternion(double& x,double& y,double& z, double& w) const;

    static Rotation RPY(double roll,double pitch,double yaw);

    void GetRPY(double& roll,double& pitch,double& yaw) const;


    inline static Rotation EulerZYX(double Alfa,double Beta,double Gamma) {
        return RPY(Gamma,Beta,Alfa);
    }

    inline void GetEulerZYX(double& Alfa,double& Beta,double& Gamma) const {
        GetRPY(Gamma,Beta,Alfa);
    }

     inline Twist operator * (const Twist& arg) const;

     inline Wrench operator * (const Wrench& arg) const;

     inline Vector UnitX() const {
         return Vector(data[0],data[3],data[6]);
     }

     inline void UnitX(const Vector& X) {
        data[0] = X(0);
        data[3] = X(1);
        data[6] = X(2);
     }

     inline Vector UnitY() const {
         return Vector(data[1],data[4],data[7]);
     }

     inline void UnitY(const Vector& X) {
        data[1] = X(0);
        data[4] = X(1);
        data[7] = X(2);
     }

     inline Vector UnitZ() const {
         return Vector(data[2],data[5],data[8]);
     }

     inline void UnitZ(const Vector& X) {
        data[2] = X(0);
        data[5] = X(1);
        data[8] = X(2);
     }

     friend bool Equal(const Rotation& a,const Rotation& b,double eps);

     friend bool operator==(const Rotation& a,const Rotation& b);
     friend bool operator!=(const Rotation& a,const Rotation& b);

     friend class Frame;
};
    bool operator==(const Rotation& a,const Rotation& b);
    bool Equal(const Rotation& a,const Rotation& b,double eps=epsilon);



class Frame {
public:
    Vector p;       
    Rotation M;     

public:

     inline Frame(const Rotation& R,const Vector& V);

     explicit inline Frame(const Vector& V);
     explicit inline Frame(const Rotation& R);

     inline Frame() {}
     inline Frame(const Frame& arg);

     //\TODO should be formulated as a constructor
     void Make4x4(double* d);

     inline double operator()(int i,int j);

     inline double operator() (int i,int j) const;

// = Inverse
     inline Frame Inverse() const;

     inline Vector Inverse(const Vector& arg) const;

     inline Wrench Inverse(const Wrench& arg) const;

     inline Twist  Inverse(const Twist& arg) const;

     inline Frame& operator = (const Frame& arg);

     inline Vector operator * (const Vector& arg) const;

     inline Wrench operator * (const Wrench& arg) const;

     inline Twist operator * (const Twist& arg) const;

     inline friend Frame operator *(const Frame& lhs,const Frame& rhs);

     inline static Frame Identity();

     inline void Integrate(const Twist& t_this,double frequency);

    /*
    // DH_Craig1989 : constructs a transformationmatrix
    // T_link(i-1)_link(i) with the Denavit-Hartenberg convention as
    // described in the Craigs book: Craig, J. J.,Introduction to
    // Robotics: Mechanics and Control, Addison-Wesley,
    // isbn:0-201-10326-5, 1986.
    //
    // Note that the frame is a redundant way to express the information
    // in the DH-convention.
    // \verbatim
    // Parameters in full : a(i-1),alpha(i-1),d(i),theta(i)
    //
    //  axis i-1 is connected by link i-1 to axis i numbering axis 1
    //  to axis n link 0 (immobile base) to link n
    //
    //  link length a(i-1) length of the mutual perpendicular line
    //  (normal) between the 2 axes.  This normal runs from (i-1) to
    //  (i) axis.
    //
    //  link twist alpha(i-1): construct plane perpendicular to the
    //  normal project axis(i-1) and axis(i) into plane angle from
    //  (i-1) to (i) measured in the direction of the normal
    //
    //  link offset d(i) signed distance between normal (i-1) to (i)
    //  and normal (i) to (i+1) along axis i joint angle theta(i)
    //  signed angle between normal (i-1) to (i) and normal (i) to
    //  (i+1) along axis i
    //
    //   First and last joints : a(0)= a(n) = 0
    //   alpha(0) = alpha(n) = 0
    //
    //   PRISMATIC : theta(1) = 0 d(1) arbitrarily
    //
    //   REVOLUTE : theta(1) arbitrarily d(1) = 0
    //
    //   Not unique : if intersecting joint axis 2 choices for normal
    //   Frame assignment of the DH convention : Z(i-1) follows axis
    //   (i-1) X(i-1) is the normal between axis(i-1) and axis(i)
    //   Y(i-1) follows out of Z(i-1) and X(i-1)
    //
    //     a(i-1)     = distance from Z(i-1) to Z(i) along X(i-1)
    //     alpha(i-1) = angle between Z(i-1) to Z(i) along X(i-1)
    //     d(i)       = distance from X(i-1) to X(i) along Z(i)
    //     theta(i)   = angle between X(i-1) to X(i) along X(i)
    // \endverbatim
    */
     static Frame DH_Craig1989(double a,double alpha,double d,double theta);

    // DH : constructs a transformationmatrix T_link(i-1)_link(i) with
    // the Denavit-Hartenberg convention as described in the original
    // publictation: Denavit, J. and Hartenberg, R. S., A kinematic
    // notation for lower-pair mechanisms based on matrices, ASME
    // Journal of Applied Mechanics, 23:215-221, 1955.

     static Frame DH(double a,double alpha,double d,double theta);


     inline friend bool Equal(const Frame& a,const Frame& b,double eps);

     inline friend bool operator==(const Frame& a,const Frame& b);
     inline friend bool operator!=(const Frame& a,const Frame& b);
};


class Twist {
public:
    Vector vel; 
    Vector rot; 
public:

    Twist():vel(),rot() {};

    Twist(const Vector& _vel,const Vector& _rot):vel(_vel),rot(_rot) {};

    inline Twist& operator-=(const Twist& arg);
    inline Twist& operator+=(const Twist& arg);
    inline double& operator()(int i);

    inline double operator()(int i) const;

     double operator[] ( int index ) const
       {
     return this->operator() ( index );
       }

     double& operator[] ( int index )
       {
     return this->operator() ( index );
       }

     inline friend Twist operator*(const Twist& lhs,double rhs);
     inline friend Twist operator*(double lhs,const Twist& rhs);
     inline friend Twist operator/(const Twist& lhs,double rhs);
     inline friend Twist operator+(const Twist& lhs,const Twist& rhs);
     inline friend Twist operator-(const Twist& lhs,const Twist& rhs);
     inline friend Twist operator-(const Twist& arg);
     inline friend double dot(const Twist& lhs,const Wrench& rhs);
     inline friend double dot(const Wrench& rhs,const Twist& lhs);
     inline friend void SetToZero(Twist& v);
    inline friend Twist operator*(const Twist& lhs,const Twist& rhs);
    inline friend Wrench operator*(const Twist& lhs,const Wrench& rhs);

     static inline Twist Zero();

     inline void ReverseSign();

     inline Twist RefPoint(const Vector& v_base_AB) const;


     inline friend bool Equal(const Twist& a,const Twist& b,double eps);

     inline friend bool operator==(const Twist& a,const Twist& b);
     inline friend bool operator!=(const Twist& a,const Twist& b);

// = Friends
    friend class Rotation;
    friend class Frame;
};


/*
class AccelerationTwist {
public:
    Vector trans; //!< The translational acceleration of that point
    Vector rot; //!< The rotational acceleration of that point.
public:

    AccelerationTwist():trans(),rot() {};

    AccelerationTwist(const Vector& _trans,const Vector& _rot):trans(_trans),rot(_rot) {};

    inline AccelerationTwist& operator-=(const AccelerationTwist& arg);
    inline AccelerationTwist& operator+=(const AccelerationTwist& arg);
    inline double& operator()(int i);

    inline double operator()(int i) const;

    double operator[] ( int index ) const
    {
        return this->operator() ( index );
    }

     double& operator[] ( int index )
     {
         return this->operator() ( index );
     }

     inline friend AccelerationTwist operator*(const AccelerationTwist& lhs,double rhs);
     inline friend AccelerationTwist operator*(double lhs,const AccelerationTwist& rhs);
     inline friend AccelerationTwist operator/(const AccelerationTwist& lhs,double rhs);
     inline friend AccelerationTwist operator+(const AccelerationTwist& lhs,const AccelerationTwist& rhs);
     inline friend AccelerationTwist operator-(const AccelerationTwist& lhs,const AccelerationTwist& rhs);
     inline friend AccelerationTwist operator-(const AccelerationTwist& arg);
     //inline friend double dot(const AccelerationTwist& lhs,const Wrench& rhs);
     //inline friend double dot(const Wrench& rhs,const AccelerationTwist& lhs);
     inline friend void SetToZero(AccelerationTwist& v);


     static inline AccelerationTwist Zero();

     inline void ReverseSign();

     inline AccelerationTwist RefPoint(const Vector& v_base_AB) const;


     inline friend bool Equal(const AccelerationTwist& a,const AccelerationTwist& b,double eps=epsilon);

     inline friend bool operator==(const AccelerationTwist& a,const AccelerationTwist& b);
     inline friend bool operator!=(const AccelerationTwist& a,const AccelerationTwist& b);

// = Friends
    friend class Rotation;
    friend class Frame;

};
*/
class Wrench
{
public:
    Vector force;       
    Vector torque;      
public:

    Wrench():force(),torque() {};
    Wrench(const Vector& _force,const Vector& _torque):force(_force),torque(_torque) {};

// = Operators
     inline Wrench& operator-=(const Wrench& arg);
     inline Wrench& operator+=(const Wrench& arg);

     inline double& operator()(int i);

     inline double operator()(int i) const;

     double operator[] ( int index ) const
       {
     return this->operator() ( index );
       }

     double& operator[] ( int index )
       {
     return this->operator() ( index );
       }

     inline friend Wrench operator*(const Wrench& lhs,double rhs);
     inline friend Wrench operator*(double lhs,const Wrench& rhs);
     inline friend Wrench operator/(const Wrench& lhs,double rhs);

     inline friend Wrench operator+(const Wrench& lhs,const Wrench& rhs);
     inline friend Wrench operator-(const Wrench& lhs,const Wrench& rhs);

     inline friend Wrench operator-(const Wrench& arg);

     inline friend void SetToZero(Wrench& v);

     static inline Wrench Zero();

     inline void ReverseSign();

     inline Wrench RefPoint(const Vector& v_base_AB) const;


     inline friend bool Equal(const Wrench& a,const Wrench& b,double eps);

     inline friend bool operator==(const Wrench& a,const Wrench& b);
     inline friend bool operator!=(const Wrench& a,const Wrench& b);

    friend class Rotation;
    friend class Frame;
};


class Vector2
{
    double data[2];
public:
     Vector2() {data[0]=data[1] = 0.0;}
     inline Vector2(double x,double y);
     inline Vector2(const Vector2& arg);

     inline Vector2& operator = ( const Vector2& arg);

     inline double operator()(int index) const;

     inline double& operator() (int index);

    double operator[] ( int index ) const
    {
        return this->operator() ( index );
    }

    double& operator[] ( int index )
    {
        return this->operator() ( index );
    }

    inline double x() const;
    inline double y() const;
    inline void x(double);
    inline void y(double);

     inline void ReverseSign();
     inline Vector2& operator-=(const Vector2& arg);
     inline Vector2& operator +=(const Vector2& arg);


     inline friend Vector2 operator*(const Vector2& lhs,double rhs);
     inline friend Vector2 operator*(double lhs,const Vector2& rhs);
     inline friend Vector2 operator/(const Vector2& lhs,double rhs);
     inline friend Vector2 operator+(const Vector2& lhs,const Vector2& rhs);
     inline friend Vector2 operator-(const Vector2& lhs,const Vector2& rhs);
     inline friend Vector2 operator*(const Vector2& lhs,const Vector2& rhs);
     inline friend Vector2 operator-(const Vector2& arg);
     inline friend void SetToZero(Vector2& v);

     inline static Vector2 Zero();

     double Normalize(double eps=epsilon);

     double Norm(double eps=epsilon) const;

     inline void Set3DXY(const Vector& v);

     inline void Set3DYZ(const Vector& v);

     inline void Set3DZX(const Vector& v);

     inline void Set3DPlane(const Frame& F_someframe_XY,const Vector& v_someframe);


     inline friend bool Equal(const Vector2& a,const Vector2& b,double eps);

    inline friend bool operator==(const Vector2& a,const Vector2& b);
    inline friend bool operator!=(const Vector2& a,const Vector2& b);

    friend class Rotation2;
};


class Rotation2
{
    double s,c;
public:
    Rotation2() {c=1.0;s=0.0;}

    explicit Rotation2(double angle_rad):s(sin(angle_rad)),c(cos(angle_rad)) {}

    Rotation2(double ca,double sa):s(sa),c(ca){}

    Rotation2(const Rotation2& arg);

     inline Rotation2& operator=(const Rotation2& arg);
     inline Vector2 operator*(const Vector2& v) const;
     inline double operator() (int i,int j) const;

     inline friend Rotation2 operator *(const Rotation2& lhs,const Rotation2& rhs);

     inline void SetInverse();
     inline Rotation2 Inverse() const;
     inline Vector2 Inverse(const Vector2& v) const;

     inline void SetIdentity();
     inline static Rotation2 Identity();


     inline void SetRot(double angle);

     inline static Rotation2 Rot(double angle);

     inline double GetRot() const;

     inline friend bool Equal(const Rotation2& a,const Rotation2& b,double eps);
};


class Frame2
 {
public:
    Vector2 p;          
    Rotation2 M;        

public:

     inline Frame2(const Rotation2& R,const Vector2& V);
     explicit inline Frame2(const Vector2& V);
     explicit inline Frame2(const Rotation2& R);
     inline Frame2(void);
     inline Frame2(const Frame2& arg);
     inline void Make4x4(double* d);

     inline double operator()(int i,int j);

     inline double operator() (int i,int j) const;

     inline void SetInverse();
     inline Frame2 Inverse() const;
     inline Vector2 Inverse(const Vector2& arg) const;
     inline Frame2& operator = (const Frame2& arg);
     inline Vector2 operator * (const Vector2& arg) const;
     inline friend Frame2 operator *(const Frame2& lhs,const Frame2& rhs);
     inline void SetIdentity();
     inline void Integrate(const Twist& t_this,double frequency);
     inline static Frame2 Identity() {
        Frame2 tmp;
        tmp.SetIdentity();
        return tmp;
     }
     inline friend bool Equal(const Frame2& a,const Frame2& b,double eps);
};

IMETHOD Vector diff(const Vector& p_w_a,const Vector& p_w_b,double dt=1);


IMETHOD Vector diff(const Rotation& R_a_b1,const Rotation& R_a_b2,double dt=1);

IMETHOD Twist diff(const Frame& F_a_b1,const Frame& F_a_b2,double dt=1);

IMETHOD Twist diff(const Twist& a,const Twist& b,double dt=1);

IMETHOD Wrench diff(const Wrench& W_a_p1,const Wrench& W_a_p2,double dt=1);

IMETHOD Vector addDelta(const Vector& p_w_a,const Vector& p_w_da,double dt=1);

IMETHOD Rotation addDelta(const Rotation& R_w_a,const Vector& da_w,double dt=1);

IMETHOD Frame addDelta(const Frame& F_w_a,const Twist& da_w,double dt=1);

IMETHOD Twist addDelta(const Twist& a,const Twist&da,double dt=1);


IMETHOD Wrench addDelta(const Wrench& a,const Wrench&da,double dt=1);

#ifdef KDL_INLINE
#include "frames.inl"
#endif

}

template<> struct std::hash<KDL::Vector>
{
    std::size_t operator()(KDL::Vector const& v) const noexcept
    {
        size_t seed = 0;
        KDL::hash_combine(seed, v.x());
        KDL::hash_combine(seed, v.y());
        KDL::hash_combine(seed, v.z());
        return seed;
    }
};

template<> struct std::hash<KDL::Rotation>
{
    std::size_t operator()(KDL::Rotation const& r) const noexcept
    {
        size_t seed = 0;
        double x, y, z, w;
        r.GetQuaternion(x, y, z, w);
        KDL::hash_combine(seed, x);
        KDL::hash_combine(seed, y);
        KDL::hash_combine(seed, z);
        KDL::hash_combine(seed, w);
        return seed;
    }
};

template<> struct std::hash<KDL::Frame>
{
    std::size_t operator()(KDL::Frame const& f) const noexcept
    {
        size_t seed = 0;
        KDL::hash_combine(seed, f.p);
        KDL::hash_combine(seed, f.M);
        return seed;
    }
};

template<> struct std::hash<KDL::Wrench>
{
    std::size_t operator()(KDL::Wrench const& w) const noexcept
    {
        size_t seed = 0;
        KDL::hash_combine(seed, w.force);
        KDL::hash_combine(seed, w.torque);
        return seed;
    }
};

template<> struct std::hash<KDL::Twist>
{
    std::size_t operator()(KDL::Twist const& t) const noexcept
    {
        size_t seed = 0;
        KDL::hash_combine(seed, t.vel);
        KDL::hash_combine(seed, t.rot);
        return seed;
    }
};

template<> struct std::hash<KDL::Vector2>
{
    std::size_t operator()(KDL::Vector2 const& v) const noexcept
    {
        size_t seed = 0;
        KDL::hash_combine(seed, v.x());
        KDL::hash_combine(seed, v.y());
        return seed;
    }
};

template<> struct std::hash<KDL::Rotation2>
{
    std::size_t operator()(KDL::Rotation2 const& r) const noexcept
    {
        size_t seed = 0;
        KDL::hash_combine(seed, r.GetRot());
        return seed;
    }
};

template<> struct std::hash<KDL::Frame2>
{
    std::size_t operator()(KDL::Frame2 const& f) const noexcept
    {
        size_t seed = 0;
        KDL::hash_combine(seed, f.p);
        KDL::hash_combine(seed, f.M);
        return seed;
    }
};

#endif