工业机械臂TCP末端标定

function eulerAngles = rotationMatrix2eulerAngles(R) % eulerAngles = rotationMatrix2eulerAngles(R) % % This function returns the rotation angles in degrees about the x, y and z axis for a % given rotation matrix % % Copyright : This code is written by david zhao from SCUT,1257650237@qq,com. The code % may be used, modified and distributed for research purposes with % acknowledgement of the author and inclusion this copyright information. % % Disclaimer : This code is provided as is without any warrantly. if abs(abs(R(3,1))-1) > 0.000001 theta1 = -asin(R(3,1)); theta2 = pi - theta1; psi1 = atan2(R(3,2)/cos(theta1), R(3,3)/cos(theta1)); psi2 = atan2(R(3,2)/cos(theta2), R(3,3)/cos(theta2)); pfi1 = atan2(R(2,1)/cos(theta1), R(1,1)/cos(theta1)); pfi2 = atan2(R(2,1)/cos(theta2), R(1,1)/cos(theta2)); theta = theta1; % could be any one of the two psi = psi1; pfi = pfi1; else pfi = 0; if abs(R(3,1)+1) < 0.00001 theta = pi/2; delta = atan2(R(1,2), R(1,3)); psi = pfi + delta; else theta = -pi/2; delta = atan2(-R(1,2), -R(1,3)); psi = -pfi + delta; end end %psi is along x-axis...........theta is along y-axis........pfi is along z %axis % eulerAngles = [psi theta pfi]; %for rad; eulerAngles = [psi*180/pi theta*180/pi pfi*180/pi]; %for degree;