Geometric primitives

Important remarks

  • All geometric primitives and there operations can be used in realtime. None of the operations lead to dynamic memory allocations and all of the operations are deterministic in time.
  • All values are in [m] for translational components and [rad] for rotational components.

Header files

  • frames.hpp: Definition of all geometric primitives and there transformations/operators.
  • frames_io.hpp: Definition of the input/output operators.

Vector

link to API

A Vector represents the 3D position of a point/object wrt the reference frame. It can be seen as a 3-element vector containing X-Y-Z values:

\[ \textrm{KDL::Vector} = \left[ \begin{array}{ccc}<br /> x \\ y \\ z \end{array}\right] \] (1)

Creating Vectors

&#10;  Vector v1; //The default constructor, X-Y-Z are initialized to zero&#10;  Vector v2(x,y,z); //X-Y-Z are initialized with the given values &#10;  Vector v3(v2); //The copy constructor&#10;  Vector v4 = Vector::Zero(); //All values are set to zero&#10;

Get/Set individual elements

The operators [ ] and ( ) use indeces from 0..2, index checking is enabled/disabled by the DEBUG/NDEBUG definitions: &#10;  v1[0]=v2[1];//copy y value of v2 to x value of v1 &#10;  v2(1)=v3(3);//copy z value of v3 to y value of v2&#10;  v3.x( v4.y() );//copy y value of v4 to x value of v3&#10;

Multiply/Divide with a scalar

You can multiply or divide a Vector with a double using the operator * and /: &#10;  v2=2*v1;&#10;  v3=v1/2;&#10;

Add and subtract vectors

&#10;  v2+=v1;&#10;  v3-=v1;&#10;  v4=v1+v2;&#10;  v5=v2-v3;&#10;

Cross and scalar product

&#10;  v3=v1*v2; //Cross product&#10;  double a=dot(v1,v2)//Scalar product&#10;

Resetting

You can reset the values of a vector to zero: &#10;  SetToZero(v1);&#10;

Comparing vectors

With or without giving an accuracy: &#10;  v1==v2;&#10;  v2!=v3;&#10;  Equal(v3,v4,eps);//with accuracy eps&#10;

Rotation

link to API

Represents the 3D rotation of an object wrt the reference frame. Internally it is represented by a 3x3 matrix which is a non-minimal representation:

\[ $ \textrm{KDL::Rotation} = \left[\begin{array}{ccc}Xx&Yx&Zx\\Xy&Yy&Zy\\Xz&Yz&Zz\end{array}\right] $ \] (2)

Creating Rotations

Safe ways to create a Rotation

The following result always in consistent Rotations. This means the rows/columns are always normalized and orthogonal: &#10;  Rotation r1; //The default constructor, initializes to an 3x3 identity matrix&#10;  Rotation r1 = Rotation::Identity();//Identity Rotation = zero rotation&#10;  Rotation r2 = Rotation::RPY(roll,pitch,yaw); //Rotation build out off Roll-Pitch-Yaw angles&#10;  Rotation r3 = Rotation::EulerZYZ(alpha,beta,gamma); //Rotation build out off Euler Z-Y-Z angles&#10;  Rotation r4 = Rotation::EulerZYX(alpha,beta,gamma); //Rotation build out off Euler Z-Y-X angles&#10;  Rotation r5 = Rotation::Rot(vector,angle); //Rotation build out off an equivalent axis(vector) and an angle.&#10;

Other ways

The following should be used with care, they can result in inconsistent rotation matrices, since there is no checking if columns/rows are normalized or orthogonal &#10;  Rotation r6( Xx,Yx,Zx,Xy,Yy,Zy,Xz,Yz,Zz);//Give each individual element (Column-Major)&#10;  Rotation r7(vectorX,vectorY,vectorZ);//Give each individual column&#10;

Getting values

Individual values, the indices go from 0..2: &#10;  double Zx = r1(0,2);&#10; Getting EulerZYZ, Euler ZYX, Roll-Pitch-Yaw angles , equivalent rotation axis with angle: &#10;  r1.GetEulerZYZ(alpha,beta,gamma);&#10;  r1.GetEulerZYX(alpha,beta,gamma);&#10;  r1.GetRPY(roll,pitch,yaw);&#10;  axis = r1.GetRot();//gives only rotation axis&#10;  angle = r1.GetRotAngle(axis);//gives both angle and rotation axis&#10; Getting the Unit vectors: &#10;  vecX=r1.UnitX();//or&#10;  r1.UnitX(vecX);&#10;  vecY=r1.UnitY();//or&#10;  r1.UnitY(vecY);&#10;  vecZ=r1.UnitZ();//or&#10;  r1.UnitZ(vecZ);&#10;

