A Practical Formulation of the Exponential Map for Rotations

Pseudocode for Jacobian Calculation

The code and functionality pertaining to the exponential map is typically a very small (but important!) part of much larger systems. It is beyond the scope of this paper to provide any kind of meaningful working system for example purposes. But to help give a feel for where computing fits into the overall scheme of things, this page contains pseudocode for computing the Jacobian contribution of a node in a transformation hierarchy, with respect to all of the end effectors below it in the hierarchy. The pseudocode routine Node_Jacobian_Contribution() makes the following assumptions:

• The function that maps an end effector ee from its local coordinate system (i.e. in a finger's frame of reference) to world coordinates is of the form

  

  where ee is a column 3-vector, is the root translation of the entire hierarchy (in the form of a 4x4 matrix), and represent the i^th joint between the root and the end effector: is the constant translation that positions the base of a limb in its parent’s frame, and is the joint's current rotation, a function of exponential map parameters.
• Node_Jacobian_Contribution() only works for a hypothetical joint type that has a fixed center of rotation and a free, unconstrained three DOF rotation. Thus it will not work for the root of the hierarchy, since the root also has translational degrees of freedom.
• The type Jacobian is essentially an m by n matrix, where m is (3 * number of end effectors), and n is the number of degrees-of-freedom in the system. Upon exit of Node_Jacobian_Contribution, jac will have the entries pertaining to node i's degrees of freedom filled in.
• The type TransformStack is an object-oriented stack supporting the same types of operations as the OpenGL stack used to traverse hierarchies. Upon entry to Node_Jacobian_Contribution, stack should contain the combined transformation of all nodes above i in the hierarchy. It will be restored to this state upon exit.
• nodeTrans is the structural translation for the node () as a 3-vector.
• nodeOrient is the vector representing the current state of the joint, i.e. its DOFs. It has parameter class inout because the act of computing its derivatives may cause it to be dynamically reparameterized. Therefore its value upon exit may differ from its value on entry, and this must be propagated into the global state vector (if your system gathers all DOFs into a global state vector).
• startDOF is the column index in jac of node i's first degree-of-freedom; it is generally the same as nodeOrient's position in the global state vector.
• effectors is a list of all of the end effectors below node i in the hierarchy. Each element has a member jacIndex that gives the first of three consecutive rows in the jacobian that correspond to the end effector. Each element has a member coords that is a 3-vector containing the i_local position of the end effector. "i_local" means the original end effector ee transformed by the combined transformation , i.e. the transformation from the originating frame of the end effector up to (but not including) node i in the hierarchy.

void Node_Jacobian_Contribution(inout Jacobian jac, inout TransformStack stack,
                                in double nodeTrans[3], inout double nodeOrient[3],
                                in int startDOF, in EffectorList effectors)
{
    int              i,j;
    EffectorList     currEff;
    double           dRdv[4][4];
    double           column[3];
 
    stack.Push();   
    stack.Translate(nodeTrans);  /* put the constant translation on the stack */     

    for j = 0 to 2 do {
        stack.Push();
        Partial_R_Partial_EM3(nodeOrient, j, dRdv);   /* provided function */
        stack.Concat_Matrix(dRdv);

        /* In the terminology used above, the partial derivative of the position of
         * the end effector with respect to the jth rotational parameter of node
         * i is:
         *    (d F)/(d nodeOrient_j) = 
         *      T_0 R_0 ... T_i (d R_i)/(d nodeOrient_j) ... T_n R_n  ee =
         *      stack.Current * i_local(ee)
         *
         *  which is what we compute below */
        for currEff = each element of effectors do {
            stack.Vector_Transform(currEff.coords, column);
            for i = 0 to 2 do
                jac[currEff.jacIndex+i][startDOF+j] = column[i];
        }
        stack.Pop();
    }
    stack.Pop();
}