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.

Link | θ | d | a | α |

1 | q1 | L1 | 0 | 90 |

2 | q2-90 | 0 | L2 | 0 |

3 | q3 | 0 | 0 | 90 |

4 | q4 | L3 | 0 | -90 |

5 | q5 | 0 | 0 | 90 |

6 | q6 | 0 | 0 | 0 |

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!

## Discussions

## Become a Hackaday.io Member

Create an account to leave a comment. Already have an account? Log In.