Inverse Rotations

Inverting the rotation: &#10;  r1.SetInverse();//r1 is inverted&#10; Getting the inverted rotation: &#10;  r2=r1.Inverse();//r2 is the inverse rotation of r1&#10;

Composing rotations

You can compose two rotations to a new rotation, the order of the rotations is important: &#10;  r3=r1*r2;&#10; You can add a rotation around X-Y-Z directly to an existing rotation &#10;  r1.DoRotX(angle);&#10;  r2.DoRotY(angle);&#10;  r3.DorotZ(angle);&#10; The rotation around X-Y-Z is applied to the rotation (r1) such that &#10;  r1 = r1*Rotation::RotX(angle)&#10;

Using a Rotation to rotate a Vector

You can rotate a Vector using a Rotation and the operator *. &#10;  v2=r1*v1;&#10; A different way to see this: You can calculate the new (v2) X-Y-Z values of a vector (v1) if the reference frame is rotated with a Rotation r1.

Comparing Rotations

This can be done in the same way as vectors are compared, with operators == and != or with the function Equal: &#10;  r1==r2;&#10;  r1!=r2;&#10;  Equal(r1,r2,eps);&#10;

Frame

link to API documentation

A Frame can represents the pose of an object/frame wrt a reference frame (4x4 matrix)

  • It contains a Rotation M that holds the rotation of the object/frame wrt the reference frame.
  • It containts a Vector p that holds the position of the origin of the object/frame in the reference frame

\[ $ \textrm{KDL::Frame} = \left[\begin{array}{cc}\mathbf{M}(3 \times 3) &p(3 \times 1)\\ 0(1 \times 3)&1 \end{array}\right] $ \] (3)

Creating Frames

Since a Frame contains a Rotation and a Vector it can only be build by supplying their values: &#10;  Frame f1;//Creates Identity frame&#10;  Frame f1=Frame::Identity();//Creates an identity frame: Rotation::Identity() and Vector::Zero()&#10;  Frame f2(your_rotation);//Create a frame with your_rotation and a zero vector&#10;  Frame f3(your_vector);//Create a frame with your_vector and a identity rotation&#10;  Frame f4(your_rotation,your_vector);//Create a frame with your_rotation&#10;  Frame f5(your_vector,your_rotation);//and your_vector&#10;  Frame f5(f6);//the copy constructor&#10;

Getting values

You can get an individual element of a frame using it's 4x4 matrix representation: &#10;  double x = f1(0,3);&#10;  double Yy = f1(1,1);&#10; A better way is to go through the underlying Rotation and vector: &#10;   Vector p = f1.p;&#10;   Rotation M = f1.M;&#10;

Composing pose transformations

You can use the operator * to compose pose transformations. If you have a Frame F_A_B that specifies the pose transformation from a frame A to a frame B and a Frame F_B_C that specifies the pose transformation from a frame B to a frame C you can calculate the Frame F_A_C as follows: &#10;  Frame F_A_C = F_A_B * F_B_C;&#10; The Frame F_A_C now specifies the pose transformation from frame A to frame C, or said otherwise, F_A_C.p is the location of the origin of frame C expressed in frame A, and F_A_C.M is the rotation of frame C expressed in frame A.

Comparing frames

Again the operators == and != or Equal can be used: &#10;  f1==f2;&#10;  f1!=f2;&#10;  Equal(f1,f2,eps);&#10;

Twist

link to API documentation

A Twist represents a 6D velocity with a 3D translational and a 3D rotational part (6x1 matrix). Internally it is represented with by two Vectors, one containing the translational velocitity, vel, and one containing the rotational velocity, rot:

\[ $\textrm{KDL::Twist} = \left[\begin{array}{c} v_x\\v_y\\v_z\\ \hline \omega_x \\ \omega_y \\ \omega_z \end{array} \right] = \left[\begin{array}{c} \textrm{vel} \\ \hline \textrm{rot}\end{array} \right] $ \] (4)

Creating Twists

&#10;  Twist t1; //Default constructor, initializes to Zero through Vector&#10;  Twist t2(vel,rot);//Vector vel, and Vector rot&#10;  Twist t3 = Twist::Zero();//Zero twist&#10;

Getting values

