%this?program?is?to?calculate?the?transformation?matrix?from?joint?1?to
%joint?6;?and?use?the?cycling?program?to?generate?the?position?data?and
%joint?data;


%this?parameter?is?for?PUMA?560
alf=[pi/2?0?-pi/2?pi/2?-pi/2?0];
a=[0?0.4318?0.0203?0?0?0];
d=[0?0?0.15?0.4318?0?0];
%?this?is?the?Home_Position
%thta=[0?0?0?0?0?0];
%?qz?position?
%thta=[0?0+pi/2?0?0?0+pi?0+pi];
%?qr?position?joint?is?exceed?the?joint?value
%thta=[0?pi/2+pi/2?-pi/2?0?0+pi?0+pi];
%?qs?position?
%thta=[0?0+pi/2?-pi/2?0?0+pi?0+pi];
%?qn?position?
%thta=[0?pi/6+pi/2?-2*pi/3?0?pi/4+pi?0+pi];

%thta=[-pi/2?pi/2-pi/4?-pi/3?-2*pi/3?pi-pi/2?pi-2*pi/3];
thta1=[-pi/2?-pi/3?0?pi/3?pi/2];
????thta2=[-pi?-pi/2?-pi/6?0?pi/6];
???????thta3=[-pi/6?0?pi/4?pi/2?2*pi/3];
????????thta4=[-pi/2?0?pi/4?pi/3?pi/2?2*pi/3];
????????thta5=[-pi/2?-pi/3??pi/6?pi/4?pi/2];
????????thta6=[-pi?-2*pi/3?-pi/2?0?pi/4?pi?4*pi/3];
????????pjoints=[];
????????ppos=[];
????????pjoint1=[];
????????save?‘pumajoints.mat‘?pjoints;
????????save?‘pumaposition.mat‘?ppos;
????????save?‘pumafirst.mat‘?pjoint1;
x=sin（alf）;
x（abs（x）<1e-16）=0;
y=cos（alf）;
y（abs（y）<1e-16）=0;

for?j=2:6
for?k=2:4
????for?l=1:5
????????for?m=2:5
????????????for?n=1:4
????????????????for?s=3:4
????????????????????total=[thta1（s）-0.001?thta2（n）?thta3（l）?thta4（m）?thta5（k）?thta6（j）];
????????????????????%actjoin=total-thta;
%Robotics?Vision?&?Control:?（c）?Peter?Corke?result?后置坐標法建模（標準建模方法std-DH）
%矩陣變換順序為Trans（00di）Rot（zthta）trans（ai00）Rot（xalf）
%運算結果與p560.fkine（q