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"><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);

Element by element comparison with or without user-defined accuracy:

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"><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);

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);

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

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)

Rotating a Vector using a Rotation and the operator *:

v2=r1*v1;

Element by element comparison with or without user-defined accuracy:

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"><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

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;

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.

Replacing a frame by its inverse:

` //not yet implemented`

Getting the inverse:

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

Element by element comparison with or without user-defined accuracy:

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"><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

Note: in contrast to the creation of Frames, the order in which vel and rot Vectors are supplied to the constructor is important.

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

The same operators as for Vector are available:

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

The same operators as for Vector are available:

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

Element by element comparison with or without user-defined accuracy:

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"><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;

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

The same operators as for Vector are available:

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

The same operators as for Twist are available:

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

Element by element comparison with or without user-defined accuracy:

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

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.

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);

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.

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.

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.