Inverse Kinematic algorithm

Inverse Kinematic for an Anthropomorphic arm with 3 DOF.

% It calculates the Inverse Kinematic of an Anthropomorphic arm with 3 DOF.
% 'q' is the solutions in radiant and K is the direct Kinematic matrix.
%
% K = [ n s a p;
% 0 0 0 1]
% where n, s, a are three vectors fo 3 elements that represents the
% end-effector's orientation, and p is the desired end-effector position.