3. Inverse Kinematics

Example project: simulation of a 6R robotic manipulator
DH notation
Forward kinematics
Twists
The manipulator Jacobian
Calculating the Jacobian in VRML
Estimating the end-effector twist
Implementing IK in VRML

The animation of articulated figures is quite popular in VRML because of the desire to use human beings as avatars in virtual environments. An articulated figure, also called kinematic linkage, consists of a series of rigid links which are connected at joints. Although there are numerous possibilities for joints, applications in Computer Animation are usually restricted to revolute joints. In this case a configuration of the linkage is described by a serious of rotation angles, which are commonly called state variables or joint angles. Roughly speaking, each of these angles describes the rotation angle between consecutive links.

There are basically two different tasks which have to be solved in animating these structures.

Forward kinematics: the motion is defined explicitly by prescribing the set of joint angles. The problem is to calculate the configuration of the linkage. Forward kinematics is intrinsically implemented in VRML. The SFRotation field in a Transform node is nothing but the description of the joint axis together with the corresponding joint angle. Thus there is generally no need in explicitly calculating forward kinematics.
Inverse kinematics (IK): sometimes called goal-directed motion, the animator defines the position of the last link, also called end-effector, of the linkage only. The problem is to recalculate the corresponding set of joint angles which give the specified end-effector position.

The inverse kinematics problem is intrinsically not unique in terms of its solutions. Given the end-effector position, there is generally more than one solution for the joint angles. It might even be that there exists an infinite number of solutions. In this case the design of the linkage is called redundant. On the other hand it is also very likely that there exists no solution at all. This is the case if the prescribed position of the end-effector is out of reach. Note that this does not necessarily mean that the position is too far away. It is also possible that the linkage is restricted in its motion capabilities due to a poor design. For example, a linkage where all joint axes are parallel is planar and thus will never be able to get out of this plane.

There are several possibilities to approach IK. Most of these approaches are based on some sort of search technique.

Algebraic approach: this approach is necessary, if one wants to calculate all possible solutions of the IK problem. Given a specified linkage, one solves the forward kinematics problem explicitly. This in general leads to a set of nonlinear algebraic expressions in the state variables of the linkage. Given an end-effector position, the problem now is to algebraicly solve this system of equations for the unknown state variables. Although it is theoretically possible to solve this using symbolic computation techniques, it is generally beyond todays computing power. Nevertheless, there exist some successful algebraic approaches for a few types of linkages.
Jacobian based approach: one idea which proves to have the highest efficiency for most industrial applications is based on the so-called maniplator Jacobian. Roughly speaking the manipulator Jacobian is a matrix which describes the relationsship between joint and end-effector velocities. Given the velocity of the end-effector, the Jacobian allows to recalculate the corresponding joint velocities simply by solving a system of linear equations. This can be used to deduce a gradient-based iterative technique for solving IK. We will persue this approach in this chapter.
Neural networks: recently, an increasing number of neural network approaches have been suggested in the area of robotics. Since this requires to teach the linkage and thus highly depends on its actual geometry, these approaches are not very useful for simulation purposes.
Genetic programming and genetic algorithms: due to steadily increasing computing power these techniques get more and more interesting in the area of Computer Animation especially when it comes to highly complex virtual environments. Genetic algorithms are directed search algorithms which try to find the solution by mimicing the natural process of evolution. Currently, however, this approach is useless in VRML since todays browsers are not capable of providing the computing power necessary to achieve anything near real-time.

3.1. Example project: simulation of a 6R robotic manipulator

The example we are going to use in this chapter is a general 6R robot as used in many industrial applications. Such a robot consists of 6 revolute joints (6R) and thus a total of 7 links including base system and end-effector. In the completed example which is seen to the right you can adjust the desired end-effectot position by dragging the blue actuators. Clicking on the green sphere in the end-effector brings the robot into the desired position by using forward kinematics only. Since all joint variable are known, this can be done simply by linear interpolation between the corresponding joint angles.

