Go to the documentation of this file.
128 #include "utilities/kdl-config.h"
129 #include "utilities/utility.h"
148 inline bool Equal(
const Vector& a,
const Vector& b,
double eps=epsilon);
149 inline bool Equal(
const Frame& a,
const Frame& b,
double eps=epsilon);
150 inline bool Equal(
const Twist& a,
const Twist& b,
double eps=epsilon);
151 inline bool Equal(
const Wrench& a,
const Wrench& b,
double eps=epsilon);
152 inline bool Equal(
const Vector2& a,
const Vector2& b,
double eps=epsilon);
153 inline bool Equal(
const Rotation2& a,
const Rotation2& b,
double eps=epsilon);
154 inline bool Equal(
const Frame2& a,
const Frame2& b,
double eps=epsilon);
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;
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()
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;
1143 IMETHOD Vector
diff(
const Vector& p_w_a,
const Vector& p_w_b,
double dt=1);
1173 IMETHOD Vector
diff(
const Rotation& R_a_b1,
const Rotation& R_a_b2,
double dt=1);
1183 IMETHOD Twist
diff(
const Frame& F_a_b1,
const Frame& F_a_b2,
double dt=1);
1189 IMETHOD Twist
diff(
const Twist& a,
const Twist& b,
double dt=1);
1195 IMETHOD Wrench
diff(
const Wrench& W_a_p1,
const Wrench& W_a_p2,
double dt=1);
1204 IMETHOD Vector
addDelta(
const Vector& p_w_a,
const Vector& p_w_da,
double dt=1);
1218 IMETHOD Rotation
addDelta(
const Rotation& R_w_a,
const Vector& da_w,
double dt=1);
1230 IMETHOD Frame
addDelta(
const Frame& F_w_a,
const Twist& da_w,
double dt=1);
1239 IMETHOD Twist
addDelta(
const Twist& a,
const Twist&da,
double dt=1);
1250 IMETHOD Wrench
addDelta(
const Wrench& a,
const Wrench&da,
double dt=1);
friend bool Equal(const Twist &a, const Twist &b, double eps)
do not use operator == because the definition of Equal(.,.) is slightly different.
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
void SetInverse()
Sets the value of *this to its inverse.
Definition: frames.inl:648
void UnitY(const Vector &X)
Access to the underlying unitvectors of the rotation matrix.
Definition: frames.hpp:527
static Rotation Rot2(const Vector &rotvec, double angle)
Along an arbitrary axes. rotvec should be normalized.
Definition: frames.cpp:304
double data[3]
Definition: frames.hpp:163
Wrench & operator+=(const Wrench &arg)
Definition: frames.inl:216
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:853
static Frame Identity()
Definition: frames.inl:695
friend Wrench operator+(const Wrench &lhs, const Wrench &rhs)
double y() const
Definition: frames.inl:76
Vector rot
The rotational velocity of that point.
Definition: frames.hpp:723
A 2D frame class, for further documentation see the Frames class for methods with unchanged semantics...
Definition: frames.hpp:1094
Frame Inverse() const
Gives back inverse transformation of a Frame.
Definition: frames.inl:422
void Set2DZX(const Vector2 &v)
a 3D vector where the 2D vector v is put in the ZX plane
Definition: frames.inl:475
void Set3DYZ(const Vector &v)
projects v in its YZ plane, and sets *this to these values
Definition: frames.inl:820
ArticulatedBodyInertia operator*(double a, const ArticulatedBodyInertia &I)
Scalar product: I_new = double * I_old.
Definition: articulatedbodyinertia.cpp:51
Vector GetRot() const
Returns a vector with the direction of the equiv.
Definition: frames.cpp:337
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
friend bool Equal(const Frame &a, const Frame &b, double eps)
do not use operator == because the definition of Equal(.,.) is slightly different.
Rotation Inverse() const
Gives back the inverse rotation matrix of *this.
Definition: frames.inl:632
friend bool operator!=(const Rotation &a, const Rotation &b)
The literal inequality operator!=()
Vector operator*(const Vector &arg) const
Transformation of the base to which the vector is expressed.
Definition: frames.inl:412
static Vector Zero()
Definition: frames.inl:138
Vector2 & operator-=(const Vector2 &arg)
Definition: frames.inl:772
Frame()
Definition: frames.hpp:584
void Integrate(const Twist &t_this, double frequency)
The twist <t_this> is expressed wrt the current frame.
Definition: frames.inl:619
friend Wrench operator-(const Wrench &lhs, const Wrench &rhs)
void DoRotZ(double angle)
The DoRot...
Definition: frames.inl:583
friend double dot(const Twist &lhs, const Wrench &rhs)
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
friend Twist operator*(const Twist &lhs, double rhs)
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
Vector p
origine of the Frame
Definition: frames.hpp:572
Rotation()
Definition: frames.hpp:306
double operator[](int index) const
Equivalent to double operator()(int index) const.
Definition: frames.hpp:977
Vector2 & operator=(const Vector2 &arg)
Definition: frames.inl:732
friend bool operator==(const Twist &a, const Twist &b)
The literal equality operator==(), also identical.
double c
Definition: frames.hpp:1052
void UnitX(const Vector &X)
Access to the underlying unitvectors of the rotation matrix.
Definition: frames.hpp:515
static Rotation RotY(double angle)
The Rot... static functions give the value of the appropriate rotation matrix back.
Definition: frames.inl:605
friend bool Equal(const Vector2 &a, const Vector2 &b, double eps)
do not use operator == because the definition of Equal(.,.) is slightly different.
Twist & operator-=(const Twist &arg)
Definition: frames.inl:312
friend bool operator==(const Vector &a, const Vector &b)
The literal equality operator==(), also identical.
friend bool operator!=(const Vector &a, const Vector &b)
The literal inequality operator!=().
void DoRotX(double angle)
The DoRot...
Definition: frames.inl:551
friend Wrench operator/(const Wrench &lhs, double rhs)
Scalar division.
void GetRPY(double &roll, double &pitch, double &yaw) const
Gives back a vector in RPY coordinates, variables are bound by.
Definition: frames.cpp:250
friend Vector2 operator/(const Vector2 &lhs, double rhs)
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
double operator[](int index) const
Definition: frames.hpp:900
void Set2DYZ(const Vector2 &v)
a 3D vector where the 2D vector v is put in the YZ plane
Definition: frames.inl:466
friend Twist operator+(const Twist &lhs, const Twist &rhs)
friend bool Equal(const Vector &a, const Vector &b, double eps)
do not use operator == because the definition of Equal(.,.) is slightly different.
friend Vector operator+(const Vector &lhs, const Vector &rhs)
void SetInverse()
Definition: frames.inl:944
Vector & operator+=(const Vector &arg)
Adds a vector from the Vector object itself.
Definition: frames.inl:120
Rotation2()
c,s represent cos(angle), sin(angle), this also represents first col.
Definition: frames.hpp:1057
void Make4x4(double *d)
Reads data from an double array.
Definition: frames.cpp:40
IMETHOD Vector addDelta(const Vector &p_w_a, const Vector &p_w_da, double dt=1)
adds vector da to vector a.
Definition: articulatedbodyinertia.cpp:28
Wrench RefPoint(const Vector &v_base_AB) const
Changes the reference point of the wrench.
Definition: frames.inl:197
double Norm() const
Definition: frames.cpp:88
friend bool operator==(const Frame &a, const Frame &b)
The literal equality operator==(), also identical.
double s
Definition: frames.hpp:1052
A concrete implementation of a 3 dimensional vector class.
Definition: frames.hpp:160
bool operator==(const Rotation &a, const Rotation &b)
Definition: frames.cpp:462
double & operator()(int i)
index-based access to components, first force(0..2), then torque(3..5)
Definition: frames.inl:223
Vector2 operator*(const Vector2 &arg) const
Definition: frames.inl:928
Vector & operator-=(const Vector &arg)
subtracts a vector from the Vector object itself
Definition: frames.inl:129
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:702
represents both translational and rotational velocities.
Definition: frames.hpp:720
void DoRotY(double angle)
The DoRot...
Definition: frames.inl:567
friend void SetToZero(Twist &v)
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
double y() const
Definition: frames.inl:795
friend void SetToZero(Vector &v)
To have a uniform operator to put an element to zero, for scalar values and for objects.
double z() const
Definition: frames.inl:77
Vector2 operator*(const Vector2 &v) const
Definition: frames.inl:849
Vector torque
Torque that is applied at the origin of the current ref frame.
Definition: frames.hpp:882
void SetIdentity()
Definition: frames.inl:938
static Frame2 Identity()
Definition: frames.hpp:1125
Frame2 & operator=(const Frame2 &arg)
Definition: frames.inl:959
void GetQuaternion(double &x, double &y, double &z, double &w) const
Get the quaternion of this matrix.
Definition: frames.cpp:205
void ReverseSign()
Definition: frames.inl:801
static Rotation RotZ(double angle)
The Rot... static functions give the value of the appropriate rotation matrix back.
Definition: frames.inl:610
static Rotation Identity()
Gives back an identity rotaton matrix.
Definition: frames.inl:547
represents both translational and rotational acceleration.
Definition: frames.hpp:878
Rotation2 Inverse() const
Definition: frames.inl:871
void SetIdentity()
Definition: frames.inl:883
void Set3DZX(const Vector &v)
projects v in its ZX plane, and sets *this to these values
Definition: frames.inl:826
friend bool operator!=(const Frame &a, const Frame &b)
The literal inequality operator!=().
Definition: frames.hpp:570
double & operator()(int i)
index-based access to components, first vel(0..2), then rot(3..5)
Definition: frames.inl:326
friend bool operator!=(const Wrench &a, const Wrench &b)
The literal inequality operator!=().
friend Twist operator-(const Twist &lhs, const Twist &rhs)
double GetRotAngle(Vector &axis, double eps=epsilon) const
Returns the rotation angle around the equiv.
Definition: frames.cpp:359
Vector operator*(const Vector &v) const
Defines a multiplication R*V between a Rotation R and a Vector V.
Definition: frames.inl:521
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
double x() const
Definition: frames.inl:794
friend Twist operator/(const Twist &lhs, double rhs)
void ReverseSign()
Reverses the sign of the twist.
Definition: frames.inl:296
double operator[](int index) const
Equivalent to double operator()(int index) const.
Definition: frames.hpp:183
friend void SetToZero(Vector2 &v)
void Integrate(const Twist &t_this, double frequency)
A 2D Rotation class, for conventions see Rotation.
Definition: frames.hpp:1050
static Rotation EulerZYZ(double Alfa, double Beta, double Gamma)
Gives back a rotation matrix specified with EulerZYZ convention :
Definition: frames.cpp:263
static Frame DH(double a, double alpha, double d, double theta)
Definition: frames.cpp:71
static Frame DH_Craig1989(double a, double alpha, double d, double theta)
Definition: frames.cpp:54
void Set2DXY(const Vector2 &v)
a 3D vector where the 2D vector v is put in the XY plane
Definition: frames.inl:458
friend bool operator==(const Wrench &a, const Wrench &b)
The literal equality operator==(), also identical.
void Set3DXY(const Vector &v)
projects v in its XY plane, and sets *this to these values
Definition: frames.inl:814
Vector vel
The velocity of that point.
Definition: frames.hpp:722
friend Vector2 operator*(const Vector2 &lhs, double rhs)
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:971
Rotation2(double angle_rad)
Definition: frames.hpp:1059
Vector2 p
origine of the Frame
Definition: frames.hpp:1097
Twist RefPoint(const Vector &v_base_AB) const
Changes the reference point of the twist.
Definition: frames.inl:302
double data[9]
Definition: frames.hpp:304
friend bool operator!=(const Twist &a, const Twist &b)
The literal inequality operator!=().
static Twist Zero()
Definition: frames.inl:290
friend Wrench operator*(const Wrench &lhs, double rhs)
Scalar multiplication.
friend bool operator==(const Vector2 &a, const Vector2 &b)
The literal equality operator==(), also identical.
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:833
static Rotation EulerZYX(double Alfa, double Beta, double Gamma)
EulerZYX constructs a Rotation from the Euler ZYX parameters:
Definition: frames.hpp:469
double Norm() const
Definition: frames.cpp:118
friend Vector operator-(const Vector &lhs, const Vector &rhs)
static Rotation Rot(const Vector &rotvec, double angle)
Along an arbitrary axes.
Definition: frames.cpp:294
static Wrench Zero()
Definition: frames.inl:185
friend bool operator==(const Rotation &a, const Rotation &b)
The literal equality operator==(), also identical.
Definition: frames.cpp:462
IMETHOD bool Equal(const FrameAcc &r1, const FrameAcc &r2, double eps=epsilon)
static Vector2 Zero()
Definition: frames.inl:779
double GetRot() const
Gets the angle (in radians)
Definition: frames.inl:897
Vector UnitY() const
Access to the underlying unitvectors of the rotation matrix.
Definition: frames.hpp:522
friend Vector operator*(const Vector &lhs, double rhs)
Scalar multiplication is defined.
friend void SetToZero(Wrench &v)
Sets the Wrench to Zero, to have a uniform function that sets an object or double to zero.
Rotation2(double ca, double sa)
Definition: frames.hpp:1061
Rotation M
Orientation of the Frame.
Definition: frames.hpp:573
void ReverseSign()
Reverses the sign of the Vector object itself.
Definition: frames.inl:440
Twist()
The default constructor initialises to Zero via the constructor of Vector.
Definition: frames.hpp:727
Frame2 Inverse() const
Definition: frames.inl:952
Rotation2 & operator=(const Rotation2 &arg)
Definition: frames.inl:844
Rotation & operator=(const Rotation &arg)
Definition: frames.inl:515
static Rotation RotX(double angle)
The Rot... static functions give the value of the appropriate rotation matrix back.
Definition: frames.inl:600
friend Vector operator/(const Vector &lhs, double rhs)
Scalar division is defined.
2D version of Vector
Definition: frames.hpp:959
double operator()(int index) const
Access to elements, range checked when NDEBUG is not set, from 0..2.
Definition: frames.inl:143
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
Vector2()
Does not initialise to Zero().
Definition: frames.hpp:964
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:662
friend bool Equal(const Frame2 &a, const Frame2 &b, double eps)
void SetRot(double angle)
The SetRot.. functions set the value of *this to the appropriate rotation matrix.
Definition: frames.inl:889
friend bool operator!=(const Vector2 &a, const Vector2 &b)
The literal inequality operator!=().
Vector2 & operator+=(const Vector2 &arg)
Definition: frames.inl:765
static Rotation2 Identity()
Definition: frames.inl:879
Vector()
Does not initialise the Vector to zero. use Vector::Zero() or SetToZero for that.
Definition: frames.hpp:165
void ReverseSign()
Reverses the sign of the current Wrench.
Definition: frames.inl:191
double data[2]
Definition: frames.hpp:961
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.
friend bool Equal(const Wrench &a, const Wrench &b, double eps)
do not use operator == because the definition of Equal(.,.) is slightly different.
Wrench(const Vector &_force, const Vector &_torque)
Definition: frames.hpp:887
void SetInverse()
Definition: frames.inl:867
friend double dot(const Vector &lhs, const Vector &rhs)
double x() const
Definition: frames.inl:75
Vector force
Force that is applied at the origin of the current ref frame.
Definition: frames.hpp:881
double operator()(int index) const
Access to elements, range checked when NDEBUG is not set, from 0..1.
Definition: frames.inl:783
friend Vector2 operator+(const Vector2 &lhs, const Vector2 &rhs)
double operator[](int index) const
Definition: frames.hpp:740
static Rotation2 Rot(double angle)
The Rot... static functions give the value of the appropriate rotation matrix bac.
Definition: frames.inl:893
void UnitZ(const Vector &X)
Access to the underlying unitvectors of the rotation matrix.
Definition: frames.hpp:539
Vector UnitZ() const
Access to the underlying unitvectors of the rotation matrix.
Definition: frames.hpp:534
Twist & operator+=(const Twist &arg)
Definition: frames.inl:319
Vector UnitX() const
Access to the underlying unitvectors of the rotation matrix.
Definition: frames.hpp:510
Wrench & operator-=(const Wrench &arg)
Definition: frames.inl:209
Vector & operator=(const Vector &arg)
Assignment operator. The normal copy by value semantics.
Definition: frames.inl:49
Frame & operator=(const Frame &arg)
Normal copy-by-value semantics.
Definition: frames.inl:428
Wrench()
Does initialise force and torque to zero via the underlying constructor of Vector.
Definition: frames.hpp:886
friend Vector2 operator-(const Vector2 &lhs, const Vector2 &rhs)
Rotation2 M
Orientation of the Frame.
Definition: frames.hpp:1098
friend bool Equal(const Rotation2 &a, const Rotation2 &b, double eps)
do not use operator == because the definition of Equal(.,.) is slightly different.
represents rotations in 3 dimensional space.
Definition: frames.hpp:301
Twist(const Vector &_vel, const Vector &_rot)
Definition: frames.hpp:729