This page describes how to construct the matrix T_RoLf whose definition is given in ICubForwardKinematics. The matrix is constructed in two steps i.e. T_RoLf = T_Ro0 * T_0n. The first matrix T_Ro0 describes the rigid roto-translation from the root reference frame to points in the 0th reference frame as defined by the Denavit-Hartenberg convention. In this case T_Ro0 is just a rigid rotation which aligns the z-axis with the first joint of the waist. The second matrix T_0n corresponds to the Denavit-Hartenberg description of the forward kinematic, i.e. the roto-translation from the 0th reference frame to the nth reference frame being n the number of degrees of freedom.
The matrix T_0n is itself the composition of n matrices as defined by the DH convention: T_0n = T_01 T_12 ... T_(n-1)n. Here is the updated matlab code for computing the forward kinematics with the Denavit Hartenberg notation Media: ICubFwdKinNew.zip.
The foot reference frame is located in the palm as shown in the CAD figure. The x axis is in red. The y axis is in green. The z axis is in blue.
Here is the matrix T_Ro0:
Here is the table of the actual DH parameters which describe T_01, T_12, ... T_(n-1)n.
|Link i / H – D||Ai (mm)||d_i (mm)||alpha_i (rad)||theta_i (deg)|
|i = 0||0||0||-pi/2||90 + (-44 -> +132)|
|i = 1||0||0||-pi/2||90 + (-119 - > +17)|
|i = 2||0||-223.6||pi/2||-90 + (-79 -> +79)|
|i = 3||-213||0||pi||90 + (-125 -> +0)|
|i = 4||0||0||-pi/2||-42 -> +21|
|i = 5||-41||0||0||-24 -> +24|