Rewrote inverse kinematic solution

Used more robust law of cosine relations and d parameter method