1098 lines
40 KiB
C++
1098 lines
40 KiB
C++
/***************************************************************************
|
|
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., 51 Franklin Street, *
|
|
* Fifth Floor, Boston, MA 02110-1301, 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 = 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
|