# 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.