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);
240 double Norm(
double eps=epsilon)
const;
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);
809 AccelerationTwist():trans(),rot() {};
811 AccelerationTwist(const Vector& _trans,const Vector& _rot):trans(_trans),rot(_rot) {};
813 inline AccelerationTwist& operator-=(const AccelerationTwist& arg);
814 inline AccelerationTwist& operator+=(const AccelerationTwist& arg);
816 inline double& operator()(int i);
820 inline double operator()(int i) const;
822 double operator[] ( int index ) const
824 return this->operator() ( index );
827 double& operator[] ( int index )
829 return this->operator() ( index );
832 inline friend AccelerationTwist operator*(const AccelerationTwist& lhs,double rhs);
833 inline friend AccelerationTwist operator*(double lhs,const AccelerationTwist& rhs);
834 inline friend AccelerationTwist operator/(const AccelerationTwist& lhs,double rhs);
835 inline friend AccelerationTwist operator+(const AccelerationTwist& lhs,const AccelerationTwist& rhs);
836 inline friend AccelerationTwist operator-(const AccelerationTwist& lhs,const AccelerationTwist& rhs);
837 inline friend AccelerationTwist operator-(const AccelerationTwist& arg);
838 //inline friend double dot(const AccelerationTwist& lhs,const Wrench& rhs);
839 //inline friend double dot(const Wrench& rhs,const AccelerationTwist& lhs);
840 inline friend void SetToZero(AccelerationTwist& v);
844 static inline AccelerationTwist Zero();
847 inline void ReverseSign();
855 inline AccelerationTwist RefPoint(const Vector& v_base_AB) const;
860 inline friend bool Equal(const AccelerationTwist& a,const AccelerationTwist& b,double eps=epsilon);
863 inline friend bool operator==(const AccelerationTwist& a,const AccelerationTwist& b);
865 inline friend bool operator!=(const AccelerationTwist& a,const AccelerationTwist& b);
868 friend class Rotation;
988 inline double x()
const;
989 inline double y()
const;
990 inline void x(
double);
991 inline void y(
double);
1018 double Norm(
double eps=epsilon)
const;
1059 explicit Rotation2(
double angle_rad):
s(sin(angle_rad)),
c(cos(angle_rad)) {}
1068 inline double operator() (
int i,
int j)
const;
1087 inline double GetRot()
const;
1117 inline double operator() (
int i,
int j)
const;
A 2D frame class, for further documentation see the Frames class for methods with unchanged semantics...
Definition frames.hpp:1097
void SetIdentity()
Definition frames.hpp:946
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:979
friend bool Equal(const Frame2 &a, const Frame2 &b, double eps)
Definition frames.hpp:1064
friend Frame2 operator*(const Frame2 &lhs, const Frame2 &rhs)
Definition frames.hpp:931
static Frame2 Identity()
Definition frames.hpp:1127
Frame2 & operator=(const Frame2 &arg)
Definition frames.hpp:967
void Integrate(const Twist &t_this, double frequency)
Frame2(void)
Definition frames.hpp:910
Rotation2 M
Orientation of the Frame.
Definition frames.hpp:1100
Vector2 p
origine of the Frame
Definition frames.hpp:1099
void SetInverse()
Definition frames.hpp:952
Frame2 Inverse() const
Definition frames.hpp:960
Definition frames.hpp:570
static Frame DH_Craig1989(double a, double alpha, double d, double theta)
Definition frames.cpp:53
static Frame Identity()
Definition frames.hpp:701
Rotation M
Orientation of the Frame.
Definition frames.hpp:573
void Make4x4(double *d)
Reads data from an double array.
Definition frames.cpp:39
void Integrate(const Twist &t_this, double frequency)
The twist <t_this> is expressed wrt the current frame.
Definition frames.hpp:625
Frame & operator=(const Frame &arg)
Normal copy-by-value semantics.
Definition frames.hpp:429
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:1040
static Frame DH(double a, double alpha, double d, double theta)
Definition frames.cpp:70
friend bool operator==(const Frame &a, const Frame &b)
The literal equality operator==(), also identical.
Definition frames.hpp:1276
Vector p
origine of the Frame
Definition frames.hpp:572
Frame Inverse() const
Gives back inverse transformation of a Frame.
Definition frames.hpp:423
friend Frame operator*(const Frame &lhs, const Frame &rhs)
Composition of two frames.
Definition frames.hpp:407
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:668
Frame()
Definition frames.hpp:584
friend bool operator!=(const Frame &a, const Frame &b)
The literal inequality operator!=().
Definition frames.hpp:1285
A 2D Rotation class, for conventions see Rotation.
Definition frames.hpp:1051
double GetRot() const
Gets the angle (in radians)
Definition frames.hpp:905
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:861
Rotation2(double ca, double sa)
Definition frames.hpp:1061
static Rotation2 Rot(double angle)
The Rot... static functions give the value of the appropriate rotation matrix bac.
Definition frames.hpp:901
void SetInverse()
Definition frames.hpp:875
Rotation2()
c,s represent cos(angle), sin(angle), this also represents first col.
Definition frames.hpp:1057
static Rotation2 Identity()
Definition frames.hpp:887
friend Rotation2 operator*(const Rotation2 &lhs, const Rotation2 &rhs)
Definition frames.hpp:871
double s
Definition frames.hpp:1052
double c
Definition frames.hpp:1052
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:1060
void SetIdentity()
Definition frames.hpp:891
Rotation2(double angle_rad)
Definition frames.hpp:1059
Rotation2 Inverse() const
Definition frames.hpp:879
void SetRot(double angle)
The SetRot.. functions set the value of *this to the appropriate rotation matrix.
Definition frames.hpp:897
Rotation2 & operator=(const Rotation2 &arg)
Definition frames.hpp:852
represents rotations in 3 dimensional space.
Definition frames.hpp:302
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:275
Vector GetRot() const
Returns a vector with the direction of the equiv.
Definition frames.cpp:336
void GetRPY(double &roll, double &pitch, double &yaw) const
Gives back a vector in RPY coordinates, variables are bound by.
Definition frames.cpp:249
void DoRotZ(double angle)
The DoRot... functions apply a rotation R to *this,such that *this = *this * Rot.
Definition frames.hpp:589
Rotation Inverse() const
Gives back the inverse rotation matrix of *this.
Definition frames.hpp:638
Rotation & operator=(const Rotation &arg)
Definition frames.hpp:521
Vector UnitY() const
Access to the underlying unitvectors of the rotation matrix.
Definition frames.hpp:522
void UnitZ(const Vector &X)
Access to the underlying unitvectors of the rotation matrix.
Definition frames.hpp:539
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:190
Vector UnitZ() const
Access to the underlying unitvectors of the rotation matrix.
Definition frames.hpp:534
Vector UnitX() const
Access to the underlying unitvectors of the rotation matrix.
Definition frames.hpp:510
Rotation()
Definition frames.hpp:306
double GetRotAngle(Vector &axis, double eps=epsilon) const
Returns the rotation angle around the equiv.
Definition frames.cpp:358
static Rotation EulerZYX(double Alfa, double Beta, double Gamma)
EulerZYX constructs a Rotation from the Euler ZYX parameters:
Definition frames.hpp:469
static Rotation Rot2(const Vector &rotvec, double angle)
Along an arbitrary axes. rotvec should be normalized.
Definition frames.cpp:303
friend Rotation operator*(const Rotation &lhs, const Rotation &rhs)
Definition frames.cpp:173
static Rotation Identity()
Gives back an identity rotaton matrix.
Definition frames.hpp:553
static Rotation RotX(double angle)
The Rot... static functions give the value of the appropriate rotation matrix back.
Definition frames.hpp:606
friend bool operator==(const Rotation &a, const Rotation &b)
The literal equality operator==(), also identical.
Definition frames.cpp:430
friend bool operator!=(const Rotation &a, const Rotation &b)
The literal inequality operator!=()
Definition frames.hpp:1328
void UnitX(const Vector &X)
Access to the underlying unitvectors of the rotation matrix.
Definition frames.hpp:515
void DoRotX(double angle)
The DoRot... functions apply a rotation R to *this,such that *this = *this * Rot.
Definition frames.hpp:557
double data[9]
Definition frames.hpp:304
static Rotation RotY(double angle)
The Rot... static functions give the value of the appropriate rotation matrix back.
Definition frames.hpp:611
void GetQuaternion(double &x, double &y, double &z, double &w) const
Get the quaternion of this matrix.
Definition frames.cpp:204
void SetInverse()
Sets the value of *this to its inverse.
Definition frames.hpp:654
static Rotation Rot(const Vector &rotvec, double angle)
Along an arbitrary axes.
Definition frames.cpp:293
static Rotation EulerZYZ(double Alfa, double Beta, double Gamma)
Gives back a rotation matrix specified with EulerZYZ convention :
Definition frames.cpp:262
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 Equal(const Rotation &a, const Rotation &b, double eps)
do not use operator == because the definition of Equal(.,.) is slightly different.
Definition frames.cpp:159
static Rotation RotZ(double angle)
The Rot... static functions give the value of the appropriate rotation matrix back.
Definition frames.hpp:616
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
void DoRotY(double angle)
The DoRot... functions apply a rotation R to *this,such that *this = *this * Rot.
Definition frames.hpp:573
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:237
represents both translational and rotational velocities.
Definition frames.hpp:720
friend Twist operator*(const Twist &lhs, double rhs)
Definition frames.hpp:346
Twist(const Vector &_vel, const Vector &_rot)
Definition frames.hpp:729
Twist & operator+=(const Twist &arg)
Definition frames.hpp:320
double & operator()(int i)
index-based access to components, first vel(0..2), then rot(3..5)
Definition frames.hpp:327
friend double dot(const Twist &lhs, const Wrench &rhs)
Definition frames.hpp:1017
friend bool operator==(const Twist &a, const Twist &b)
The literal equality operator==(), also identical.
Definition frames.hpp:1303
Vector rot
The rotational velocity of that point.
Definition frames.hpp:723
friend Twist operator-(const Twist &lhs, const Twist &rhs)
Definition frames.hpp:367
Twist()
The default constructor initialises to Zero via the constructor of Vector.
Definition frames.hpp:727
friend void SetToZero(Twist &v)
Definition frames.hpp:1072
friend Twist operator/(const Twist &lhs, double rhs)
Definition frames.hpp:356
void ReverseSign()
Reverses the sign of the twist.
Definition frames.hpp:297
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:1050
Twist RefPoint(const Vector &v_base_AB) const
Changes the reference point of the twist.
Definition frames.hpp:303
friend Twist operator+(const Twist &lhs, const Twist &rhs)
Definition frames.hpp:362
Twist & operator-=(const Twist &arg)
Definition frames.hpp:313
double operator[](int index) const
Definition frames.hpp:740
Vector vel
The velocity of that point.
Definition frames.hpp:722
static Twist Zero()
Definition frames.hpp:291
friend bool operator!=(const Twist &a, const Twist &b)
The literal inequality operator!=().
Definition frames.hpp:1312
2D version of Vector
Definition frames.hpp:960
friend bool operator!=(const Vector2 &a, const Vector2 &b)
The literal inequality operator!=().
Definition frames.hpp:1341
static Vector2 Zero()
Definition frames.hpp:785
friend Vector2 operator-(const Vector2 &lhs, const Vector2 &rhs)
Definition frames.hpp:751
void ReverseSign()
Definition frames.hpp:807
friend Vector2 operator*(const Vector2 &lhs, double rhs)
Definition frames.hpp:756
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:1055
friend bool operator==(const Vector2 &a, const Vector2 &b)
The literal equality operator==(), also identical.
Definition frames.hpp:1332
double x() const
Definition frames.hpp:800
void Set3DZX(const Vector &v)
projects v in its ZX plane, and sets *this to these values
Definition frames.hpp:832
Vector2 & operator=(const Vector2 &arg)
Definition frames.hpp:738
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:104
void Set3DXY(const Vector &v)
projects v in its XY plane, and sets *this to these values
Definition frames.hpp:820
Vector2()
Does not initialise to Zero().
Definition frames.hpp:964
friend Vector2 operator/(const Vector2 &lhs, double rhs)
Definition frames.hpp:766
Vector2 & operator+=(const Vector2 &arg)
Definition frames.hpp:771
void Set3DYZ(const Vector &v)
projects v in its YZ plane, and sets *this to these values
Definition frames.hpp:826
friend void SetToZero(Vector2 &v)
Definition frames.hpp:1081
friend Vector2 operator*(const Vector2 &lhs, const Vector2 &rhs)
friend Vector2 operator+(const Vector2 &lhs, const Vector2 &rhs)
Definition frames.hpp:746
Vector2 & operator-=(const Vector2 &arg)
Definition frames.hpp:778
double Norm(double eps=epsilon) const
Definition frames.cpp:87
double operator()(int index) const
Access to elements, range checked when NDEBUG is not set, from 0..1.
Definition frames.hpp:789
double y() const
Definition frames.hpp:801
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:839
double data[2]
Definition frames.hpp:961
A concrete implementation of a 3 dimensional vector class.
Definition frames.hpp:161
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:708
friend double dot(const Vector &lhs, const Vector &rhs)
Definition frames.hpp:1013
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:1033
void ReverseSign()
Reverses the sign of the Vector object itself.
Definition frames.hpp:441
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:1069
Vector & operator-=(const Vector &arg)
subtracts a vector from the Vector object itself
Definition frames.hpp:130
friend Vector operator+(const Vector &lhs, const Vector &rhs)
Definition frames.hpp:58
friend Vector operator/(const Vector &lhs, double rhs)
Scalar division is defined.
Definition frames.hpp:102
friend bool operator==(const Vector &a, const Vector &b)
The literal equality operator==(), also identical.
Definition frames.hpp:1289
double z() const
Definition frames.hpp:78
double operator[](int index) const
Equivalent to double operator()(int index) const.
Definition frames.hpp:183
double y() const
Definition frames.hpp:77
void Set2DZX(const Vector2 &v)
a 3D vector where the 2D vector v is put in the ZX plane
Definition frames.hpp:476
Vector & operator+=(const Vector &arg)
Adds a vector from the Vector object itself.
Definition frames.hpp:121
Vector & operator=(const Vector &arg)
Assignment operator. The normal copy by value semantics.
Definition frames.hpp:50
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:147
Vector()
Does not initialise the Vector to zero. use Vector::Zero() or SetToZero for that.
Definition frames.hpp:165
double data[3]
Definition frames.hpp:163
double x() const
Definition frames.hpp:76
static Vector Zero()
Definition frames.hpp:139
void Set2DXY(const Vector2 &v)
a 3D vector where the 2D vector v is put in the XY plane
Definition frames.hpp:459
double operator()(int index) const
Access to elements, range checked when NDEBUG is not set, from 0..2.
Definition frames.hpp:144
friend Vector operator*(const Vector &lhs, double rhs)
Scalar multiplication is defined.
Definition frames.hpp:84
friend Vector operator-(const Vector &lhs, const Vector &rhs)
Definition frames.hpp:67
double Norm(double eps=epsilon) const
Definition frames.cpp:117
friend bool operator!=(const Vector &a, const Vector &b)
The literal inequality operator!=().
Definition frames.hpp:1299
void Set2DYZ(const Vector2 &v)
a 3D vector where the 2D vector v is put in the YZ plane
Definition frames.hpp:467
represents both translational and rotational acceleration.
Definition frames.hpp:879
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:1045
Wrench & operator+=(const Wrench &arg)
Definition frames.hpp:217
friend Wrench operator+(const Wrench &lhs, const Wrench &rhs)
Definition frames.hpp:259
Wrench & operator-=(const Wrench &arg)
Definition frames.hpp:210
Vector force
Force that is applied at the origin of the current ref frame.
Definition frames.hpp:881
friend bool operator==(const Wrench &a, const Wrench &b)
The literal equality operator==(), also identical.
Definition frames.hpp:1316
friend Wrench operator/(const Wrench &lhs, double rhs)
Scalar division.
Definition frames.hpp:253
Wrench()
Does initialise force and torque to zero via the underlying constructor of Vector.
Definition frames.hpp:886
static Wrench Zero()
Definition frames.hpp:186
double & operator()(int i)
index-based access to components, first force(0..2), then torque(3..5)
Definition frames.hpp:224
friend Wrench operator-(const Wrench &lhs, const Wrench &rhs)
Definition frames.hpp:264
friend bool operator!=(const Wrench &a, const Wrench &b)
The literal inequality operator!=().
Definition frames.hpp:1325
Vector torque
Torque that is applied at the origin of the current ref frame.
Definition frames.hpp:882
double operator[](int index) const
Definition frames.hpp:900
Wrench RefPoint(const Vector &v_base_AB) const
Changes the reference point of the wrench.
Definition frames.hpp:198
friend Wrench operator*(const Wrench &lhs, double rhs)
Scalar multiplication.
Definition frames.hpp:243
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:1076
void ReverseSign()
Reverses the sign of the current Wrench.
Definition frames.hpp:192
Inlined member functions and global functions that relate to the classes in frames....
Definition articulatedbodyinertia.cpp:26
bool operator==(const Rotation &a, const Rotation &b)
Definition frames.cpp:430
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:1130
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:1157
IMETHOD bool Equal(const FrameAcc &r1, const FrameAcc &r2, double eps=epsilon)
Definition frameacc.hpp:394