Clicking on the green base activates a Jacobian based IK algorithm. Each click shows one iteration. Usually the algorithm starts to converge immdiately, but in case the desired position is far away from the robots initial configuration, it might take some 20 iteration until the algorithm stabilizes. Note that the initial position of the robot is a so-called singular position in which case the algorithm fails. We will discuss this case later. In order to demonstrate the IK algorithm one therefore needs to bring the robot out of its singular position first. This can be done by using the above forward kinematics implementation. If you repeat the algorithm a couple of times you will see that the algorithm in general converges to a different solution than the one we used to input the desired end-effector position. In fact, it has been proven that the IK of a robot of this type has in general 16 solution. The IK algorithm will find one of these solutions. Which one depends on the initial configuration only.

3.2. DH notation

Before we can start thinking about the IK problem we first have to discuss how the geometry of the robot is specified. During the last years, a method called Denavit-Hartenberg (DH) notation has become standard. In the right window you can see the situation with only two links. This two links contain three revolute axis. One, which connects the two links and two other axis which connect the linkage to the previous and the following link. Basically the geometry of the linkage is uniquely described, if one prescribes the distances between those axis, measured at the common normal, the angle between consecutive axis and the distance between the common normals of consecutive links. In the image to the right, the axis are colored blue whereas the common normals are red. The value a denotes the distance between the blue axis, d the distance between the red normals, and alpha the angle between the axis. We have a total of 6 axis denoted by

v₁ , v₂ , ... , v₆ .

Thus we need 5 a values, 5 alpha angles and 4 d values to uniquely describe the geometry of the linkage. These 14 parameters are called the Denavit-Hartenberg (DH) parameters of the 6R robot. To describe an individual configuration of the robot, one additionally needs the joint angles, which are basically the angles between consecutive common normals.

For the VRML implementation we will use a PROTO to describe the links. In contrast to the previous examples this PROTO will also take care of a significant part of the computation of the IK algorithm. The local coordinate system within each link is chosen according to the general used standard. Here, the axis v_i is the z axis of the links local coordinate frame. The x axis is chosen to be the common normal. We are going to use a CylinderSensor for adjusting the joint angles. Since this sensor node is aligned at the y axis we need to trick a little.

