Geometric primitives

KDL::Vector

Browse KDL API Documentation

A Vector is a 3x1 matrix containing X-Y-Z coordinate values. It is used for representing: 3D position of a point wrt a reference frame, rotational and translational part of a 6D motion or force entity : <equation id="vector">$\textrm{KDL::Vector} = \left[ \begin{array}{ccc} x \\ y \\ z \end{array}\right]$<equation>

Creating Vectors

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

Get/Set individual elements

The operators [ ] and ( ) use indices from 0..2, index checking is enabled/disabled by the DEBUG/NDEBUG definitions: 

  v1[0]=v2[1];//copy y value of v2 to x value of v1 
  v2(1)=v3(3);//copy z value of v3 to y value of v2
  v3.x( v4.y() );//copy y value of v4 to x value of v3

Multiply/Divide with a scalar

You can multiply or divide a Vector with a double using the operator * and /: 

  v2=2*v1;
  v3=v1/2;

Add and subtract vectors

  v2+=v1;
  v3-=v1;
  v4=v1+v2;
  v5=v2-v3;

Cross and scalar product

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

Resetting

You can reset the values of a vector to zero: 

  SetToZero(v1);

Comparing vectors

Element by element comparison with or without user-defined accuracy: 
  v1==v2;
  v2!=v3;
  Equal(v3,v4,eps);//with accuracy eps

KDL::Rotation

link to API

A Rotation is the 3x3 matrix that represents the 3D rotation of an object wrt the reference frame.

<equation id="rotation">$ \textrm{KDL::Rotation} = \left[\begin{array}{ccc}Xx&Yx&Zx\\Xy&Yy&Zy\\Xz&Yz&Zz\end{array}\right] $<equation>

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: 

  Rotation r1; //The default constructor, initializes to an 3x3 identity matrix
  Rotation r1 = Rotation::Identity();//Identity Rotation = zero rotation
  Rotation r2 = Rotation::RPY(roll,pitch,yaw); //Rotation built from Roll-Pitch-Yaw angles
  Rotation r3 = Rotation::EulerZYZ(alpha,beta,gamma); //Rotation built from Euler Z-Y-Z angles
  Rotation r4 = Rotation::EulerZYX(alpha,beta,gamma); //Rotation built from Euler Z-Y-X angles
  Rotation r5 = Rotation::Rot(vector,angle); //Rotation built from an equivalent axis(vector) and an angle.

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 

  Rotation r6( Xx,Yx,Zx,Xy,Yy,Zy,Xz,Yz,Zz);//Give each individual element (Column-Major)
  Rotation r7(vectorX,vectorY,vectorZ);//Give each individual column

Getting values

Individual values, the indices go from 0..2: 

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

Inverting Rotations

Replacing a rotation by its inverse: 

  r1.SetInverse();//r1 is inverted and overwritten
Getting the inverse rotation without overwriting the original: 
  r2=r1.Inverse();//r2 is the inverse rotation of r1

Composing rotations

Compose two rotations to a new rotation, the order of the rotations is important: 

  r3=r1*r2;
 
Compose a rotation with elementary rotations around X-Y-Z: 
  r1.DoRotX(angle);
  r2.DoRotY(angle);
  r3.DoRotZ(angle);
this is the shorthand version of: 
  r1 = r1*Rotation::RotX(angle)

Rotation of a Vector

Rotating a Vector using a Rotation and the operator *: 
  v2=r1*v1;

Comparing Rotations

Element by element comparison with or without user-defined accuracy: 
  r1==r2;
  r1!=r2;
  Equal(r1,r2,eps);

KDL::Frame

link to API documentation

A Frame is the 4x4 matrix that represents the pose of an object/frame wrt a reference frame. It contains:

  • a Rotation M for the rotation of the object/frame wrt the reference frame.
  • a Vector p for the position of the origin of the object/frame in the reference frame

<equation id="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] $<equation>

Creating Frames

  Frame f1;//Creates Identity frame
  Frame f1=Frame::Identity();//Creates an identity frame: Rotation::Identity() and Vector::Zero()
  Frame f2(your_rotation);//Create a frame with your_rotation and a zero vector
  Frame f3(your_vector);//Create a frame with your_vector and a identity rotation
  Frame f4(your_rotation,your_vector);//Create a frame with your_rotation
  Frame f5(your_vector,your_rotation);//and your_vector
  Frame f5(f6);//the copy constructor

Getting values

Individual values from the 4x4 matrix, the indices go from 0..3: 
  double x = f1(0,3);
  double Yy = f1(1,1);
Another way is to go through the underlying Rotation and Vector: 
   Vector p = f1.p;
   Rotation M = f1.M;

