diff options
Diffstat (limited to 'intern/itasc/kdl/frames.hpp')
| -rw-r--r-- | intern/itasc/kdl/frames.hpp | 1097 |
1 files changed, 1097 insertions, 0 deletions
diff --git a/intern/itasc/kdl/frames.hpp b/intern/itasc/kdl/frames.hpp new file mode 100644 index 00000000000..20590c5303e --- /dev/null +++ b/intern/itasc/kdl/frames.hpp @@ -0,0 +1,1097 @@ +/*************************************************************************** + 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 * + * * + ***************************************************************************/ + +/** + * \file + * \warning + * Efficienty can be improved by writing p2 = A*(B*(C*p1))) instead of + * p2=A*B*C*p1 + * + * \par PROPOSED NAMING CONVENTION FOR FRAME-like OBJECTS + * + * \verbatim + * A naming convention of objects of the type defined in this file : + * (1) Frame : F... + * Rotation : R ... + * (2) Twist : T ... + * Wrench : W ... + * Vector : V ... + * This prefix is followed by : + * for category (1) : + * F_A_B : w.r.t. frame A, frame B expressed + * ( each column of F_A_B corresponds to an axis of B, + * expressed w.r.t. frame A ) + * in mathematical convention : + * A + * F_A_B == F + * B + * + * for category (2) : + * V_B : a vector expressed w.r.t. frame B + * + * This can also be prepended by a name : + * e.g. : temporaryV_B + * + * With this convention one can write : + * + * F_A_B = F_B_A.Inverse(); + * F_A_C = F_A_B * F_B_C; + * V_B = F_B_C * V_C; // both translation and rotation + * V_B = R_B_C * V_C; // only rotation + * \endverbatim + * + * \par CONVENTIONS FOR WHEN USED WITH ROBOTS : + * + * \verbatim + * world : represents the frame ([1 0 0,0 1 0,0 0 1],[0 0 0]') + * mp : represents mounting plate of a robot + * (i.e. everything before MP is constructed by robot manufacturer + * everything after MP is tool ) + * tf : represents task frame of a robot + * (i.e. frame in which motion and force control is expressed) + * sf : represents sensor frame of a robot + * (i.e. frame at which the forces measured by the force sensor + * are expressed ) + * + * Frame F_world_mp=...; + * Frame F_mp_sf(..) + * Frame F_mp_tf(,.) + * + * Wrench are measured in sensor frame SF, so one could write : + * Wrench_tf = F_mp_tf.Inverse()* ( F_mp_sf * Wrench_sf ); + * \endverbatim + * + * \par CONVENTIONS REGARDING UNITS : + * Any consistent series of units can be used, e.g. N,mm,Nmm,..mm/sec + * + * \par Twist and Wrench transformations + * 3 different types of transformations do exist for the twists + * and wrenches. + * + * \verbatim + * 1) Frame * Twist or Frame * Wrench : + * this transforms both the velocity/force reference point + * and the basis to which the twist/wrench are expressed. + * 2) Rotation * Twist or Rotation * Wrench : + * this transforms the basis to which the twist/wrench are + * expressed, but leaves the reference point intact. + * 3) Twist.RefPoint(v_base_AB) or Wrench.RefPoint(v_base_AB) + * this transforms only the reference point. v is expressed + * in the same base as the twist/wrench and points from the + * old reference point to the new reference point. + * \endverbatim + * + * \par Complexity + * Sometimes the amount of work is given in the documentation + * e.g. 6M+3A means 6 multiplications and 3 additions. + * + * \author + * Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven + * + ****************************************************************************/ +#ifndef KDL_FRAMES_H +#define KDL_FRAMES_H + + +#include "utilities/kdl-config.h" +#include "utilities/utility.h" + +///////////////////////////////////////////////////////////// + +namespace KDL { + + + +class Vector; +class Rotation; +class Frame; +class Wrench; +class Twist; +class Vector2; +class Rotation2; +class Frame2; + + + +/** + * \brief A concrete implementation of a 3 dimensional vector class + */ +class Vector +{ +public: + double data[3]; + //! Does not initialise the Vector to zero. use Vector::Zero() or SetToZero for that + inline Vector() {data[0]=data[1]=data[2] = 0.0;} + + //! Constructs a vector out of the three values x, y and z + inline Vector(double x,double y, double z); + + //! Constructs a vector out of an array of three values x, y and z + inline Vector(double* xyz); + + //! Constructs a vector out of an array of three values x, y and z + inline Vector(float* xyz); + + //! Assignment operator. The normal copy by value semantics. + inline Vector(const Vector& arg); + + //! store vector components in array + inline void GetValue(double* xyz) const; + + //! Assignment operator. The normal copy by value semantics. + inline Vector& operator = ( const Vector& arg); + + //! Access to elements, range checked when NDEBUG is not set, from 0..2 + inline double operator()(int index) const; + + //! Access to elements, range checked when NDEBUG is not set, from 0..2 + inline double& operator() (int index); + + //! Equivalent to double operator()(int index) const + double operator[] ( int index ) const + { + return this->operator() ( index ); + } + + //! Equivalent to double& operator()(int 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); + + //! Reverses the sign of the Vector object itself + inline void ReverseSign(); + + + //! subtracts a vector from the Vector object itself + inline Vector& operator-=(const Vector& arg); + + + //! Adds a vector from the Vector object itself + inline Vector& operator +=(const Vector& arg); + + //! Scalar multiplication is defined + inline friend Vector operator*(const Vector& lhs,double rhs); + //! Scalar multiplication is defined + inline friend Vector operator*(double lhs,const Vector& rhs); + //! Scalar division is defined + + 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); + + //! To have a uniform operator to put an element to zero, for scalar values + //! and for objects. + inline friend void SetToZero(Vector& v); + + //! @return a zero vector + inline static Vector Zero(); + + /** Normalizes this vector and returns it norm + * makes v a unitvector and returns the norm of v. + * if v is smaller than eps, Vector(1,0,0) is returned with norm 0. + * if this is not good, check the return value of this method. + */ + double Normalize(double eps=epsilon); + + //! @return the norm of the vector + double Norm() const; + + + + //! a 3D vector where the 2D vector v is put in the XY plane + inline void Set2DXY(const Vector2& v); + //! a 3D vector where the 2D vector v is put in the YZ plane + inline void Set2DYZ(const Vector2& v); + //! a 3D vector where the 2D vector v is put in the ZX plane + inline void Set2DZX(const Vector2& v); + //! a 3D vector where the 2D vector v_XY is put in the XY plane of the frame F_someframe_XY. + inline void Set2DPlane(const Frame& F_someframe_XY,const Vector2& v_XY); + + + //! do not use operator == because the definition of Equal(.,.) is slightly + //! different. It compares whether the 2 arguments are equal in an eps-interval + inline friend bool Equal(const Vector& a,const Vector& b,double eps=epsilon); + + //! return a normalized vector + inline friend Vector Normalize(const Vector& a, double eps=epsilon); + + //! The literal equality operator==(), also identical. + inline friend bool operator==(const Vector& a,const Vector& b); + //! The literal inequality operator!=(). + inline friend bool operator!=(const Vector& a,const Vector& b); + + friend class Rotation; + friend class Frame; +}; + + +/** + \brief represents rotations in 3 dimensional space. + + This class represents a rotation matrix with the following + conventions : + \verbatim + Suppose V2 = R*V, (1) + V is expressed in frame B + V2 is expressed in frame A + This matrix R consists of 3 collumns [ X,Y,Z ], + X,Y, and Z contain the axes of frame B, expressed in frame A + Because of linearity expr(1) is valid. + \endverbatim + This class only represents rotational_interpolation, not translation + Two interpretations are possible for rotation angles. + * if you rotate with angle around X frame A to have frame B, + then the result of SetRotX is equal to frame B expressed wrt A. + In code: + \verbatim + Rotation R; + F_A_B = R.SetRotX(angle); + \endverbatim + * Secondly, if you take the following code : + \verbatim + Vector p,p2; Rotation R; + R.SetRotX(angle); + p2 = R*p; + \endverbatim + then the frame p2 is rotated around X axis with (-angle). + Analogue reasonings can be applyd to SetRotY,SetRotZ,SetRot + \par type + Concrete implementation +*/ +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); + // default copy constructor is sufficient + + inline void setValue(float* oglmat); + inline void getValue(float* oglmat) const; + + inline Rotation& operator=(const Rotation& arg); + + //! Defines a multiplication R*V between a Rotation R and a Vector V. + //! Complexity : 9M+6A + inline Vector operator*(const Vector& v) const; + + //! Access to elements 0..2,0..2, bounds are checked when NDEBUG is not set + inline double& operator()(int i,int j); + + //! Access to elements 0..2,0..2, bounds are checked when NDEBUG is not set + inline double operator() (int i,int j) const; + + friend Rotation operator *(const Rotation& lhs,const Rotation& rhs); + + //! Sets the value of *this to its inverse. + inline void SetInverse(); + + //! Gives back the inverse rotation matrix of *this. + inline Rotation Inverse() const; + + //! The same as R.Inverse()*v but more efficient. + inline Vector Inverse(const Vector& v) const; + + //! The same as R.Inverse()*arg but more efficient. + inline Wrench Inverse(const Wrench& arg) const; + + //! The same as R.Inverse()*arg but more efficient. + inline Twist Inverse(const Twist& arg) const; + + //! Gives back an identity rotaton matrix + inline static Rotation Identity(); + + +// = Rotations + //! The Rot... static functions give the value of the appropriate rotation matrix back. + inline static Rotation RotX(double angle); + //! The Rot... static functions give the value of the appropriate rotation matrix back. + inline static Rotation RotY(double angle); + //! The Rot... static functions give the value of the appropriate rotation matrix back. + inline static Rotation RotZ(double angle); + //! The DoRot... functions apply a rotation R to *this,such that *this = *this * Rot.. + //! DoRot... functions are only defined when they can be executed more efficiently + inline void DoRotX(double angle); + //! The DoRot... functions apply a rotation R to *this,such that *this = *this * Rot.. + //! DoRot... functions are only defined when they can be executed more efficiently + inline void DoRotY(double angle); + //! The DoRot... functions apply a rotation R to *this,such that *this = *this * Rot.. + //! DoRot... functions are only defined when they can be executed more efficiently + inline void DoRotZ(double angle); + + //! Along an arbitrary axes. It is not necessary to normalize rotaxis. + //! returns identity rotation matrix in the case that the norm of rotaxis + //! is to small to be used. + // @see Rot2 if you want to handle this error in another way. + static Rotation Rot(const Vector& rotaxis,double angle); + + //! Along an arbitrary axes. rotvec should be normalized. + static Rotation Rot2(const Vector& rotvec,double angle); + + // make sure the matrix is a pure rotation (no scaling) + void Ortho(); + + //! Returns a vector with the direction of the equiv. axis + //! and its norm is angle + Vector GetRot() const; + + //! Returns a 2D vector representing the equivalent rotation in the XZ plane that brings the + //! Y axis onto the Matrix Y axis and its norm is angle + Vector2 GetXZRot() const; + + /** Returns the rotation angle around the equiv. axis + * @param axis the rotation axis is returned in this variable + * @param eps : in the case of angle == 0 : rot axis is undefined and choosen + * to be +/- Z-axis + * in the case of angle == PI : 2 solutions, positive Z-component + * of the axis is choosen. + * @result returns the rotation angle (between [0..PI] ) + */ + double GetRotAngle(Vector& axis,double eps=epsilon) const; + + + //! Gives back a rotation matrix specified with EulerZYZ convention : + //! First rotate around Z with alfa, + //! then around the new Y with beta, then around + //! new Z with gamma. + static Rotation EulerZYZ(double Alfa,double Beta,double Gamma); + + //! Gives back the EulerZYZ convention description of the rotation matrix : + //! First rotate around Z with alfa, + //! then around the new Y with beta, then around + //! new Z with gamma. + //! + //! Variables are bound by + //! (-PI <= alfa <= PI), + //! (0 <= beta <= PI), + //! (-PI <= alfa <= PI) + void GetEulerZYZ(double& alfa,double& beta,double& gamma) const; + + + //! Sets the value of this object to a rotation specified with RPY convention: + //! first rotate around X with roll, then around the + //! old Y with pitch, then around old Z with alfa + static Rotation RPY(double roll,double pitch,double yaw); + + //! Gives back a vector in RPY coordinates, variables are bound by + //! -PI <= roll <= PI + //! -PI <= Yaw <= PI + //! -PI/2 <= PITCH <= PI/2 + //! + //! convention : first rotate around X with roll, then around the + //! old Y with pitch, then around old Z with alfa + void GetRPY(double& roll,double& pitch,double& yaw) const; + + + //! Gives back a rotation matrix specified with EulerZYX convention : + //! First rotate around Z with alfa, + //! then around the new Y with beta, then around + //! new X with gamma. + //! + //! closely related to RPY-convention + inline static Rotation EulerZYX(double Alfa,double Beta,double Gamma) { + return RPY(Gamma,Beta,Alfa); + } + + //! GetEulerZYX gets the euler ZYX parameters of a rotation : + //! First rotate around Z with alfa, + //! then around the new Y with beta, then around + //! new X with gamma. + //! + //! Range of the results of GetEulerZYX : + //! -PI <= alfa <= PI + //! -PI <= gamma <= PI + //! -PI/2 <= beta <= PI/2 + //! + //! Closely related to RPY-convention. + inline void GetEulerZYX(double& Alfa,double& Beta,double& Gamma) const { + GetRPY(Gamma,Beta,Alfa); + } + + //! Transformation of the base to which the twist is expressed. + //! Complexity : 18M+12A + //! @see Frame*Twist for a transformation that also transforms + //! the velocity reference point. + inline Twist operator * (const Twist& arg) const; + + //! Transformation of the base to which the wrench is expressed. + //! Complexity : 18M+12A + //! @see Frame*Wrench for a transformation that also transforms + //! the force reference point. + inline Wrench operator * (const Wrench& arg) const; + + //! Access to the underlying unitvectors of the rotation matrix + inline Vector UnitX() const { + return Vector(data[0],data[3],data[6]); + } + + //! Access to the underlying unitvectors of the rotation matrix + inline void UnitX(const Vector& X) { + data[0] = X(0); + data[3] = X(1); + data[6] = X(2); + } + + //! Access to the underlying unitvectors of the rotation matrix + inline Vector UnitY() const { + return Vector(data[1],data[4],data[7]); + } + + //! Access to the underlying unitvectors of the rotation matrix + inline void UnitY(const Vector& X) { + data[1] = X(0); + data[4] = X(1); + data[7] = X(2); + } + + //! Access to the underlying unitvectors of the rotation matrix + inline Vector UnitZ() const { + return Vector(data[2],data[5],data[8]); + } + + //! Access to the underlying unitvectors of the rotation matrix + inline void UnitZ(const Vector& X) { + data[2] = X(0); + data[5] = X(1); + data[8] = X(2); + } + + //! do not use operator == because the definition of Equal(.,.) is slightly + //! different. It compares whether the 2 arguments are equal in an eps-interval + friend bool Equal(const Rotation& a,const Rotation& b,double eps=epsilon); + + //! The literal equality operator==(), also identical. + friend bool operator==(const Rotation& a,const Rotation& b); + //! The literal inequality operator!=() + friend bool operator!=(const Rotation& a,const Rotation& b); + + friend class Frame; +}; + bool operator==(const Rotation& a,const Rotation& b); + + + +/** + \brief represents a frame transformation in 3D space (rotation + translation) + + if V2 = Frame*V1 (V2 expressed in frame A, V1 expressed in frame B) + then V2 = Frame.M*V1+Frame.p + + Frame.M contains columns that represent the axes of frame B wrt frame A + Frame.p contains the origin of frame B expressed in frame A. +*/ +class Frame { +public: + Vector p; //!< origine of the Frame + Rotation M; //!< Orientation of the Frame + +public: + + inline Frame(const Rotation& R,const Vector& V); + + //! The rotation matrix defaults to identity + explicit inline Frame(const Vector& V); + //! The position matrix defaults to zero + explicit inline Frame(const Rotation& R); + + inline void setValue(float* oglmat); + inline void getValue(float* oglmat) const; + + inline Frame() {} + //! The copy constructor. Normal copy by value semantics. + inline Frame(const Frame& arg); + + //! Reads data from an double array + //\TODO should be formulated as a constructor + void Make4x4(double* d); + + //! Treats a frame as a 4x4 matrix and returns element i,j + //! Access to elements 0..3,0..3, bounds are checked when NDEBUG is not set + inline double operator()(int i,int j); + + //! Treats a frame as a 4x4 matrix and returns element i,j + //! Access to elements 0..3,0..3, bounds are checked when NDEBUG is not set + inline double operator() (int i,int j) const; + + // = Inverse + //! Gives back inverse transformation of a Frame + inline Frame Inverse() const; + + //! The same as p2=R.Inverse()*p but more efficient. + inline Vector Inverse(const Vector& arg) const; + + //! The same as p2=R.Inverse()*p but more efficient. + inline Wrench Inverse(const Wrench& arg) const; + + //! The same as p2=R.Inverse()*p but more efficient. + inline Twist Inverse(const Twist& arg) const; + + //! Normal copy-by-value semantics. + inline Frame& operator = (const Frame& arg); + + //! Transformation of the base to which the vector + //! is expressed. + inline Vector operator * (const Vector& arg) const; + + //! Transformation of both the force reference point + //! and of the base to which the wrench is expressed. + //! look at Rotation*Wrench operator for a transformation + //! of only the base to which the twist is expressed. + //! + //! Complexity : 24M+18A + inline Wrench operator * (const Wrench& arg) const; + + //! Transformation of both the velocity reference point + //! and of the base to which the twist is expressed. + //! look at Rotation*Twist for a transformation of only the + //! base to which the twist is expressed. + //! + //! Complexity : 24M+18A + inline Twist operator * (const Twist& arg) const; + + //! Composition of two frames. + inline friend Frame operator *(const Frame& lhs,const Frame& rhs); + + //! @return the identity transformation Frame(Rotation::Identity(),Vector::Zero()). + inline static Frame Identity(); + + //! The twist <t_this> is expressed wrt the current + //! frame. This frame is integrated into an updated frame with + //! <samplefrequency>. Very simple first order integration rule. + 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); + + + //! do not use operator == because the definition of Equal(.,.) is slightly + //! different. It compares whether the 2 arguments are equal in an eps-interval + inline friend bool Equal(const Frame& a,const Frame& b,double eps=epsilon); + + //! The literal equality operator==(), also identical. + inline friend bool operator==(const Frame& a,const Frame& b); + //! The literal inequality operator!=(). + inline friend bool operator!=(const Frame& a,const Frame& b); +}; + +/** + * \brief represents both translational and rotational velocities. + * + * This class represents a twist. A twist is the combination of translational + * velocity and rotational velocity applied at one point. +*/ +class Twist { +public: + Vector vel; //!< The velocity of that point + Vector rot; //!< The rotational velocity of that point. +public: + + //! The default constructor initialises to Zero via the constructor of Vector. + 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); + //! index-based access to components, first vel(0..2), then rot(3..5) + inline double& operator()(int i); + + //! index-based access to components, first vel(0..2), then rot(3..5) + //! For use with a const Twist + 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); + + + //! @return a zero Twist : Twist(Vector::Zero(),Vector::Zero()) + static inline Twist Zero(); + + //! Reverses the sign of the twist + inline void ReverseSign(); + + //! Changes the reference point of the twist. + //! The vector v_base_AB is expressed in the same base as the twist + //! The vector v_base_AB is a vector from the old point to + //! the new point. + //! + //! Complexity : 6M+6A + inline Twist RefPoint(const Vector& v_base_AB) const; + + + //! do not use operator == because the definition of Equal(.,.) is slightly + //! different. It compares whether the 2 arguments are equal in an eps-interval + inline friend bool Equal(const Twist& a,const Twist& b,double eps=epsilon); + + //! The literal equality operator==(), also identical. + inline friend bool operator==(const Twist& a,const Twist& b); + //! The literal inequality operator!=(). + inline friend bool operator!=(const Twist& a,const Twist& b); + +// = Friends + friend class Rotation; + friend class Frame; + +}; + +/** + * \brief represents both translational and rotational acceleration. + * + * This class represents an acceleration twist. A acceleration twist is + * the combination of translational + * acceleration and rotational acceleration applied at one point. +*/ +/* +class AccelerationTwist { +public: + Vector trans; //!< The translational acceleration of that point + Vector rot; //!< The rotational acceleration of that point. +public: + + //! The default constructor initialises to Zero via the constructor of Vector. + 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); + //! index-based access to components, first vel(0..2), then rot(3..5) + inline double& operator()(int i); + + //! index-based access to components, first vel(0..2), then rot(3..5) + //! For use with a const AccelerationTwist + 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); + + + //! @return a zero AccelerationTwist : AccelerationTwist(Vector::Zero(),Vector::Zero()) + static inline AccelerationTwist Zero(); + + //! Reverses the sign of the AccelerationTwist + inline void ReverseSign(); + + //! Changes the reference point of the AccelerationTwist. + //! The vector v_base_AB is expressed in the same base as the AccelerationTwist + //! The vector v_base_AB is a vector from the old point to + //! the new point. + //! + //! Complexity : 6M+6A + inline AccelerationTwist RefPoint(const Vector& v_base_AB) const; + + + //! do not use operator == because the definition of Equal(.,.) is slightly + //! different. It compares whether the 2 arguments are equal in an eps-interval + inline friend bool Equal(const AccelerationTwist& a,const AccelerationTwist& b,double eps=epsilon); + + //! The literal equality operator==(), also identical. + inline friend bool operator==(const AccelerationTwist& a,const AccelerationTwist& b); + //! The literal inequality operator!=(). + inline friend bool operator!=(const AccelerationTwist& a,const AccelerationTwist& b); + +// = Friends + friend class Rotation; + friend class Frame; + +}; +*/ +/** + * \brief represents the combination of a force and a torque. + * + * This class represents a Wrench. A Wrench is the force and torque applied at a point + */ +class Wrench +{ +public: + Vector force; //!< Force that is applied at the origin of the current ref frame + Vector torque; //!< Torque that is applied at the origin of the current ref frame +public: + + //! Does initialise force and torque to zero via the underlying constructor of Vector + 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); + + //! index-based access to components, first force(0..2), then torque(3..5) + inline double& operator()(int i); + + //! index-based access to components, first force(0..2), then torque(3..5) + //! for use with a const Wrench + inline double operator()(int i) const; + + double operator[] ( int index ) const + { + return this->operator() ( index ); + } + + double& operator[] ( int index ) + { + return this->operator() ( index ); + } + + //! Scalar multiplication + inline friend Wrench operator*(const Wrench& lhs,double rhs); + //! Scalar multiplication + inline friend Wrench operator*(double lhs,const Wrench& rhs); + //! Scalar division + 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); + + //! An unary - operator + inline friend Wrench operator-(const Wrench& arg); + + //! Sets the Wrench to Zero, to have a uniform function that sets an object or + //! double to zero. + inline friend void SetToZero(Wrench& v); + + //! @return a zero Wrench + static inline Wrench Zero(); + + //! Reverses the sign of the current Wrench + inline void ReverseSign(); + + //! Changes the reference point of the wrench. + //! The vector v_base_AB is expressed in the same base as the twist + //! The vector v_base_AB is a vector from the old point to + //! the new point. + //! + //! Complexity : 6M+6A + inline Wrench RefPoint(const Vector& v_base_AB) const; + + + //! do not use operator == because the definition of Equal(.,.) is slightly + //! different. It compares whether the 2 arguments are equal in an eps-interval + inline friend bool Equal(const Wrench& a,const Wrench& b,double eps=epsilon); + + //! The literal equality operator==(), also identical. + inline friend bool operator==(const Wrench& a,const Wrench& b); + //! The literal inequality operator!=(). + inline friend bool operator!=(const Wrench& a,const Wrench& b); + + friend class Rotation; + friend class Frame; + + +}; + + +//! 2D version of Vector +class Vector2 +{ + double data[2]; +public: + //! Does not initialise to Zero(). + Vector2() {data[0]=data[1] = 0.0;} + inline Vector2(double x,double y); + inline Vector2(const Vector2& arg); + inline Vector2(double* xyz); + inline Vector2(float* xyz); + + inline Vector2& operator = ( const Vector2& arg); + + //! Access to elements, range checked when NDEBUG is not set, from 0..1 + inline double operator()(int index) const; + + //! Access to elements, range checked when NDEBUG is not set, from 0..1 + inline double& operator() (int index); + + //! store vector components in array + inline void GetValue(double* xy) const; + + 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); + + //! @return a zero 2D vector. + inline static Vector2 Zero(); + + /** Normalizes this vector and returns it norm + * makes v a unitvector and returns the norm of v. + * if v is smaller than eps, Vector(1,0,0) is returned with norm 0. + * if this is not good, check the return value of this method. + */ + double Normalize(double eps=epsilon); + + //! @return the norm of the vector + inline double Norm() const; + + //! projects v in its XY plane, and sets *this to these values + inline void Set3DXY(const Vector& v); + + //! projects v in its YZ plane, and sets *this to these values + inline void Set3DYZ(const Vector& v); + + //! projects v in its ZX plane, and sets *this to these values + inline void Set3DZX(const Vector& v); + + //! projects v_someframe in the XY plane of F_someframe_XY, + //! and sets *this to these values + //! expressed wrt someframe. + inline void Set3DPlane(const Frame& F_someframe_XY,const Vector& v_someframe); + + + //! do not use operator == because the definition of Equal(.,.) is slightly + //! different. It compares whether the 2 arguments are equal in an eps-interval + inline friend bool Equal(const Vector2& a,const Vector2& b,double eps=epsilon); + + friend class Rotation2; +}; + + +//! A 2D Rotation class, for conventions see Rotation. For further documentation +//! of the methods see Rotation class. +class Rotation2 +{ + double s,c; + //! c,s represent cos(angle), sin(angle), this also represents first col. of rot matrix + //! from outside, this class behaves as if it would store the complete 2x2 matrix. +public: + //! Default constructor does NOT initialise to Zero(). + 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){} + + inline Rotation2& operator=(const Rotation2& arg); + inline Vector2 operator*(const Vector2& v) const; + //! Access to elements 0..1,0..1, bounds are checked when NDEBUG is not set + 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(); + + + //! The SetRot.. functions set the value of *this to the appropriate rotation matrix. + inline void SetRot(double angle); + + //! The Rot... static functions give the value of the appropriate rotation matrix bac + inline static Rotation2 Rot(double angle); + + //! Gets the angle (in radians) + inline double GetRot() const; + + //! do not use operator == because the definition of Equal(.,.) is slightly + //! different. It compares whether the 2 arguments are equal in an eps-interval + inline friend bool Equal(const Rotation2& a,const Rotation2& b,double eps=epsilon); +}; + +//! A 2D frame class, for further documentation see the Frames class +//! for methods with unchanged semantics. +class Frame2 + { +public: + Vector2 p; //!< origine of the Frame + Rotation2 M; //!< Orientation of the Frame + +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); + + //! Treats a frame as a 3x3 matrix and returns element i,j + //! Access to elements 0..2,0..2, bounds are checked when NDEBUG is not set + inline double operator()(int i,int j); + + //! Treats a frame as a 4x4 matrix and returns element i,j + //! Access to elements 0..3,0..3, bounds are checked when NDEBUG is not set + 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); + 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=epsilon); +}; + +IMETHOD Vector diff(const Vector& a,const Vector& 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& a,const Vector&da,double dt=1); +IMETHOD Rotation addDelta(const Rotation& a,const Vector&da,double dt=1); +IMETHOD Frame addDelta(const Frame& a,const Twist& da,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 "vector.inl" +// #include "wrench.inl" + //#include "rotation.inl" + //#include "frame.inl" + //#include "twist.inl" + //#include "vector2.inl" + //#include "rotation2.inl" + //#include "frame2.inl" +#include "frames.inl" +#endif + + + +} + + +#endif |