128 #include "utilities/kdl-config.h" 129 #include "utilities/utility.h" 168 inline Vector(
double x,
double y,
double z);
194 inline double x()
const;
195 inline double y()
const;
196 inline double z()
const;
197 inline void x(
double);
198 inline void y(
double);
199 inline void z(
double);
309 inline Rotation(
double Xx,
double Yx,
double Zx,
310 double Xy,
double Yy,
double Zy,
311 double Xz,
double Yz,
double Zz);
358 inline void DoRotX(
double angle);
361 inline void DoRotY(
double angle);
364 inline void DoRotZ(
double angle);
415 void GetEulerZYZ(
double& alpha,
double& beta,
double& gamma)
const;
435 static Rotation RPY(
double roll,
double pitch,
double yaw);
455 void GetRPY(
double& roll,
double& pitch,
double& yaw)
const;
470 return RPY(Gamma,Beta,Alfa);
493 inline void GetEulerZYX(
double& Alfa,
double& Beta,
double& Gamma)
const {
701 static Frame DH(
double a,
double alpha,
double d,
double theta);
988 inline double x()
const;
989 inline double y()
const;
990 inline void x(
double);
991 inline void y(
double);
1018 double Norm()
const;
1059 explicit Rotation2(
double angle_rad):
s(sin(angle_rad)),
c(cos(angle_rad)) {}
1066 inline double operator() (
int i,
int j)
const;
1079 inline void SetRot(
double angle);
1085 inline double GetRot()
const;
1107 inline void Make4x4(
double* d);
1115 inline double operator() (
int i,
int j)
const;
friend Wrench operator*(const Wrench &lhs, double rhs)
Scalar multiplication.
Definition: frames.hpp:243
Rotation2 M
Orientation of the Frame.
Definition: frames.hpp:1098
double x() const
Definition: frames.hpp:795
double Norm() const
Definition: frames.cpp:118
friend Vector operator+(const Vector &lhs, const Vector &rhs)
Definition: frames.hpp:58
friend bool operator!=(const Rotation &a, const Rotation &b)
The literal inequality operator!=()
Definition: frames.hpp:1321
Twist & operator-=(const Twist &arg)
Definition: frames.hpp:313
represents rotations in 3 dimensional space.
Definition: frames.hpp:301
void UnitZ(const Vector &X)
Access to the underlying unitvectors of the rotation matrix.
Definition: frames.hpp:539
void Set3DYZ(const Vector &v)
projects v in its YZ plane, and sets *this to these values
Definition: frames.hpp:821
double Normalize(double eps=epsilon)
Normalizes this vector and returns it norm makes v a unitvector and returns the norm of v.
Definition: frames.cpp:105
static Vector Zero()
Definition: frames.hpp:139
friend Twist operator-(const Twist &lhs, const Twist &rhs)
Definition: frames.hpp:367
double operator[](int index) const
Equivalent to double operator()(int index) const.
Definition: frames.hpp:183
void GetEulerZYX(double &Alfa, double &Beta, double &Gamma) const
GetEulerZYX gets the euler ZYX parameters of a rotation : First rotate around Z with alfa,...
Definition: frames.hpp:493
friend bool operator==(const Frame &a, const Frame &b)
The literal equality operator==(), also identical.
Definition: frames.hpp:1269
static Rotation2 Identity()
Definition: frames.hpp:880
void UnitX(const Vector &X)
Access to the underlying unitvectors of the rotation matrix.
Definition: frames.hpp:515
Frame2(void)
Definition: frames.hpp:903
friend Vector2 operator+(const Vector2 &lhs, const Vector2 &rhs)
Definition: frames.hpp:741
void SetIdentity()
Definition: frames.hpp:939
void Set2DXY(const Vector2 &v)
a 3D vector where the 2D vector v is put in the XY plane
Definition: frames.hpp:459
Vector2 & operator+=(const Vector2 &arg)
Definition: frames.hpp:766
double z() const
Definition: frames.hpp:78
void UnitY(const Vector &X)
Access to the underlying unitvectors of the rotation matrix.
Definition: frames.hpp:527
void GetRPY(double &roll, double &pitch, double &yaw) const
Gives back a vector in RPY coordinates, variables are bound by.
Definition: frames.cpp:250
void ReverseSign()
Reverses the sign of the twist.
Definition: frames.hpp:297
double operator[](int index) const
Equivalent to double operator()(int index) const.
Definition: frames.hpp:977
double Normalize(double eps=epsilon)
Normalizes this vector and returns it norm makes v a unitvector and returns the norm of v.
Definition: frames.cpp:148
Vector vel
The velocity of that point.
Definition: frames.hpp:722
Vector & operator+=(const Vector &arg)
Adds a vector from the Vector object itself.
Definition: frames.hpp:121
Frame & operator=(const Frame &arg)
Normal copy-by-value semantics.
Definition: frames.hpp:429
Wrench()
Does initialise force and torque to zero via the underlying constructor of Vector.
Definition: frames.hpp:886
Vector2 & operator=(const Vector2 &arg)
Definition: frames.hpp:733
friend bool operator!=(const Wrench &a, const Wrench &b)
The literal inequality operator!=().
Definition: frames.hpp:1318
void SetRot(double angle)
The SetRot.. functions set the value of *this to the appropriate rotation matrix.
Definition: frames.hpp:890
Wrench RefPoint(const Vector &v_base_AB) const
Changes the reference point of the wrench.
Definition: frames.hpp:198
static Rotation2 Rot(double angle)
The Rot... static functions give the value of the appropriate rotation matrix bac.
Definition: frames.hpp:894
friend bool operator!=(const Vector2 &a, const Vector2 &b)
The literal inequality operator!=().
Definition: frames.hpp:1334
void SetInverse()
Definition: frames.hpp:945
Inlined member functions and global functions that relate to the classes in frames....
friend double dot(const Vector &lhs, const Vector &rhs)
Definition: frames.hpp:1006
IMETHOD Vector diff(const Vector &p_w_a, const Vector &p_w_b, double dt=1)
determines the difference of vector b with vector a.
Definition: frames.hpp:1123
Vector2()
Does not initialise to Zero().
Definition: frames.hpp:964
void Set2DYZ(const Vector2 &v)
a 3D vector where the 2D vector v is put in the YZ plane
Definition: frames.hpp:467
friend bool Equal(const Rotation &a, const Rotation &b, double eps)
do not use operator == because the definition of Equal(.,.) is slightly different.
Definition: frames.cpp:160
A 2D Rotation class, for conventions see Rotation.
Definition: frames.hpp:1050
void SetIdentity()
Definition: frames.hpp:884
static Rotation Quaternion(double x, double y, double z, double w)
Gives back a rotation matrix specified with Quaternion convention the norm of (x,y,...
Definition: frames.cpp:191
2D version of Vector
Definition: frames.hpp:959
void SetInverse()
Definition: frames.hpp:868
Rotation Inverse() const
Gives back the inverse rotation matrix of *this.
Definition: frames.hpp:633
Vector & operator-=(const Vector &arg)
subtracts a vector from the Vector object itself
Definition: frames.hpp:130
void DoRotZ(double angle)
The DoRot...
Definition: frames.hpp:584
Twist()
The default constructor initialises to Zero via the constructor of Vector.
Definition: frames.hpp:727
void Integrate(const Twist &t_this, double frequency)
friend Vector2 operator-(const Vector2 &lhs, const Vector2 &rhs)
Definition: frames.hpp:746
Vector torque
Torque that is applied at the origin of the current ref frame.
Definition: frames.hpp:882
Rotation()
Definition: frames.hpp:306
Rotation2(double angle_rad)
Definition: frames.hpp:1059
void SetInverse()
Sets the value of *this to its inverse.
Definition: frames.hpp:649
double operator()(int index) const
Access to elements, range checked when NDEBUG is not set, from 0..2.
Definition: frames.hpp:144
friend bool Equal(const Frame2 &a, const Frame2 &b, double eps)
Definition: frames.hpp:1057
friend Twist operator*(const Twist &lhs, double rhs)
Definition: frames.hpp:346
void Set3DZX(const Vector &v)
projects v in its ZX plane, and sets *this to these values
Definition: frames.hpp:827
double operator[](int index) const
Definition: frames.hpp:740
friend Twist operator/(const Twist &lhs, double rhs)
Definition: frames.hpp:356
double operator()(int i, int j)
Treats a frame as a 3x3 matrix and returns element i,j Access to elements 0..2,0.....
Definition: frames.hpp:972
static Rotation RotY(double angle)
The Rot... static functions give the value of the appropriate rotation matrix back.
Definition: frames.hpp:606
Vector()
Does not initialise the Vector to zero. use Vector::Zero() or SetToZero for that.
Definition: frames.hpp:165
friend bool operator==(const Vector &a, const Vector &b)
The literal equality operator==(), also identical.
Definition: frames.hpp:1282
Twist & operator+=(const Twist &arg)
Definition: frames.hpp:320
friend bool Equal(const Vector &a, const Vector &b, double eps)
do not use operator == because the definition of Equal(.,.) is slightly different.
Definition: frames.hpp:1026
Rotation M
Orientation of the Frame.
Definition: frames.hpp:573
static Frame Identity()
Definition: frames.hpp:696
Rotation2(double ca, double sa)
Definition: frames.hpp:1061
static Rotation EulerZYX(double Alfa, double Beta, double Gamma)
EulerZYX constructs a Rotation from the Euler ZYX parameters:
Definition: frames.hpp:469
Vector2 & operator-=(const Vector2 &arg)
Definition: frames.hpp:773
friend Vector2 operator/(const Vector2 &lhs, double rhs)
Definition: frames.hpp:761
friend bool operator==(const Rotation &a, const Rotation &b)
The literal equality operator==(), also identical.
Definition: frames.cpp:462
double GetRotAngle(Vector &axis, double eps=epsilon) const
Returns the rotation angle around the equiv.
Definition: frames.cpp:359
Vector GetRot() const
Returns a vector with the direction of the equiv.
Definition: frames.cpp:337
friend bool operator==(const Wrench &a, const Wrench &b)
The literal equality operator==(), also identical.
Definition: frames.hpp:1309
friend void SetToZero(Wrench &v)
Sets the Wrench to Zero, to have a uniform function that sets an object or double to zero.
Definition: frames.hpp:1069
represents both translational and rotational velocities.
Definition: frames.hpp:720
Vector operator*(const Vector &v) const
Defines a multiplication R*V between a Rotation R and a Vector V.
Definition: frames.hpp:522
friend Vector operator*(const Vector &lhs, double rhs)
Scalar multiplication is defined.
Definition: frames.hpp:84
static Frame DH_Craig1989(double a, double alpha, double d, double theta)
Definition: frames.cpp:54
void Integrate(const Twist &t_this, double frequency)
The twist <t_this> is expressed wrt the current frame.
Definition: frames.hpp:620
IMETHOD bool Equal(const FrameAcc &r1, const FrameAcc &r2, double eps=epsilon)
Definition: frameacc.hpp:394
void Set3DPlane(const Frame &F_someframe_XY, const Vector &v_someframe)
projects v_someframe in the XY plane of F_someframe_XY, and sets *this to these values expressed wrt ...
Definition: frames.hpp:834
Definition: articulatedbodyinertia.cpp:28
double GetRot() const
Gets the angle (in radians)
Definition: frames.hpp:898
A concrete implementation of a 3 dimensional vector class.
Definition: frames.hpp:160
void GetEulerZYZ(double &alpha, double &beta, double &gamma) const
Gives back the EulerZYZ convention description of the rotation matrix : First rotate around Z with al...
Definition: frames.cpp:276
friend bool Equal(const Vector2 &a, const Vector2 &b, double eps)
do not use operator == because the definition of Equal(.,.) is slightly different.
Definition: frames.hpp:1048
Vector rot
The rotational velocity of that point.
Definition: frames.hpp:723
static Rotation RPY(double roll, double pitch, double yaw)
Gives back a rotation matrix specified with RPY convention: first rotate around X with roll,...
Definition: frames.cpp:238
static Frame2 Identity()
Definition: frames.hpp:1125
Frame2 & operator=(const Frame2 &arg)
Definition: frames.hpp:960
double & operator()(int i)
index-based access to components, first force(0..2), then torque(3..5)
Definition: frames.hpp:224
static Frame DH(double a, double alpha, double d, double theta)
Definition: frames.cpp:71
void Set3DXY(const Vector &v)
projects v in its XY plane, and sets *this to these values
Definition: frames.hpp:815
friend bool Equal(const Twist &a, const Twist &b, double eps)
do not use operator == because the definition of Equal(.,.) is slightly different.
Definition: frames.hpp:1043
Vector operator*(const Vector &arg) const
Transformation of the base to which the vector is expressed.
Definition: frames.hpp:413
Rotation2 & operator=(const Rotation2 &arg)
Definition: frames.hpp:845
Frame Inverse() const
Gives back inverse transformation of a Frame.
Definition: frames.hpp:423
friend bool operator!=(const Twist &a, const Twist &b)
The literal inequality operator!=().
Definition: frames.hpp:1305
friend Vector2 operator*(const Vector2 &lhs, double rhs)
Definition: frames.hpp:751
double data[3]
Definition: frames.hpp:163
Frame2 Inverse() const
Definition: frames.hpp:953
double operator()(int index) const
Access to elements, range checked when NDEBUG is not set, from 0..1.
Definition: frames.hpp:784
friend Wrench operator+(const Wrench &lhs, const Wrench &rhs)
Definition: frames.hpp:259
void Set2DZX(const Vector2 &v)
a 3D vector where the 2D vector v is put in the ZX plane
Definition: frames.hpp:476
void DoRotX(double angle)
The DoRot...
Definition: frames.hpp:552
Vector p
origine of the Frame
Definition: frames.hpp:572
static Wrench Zero()
Definition: frames.hpp:186
static Rotation RotZ(double angle)
The Rot... static functions give the value of the appropriate rotation matrix back.
Definition: frames.hpp:611
double Norm() const
Definition: frames.cpp:88
friend Vector operator-(const Vector &lhs, const Vector &rhs)
Definition: frames.hpp:67
void Make4x4(double *d)
Reads data from an double array.
Definition: frames.cpp:40
Vector UnitX() const
Access to the underlying unitvectors of the rotation matrix.
Definition: frames.hpp:510
friend bool Equal(const Wrench &a, const Wrench &b, double eps)
do not use operator == because the definition of Equal(.,.) is slightly different.
Definition: frames.hpp:1038
Vector & operator=(const Vector &arg)
Assignment operator. The normal copy by value semantics.
Definition: frames.hpp:50
Rotation & operator=(const Rotation &arg)
Definition: frames.hpp:516
Vector force
Force that is applied at the origin of the current ref frame.
Definition: frames.hpp:881
friend Vector operator/(const Vector &lhs, double rhs)
Scalar division is defined.
Definition: frames.hpp:102
static Rotation RotX(double angle)
The Rot... static functions give the value of the appropriate rotation matrix back.
Definition: frames.hpp:601
Frame()
Definition: frames.hpp:584
friend double dot(const Twist &lhs, const Wrench &rhs)
Definition: frames.hpp:1010
double y() const
Definition: frames.hpp:796
double operator()(int i, int j)
Treats a frame as a 4x4 matrix and returns element i,j Access to elements 0..3,0.....
Definition: frames.hpp:663
void Set2DPlane(const Frame &F_someframe_XY, const Vector2 &v_XY)
a 3D vector where the 2D vector v_XY is put in the XY plane of the frame F_someframe_XY.
Definition: frames.hpp:703
Vector2 operator*(const Vector2 &v) const
Definition: frames.hpp:850
friend void SetToZero(Vector2 &v)
Definition: frames.hpp:1074
void DoRotY(double angle)
The DoRot...
Definition: frames.hpp:568
friend bool operator==(const Vector2 &a, const Vector2 &b)
The literal equality operator==(), also identical.
Definition: frames.hpp:1325
double & operator()(int i)
index-based access to components, first vel(0..2), then rot(3..5)
Definition: frames.hpp:327
friend bool Equal(const Rotation2 &a, const Rotation2 &b, double eps)
do not use operator == because the definition of Equal(.,.) is slightly different.
Definition: frames.hpp:1053
friend bool operator!=(const Vector &a, const Vector &b)
The literal inequality operator!=().
Definition: frames.hpp:1292
friend bool operator==(const Twist &a, const Twist &b)
The literal equality operator==(), also identical.
Definition: frames.hpp:1296
static Rotation EulerZYZ(double Alfa, double Beta, double Gamma)
Gives back a rotation matrix specified with EulerZYZ convention :
Definition: frames.cpp:263
double y() const
Definition: frames.hpp:77
Definition: frames.hpp:570
double x() const
Definition: frames.hpp:76
double operator()(int i, int j) const
Access to elements 0..1,0..1, bounds are checked when NDEBUG is not set.
Definition: frames.hpp:854
void GetQuaternion(double &x, double &y, double &z, double &w) const
Get the quaternion of this matrix.
Definition: frames.cpp:205
Twist(const Vector &_vel, const Vector &_rot)
Definition: frames.hpp:729
Vector2 p
origine of the Frame
Definition: frames.hpp:1097
static Rotation Identity()
Gives back an identity rotaton matrix.
Definition: frames.hpp:548
represents both translational and rotational acceleration.
Definition: frames.hpp:878
IMETHOD Vector addDelta(const Vector &p_w_a, const Vector &p_w_da, double dt=1)
adds vector da to vector a.
Definition: frames.hpp:1150
void ReverseSign()
Reverses the sign of the Vector object itself.
Definition: frames.hpp:441
Vector UnitZ() const
Access to the underlying unitvectors of the rotation matrix.
Definition: frames.hpp:534
friend bool Equal(const Frame &a, const Frame &b, double eps)
do not use operator == because the definition of Equal(.,.) is slightly different.
Definition: frames.hpp:1033
static Rotation Rot(const Vector &rotvec, double angle)
Along an arbitrary axes.
Definition: frames.cpp:294
Rotation2 Inverse() const
Definition: frames.hpp:872
double & operator()(int i, int j)
Access to elements 0..2,0..2, bounds are checked when NDEBUG is not set.
Definition: frames.hpp:489
Vector UnitY() const
Access to the underlying unitvectors of the rotation matrix.
Definition: frames.hpp:522
Vector2 operator*(const Vector2 &arg) const
Definition: frames.hpp:929
friend Twist operator+(const Twist &lhs, const Twist &rhs)
Definition: frames.hpp:362
Twist RefPoint(const Vector &v_base_AB) const
Changes the reference point of the twist.
Definition: frames.hpp:303
Wrench & operator+=(const Wrench &arg)
Definition: frames.hpp:217
static Twist Zero()
Definition: frames.hpp:291
friend bool operator!=(const Frame &a, const Frame &b)
The literal inequality operator!=().
Definition: frames.hpp:1278
Wrench & operator-=(const Wrench &arg)
Definition: frames.hpp:210
static Rotation Rot2(const Vector &rotvec, double angle)
Along an arbitrary axes. rotvec should be normalized.
Definition: frames.cpp:304
friend void SetToZero(Vector &v)
To have a uniform operator to put an element to zero, for scalar values and for objects.
Definition: frames.hpp:1062
Wrench(const Vector &_force, const Vector &_torque)
Definition: frames.hpp:887
friend Wrench operator-(const Wrench &lhs, const Wrench &rhs)
Definition: frames.hpp:264
friend void SetToZero(Twist &v)
Definition: frames.hpp:1065
void ReverseSign()
Definition: frames.hpp:802
double data[2]
Definition: frames.hpp:961
bool operator==(const Rotation &a, const Rotation &b)
Definition: frames.cpp:462
friend Wrench operator/(const Wrench &lhs, double rhs)
Scalar division.
Definition: frames.hpp:253
A 2D frame class, for further documentation see the Frames class for methods with unchanged semantics...
Definition: frames.hpp:1094
Rotation2()
c,s represent cos(angle), sin(angle), this also represents first col.
Definition: frames.hpp:1057
double c
Definition: frames.hpp:1052
double operator[](int index) const
Definition: frames.hpp:900
double data[9]
Definition: frames.hpp:304
static Vector2 Zero()
Definition: frames.hpp:780
double s
Definition: frames.hpp:1052
void ReverseSign()
Reverses the sign of the current Wrench.
Definition: frames.hpp:192