Using the operators [ ] and ( ), the indeces from 0..2 return the elements of vel, the indeces from 3..5 return the elements of rot: &#10;  double vx = t1(0);&#10;  double omega_y = t1[4];&#10;  t1(1) = vy;&#10;  t1[5] = omega_z;&#10; Because some robotics literature put the rotation part on top it is safer to use the vel, rot members to access the individual elements: &#10;  double vx = t1.vel.x();//or&#10;  vx = t1.vel(0);&#10;  double omega_y = t1.rot.y();//or&#10;  omega_y = t1.rot(1);&#10;  t1.vel.y(v_y);//or&#10;  t1.vel(1)=v_y;//etc&#10;

Multiply/Divide with a scalar

The same operators as for Vector are available: &#10;  t2=2*t1;&#10;  t2=t1*2;&#10;  t2=t1/2;&#10;

Adding/subtracting Twists

The same operators as for Vector are available: &#10;  t1+=t2;&#10;  t1-=t2;&#10;  t3=t1+t2;&#10;  t3=t1-t2;&#10;

Comparing Twists

Again the operators == and != or Equal can be used: &#10;  t1==t2;&#10;  t1!=t2;&#10;  Equal(t1,t2,eps);&#10;

Wrench

A Wrench represents a 6D force with a 3D force and a 3D torque part (6x1 matrix). Internally it is represented with by two Vectors, one containing the forces, force, and one containing the torques, torque:

\[ $\textrm{KDL::Wrench} = \left[\begin{array}{c} f_x\\f_y\\f_z\\ \hline t_x \\ t_y \\ t_z \end{array} \right] = \left[\begin{array}{c} \textrm{force} \\ \hline \textrm{torque}\end{array} \right] $ \] (5)

Creating Wrenchs

&#10;  Wrench w1; //Default constructor, initializes to Zero through Vector&#10;  Wrench w2(force,torque);//Vector force, and Vector torque&#10;  Wrench w3 = Wrench::Zero();//Zero wrench&#10;

Getting values

Using the operators [ ] and ( ), the indeces from 0..2 return the elements of force, the indeces from 3..5 return the elements of torque: &#10;  double fx = w1(0);&#10;  double ty = w1[4];&#10;  w1(1) = fy;&#10;  w1[5] = tz;&#10; Because some robotics literature put the torque part on top it is safer to use the torque, force members to access the individual elements: &#10;  double fx = w1.force.x();//or&#10;  fx = w1.force(0);&#10;  double ty = w1.torque.y();//or&#10;  ty = w1.torque(1);&#10;  w1.force.y(fy);//or&#10;  w1.force(1)=fy;//etc&#10;

Multiply/Divide with a scalar

The same operators as for Vector are available: &#10;  w2=2*w1;&#10;  w2=w1*2;&#10;  w2=w1/2;&#10;

Adding/subtracting Wrenchs

The same operators as for Twist are available: &#10;  w1+=w2;&#10;  w1-=w2;&#10;  w3=w1+w2;&#10;  w3=w1-w2;&#10;

Comparing Wrenchs

Again the operators == and != or Equal can be used: &#10;  w1==w2;&#10;  w1!=w2;&#10;  Equal(w1,w2,eps);&#10;

Twist and Wrench transformations

A Wrench or Twist is expressed in a certain reference frame and reference point, most of the times the reference point coincides with the base point of the reference frame, but this does not have to be the case.

The values of a Wrench or Twist changes if the reference frame or reference point is changed.

Changing the reference point

If you want to change the reference point you need the vector p from the old reference point to the new reference point expressed in the reference frame of the Wrench or Twist:

&#10;t2 = t1.RefPoint(v_old_new);&#10;w2 = w1.RefPoint(v_old_new);&#10;

Changing the reference frame

If you want to change the reference frame but want to keep the reference point intact you can use a Rotation matrix which expresses the rotation of the current reference frame B wrt to the new reference frame A:

&#10;ta = R_AB*tb;&#10;wa = R_AB*wb;&#10;

Changing both the reference frame and the reference point

Use a Frame that contains both the rotation and the pose change, changing the reference frame and reference point from B to A:

&#10;ta = F_AB*tb;&#10;wa = F_AB*wb;&#10;

First order differentiation and integration

&#10;t_ab = diff(F_b_A,F_b_B,timestep)&#10;&#10;F_b_B = F_b_A.addDelta(t_ab,timestep)&#10;

‹ User ManualupKinematic chains ›
»