Today I'll share Thor's Forward Kinematics calculus.

Forward kinematics allows to compute the position of the end-effector from specified values for the joint parameters.

In order to resolve the Forward Kinematics I have used the Denavit-Hartenberg parameters.

After obtaining the previous data, we have to calculate the transformation matrices:

Replacing with our data:

In order to obtain the transformation matrix (T) between the base and the end of the robot we have to multiply the previous matrices as shown:

The resulting T matrix is formed by a 3x3 sub-matrix destined to orientation (n,o,a) and by a 3x1 vector destined to position (p).

As the calculus of this transformation matrix is not trivial, I have used the OpenSource tool SymPy to do the symbolic operations (code available below).

The SymPy code used:

C1 = Symbol('C1')

C2 = Symbol('C2')

C3 = Symbol('C3')

C4 = Symbol('C4')

C5 = Symbol('C5')

C6 = Symbol('C6')

S1 = Symbol('S1')

S2 = Symbol('S2')

S3 = Symbol('S3')

S4 = Symbol('S4')

S5 = Symbol('S5')

S6 = Symbol('S6')

L1 = Symbol('L1')

L2 = Symbol('L2')

L3 = Symbol('L3')

L4 = Symbol('L4')

A1=Matrix([[C1,0,S1,0],[S1,0,-C1,0],[0,1,0,L1],[0,0,0,1]])

A2=Matrix([[S2,C2,0,L2*S2],[-C2,S2,0,-L2*C2],[0,0,1,0],[0,0,0,1]])

A3=Matrix([[C3,0,S3,0],[S3,0,-C3,0],[0,1,0,0],[0,0,0,1]])

A4=Matrix([[C4,0,-S4,0],[S4,0,C4,0],[0,-1,0,L3],[0,0,0,1]])

A5=Matrix([[C5,0,S5,0],[S5,0,-C5,0],[0,1,0,0],[0,0,0,1]])

A6=Matrix([[C6,-S6,0,0],[S6,C6,0,0],[0,0,1,L4],[0,0,0,1]])

A1*A2*A3*A4*A5*A6

Hope you like it!