Go to the documentation of this file.
150 inline bool Equal(
const Vector& a,
const Vector& b,
double eps=
epsilon);
151 inline bool Equal(
const Frame& a,
const Frame& b,
double eps=
epsilon);
152 inline bool Equal(
const Twist& a,
const Twist& b,
double eps=
epsilon);
153 inline bool Equal(
const Wrench& a,
const Wrench& b,
double eps=
epsilon);
154 inline bool Equal(
const Vector2& a,
const Vector2& b,
double eps=
epsilon);
155 inline bool Equal(
const Rotation2& a,
const Rotation2& b,
double eps=
epsilon);
156 inline bool Equal(
const Frame2& a,
const Frame2& b,
double eps=
epsilon);
170 inline Vector(
double x,
double y,
double z);
196 inline double x()
const;
197 inline double y()
const;
198 inline double z()
const;
199 inline void x(
double);
200 inline void y(
double);
201 inline void z(
double);
311 inline Rotation(
double Xx,
double Yx,
double Zx,
312 double Xy,
double Yy,
double Zy,
313 double Xz,
double Yz,
double Zz);
360 inline void DoRotX(
double angle);
363 inline void DoRotY(
double angle);
366 inline void DoRotZ(
double angle);
417 void GetEulerZYZ(
double& alpha,
double& beta,
double& gamma)
const;
425 void GetQuaternion(
double& x,
double& y,
double& z,
double& w)
const;
437 static Rotation RPY(
double roll,
double pitch,
double yaw);
457 void GetRPY(
double& roll,
double& pitch,
double& yaw)
const;
472 return RPY(Gamma,Beta,Alfa);
495 inline void GetEulerZYX(
double& Alfa,
double& Beta,
double& Gamma)
const {
703 static Frame DH(
double a,
double alpha,
double d,
double theta);
989 inline double x()
const;
990 inline double y()
const;
991 inline void x(
double);
992 inline void y(
double);
1069 inline double operator() (
int i,
int j)
const;
1082 inline void SetRot(
double angle);
1088 inline double GetRot()
const;
1111 inline void Make4x4(
double* d);
1119 inline double operator() (
int i,
int j)
const;
1147 IMETHOD Vector
diff(
const Vector& p_w_a,
const Vector& p_w_b,
double dt=1);
1177 IMETHOD Vector
diff(
const Rotation& R_a_b1,
const Rotation& R_a_b2,
double dt=1);
1187 IMETHOD Twist
diff(
const Frame& F_a_b1,
const Frame& F_a_b2,
double dt=1);
1193 IMETHOD Twist
diff(
const Twist& a,
const Twist& b,
double dt=1);
1199 IMETHOD Wrench
diff(
const Wrench& W_a_p1,
const Wrench& W_a_p2,
double dt=1);
1208 IMETHOD Vector
addDelta(
const Vector& p_w_a,
const Vector& p_w_da,
double dt=1);
1222 IMETHOD Rotation
addDelta(
const Rotation& R_w_a,
const Vector& da_w,
double dt=1);
1234 IMETHOD Frame
addDelta(
const Frame& F_w_a,
const Twist& da_w,
double dt=1);
1280 r.GetQuaternion(x, y, z, w);
friend bool Equal(const Twist &a, const Twist &b, double eps)
double Normalize(double eps=epsilon)
void SetInverse()
Sets the value of *this to its inverse.
void UnitY(const Vector &X)
Access to the underlying unitvectors of the rotation matrix.
static Rotation Rot2(const Vector &rotvec, double angle)
Along an arbitrary axes. rotvec should be normalized.
Wrench & operator+=(const Wrench &arg)
double operator()(int i, int j) const
Access to elements 0..1,0..1, bounds are checked when NDEBUG is not set.
friend Wrench operator+(const Wrench &lhs, const Wrench &rhs)
Vector rot
The rotational velocity of that point.
Frame Inverse() const
Gives back inverse transformation of a Frame.
void Set2DZX(const Vector2 &v)
a 3D vector where the 2D vector v is put in the ZX plane
void Set3DYZ(const Vector &v)
projects v in its YZ plane, and sets *this to these values
static Rotation Quaternion(double x, double y, double z, double w)
friend bool Equal(const Frame &a, const Frame &b, double eps)
Rotation Inverse() const
Gives back the inverse rotation matrix of *this.
friend bool operator!=(const Rotation &a, const Rotation &b)
The literal inequality operator!=()
Vector operator*(const Vector &arg) const
Vector2 & operator-=(const Vector2 &arg)
void Integrate(const Twist &t_this, double frequency)
friend Wrench operator-(const Wrench &lhs, const Wrench &rhs)
void DoRotZ(double angle)
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.
friend Twist operator*(const Twist &lhs, double rhs)
void GetEulerZYZ(double &alpha, double &beta, double &gamma) const
Vector p
origine of the Frame
double operator[](int index) const
Equivalent to double operator()(int index) const.
Vector2 & operator=(const Vector2 &arg)
friend bool operator==(const Twist &a, const Twist &b)
The literal equality operator==(), also identical.
void UnitX(const Vector &X)
Access to the underlying unitvectors of the rotation matrix.
double Norm(double eps=epsilon) const
static Rotation RotY(double angle)
The Rot... static functions give the value of the appropriate rotation matrix back.
friend bool Equal(const Vector2 &a, const Vector2 &b, double eps)
Twist & operator-=(const Twist &arg)
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)
friend Wrench operator/(const Wrench &lhs, double rhs)
Scalar division.
void GetRPY(double &roll, double &pitch, double &yaw) const
INLINE Rall1d< T, V, S > sin(const Rall1d< T, V, S > &arg)
friend Vector2 operator/(const Vector2 &lhs, double rhs)
void hash_combine(std::size_t &seed, const T &v)
Combine hash of object v to the seed.
double Normalize(double eps=epsilon)
double operator[](int index) const
void Set2DYZ(const Vector2 &v)
a 3D vector where the 2D vector v is put in the YZ plane
friend Twist operator+(const Twist &lhs, const Twist &rhs)
friend bool Equal(const Vector &a, const Vector &b, double eps)
friend Vector operator+(const Vector &lhs, const Vector &rhs)
Vector & operator+=(const Vector &arg)
Adds a vector from the Vector object itself.
Rotation2()
Default constructor does NOT initialise to Zero().
void Make4x4(double *d)
Reads data from an double array.
IMETHOD Vector addDelta(const Vector &p_w_a, const Vector &p_w_da, double dt=1)
adds vector da to vector a. see also the corresponding diff() routine.
Wrench RefPoint(const Vector &v_base_AB) const
friend bool operator==(const Frame &a, const Frame &b)
The literal equality operator==(), also identical.
std::size_t operator()(KDL::Frame2 const &f) const noexcept
A concrete implementation of a 3 dimensional vector class.
bool operator==(const Rotation &a, const Rotation &b)
double & operator()(int i)
index-based access to components, first force(0..2), then torque(3..5)
Vector2 operator*(const Vector2 &arg) const
INLINE Rall1d< T, V, S > cos(const Rall1d< T, V, S > &arg)
Vector & operator-=(const Vector &arg)
subtracts a vector from the Vector object itself
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.
double epsilon
default precision while comparing with Equal(..,..) functions. Initialized at 0.0000001.
represents both translational and rotational velocities.
void DoRotY(double angle)
friend void SetToZero(Twist &v)
void GetEulerZYX(double &Alfa, double &Beta, double &Gamma) const
friend void SetToZero(Vector &v)
Vector2 operator*(const Vector2 &v) const
Vector torque
Torque that is applied at the origin of the current ref frame.
Frame2 & operator=(const Frame2 &arg)
void GetQuaternion(double &x, double &y, double &z, double &w) const
static Rotation RotZ(double angle)
The Rot... static functions give the value of the appropriate rotation matrix back.
static Rotation Identity()
Gives back an identity rotaton matrix.
represents both translational and rotational acceleration.
Rotation2 Inverse() const
void Set3DZX(const Vector &v)
projects v in its ZX plane, and sets *this to these values
friend bool operator!=(const Frame &a, const Frame &b)
The literal inequality operator!=().
represents a frame transformation in 3D space (rotation + translation)
double & operator()(int i)
index-based access to components, first vel(0..2), then rot(3..5)
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
Vector operator*(const Vector &v) const
friend bool Equal(const Rotation &a, const Rotation &b, double eps)
friend Twist operator/(const Twist &lhs, double rhs)
void ReverseSign()
Reverses the sign of the twist.
double operator[](int index) const
Equivalent to double operator()(int index) const.
friend void SetToZero(Vector2 &v)
void Integrate(const Twist &t_this, double frequency)
std::size_t operator()(KDL::Frame const &f) const noexcept
std::size_t operator()(KDL::Vector const &v) const noexcept
static Rotation EulerZYZ(double Alfa, double Beta, double Gamma)
static Frame DH(double a, double alpha, double d, double theta)
static Frame DH_Craig1989(double a, double alpha, double d, double theta)
std::size_t operator()(KDL::Twist const &t) const noexcept
void Set2DXY(const Vector2 &v)
a 3D vector where the 2D vector v is put in the XY plane
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
Vector vel
The velocity of that point.
friend Vector2 operator*(const Vector2 &lhs, double rhs)
double operator()(int i, int j)
Rotation2(double angle_rad)
Vector2 p
origine of the Frame
Twist RefPoint(const Vector &v_base_AB) const
friend bool operator!=(const Twist &a, const Twist &b)
The literal inequality operator!=().
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.
std::size_t operator()(KDL::Wrench const &w) const noexcept
void Set3DPlane(const Frame &F_someframe_XY, const Vector &v_someframe)
static Rotation EulerZYX(double Alfa, double Beta, double Gamma)
friend Vector operator-(const Vector &lhs, const Vector &rhs)
static Rotation Rot(const Vector &rotvec, double angle)
friend bool operator==(const Rotation &a, const Rotation &b)
The literal equality operator==(), also identical.
IMETHOD bool Equal(const FrameAcc &r1, const FrameAcc &r2, double eps=epsilon)
double GetRot() const
Gets the angle (in radians)
Vector UnitY() const
Access to the underlying unitvectors of the rotation matrix.
friend Vector operator*(const Vector &lhs, double rhs)
Scalar multiplication is defined.
friend void SetToZero(Wrench &v)
Rotation2(double ca, double sa)
Rotation M
Orientation of the Frame.
std::size_t operator()(KDL::Vector2 const &v) const noexcept
void ReverseSign()
Reverses the sign of the Vector object itself.
Twist()
The default constructor initialises to Zero via the constructor of Vector.
Rotation2 & operator=(const Rotation2 &arg)
Rotation & operator=(const Rotation &arg)
static Rotation RotX(double angle)
The Rot... static functions give the value of the appropriate rotation matrix back.
friend Vector operator/(const Vector &lhs, double rhs)
Scalar division is defined.
double operator()(int index) const
Access to elements, range checked when NDEBUG is not set, from 0..2.
std::size_t operator()(KDL::Rotation const &r) const noexcept
static Rotation RPY(double roll, double pitch, double yaw)
Vector2()
Does not initialise to Zero().
double Norm(double eps=epsilon) const
double operator()(int i, int j)
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.
friend bool operator!=(const Vector2 &a, const Vector2 &b)
The literal inequality operator!=().
Vector2 & operator+=(const Vector2 &arg)
static Rotation2 Identity()
Vector()
Does not initialise the Vector to zero. use Vector::Zero() or SetToZero for that.
void ReverseSign()
Reverses the sign of the current Wrench.
IMETHOD Vector diff(const Vector &p_w_a, const Vector &p_w_b, double dt=1)
friend bool Equal(const Wrench &a, const Wrench &b, double eps)
Wrench(const Vector &_force, const Vector &_torque)
std::size_t operator()(KDL::Rotation2 const &r) const noexcept
friend double dot(const Vector &lhs, const Vector &rhs)
Vector force
Force that is applied at the origin of the current ref frame.
double operator()(int index) const
Access to elements, range checked when NDEBUG is not set, from 0..1.
friend Vector2 operator+(const Vector2 &lhs, const Vector2 &rhs)
double operator[](int index) const
static Rotation2 Rot(double angle)
The Rot... static functions give the value of the appropriate rotation matrix bac.
void UnitZ(const Vector &X)
Access to the underlying unitvectors of the rotation matrix.
Vector UnitZ() const
Access to the underlying unitvectors of the rotation matrix.
Twist & operator+=(const Twist &arg)
Vector UnitX() const
Access to the underlying unitvectors of the rotation matrix.
Wrench & operator-=(const Wrench &arg)
Vector & operator=(const Vector &arg)
Assignment operator. The normal copy by value semantics.
Frame & operator=(const Frame &arg)
Normal copy-by-value semantics.
Wrench()
Does initialise force and torque to zero via the underlying constructor of Vector.
friend Vector2 operator-(const Vector2 &lhs, const Vector2 &rhs)
Rotation2 M
Orientation of the Frame.
friend bool Equal(const Rotation2 &a, const Rotation2 &b, double eps)
represents rotations in 3 dimensional space.
Twist(const Vector &_vel, const Vector &_rot)