Available Primitives

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>

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

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

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

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

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

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

You can reset the values of a vector to zero:

SetToZero(v1);

v1==v2; v2!=v3; Equal(v3,v4,eps);//with accuracy eps

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>

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.

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

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

double Zx = r1(0,2);

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

vecX=r1.UnitX();//or r1.UnitX(vecX); vecY=r1.UnitY();//or r1.UnitY(vecY); vecZ=r1.UnitZ();//or r1.UnitZ(vecZ);

Replacing a rotation by its inverse:

r1.SetInverse();//r1 is inverted and overwritten

r2=r1.Inverse();//r2 is the inverse rotation of r1

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

r3=r1*r2;

r1.DoRotX(angle); r2.DoRotY(angle); r3.DoRotZ(angle);

r1 = r1*Rotation::RotX(angle)

v2=r1*v1;

r1==r2; r1!=r2; Equal(r1,r2,eps);

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>

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

double x = f1(0,3); double Yy = f1(1,1);

Vector p = f1.p; Rotation M = f1.M;

Frame F_A_C = F_A_B * F_B_C;

Replacing a frame by its inverse:

` //not yet implemented`

f2=f1.Inverse();//f2 is the inverse of f1

f1==f2; f1!=f2; Equal(f1,f2,eps);

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>

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

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;

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

t2=2*t1; t2=t1*2; t2=t1/2;

t1+=t2; t1-=t2; t3=t1+t2; t3=t1-t2;

t1==t2; t1!=t2; Equal(t1,t2,eps);

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>

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

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;

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

w2=2*w1; w2=w1*2; w2=w1/2;

w1+=w2; w1-=w2; w3=w1+w2; w3=w1-w2;

w1==w2; w1!=w2; Equal(w1,w2,eps);

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

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

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.

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.

t = diff(F_w_A,F_w_B,timestep)//differentiation F_w_B = F_w_A.addDelta(t,timestep)//integration