ICubFowardKinematics v2

From Wiki for iCub and Friends
Jump to: navigation, search

These are the Denavit-Hartenberg parameters for the first seven joints.

Here is the matlab code for computing the forward kinematics with the Denavit Hartenberg notation Media: FwdKin7DOF.zip.

DHnotation v2.jpg

Link i / H – D Ai (mm) di+1 (mm) alpha i (rad) theta_i+1 (deg)
i = 0 0 LA0 = 109.8 pi/2 -40 -> 230
i = 1 0 0 -pi/2 (0 -> 180)-90
i = 2 0 -151.46 pi/2 -90 -> 90
i = 3 -15 0 pi/2 15 -> 90
i = 4 0 137.5 pi/2 -90 -> 90
i = 5 0 0 pi/2 (-45 -> 90) +90
i = 6 0 0 0 (-45 -> 45)

The matrix which rotates 'joint i-1' to 'joint i' is:

cos(thet) -sin(thet)*cos(alph) sin(thet)*sin(alph) cos(thet)*a
sin(thet) cos(thet)*cos(alph) -cos(thet)*sin(alph) sin(thet)*a
0 sin(alph) cos(alph) d
0 0 0 1