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:

(1)

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

With or without giving an accuracy:

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

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:

(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:

  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 build out off Roll-Pitch-Yaw angles
  Rotation r3 = Rotation::EulerZYZ(alpha,beta,gamma); //Rotation build out off Euler Z-Y-Z angles
  Rotation r4 = Rotation::EulerZYX(alpha,beta,gamma); //Rotation build out off Euler Z-Y-X angles
  Rotation r5 = Rotation::Rot(vector,angle); //Rotation build out off 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);

Inverse Rotations

Inverting the rotation:

  r1.SetInverse();//r1 is inverted

Getting the inverted rotation:

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

Composing rotations

You can compose two rotations to a new rotation, the order of the rotations is important:

  r3=r1*r2;
 

You can add a rotation around X-Y-Z directly to an existing rotation

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

The rotation around X-Y-Z is applied to the rotation (r1) such that

  r1 = r1*Rotation::RotX(angle)

Using a Rotation to rotate a Vector

You can rotate a Vector using a Rotation and the operator *.

  v2=r1*v1;

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:

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

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

(3)

Creating Frames

Since a Frame contains a Rotation and a Vector it can only be build by supplying their values:

  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

You can get an individual element of a frame using it's 4x4 matrix representation:

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

A better way is to go through the underlying Rotation and vector:

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

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:

  Frame F_A_C = F_A_B * F_B_C;

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:

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

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:

(4)

Creating Twists

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

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:

  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

Again the operators == and != or Equal can be used:

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

Wrench

• link to API documentation

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:

(5)

Creating Wrenchs

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

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:

  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

Again the operators == and != or Equal can be used:

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

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:

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

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:

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

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:

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

First order differentiation and integration

t_ab = diff(F_b_A,F_b_B,timestep)
F_b_B = F_b_A.addDelta(t_ab,timestep)

t_ab is the twist to move from frame A to frame B in timestep seconds, with reference frame b and reference point the origin of A.

Kinematic chains

A KDL::Chain is

• a kinematic description of a serial chain of bodies connected by joints.
• build out of KDL::Segments.

A Segment contains

• a KDL::Frame describing the link, it is the pose between the root and the tip of the segment. It cannot only contain a translational part but also a rotational part.
• a joint located at the root of a segment, it can be a translational or a rotational joint, or a fixed joint if the segment is fixed rigidly.

To start from the bottom:

KDL::Joint

When constructing a Joint, you have to define the type of the joint, you can choose from the following:

Joint rx = Joint(Joint::RotX);
Joint ry = Joint(Joint::RotY);
Joint rz = Joint(Joint::RotZ);
Joint tx = Joint(Joint::TransX);
Joint ty = Joint(Joint::TransY);
Joint tz = Joint(Joint::TransZ);
Joint fixed = Joint(Joint::None);

A joint is 1D.

You can get the pose and twist of the joint:

double q = M_PI/4;
Frame f = rx.pose(q);
double qdot = 0.1;
Twist t = rx.twist(qdot);

KDL::Segment

When constructing a Segment you have to give the Joint at the root and the Frame describing the pose to the tip, the constructor takes copies of the arguments, you cannot change the frame or joint afterwards!!!:

Segment s = Segment(Joint(Joint::RotX),
                                     Frame(Rotation::RPY(0.0,M_PI/4,0.0),
                                         Vector(0.1,0.2,0.3) ));

A segment is still 1D, you can get the pose and the twist of the tip of the segment wrt the root:

double q=M_PI/2;
Frame f = s.pose(q);
double qdot=0.1;
Twist t = s.twist(q,qdot);

When asking the twist of the tip you have to give the position value of the joint because the twist is expressed in the root frame of the segment but the reference point of the Twist is at the tip of the segment.

You can get the pose of the link of the segment, this does not include the pose of the joint!!!

Frame F_link = segment1.getFrameToTip();

KDL::Chain

A Chain has a default constructor, creating an empty chain without any segments and a copy-constructor, creating a copy of an existing chain:

Chain chain1();
Chain chain2(chain3);

Chain are constructed by adding Segments or adding existing chains, to the end of the chain, these functions add copies of the arguments, not the arguments themselves!!!:

chain1.addSegment(segment1);
chain1.addChain(chain2);

You can get the number of joints and number of segments (this is not always the same since a segment can have a Joint::None, which is not included in the number of joints):

unsigned int nj = chain1.getNrOfJoints();
unsigned int js = chain1.getNrOfSegments();

You can iterate over the segments of a chain by getting a reference to each successive segment:

Segment& segment3 = chain1.getSegment(3);

Kinematic Solvers

KDL contains for the moment only generic solvers for kinematic chains. They can be used (with care) for every KDL::Chain.

The idea behind the generic solvers is to have a uniform API. We do this by inheriting from the abstract classes for each type of solver:

• ChainFkSolverPos
• ChainFkSolverVel
• ChainIkSolverVel
• ChainIkSolverPos

A seperate solver has to be created for each chain. At construction time, it will allocate all necessary resources.

A specific type of solver can add some solver-specific functions/parameters to the interface, but still has to use the generic interface for it's main solving purpose.

The forward kinematics use the function JntToCart(...) to calculate the Cartesian space values from the Joint space values. The inverse kinematics use the function CartToJnt(...) to calculate the Joint space values from the Cartesian space values.

Recursive forward kinematic solvers

For now we only have one generic forward and velocity position kinematics solver.

It recursively adds the poses/velocity of the successive segments, going from the first to the last segment, you can also get intermediate results by giving a Segment number:

ChainFkSolverPos_recursive fksolver(chain1);
JntArray q(chain1.getNrOfJoints);
q=...
Frame F_result;
fksolver.JntToCart(q,F_result,segment_nr);