Was hounter gesagt hat ist richtig und sollte bei der Umsetzung auf jeden Fall beachtet werden.
Ich kann auchnoch ein bissl Senf dazu geben der (glaube ich) noch nciht genannt wurde. Hier wird ja schon fleißig von Vorwärts und Rückwärtskinematik gesprochen. Du Antwortest immer, dass du dich da noch durchlesen musst. Deshalb: Der Überbegriff für den ganzen Mathekrämpel der dahinter steckt lautet MKS (Mehrkörpersysteme). Darunter solltest du einiges an Input finden können. Falls Softwareseitig Probleme bei der Umsetzung deines Projektes bestehen schau dir mal Orocos an. Ist nen OpenSource KinematikToolChain Lib.

http://www.orocos.org/