Calibrating robot FK and transformation from Optitrack system to the robot frame - View it on GitHub