Composing frames

You can use the operator * to compose frames. If you have a Frame F_A_B that expresses the pose of frame B wrt frame A, and a Frame F_B_C that expresses the pose of frame C wrt to frame B, the calculation of Frame F_A_C that expresses the pose of frame C wrt to frame A is as follows: 
  Frame F_A_C = F_A_B * F_B_C;
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.

Inverting Frames

Replacing a frame by its inverse: 

  //not yet implemented
Getting the inverse: 
  f2=f1.Inverse();//f2 is the inverse of f1

Comparing frames

Element by element comparison with or without user-defined accuracy: 
  f1==f2;
  f1!=f2;
  Equal(f1,f2,eps);

KDL::Twist

link to API documentation

A Twist is the 6x1 matrix that represents the velocity of a Frame using a 3D translational velocity Vector vel and a 3D angular velocity Vector rot:

<equation id="twist">$\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] $<equation>

Creating Twists

  Twist t1; //Default constructor, initializes both vel and rot to Zero
  Twist t2(vel,rot);//Vector vel, and Vector rot
  Twist t3 = Twist::Zero();//Zero twist
Note: in contrast to the creation of Frames, the order in which vel and rot Vectors are supplied to the constructor is important.

Getting values

Using the operators [ ] and ( ), the indices from 0..2 return the elements of vel, the indices from 3..5 return the elements of rot: 

  double vx = t1(0);
  double omega_y = t1[4];
  t1(1) = vy;
  t1[5] = omega_z;
Because some robotics literature put the rotation part on top it is safer to use the vel, rot members to access the individual elements: 
  double vx = t1.vel.x();//or
  vx = t1.vel(0);
  double omega_y = t1.rot.y();//or
  omega_y = t1.rot(1);
  t1.vel.y(v_y);//or
  t1.vel(1)=v_y;
  //etc

Multiply/Divide with a scalar

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

Adding/subtracting Twists

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

Comparing Twists

Element by element comparison with or without user-defined accuracy: 
  t1==t2;
  t1!=t2;
  Equal(t1,t2,eps);

KDL::Wrench

A Wrench is the 6x1 matrix that represents a force on a Frame using a 3D translational force Vector force and a 3D moment Vector torque:

<equation id="wrench">$\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] $<equation>

Creating Wrenches

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

Getting values

Using the operators [ ] and ( ), the indices from 0..2 return the elements of force, the indices from 3..5 return the elements of torque: 

  double fx = w1(0);
  double ty = w1[4];
  w1(1) = fy;
  w1[5] = tz;
Because some robotics literature put the torque part on top it is safer to use the torque, force members to access the individual elements: 
  double fx = w1.force.x();//or
  fx = w1.force(0);
  double ty = w1.torque.y();//or
  ty = w1.torque(1);
  w1.force.y(fy);//or
  w1.force(1)=fy;//etc

Multiply/Divide with a scalar

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

Adding/subtracting Wrenchs

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

Comparing Wrenchs

Element by element comparison with or without user-defined accuracy: 
  w1==w2;
  w1!=w2;
  Equal(w1,w2,eps);

Twist and Wrench transformations

Wrenches and Twists are expressed in a certain reference frame; the translational Vector vel of the Twists and the moment Vector torque of the Wrenches represent the velocity of, resp. the moment on, a certain reference point in that frame. Common choices for the reference point are the origin of the reference frame or a task specific point.

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

Changing only the reference point

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

t2 = t1.RefPoint(v_old_new);
w2 = w1.RefPoint(v_old_new);

Changing only 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 R_AB which expresses the rotation of the current reference frame B wrt to the new reference frame A:

ta = R_AB*tb;
wa = R_AB*wb;

Note: This operation seems to multiply a 3x3 matrix R_AB with 6x1 matrices tb or wb, while in reality it uses the 6x6 Screw transformation matrix derived from R_AB.

Changing both the reference frame and the reference point

If you want to change both the reference frame and the reference point you can use a Frame F_AB which contains (i) Rotation matrix R_AB which expresses the rotation of the current reference frame B wrt to the new reference frame A and (ii) the Vector v_old_new from the old reference point to the new reference point expressed in A:

ta = F_AB*tb;
wa = F_AB*wb;

Note: This operation seems to multiply a 4x4 matrix F_AB with 6x1 matrices tb or wb, while in reality it uses the 6x6 Screw transformation matrix derived from F_AB.

First order differentiation and integration

t = diff(F_w_A,F_w_B,timestep)//differentiation
F_w_B = F_w_A.addDelta(t,timestep)//integration
t is the twist that moves frame A to frame B in timestep seconds. t is expressed in reference frame w using the origin of A as velocity reference point.