PROTO Link [field SFFloat DH_a 0
            field SFFloat DH_d 0
            field SFFloat DH_alpha 0
            eventIn SFFloat set_jointAngle
            eventOut SFFloat jointAngle_changed
            eventOut SFBool isActive
            field SFBool sensorEnabled TRUE
            exposedField MFNode children []
           ] {
  DEF ELEMENT Transform {
    children Transform {
      rotation 1 0 0 1.5708
      children [
        Transform {
          children [
            DEF SENSOR CylinderSensor {
              enabled IS sensorEnabled
              isActive IS isActive
            }
            DEF ACTUATOR Transform {}
          ]
        }
        DEF LINK Transform {
          children [
            Transform {
              rotation 0 1 0 1.5708
              children DEF JOINT Transform {}
            }
            DEF CHILD Transform {
              children IS children
            }
          ]
        }
      ]
    }
  }
  DEF CALCULATOR Script {
    field SFFloat DH_a IS DH_a
    field SFFloat DH_d IS DH_d
    field SFFloat DH_alpha IS DH_alpha
    field SFFloat jointAngle 0
    field SFNode child USE CHILD
    field SFNode actuator USE ACTUATOR
    field SFNode joint USE JOINT
    eventIn SFFloat set_jointAngle IS set_jointAngle
    eventIn SFRotation set_jointRotation
    eventOut SFFloat jointAngle_changed IS jointAngle_changed
    directOutput TRUE
    url "vrmlscript:
      function initialize() {
        child.set_translation=new SFVec3f(DH_a,DH_d,0);
        child.set_rotation=new SFRotation(1,0,0,DH_alpha-1.5708);
        actuator.set_translation=new SFVec3f(0,DH_d/2,0);
        actuator.addChildren=Browser.createVrmlFromString('Shape {'+
          '  appearance Appearance {'+
          '    material Material {'+
          '      diffuseColor 0 0 1'+
          '    }'+
          '  }'+
          '  geometry Cylinder {'+
          '    radius 0.08'+
          '    height '+(DH_d+0.2)+
          '  }'+
          '}');
        if(DH_a>0.01) {
          joint.set_rotation=new SFRotation(1,0,0,1.5708);
          joint.set_translation=new SFVec3f(0,DH_d,DH_a/2);
          joint.addChildren=Browser.createVrmlFromString('Shape {'+
            '  appearance Appearance {'+
            '    material Material {'+
            '      diffuseColor 0.8 0.8 0.8'+
            '    }'+
            '  }'+
            '  geometry Cylinder {'+
            '    radius 0.06'+
            '    height '+DH_a+
            '  }'+
            '}');
        }
      }
      function set_jointAngle(value) {
        jointAngle=value;
        element.rotation=new SFRotation(0,0,1,jointAngle);
      }
      function set_jointRotation(value) {
        jointAngle=value[1]*value[3]/Math.abs(value[1]);
        // value[1] holds the orientation of the axis
        jointAngle_changed=jointAngle;
      }
    "
  }
  ROUTE CALCULATOR.enabled_changed TO SENSOR.set_enabled
  ROUTE SENSOR.rotation_changed TO LINK.set_rotation
  ROUTE SENSOR.rotation_changed TO CALCULATOR.set_jointRotation
}

It is not necessary to understand this code in every detail. Basically, the user specifies the fields DH_a, DH_d and DH_alpha, which are the DH parameters for the link. The function initialize() in the script then computes the geometry of the link. It consist of a blue cylinder which is added to the children of ACTUATOR and a white cylinder which becomes a child of JOINT. Additionally the PROTO implements events for retreiving and setting the joint angle and for disabling the dragsensor. The basic 6R robot geometry reads

DEF DL1 Link {
  DH_a 0.5
  DH_alpha 0
  children DEF DL2 Link {
    DH_a 0.5
    DH_d 0.5
    DH_alpha 0.7854
    children DEF DL3 Link {
      DH_a 0.5
      DH_d 0.5
      DH_alpha 0.7854
      children DEF DL4 Link {
        DH_a 0.25
        DH_d 0.5
        DH_alpha 1.5708
        children DEF DL5 Link {
          DH_a 0.25
          DH_d 0.25
          DH_alpha -1.5708
          children DEF DL6 Link {
            children WorkPiece {}
          }
        }
      }
    }
  }
}

In the last link, WorkPiece contains the geometry of the end-effector.

3.3. Forward kinematics

As mentioned above, one normally does not need to calculate the forward kinematics in VRML, since it is intrinsically implemented in how Transform nodes are nested. In our example, however, we will need to calculate the transformation for each individual link with respect to the base coordinate system. We need to do this because of two reasons. First of all, we need to know the position of the end-effector and second, for the Jacobian, we will have to calculate the line coordinates of each of the revolute axis. All these have to be computed with respect to the base coordinate system. The most straightforward way to do the forward kinematics is to calculate it in the links themselves. The idea is the following. We will implement an eventIn through which we will pass the transformation data of the parent link. Within the link, this information will be nested with the link's transformation and the result will be passed to the next link. Because of their simplicity we will again use dual quaternions for these computations.

First we need to add two events

eventIn MFFloat set_parentTransform
eventOut MFFloat childTransform_changed

with the proper definition in the script node, which reads

