Solved it,
my error was that I directly applied the formula from the link when instead I would have needed to create a vector that represents the base and rotate that around the rotation vector of the robot.
After that I can apply the calculation from the link and get the result I want.
1 Like