schon klar, das sind die oben zitierten Matrizen, am ehesten verkettet nach Denavit–Hartenberg, und berechnet "vorwärts" und dann auch invers.
Aber wenn du das ausrechnen und programiieren kannst, das wäre großartig - ICH kann es nicht, daher Suche ich eine fertige Lib.