Inverse Kinematics for 4 joint arm