hrz.m

针对移动机械臂运动学

function hr = hrz (a,o)
%HRZ(ANGLE) or HRZ(ANGLE,"OPTION") Homogeneous Rotation over Z-axis.

%July ~ December 2005, by Reinaldo M. do Nascimento

try,a=mod(a,2*pi);o=0;catch,o=1;end
	ca = cos(a);
	sa = sin(a);
if nargin==2 & o==0
    if abs(ca) <= eps
        ca=0;
    end
    if abs(sa) <= eps
        sa=0;
    end
end
	hr =   [ca	-sa	 0	0
		    sa	 ca	 0	0
		     0	  0	 1	0
		     0    0	 0	1];