ICubFowardKinematics left
This page describes how to construct the matrix T_RoLa whose definition is given in ICubForwardKinematics. The matrix is constructed in two steps i.e. T_RoLa = 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 forward kinematic in this case includes the waist and the left arm forward kinematics.
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 hand 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:
| 0 | -1 | 0 | 0 |
| 0 | 0 | -1 | 0 |
| 1 | 0 | 0 | 0 |
| 0 | 0 | 0 | 1 |
Here is the table of the actual DH parameters for left arm v1.
| Link i / H – D | Ai (mm) | d_i (mm) | alpha_i (rad) | theta_i (deg) |
|---|---|---|---|---|
| i = 0 | 32 | 0 | pi/2 | -22 -> 84 |
| i = 1 | 0 | -5.5 | pi/2 | -90 + (-39 -> 39) |
| i = 2 | 23.3647 | -143.3 | -pi/2 | 105 + (-59 -> 59) |
| i = 3 | 0 | 107.74 | -pi/2 | 90 + (5 -> -95) |
| i = 4 | 0 | 0 | pi/2 | -90 + (0 -> 160.8) |
| i = 5 | 15 | 152.28 | -pi/2 | 75 + (-37 -> 100) |
| i = 6 | -15 | 0 | pi/2 | 5.5 -> 106 |
| i = 7 | 0 | 137.3 | pi/2 | -90 + (-50 -> 50) |
| i = 8 | 0 | 0 | pi/2 | 90 + (10 -> -65) |
| i = 9 | 62.5 | -16 | 0 | (-25 -> 25) |
Here is the table of the actual DH parameters for left arm v1.7.
| Link i / H – D | Ai (mm) | d_i (mm) | alpha_i (rad) | theta_i (deg) |
|---|---|---|---|---|
| i = 0 | 32 | 0 | pi/2 | -22 -> 84 |
| i = 1 | 0 | -5.5 | pi/2 | -90 + (-39 -> 39) |
| i = 2 | 23.3647 | -143.3 | -pi/2 | 105 + (-59 -> 59) |
| i = 3 | 0 | 107.74 | -pi/2 | 90 + (5 -> -95) |
| i = 4 | 0 | 0 | pi/2 | -90 + (0 -> 160.8) |
| i = 5 | 15 | 152.28 | -pi/2 | 75 + (-37 -> 100) |
| i = 6 | -15 | 0 | pi/2 | 5.5 -> 106 |
| i = 7 | 0 | 141.3 | pi/2 | -90 + (-50 -> 50) |
| i = 8 | 0 | 0 | pi/2 | 90 + (10 -> -65) |
| i = 9 | 62.5 | -16 | 0 | (-25 -> 25) |
Here is the table of the actual DH parameters for left arm v2.
| Link i / H – D | Ai (mm) | d_i (mm) | alpha_i (rad) | theta_i (deg) |
|---|---|---|---|---|
| i = 0 | 32 | 0 | pi/2 | -22 -> 84 |
| i = 1 | 0 | -5.5 | pi/2 | -90 + (-39 -> 39) |
| i = 2 | 23.3647 | -143.3 | -pi/2 | 105 + (-59 -> 59) |
| i = 3 | 0 | 107.74 | -pi/2 | 90 + (5 -> -95) |
| i = 4 | 0 | 0 | pi/2 | -90 + (0 -> 160.8) |
| i = 5 | 15 | 152.28 | -pi/2 | 75 + (-37 -> 100) |
| i = 6 | -15 | 0 | pi/2 | 5.5 -> 106 |
| i = 7 | 0 | 141.3 | pi/2 | -90 + (-50 -> 50) |
| i = 8 | 0 | 0 | pi/2 | 90 + (10 -> -65) |
| i = 9 | 62.5 | -25.98 | 0 | (-25 -> 25) |