A small function to calculate the Inverse Kinematics of a robot
This is a small function I wrote while starting with my robotics lectures back in 2013. The goal is to calculate the Inverse Kinematics of the arm of the famous NAO Robot (https://www.softbankrobotics.com/emea/en/nao).
Basically the Inverse Kinematics algorithm calculates the angular position to which the servomotors of the arm need to move in order to reach a desired set of XYZ coordinates, given an initial set of XYZ coordinates.