The process is explained in the following blog: http://dagmarobotix.blogspot.pt/2016/09/6-dof-robotic-arm-modelation_81.html ...