Matlab Code for IK of Fanuc Robotic Arm with trajectory following