function set_parentTransform(parent) {
  angle=jointAngle/2;
  JAQuat=new MFFloat(0,0,Math.sin(angle),Math.cos(angle),0,0,0,0);
  qu=multiply(parent,JAQuat);
  relQuat=new MFFloat(qu[0],qu[1],qu[2],qu[3],qu[4],qu[5],qu[6],qu[7]);
  qu=multiply(relQuat,DHQuat);
  childTransform_changed=new MFFloat(qu[0],qu[1],qu[2],qu[3],qu[4],qu[5],qu[6],qu[7]);
}

The result is the dual quaternion which is the product of the parent transformation, the rotation JAQuat about the joint axis and the transformation DHQuat due to the DH parameters. The latter is precalculated by adding

array=new MFFloat(1,0,0,DH_alpha,DH_a,0,DH_d,0);
qu=toDualQuaternion(array);
DHQuat=new MFFloat(qu[0],qu[1],qu[2],qu[3],qu[4],qu[5],qu[6],qu[7]);

to the initialize method of the script. For these purposes we use the same functions toDualQuaternion and multiply as in the previous example. Finally we need to set up a sequence of ROUTEs which will route the dual quaternion through the links thereby accumulating the transfomration information.

ROUTE MOVER.designBaseTransform_changed TO DL1.set_parentTransform
ROUTE DL1.childTransform_changed TO DL2.set_parentTransform
ROUTE DL2.childTransform_changed TO DL3.set_parentTransform
ROUTE DL3.childTransform_changed TO DL4.set_parentTransform
ROUTE DL4.childTransform_changed TO DL5.set_parentTransform
ROUTE DL5.childTransform_changed TO DL6.set_parentTransform
ROUTE DL6.childTransform_changed TO MOVER.set_designEndeffectorTransform

This process will be activated by a script called MOVER which will send a unit dual quaternion (0,0,0,1,0,0,0,0) to the first link. The last link on the other hand will send its position back to the script after the calculations have been completed.

We are now ready to enter the meaty part of the algorithm.

3.4. Twists

In differential geometry the velocity information of a curve is given by its tangent vector. In kinematics there is a similar concept. It is well know that for any motion each body behaves at each instant as if it would be subject to a continuous screw motion around a fixed axis with constant pitch. In some sense this screw motion is an analogy to the tanget of a curve. In the following we will extend the concept of line coordinates to what is called twists or screws. These twists will hold six coordinates which uniquely identify the above moment screw.

Again, the basic concept is simple. Consider a continuous screw motion around a fixed axis with homogeneous line coordinates collected in a normalized dual vector v. Further let's assume that p denotes the pitch and a its angular velocity. We then define the twist s associated with the screw motion by

s = a ( 1 + e p ) v .

Obviously, if this screw motion is a pure rotation, in which case p=0, the twist consists just of the normalized line coordinates of the rotation axis multiplied by the angular velocity.

3.5. The manipulator Jacobian

Consider the motion of a link with respect to the previous link. Obviously, since this motion is a pure rotation around the joint axis, the corresponding twist results from multiplying the normalized line coordinates of the joint axis by the angular velocity, also called joint velocity. For link number i this would be

r_i = a_i v_i.

Once again, a_i denotes the joint velocity of this link and v_i the dual vector that holds the normalized line coordinates of the corresponding joint axis.

Further, let s denote the twist of the motion of the end-effector. Restricting our investigations to the case of a 6R robot these twists then hold

r₁ + r₂ + r₃ + r₄ + r₅ + r₆ = s ,

provided that all twists are measured in the same coordinate system. The above equality is equivalent to

a₁ v₁ + a₂ v₂ + a₃ v₃ + a₄ v₄ + a₅ v₅ + a₆ v₆ = s .

Changing our notation from dual vectors to vector pairs again, we see that we can collect the line coordinates on the left side in a 6 by 6 matrix J and the equation becomes

J · ( a₁ , a₂ , a₃ , a₄ , a₅ , a₆ )^T = ( s , s_e )^T.

We use the standard matrix notation where · denotes the matrix product and the superscript T the transpose. The rows of J hold the normalized line coordinates of the joint axis.

