Inverse Kinematic for 6DOF arm

% Anthropomorphic arm with 6 DOF and spherical wrist
% It calculates the Inverse Kinematic of an Anthropomorphic arm with 6 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.