Ich denke der Lösungsweg ist nicht das Problem. Es wird halt jemand gesucht, der ihn gehen soll.

Weg a) geht im Prinzip so
https://de.wikipedia.org/wiki/Denavi...Transformation

Weg b) macht man wohl heute meist mit einer Kombination aus
https://de.wikipedia.org/wiki/Jacobi-Matrix
https://de.wikipedia.org/wiki/Pseudoinverse

Eigentlich muss man das aber nicht wirklich wissen, weil es diverse C++ Bibliotheken gibt, die das schon fertig können.
http://roboop.sourceforge.net/
https://moveit.ros.org/
http://www.orocos.org/kdl
Die sind aber nicht geeignet, weil keine fertigen Portierungen auf Arduino existieren oder möglich sind.

Auch Rechnen mit mehr Handarbeit mit den gängigen C++ Matrix Libs wie z.B.
http://eigen.tuxfamily.org/index.php?title=Main_Page
http://arma.sourceforge.net/
ist wohl nicht gefragt, weil es zumindest keine fertigen Portierungen auf Arduino gibt.

Das ist alles, was ich beitragen könnte, ist aber keine fertige Lösung, daher zu verwerfen.