The matrix J is called the manipulator Jacobian. It can be used for solving the IK problem the following way. The idea is to push the manipulator towards its desired position. We therefore first estimate the twist of the end-effector such that the robot will move towards the desired position. Then, using the above formula, we recalculate the joint velocities necessary to realize this push. Afterwards we apply the push and continue on this motion for a specified amount of time. Although we pushed directly towards the desired position, the manipulator will only approximately follow this way. However, it still will be guaranteed that the manipulator will move into a position which is nearer to the desired position than the previous one. In order to solve the IK problem we therefore iterate this process.

Before we continue with the implementation of this algorithm it is necessary to make a few remarks on the theoretical problems that arise from this technique. Notice that solving for the joint velocities means inverting the manipulator Jacobian. As a result, the algorithm will fail, if the Jacobian is singular. Positions in which this is the case are called singular positions. In fact, the initial position of the robot in the example project is such a singular position. Handling these positions is an important problem in robotics. For industrial robots, the configurations which lead to singular positions are usually included in the user manual. In our example, we will simply ignore such positions. If a singularity is encountered, the robot will not move.

3.6. Calculating the Jacobian in VRML

Our task is clear. We have to calculate the line coordinates of each joint axis with respect to the base system. We do this by applying quaternion algebra. Consider a line with line coordinates v and a transformation given in terms of the dual quaternion Q. Using the quaternion product, the line coordinates of the transformed line v' then read

v' = Q * ( v , 0 ) * Q^c .

In our case we have to transform the joint axis from the link's local coordinate system to the base coordinate system. For each joint, the joint axis is the z axis of the local coordinate system. Thus, the local line coordinates of the joint axis read (0,0,1,0,0,0). Notice that the transfomration which transforms the link's coordinate system into the base coordinate system is calculated during the forward kinematics evaluation. We can therefore use this procedure to calculate the transformed line coordinates and thus the corresponding row in the Jacobian at the same time.

This is done by adding the function

function calculateJacobian(quat) {
  lineQuat=new MFFloat(0,0,1,0,0,0,0,0);
  // z-axis in line coordinates
  conjQuat=new MFFloat(-quat[0],-quat[1],-quat[2],quat[3],-quat[4],-quat[5],-quat[6],quat[7]);
  qu=multiply(lineQuat,conjQuat);
  relQuat=new MFFloat(qu[0],qu[1],qu[2],qu[3],qu[4],qu[5],qu[6],qu[7]);
  qu=multiply(quat,relQuat);
  len=qu[0]*qu[0]+qu[1]*qu[1]+qu[2]*qu[2];
  // normalize line coordinates
  if (len>tiny)
    return new MFFloat(qu[0]/len,qu[1]/len,qu[2]/len,qu[4]/len,qu[5]/len,qu[6]/len);
  else
    return new MFFloat(0,0,0,qu[4],qu[5],qu[6]);
}

to the script in the definition of the Link prototype. This function is called whenever a set_parentTransform event is received. The result is routed back to the script which initiated the forward kinematics evaluation.

During this evaluation the script receives the Jacobian row by row. Each link will report exactly one row. After all rows have been received, the script can proceed evaluating the IK algorithm.

3.7. Estimating the end-effector twist

For the estimation of the end-effector's twist, we use the following technique. We first calculate the transformation which maps the current end-effector position to the desired end position. If C denotes the current and D the desired position this transformation reads

Q = D * C^c.

Recall that and rigid body transformation such as Q can be decomposed into a rotation and a translation in direction of the rotation vector. In other words Q is associated with a continuous screw motion with fixed axis which maps C to D. The twist of this screw motion will serve as our estimate. To obtain the corresponding screw axis we transform Q back into VRML format. The orientation field holds the information about the direction d of the screw axis and the rotation angle a, the translation field gives the translation vector t.

