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;
423 void GetQuaternion(
double& x,
double& y,
double& z,
double& w)
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(
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;
1081 inline void SetRot(
double angle);
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.inl:945
Vector2 operator*(const Vector2 &arg) const
Definition: frames.inl:935
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.inl:978
static Frame2 Identity()
Definition: frames.hpp:1127
Frame2 & operator=(const Frame2 &arg)
Definition: frames.inl:966
void Integrate(const Twist &t_this, double frequency)
Rotation2 M
Orientation of the Frame.
Definition: frames.hpp:1100
friend bool Equal(const Frame2 &a, const Frame2 &b, double eps)
Vector2 p
origine of the Frame
Definition: frames.hpp:1099
void SetInverse()
Definition: frames.inl:951
Frame2 Inverse() const
Definition: frames.inl:959
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.inl:700
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.inl:624
Frame & operator=(const Frame &arg)
Normal copy-by-value semantics.
Definition: frames.inl:428
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.
Vector operator*(const Vector &arg) const
Transformation of the base to which the vector is expressed.
Definition: frames.inl:412
friend bool Equal(const Frame &a, const Frame &b, double eps)
do not use operator == because the definition of Equal(.,.) is slightly different.
Vector p
origine of the Frame
Definition: frames.hpp:572
Frame Inverse() const
Gives back inverse transformation of a Frame.
Definition: frames.inl:422
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.inl:667
Frame()
Definition: frames.hpp:584
friend bool operator!=(const Frame &a, const Frame &b)
The literal inequality operator!=().
A 2D Rotation class, for conventions see Rotation.
Definition: frames.hpp:1051
double GetRot() const
Gets the angle (in radians)
Definition: frames.inl:904
Vector2 operator*(const Vector2 &v) const
Definition: frames.inl:856
double operator()(int i, int j) const
Access to elements 0..1,0..1, bounds are checked when NDEBUG is not set.
Definition: frames.inl:860
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.inl:900
void SetInverse()
Definition: frames.inl:874
Rotation2()
c,s represent cos(angle), sin(angle), this also represents first col.
Definition: frames.hpp:1057
static Rotation2 Identity()
Definition: frames.inl:886
friend bool Equal(const Rotation2 &a, const Rotation2 &b, double eps)
do not use operator == because the definition of Equal(.,.) is slightly different.
double s
Definition: frames.hpp:1052
double c
Definition: frames.hpp:1052
void SetIdentity()
Definition: frames.inl:890
Rotation2(double angle_rad)
Definition: frames.hpp:1059
Rotation2 Inverse() const
Definition: frames.inl:878
void SetRot(double angle)
The SetRot.. functions set the value of *this to the appropriate rotation matrix.
Definition: frames.inl:896
Rotation2 & operator=(const Rotation2 &arg)
Definition: frames.inl:851
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...
Definition: frames.inl:588
Rotation Inverse() const
Gives back the inverse rotation matrix of *this.
Definition: frames.inl:637
Rotation & operator=(const Rotation &arg)
Definition: frames.inl:520
Vector operator*(const Vector &v) const
Defines a multiplication R*V between a Rotation R and a Vector V.
Definition: frames.inl:526
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
static Rotation Identity()
Gives back an identity rotaton matrix.
Definition: frames.inl:552
static Rotation RotX(double angle)
The Rot... static functions give the value of the appropriate rotation matrix back.
Definition: frames.inl:605
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!=()
void UnitX(const Vector &X)
Access to the underlying unitvectors of the rotation matrix.
Definition: frames.hpp:515
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
void DoRotX(double angle)
The DoRot...
Definition: frames.inl:556
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.inl:610
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.inl:653
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
void UnitY(const Vector &X)
Access to the underlying unitvectors of the rotation matrix.
Definition: frames.hpp:527
static Rotation RotZ(double angle)
The Rot... static functions give the value of the appropriate rotation matrix back.
Definition: frames.inl:615
double & operator()(int i, int j)
Access to elements 0..2,0..2, bounds are checked when NDEBUG is not set.
Definition: frames.inl:488
void DoRotY(double angle)
The DoRot...
Definition: frames.inl:572
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)
friend Wrench operator*(const Twist &lhs, const Wrench &rhs)
Spatial cross product for 6d force vectors, beware all of them have to be expressed in the same refer...
Twist(const Vector &_vel, const Vector &_rot)
Definition: frames.hpp:729
Twist & operator+=(const Twist &arg)
Definition: frames.inl:319
double & operator()(int i)
index-based access to components, first vel(0..2), then rot(3..5)
Definition: frames.inl:326
friend double dot(const Twist &lhs, const Wrench &rhs)
friend double dot(const Wrench &rhs, const Twist &lhs)
friend bool Equal(const Twist &a, const Twist &b, double eps)
do not use operator == because the definition of Equal(.,.) is slightly different.
friend bool operator==(const Twist &a, const Twist &b)
The literal equality operator==(), also identical.
Vector rot
The rotational velocity of that point.
Definition: frames.hpp:723
friend Twist operator-(const Twist &lhs, const Twist &rhs)
Twist()
The default constructor initialises to Zero via the constructor of Vector.
Definition: frames.hpp:727
friend void SetToZero(Twist &v)
friend Twist operator/(const Twist &lhs, double rhs)
friend Twist operator*(const Twist &lhs, const Twist &rhs)
Spatial cross product for 6d motion vectors, beware all of them have to be expressed in the same refe...
friend Twist operator-(const Twist &arg)
void ReverseSign()
Reverses the sign of the twist.
Definition: frames.inl:296
Twist RefPoint(const Vector &v_base_AB) const
Changes the reference point of the twist.
Definition: frames.inl:302
friend Twist operator*(double lhs, const Twist &rhs)
friend Twist operator+(const Twist &lhs, const Twist &rhs)
Twist & operator-=(const Twist &arg)
Definition: frames.inl:312
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.inl:290
friend bool operator!=(const Twist &a, const Twist &b)
The literal inequality operator!=().
2D version of Vector
Definition: frames.hpp:960
friend bool operator!=(const Vector2 &a, const Vector2 &b)
The literal inequality operator!=().
static Vector2 Zero()
Definition: frames.inl:784
friend Vector2 operator-(const Vector2 &arg)
friend Vector2 operator-(const Vector2 &lhs, const Vector2 &rhs)
void ReverseSign()
Definition: frames.inl:806
friend Vector2 operator*(const Vector2 &lhs, double rhs)
friend bool operator==(const Vector2 &a, const Vector2 &b)
The literal equality operator==(), also identical.
double x() const
Definition: frames.inl:799
void Set3DZX(const Vector &v)
projects v in its ZX plane, and sets *this to these values
Definition: frames.inl:831
Vector2 & operator=(const Vector2 &arg)
Definition: frames.inl:737
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.inl:819
Vector2()
Does not initialise to Zero().
Definition: frames.hpp:964
friend Vector2 operator/(const Vector2 &lhs, double rhs)
Vector2 & operator+=(const Vector2 &arg)
Definition: frames.inl:770
void Set3DYZ(const Vector &v)
projects v in its YZ plane, and sets *this to these values
Definition: frames.inl:825
friend void SetToZero(Vector2 &v)
friend Vector2 operator*(const Vector2 &lhs, const Vector2 &rhs)
friend Vector2 operator+(const Vector2 &lhs, const Vector2 &rhs)
Vector2 & operator-=(const Vector2 &arg)
Definition: frames.inl:777
friend bool Equal(const Vector2 &a, const Vector2 &b, double eps)
do not use operator == because the definition of Equal(.,.) is slightly different.
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.inl:788
double y() const
Definition: frames.inl:800
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.inl:838
friend Vector2 operator*(double lhs, const Vector2 &rhs)
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.inl:707
friend double dot(const Vector &lhs, const Vector &rhs)
void ReverseSign()
Reverses the sign of the Vector object itself.
Definition: frames.inl:440
friend void SetToZero(Vector &v)
To have a uniform operator to put an element to zero, for scalar values and for objects.
Vector & operator-=(const Vector &arg)
subtracts a vector from the Vector object itself
Definition: frames.inl:129
friend Vector operator+(const Vector &lhs, const Vector &rhs)
friend Vector operator/(const Vector &lhs, double rhs)
Scalar division is defined.
friend bool operator==(const Vector &a, const Vector &b)
The literal equality operator==(), also identical.
friend bool Equal(const Vector &a, const Vector &b, double eps)
do not use operator == because the definition of Equal(.,.) is slightly different.
double z() const
Definition: frames.inl:77
double operator[](int index) const
Equivalent to double operator()(int index) const.
Definition: frames.hpp:183
double y() const
Definition: frames.inl:76
friend Vector operator-(const Vector &arg)
void Set2DZX(const Vector2 &v)
a 3D vector where the 2D vector v is put in the ZX plane
Definition: frames.inl:475
Vector & operator+=(const Vector &arg)
Adds a vector from the Vector object itself.
Definition: frames.inl:120
friend Vector operator*(const Vector &lhs, const Vector &rhs)
Vector & operator=(const Vector &arg)
Assignment operator. The normal copy by value semantics.
Definition: frames.inl:49
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.inl:75
static Vector Zero()
Definition: frames.inl:138
void Set2DXY(const Vector2 &v)
a 3D vector where the 2D vector v is put in the XY plane
Definition: frames.inl:458
double operator()(int index) const
Access to elements, range checked when NDEBUG is not set, from 0..2.
Definition: frames.inl:143
friend Vector operator*(const Vector &lhs, double rhs)
Scalar multiplication is defined.
friend Vector operator-(const Vector &lhs, const Vector &rhs)
double Norm(double eps=epsilon) const
Definition: frames.cpp:117
friend bool operator!=(const Vector &a, const Vector &b)
The literal inequality operator!=().
void Set2DYZ(const Vector2 &v)
a 3D vector where the 2D vector v is put in the YZ plane
Definition: frames.inl:466
friend Vector operator*(double lhs, const Vector &rhs)
Scalar multiplication is defined.
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.
Wrench & operator+=(const Wrench &arg)
Definition: frames.inl:216
friend Wrench operator+(const Wrench &lhs, const Wrench &rhs)
Wrench & operator-=(const Wrench &arg)
Definition: frames.inl:209
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.
friend Wrench operator/(const Wrench &lhs, double rhs)
Scalar division.
Wrench()
Does initialise force and torque to zero via the underlying constructor of Vector.
Definition: frames.hpp:886
static Wrench Zero()
Definition: frames.inl:185
Wrench(const Vector &_force, const Vector &_torque)
Definition: frames.hpp:887
double & operator()(int i)
index-based access to components, first force(0..2), then torque(3..5)
Definition: frames.inl:223
friend Wrench operator-(const Wrench &lhs, const Wrench &rhs)
friend Wrench operator-(const Wrench &arg)
An unary - operator.
friend bool operator!=(const Wrench &a, const Wrench &b)
The literal inequality operator!=().
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.inl:197
friend Wrench operator*(const Wrench &lhs, double rhs)
Scalar multiplication.
friend Wrench operator*(double lhs, const Wrench &rhs)
Scalar multiplication.
friend void SetToZero(Wrench &v)
Sets the Wrench to Zero, to have a uniform function that sets an object or double to zero.
void ReverseSign()
Reverses the sign of the current Wrench.
Definition: frames.inl:191
Inlined member functions and global functions that relate to the classes in frames....
Definition: articulatedbodyinertia.cpp:28
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.
IMETHOD Vector addDelta(const Vector &p_w_a, const Vector &p_w_da, double dt=1)
adds vector da to vector a.
IMETHOD bool Equal(const FrameAcc &r1, const FrameAcc &r2, double eps=epsilon)