Wenn man seinen Arm in ROS mit URDF modelliert kriegt man mit TF2 beide Richtungen der Kinematik quasi geschenkt. Ansonsten verstehe ich auch nicht dein Problem mit mehr Gelenken nicht: Das ist einfach nur die Modellierung für 2 in Reihe gepackt. Auch mehr Freiheitsgrade sind nur: Mach deine Matrix größer