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!