clear;clc;
lb=[-300200200000];
ub=[0300300700700700];
rng?default
x0=100*randn（61）;

MIN_leglength=300;%支鏈最短值
MAX_leglength=600;%支鏈最長值
interval=1.5;%數據細分度
MAX_angle=40;%球面副最大傾角
currentnum=0;%當前循環個數，用于百分比表示計算進度
osnumber=1;%動平面中心的坐標指針
erropost=0;%不符合機構轉角限制的位姿個數

L_1=500;%固定平臺邊長
L_2=500;%動平臺邊長
Rx=[-L_1/2;L_1/2;0];
Ry=[-sqrt（3）.*L_1/6;-sqrt（3）.*L_1/6;sqrt（3）.*L_1/3];
Rz=[0;0;0];
R1_steady=[Rx（11）;Ry（11）;Rz（11）];
R2_steady=[Rx（21）;Ry（21）;Rz（21）];
R3_steady=[Rx（31）;Ry（31）;Rz（31）];
G1min=MIN_leglength;
G2min=MIN_leglength;
G3min=MIN_leglength;
leenum=（MAX_leglength-MIN_leglength）/interval+1;%單支鏈長數據個數
totalprocess=leenum+leenum*（leenum-1）+nchoosek（leenum3）;
%totalprocess=leenum^3;

Rz120_matrix=RZ（2*pi/3）;
Rz240_matrix=RZ（-2*pi/3）;

for?G1=G1min:interval:MAX_leglength?
?????for?G2=G2min:interval:MAX_leglength???????????
????????????for?G3=G3min:interval:MAX_leglength

p=setcoeffirent（L_1L_2G1G2G3）;
[xres]=lsqnonlin（px0lbub）;
for?i=1:1:3????
????Z（i）=x（i+3）;???
end
X（1）=x（1）;
X（2）=x（2）;
X（3）=0;
Y（1）=sqrt（3）*X（1）/3;
Y（2）=-sqrt（3）*X（2）/3;
Y（3）=x（3）;

OX=（X（1）+X（2）+X（3））/3;
OY=（Y（1）+Y（2）+Y（3））/3;
OZ=（Z（1）+Z（2）+Z（3））/3;

vectorOS_S1=[X（1）-OX;Y（1）-OY;Z（1）-OZ]/sqrt（（X（1）-OX）^2+（Y（1）-OY）^2+（Z（1）-OZ）^2）;
vectorOS_S2=[X（2）-OX;Y（2）-OY;Z（2）-OZ]/sqrt（（X（2）-OX）^2+（Y（2）-OY）^2+（Z（2）-OZ）^2）;
vectorOS_S3=[X（3）-OX;Y（3）-OY;Z（3）-OZ]/sqrt（（X（3）-OX）^2+（Y（3）-OY）^2+（Z（3）-OZ）^2）;
vectorOS_S=cross（vectorOS_S1vectorOS_S2）;%動平臺法向量

vectorR1_S1=[X（1）-Rx（11）;Y（1）-Ry（11）;Z（1）-Rz（11）]/sqrt（（X（1）-Rx（11））^2+（Y（1）-Ry（11））^2+（Z（1）-Rz（11））^2）;
vectorR2_S2=[X（2）-Rx（21）;Y（2）-Ry（21）;Z（2）-Rz（21）]/sqrt（（X（2）-Rx（21））^2+（Y（2）-Ry（21））^2+（Z（2）-Rz（21））^2）;
vectorR3_S3=[X（3）-Rx（31）;Y（3）-Ry（31）;Z（3）-Rz（31）]/sqrt（（X（3）-Rx（31））^2+（Y（3）-Ry（31））^2+（Z（3）-Rz（31））^2）;

currentnum=currentnum+1;
disp（[‘當前計算完成????‘num2str（currentnum*100/totalprocess）‘%‘]）
xgroup=[X（1）;X（2）;X（3）];
ygroup=[Y（1）;Y（2）;Y（3）];
zgroup=[Z（1）;Z（2）;Z（3）];
theta1=asin（Z（1）/G1）*180/pi;%旋轉副夾角
theta2=asin（Z（2）/G2）*180/pi;
theta3=asin（Z（3）/G3）*180/pi;
phy1=subspace（vectorOS_SvectorR1_S1）*180/pi;%球副夾角
phy2=subspace（vectorOS_SvectorR2_S2）*180/pi;
phy3=subspace（vectorOS_SvectorR3_S3）*180/pi;
alphaOS_OR=subspace（vectorOS_S[0;0;1]）*180/pi;%動平臺相對于定平臺的偏角
alphaOS3_OR3=subspace（[vectorOS_S（11）;vectorOS_S（21）][1;0]）*180/pi;%動平臺相對于定平臺轉角
%alphaOS3_OR3=（asin（vectorOS_S（11）/sqrt（vectorOS_S（11）^2+vectorOS_S（21）^2）））*180/pi;
%subspace（[vectorOS_S（11）;vectorOS_S（21）][0;1]）*180/pi;

if?phy1>MAX_angle
????erropost=erropost+1;?
????EROG（erropost1）=G1;
????EROG（erropost2）=G2;
????EROG（erropost3）=G3;
????EROG（erropost4）=phy1;
????????
????continue;
elseif?phy2>MAX_angle
????erropost=erropost+1;??
????EROG（erropost1）=G1;
????EROG（erropost2）=G2;
????EROG（erropost3）=G3;
????EROG（erropost5）=phy2;
????
????continue;
elseif?phy3>MAX_angle
????erropost=erropost+1;??
????EROG（erropost1）=G1;
????EROG（erropost