MATLAB 并联机构工作空间搜索代码

function pan=subprog1（x0y0z0）
%***********************
%x0=0;y0=0;z0=919;a=0;b=0;r=0;%为输入参数
a0=pi/180;b0=180/pi;
%a=a*a0;b=b*a0;r=r*a0;%must be out of the circle
P0=[x0y0z0]‘;%P1=[abr]‘;%输入初始参数；当P1=[000]Zmax=1576Zmin=919;P0=[001000]a=+-19.2度;
lmax=4400;lmin=800;Rp=1732;Rb=2078;smax=30;%教练某些位置可达80度
%Rotz=[cos（a）sin（a）0;sin（a）cos（a）0;001];
%Roty=[cos（b）0sin（b）;010;-sin（b）0cos（b）];
%Rotx=[100;0cos（r）-sin（r）;0sin（r）cos（r）];
%T=Rotz*Roty*Rotx;           %余弦矩阵坐标转换
%ap=50;ab=10;
A=[0120240]‘*a0;
B=[0120240]‘*a0;
A0=cos（A）*Rb;A01=sin（A）*Rb;A02=[000]‘;
B0=cos（B）*Rp;B01=sin（B）*Rp;B02=[000]‘;
C=[A0A01A02];
B1B=C（1:）‘;B2B=C（2:）‘;B3B=C（3:）‘;%B4B=C（4:）‘;B5B=C（5:）‘;B6B=C（6:）‘;
D1=[B1BB2BB3B];%B4BB5BB6B];%底座各点绝对坐标
D=[B0B01B02];
P1p=D（1:）‘;P2p=D（2:）‘;P3p=D（3:）‘;%P4p=D（4:）‘;P5p=D（5:）‘;P6p=D（6:）‘;
C1=[P1pP2pP3p];%P4pP5pP6p];%工作台各点相对坐标
P1B=P1p+P0;P2B=P2p+P0;P3B=P3p+P0;%P4B=P4p+P0;P5B=P5p+P0;P6B=P6p+P0;%工作台各点绝对坐标
DB=[P1BP2BP3B];%P4BP5BP6B];