iCub Forward Kinematics - IMU

V1

Here's described how to construct the matrix T_RoIs whose definition is given in ICubForwardKinematics. The matrix is constructed in three steps i.e. T_RoIs = T_Ro0 * T_0n * T_nIs. The first matrix T_Ro0 describes the rigid roto-translation from the root reference frame to points in the 0^th 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 correspond to the Denavit-Hartenberg description of the waist and neck forward kinematic, i.e. the roto-translation from the 0^th reference frame to the nth reference frame being n the number of degrees of freedom. The forward kinematic T_0n in this case includes the waist and the neck 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.

The sensor 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 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	32	0	pi/2	-22 -> 84
i = 1	0	-5.5	pi/2	-90 + (-39 -> 39)
i = 2	2.31	-193.3	-pi/2	-90 + (-59 -> 59)
i = 3	33	0	pi/2	90 + (-40 -> 30)
i = 4	0	1	-pi/2	-90 + (-70 -> 60)
i = 5	22.5	100.5	-pi/2	90 + (-55 -> 55)

Here is the matrix T\_nIs:


1	0	0	0
0	0	-1	0
0	1	0	6.6
0	0	0	1

In some circumstances it might be convenient to think of coding T_nls as a further virtual link located at the end of the chain and with its joint constantly kept at 0 value. The DH parameters of this virtual link are:

Link i / H – D	Ai (mm)	d_i (mm)	alpha_i (rad)	theta_i (deg)
i = 6	0	6.6	pi/2	0

V2

Here's described how to construct the matrix T_RoIs whose definition is given in ICubForwardKinematics. The matrix is constructed in three steps i.e. T_RoIs = T_Ro0 * T_0n * T_nIs. The first matrix T_Ro0 describes the rigid roto-translation from the root reference frame to points in the 0^th 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 correspond to the Denavit-Hartenberg description of the waist and neck forward kinematic, i.e. the roto-translation from the 0^th reference frame to the nth reference frame being n the number of degrees of freedom. The forward kinematic T_0n in this case includes the waist and the neck forward kinematics.