128 #include "utilities/kdl-config.h" 129 #include "utilities/utility.h" 165 inline Vector() {data[0]=data[1]=data[2] = 0.0;}
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);
331 inline void SetInverse();
353 inline static Rotation RotY(
double angle);
355 inline static Rotation RotZ(
double angle);
358 inline void DoRotX(
double angle);
361 inline void DoRotY(
double angle);
364 inline void DoRotZ(
double angle);
387 double GetRotAngle(
Vector& axis,
double eps=epsilon)
const;
398 static Rotation EulerZYZ(
double Alfa,
double Beta,
double Gamma);
415 void GetEulerZYZ(
double& alpha,
double& beta,
double& gamma)
const;
419 static Rotation Quaternion(
double x,
double y,
double z,
double w);
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 {
494 GetRPY(Gamma,Beta,Alfa);
511 return Vector(data[0],data[3],data[6]);
523 return Vector(data[1],data[4],data[7]);
535 return Vector(data[2],data[5],data[8]);
590 void Make4x4(
double* d);
602 inline Frame Inverse()
const;
645 inline void Integrate(
const Twist& t_this,
double frequency);
693 static Frame DH_Craig1989(
double a,
double alpha,
double d,
double theta);
701 static Frame DH(
double a,
double alpha,
double d,
double theta);
776 inline Twist RefPoint(
const Vector& v_base_AB)
const;
988 inline double x()
const;
989 inline double y()
const;
990 inline void x(
double);
991 inline void y(
double);
1018 double Norm()
const;
1021 inline void Set3DXY(
const Vector& v);
1024 inline void Set3DYZ(
const Vector& v);
1027 inline void Set3DZX(
const Vector& v);
1032 inline void Set3DPlane(
const Frame& F_someframe_XY,
const Vector& v_someframe);
1059 explicit Rotation2(
double angle_rad):s(sin(angle_rad)),c(cos(angle_rad)) {}
1066 inline double operator() (
int i,
int j)
const;
1070 inline void SetInverse();
1074 inline void SetIdentity();
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;
1117 inline void SetInverse();
1118 inline Frame2 Inverse()
const;
1123 inline void SetIdentity();
1124 inline void Integrate(
const Twist& t_this,
double frequency);
Rotation2 M
Orientation of the Frame.
Definition: frames.hpp:1098
double Norm() const
Definition: frames.cpp:118
friend Vector operator+(const Vector &lhs, const Vector &rhs)
Definition: frames.hpp:58
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
static Vector Zero()
Definition: frames.hpp:139
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
void UnitX(const Vector &X)
Access to the underlying unitvectors of the rotation matrix.
Definition: frames.hpp:515
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
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
IMETHOD Rotation Rot(const Vector &axis_a_b)
axis_a_b is a rotation vector, its norm is a rotation angle axis_a_b rotates the a frame towards the ...
Definition: frames.hpp:1101
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
Wrench()
Does initialise force and torque to zero via the underlying constructor of Vector.
Definition: frames.hpp:886
Inlined member functions and global functions that relate to the classes in frames.cpp.
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
A 2D Rotation class, for conventions see Rotation.
Definition: frames.hpp:1050
2D version of Vector
Definition: frames.hpp:959
Vector & operator-=(const Vector &arg)
subtracts a vector from the Vector object itself
Definition: frames.hpp:130
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
double operator()(int index) const
Access to elements, range checked when NDEBUG is not set, from 0..2.
Definition: frames.hpp:144
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
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
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
represents both translational and rotational velocities.
Definition: frames.hpp:720
friend Vector operator*(const Vector &lhs, double rhs)
Scalar multiplication is defined.
Definition: frames.hpp:84
IMETHOD bool Equal(const FrameAcc &r1, const FrameAcc &r2, double eps=epsilon)
Definition: frameacc.hpp:394
Definition: articulatedbodyinertia.cpp:28
A concrete implementation of a 3 dimensional vector class.
Definition: frames.hpp:160
Vector rot
The rotational velocity of that point.
Definition: frames.hpp:723
static Frame2 Identity()
Definition: frames.hpp:1125
double data[3]
Definition: frames.hpp:163
void Set2DZX(const Vector2 &v)
a 3D vector where the 2D vector v is put in the ZX plane
Definition: frames.hpp:476
Vector p
origine of the Frame
Definition: frames.hpp:572
friend Vector operator-(const Vector &lhs, const Vector &rhs)
Definition: frames.hpp:67
Vector UnitX() const
Access to the underlying unitvectors of the rotation matrix.
Definition: frames.hpp:510
Vector & operator=(const Vector &arg)
Assignment operator. The normal copy by value semantics.
Definition: frames.hpp:50
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
Frame()
Definition: frames.hpp:584
friend class Frame
Definition: frames.hpp:264
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
friend bool operator!=(const Vector &a, const Vector &b)
The literal inequality operator!=().
Definition: frames.hpp:1292
double y() const
Definition: frames.hpp:77
represents a frame transformation in 3D space (rotation + translation)
Definition: frames.hpp:570
double x() const
Definition: frames.hpp:76
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
friend class Rotation
Definition: frames.hpp:263
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
Vector UnitY() const
Access to the underlying unitvectors of the rotation matrix.
Definition: frames.hpp:522
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
A 2D frame class, for further documentation see the Frames class for methods with unchanged semantics...
Definition: frames.hpp:1094
double s
Definition: frames.hpp:1052