Next we calculate amount of translation in the direction of d. Assuming that d is normalized, this amount is

a_e = d · t .

The computation of the moment vector of the screw axis is a little tricky. We therefore just give the result

d_e = 0.5 ( t × d + ctg( a/2 ) ( d × t ) × d ) .

The easiest way to derive this formula is by decomposing the dual quaternion Q into the product of a dual angle and a dual vector describing a line and thus satisfying Plücker's identity.

Finally we calculate a proper estimate for the end-effector twist by

s = 0.1 ( a + e a_e ) d .

This guarantees that the end-effector is moving towards the desired end position. Tests showed that multiplying the velocity by 0.1 gives the best results. It basically ensures that the algorithms does not overshoot in the sense that the push is never too strong to push the end-effector beyond the desired position.

3.8. Implementing IK in VRML

The implementation is done in a function solveIK which is called whenever the manipulator Jabobian has been reported to the script. It uses a method called LU decomposition to solve the system of linear equations. This method is split into two parts. First a function ludcmp is called which calculates the LU decomposition of the Jacobian. If the Jacobian is singular this function will return a FALSE value. After the end-effector twist has been calculated a dn stored in the field deltaAngle a second function lubksb is called which takes the decomposed matrix and solves the system. The result is again stored in deltaAngle. Finally the changes in the joint angles are added to the initial values and reported back to the geometry.

function solveIK() {
  if(ludcmp()==TRUE) {
    conjQuaternion=new MFFloat(-currentQuaternion[0],-currentQuaternion[1],
                               -currentQuaternion[2],currentQuaternion[3],
                               -currentQuaternion[4],-currentQuaternion[5],
                               -currentQuaternion[6],currentQuaternion[7]);
    deltaQuaternion=multiply(desiredQuaternion,conjQuaternion);
    array=fromDualQuaternion(deltaQuaternion);
    va=array[3]; // angular velocity
    len=array[0]*array[0]+array[1]*array[1]+array[2]*array[2];
    vo=new SFVec3f(array[4],array[5],array[6]);
    if(len>tiny) {
      vd=array[0]*array[4]+array[1]*array[5]+array[2]*array[6];
      dir=new SFVec3f(array[0],array[1],array[2]);
    } else {
      vd=Math.sqrt(array[4]*array[4]+array[5]*array[5]+array[6]*array[6]);
      dir=new SFVec3f(0,0,1);
      va=0;
    }
    vdum=cross(dir,vo);
    mom=cross(vdum,dir);
    tan=Math.tan(va/2);
    if(Math.abs(tan)>tiny)
      ctg=1/tan;
    else
      ctg=0;
    mom[0]=0.5*(mom[0]*ctg-vdum[0]);
    mom[1]=0.5*(mom[1]*ctg-vdum[1]);
    mom[2]=0.5*(mom[2]*ctg-vdum[2]); // moment vector
    va/=10;
    vd/=10;
    deltaAngle=new MFFloat(va*dir[0],va*dir[1],va*dir[2],
                           vd*dir[0]+va*mom[0],vd*dir[1]+va*mom[1],vd*dir[2]+va*mom[2]);
    lubksb();
    oldAngle[0]+=deltaAngle[0];
    oldAngle[1]+=deltaAngle[1];
    oldAngle[2]+=deltaAngle[2];
    oldAngle[3]+=deltaAngle[3];
    oldAngle[4]+=deltaAngle[4];
    oldAngle[5]+=deltaAngle[5];
    jointAngle1_changed=oldAngle[0];
    jointAngle2_changed=oldAngle[1];
    jointAngle3_changed=oldAngle[2];
    jointAngle4_changed=oldAngle[3];
    jointAngle5_changed=oldAngle[4];
    jointAngle6_changed=oldAngle[5];
  }
}

An explanation of the LU decomposition method is omitted here. It turns out to be fast and stable enough to provide real-time results. The details can be found in the source code. To explore the simulation, click here.