close?all
clc
lui=0.378;?%質心到動平臺胡克鉸ui的距離??
lsi=0.452;?%質心到動平臺鉸鏈si的距離
mui=26.15;%擺動桿的質量??
msi=8.45;?%伸縮桿的質量
mB=36.28;%并聯機構動平臺的質量?
mdj=0;%各種機床參數賦值（mdj為刀頭質量）

h=0;?
rA=0.645;%定平臺上四個虎克鉸u1u2u3u4均勻布置于半徑645mm的圓上
rB=0.202;%動平臺上五個鉸鏈s1s2s3s4s5均勻布置在本經202mm的圓上
mq=pi/4;%定平臺上四個胡克鉸的間隔角度
Q=2*pi/5;%動平臺上五個鉸鏈的間隔角度
W=0.71707;%球副S中心位于半徑717.07mm處
g=[9.800].‘;%重力取值?%鉸節點位置參數賦值，1.1支鏈坐標系的建立

a=0.025;%動平臺運動軌跡中的給定參數
syms?t?f1x?f1z?f2x?f3x?f4x?f5x??%syms函數：創建符號變量的捷徑
tt=0:0.01:4;
for?j=1:length（tt）
????t=tt（j）
ms1=0;ms2=0;%對于FEDi、FEmi的疊加運算賦初值?
%ma=（pi/36）*t*t*t;mb=0;mc=-pi/2;%歐拉角賦值?
%ma=（pi/72）*t;mb=（pi/72）*t;mc=-pi/2;%歐拉角賦值
%ma=0;mb=0;mc=（-pi/2）*t;%歐拉角賦值
ma=0;mb=0;mc=-pi/2;%歐拉角賦值
%ma=-（pi/18）*t;mb=0;mc=-pi/2;%歐拉角賦值
%aaq1=（pi/12）*t*t;bbq1=0;ccq1=0;%對歐拉角求一階導數
%aaq11=（pi/6）*t;bbq11=0;ccq11=0;%對歐拉角求二階導數
%aaq1=0;bbq1=0;ccq1=-pi/2;%對歐拉角求一階導數
%aaq11=0;bbq11=0;ccq11=0;%對歐拉角求二階導數
%aaq1=-pi/18;bbq1=0;ccq1=0;%對歐拉角求一階導數
aaq1=0;bbq1=0;ccq1=0;%對歐拉角求一階導數
aaq11=0;bbq11=0;ccq11=0;%對歐拉角求二階導數
A=[cos（ma）?-sin（ma）?0;sin（ma）?cos（ma）?0;0?0?1];
B=[cos（mb）?0?sin（mb）;0?1?0;-sin（mb）?0?cos（mb）];????
C=[1?0?0;0?cos（mc）?-sin（mc）;0?sin（mc）?cos（mc）];
ABR=A*B*C;?%歐拉轉換矩陣（B相對于A的）?3*3
N=inv（ABR）;%歐拉轉換矩陣（A相對于B的）后面有用?3*3
X=0.90+0.025*cos（2*t）;%動系坐標原點在靜坐標系下的坐標賦值??式40
Y=-0.10+0.025*sin（2*t）;
Z=0.202*cos（mb）*cos（mc）;%動系坐標原點在靜坐標系下的坐標賦值?
P=[X;Y;Z];?%3*1
%AaVpB=ABR*[-0.120*cos（2*t）;-0.120*sin（2*t）;0]%動平臺在靜坐標下的加速度
%求基本量mni（沿著桿長方向的單位向量）5個桿的，RPS也要用到
ma1=[0;W;0];???????????%定平臺上5個胡克鉸在靜坐標系下的坐標
ma2=[0;rA*cos（mq）;rA*sin（mq）];
ma3=[0;rA*cos（3*mq）;rA*sin（3*mq）];
ma4=[0;rA*cos（5*mq）;rA*sin（5*mq）];
ma5=[0;rA*cos（7*mq）;rA*sin（7*mq）];??%3*1

lApu1=[ma1（21）.^2].^（1/2）;?%定平臺上5個胡克鉸在靜坐標系A下的模
lApu2=[ma2（31）.^2+ma2（21）.^2].^（1/2）;
lApu3=[ma3（31）.^2+ma3（21）.^2].^（1/2）;
lApu4=[ma4（31）.^2+ma4（21）.^2].^（1/2）;
lApu5=[ma5（31）.^2+ma5（21）.^2].^（1/2）;??%1*1

mA1=N*（ma1-P）;??????????%定平臺上5個胡克鉸在動坐標系下的坐標
mA2=N*（ma2-P）;
mA3=N*（ma3-P）;
mA4=N*（ma4-P）;
mA5=N*（ma5-P）;??????????%N=inv（ABR）歐拉轉換矩陣（A相對于B的）?3*1

mb1=[0;0;-rB];????????%動平臺上5個鉸在動坐標系下的坐標
mb2=[0;rB*sin（Q）;-rB*cos（Q）];
mb3=[0;rB*sin（2*Q）;-rB*cos（2*Q）];
mb4=[0;rB*sin（3*Q）;-rB*cos（3*Q）];
mb5=[0;rB*sin（4*Q）;-rB*cos（4*Q）];??%3*1

mL1=[（0-mA1（11））^2+（0-mA1（21））^2+（-rB-mA1（31））^2].^（1/2）;%5個桿的長度
mL2=[（0-mA2（11））^2+（rB*sin（Q）-mA2（21））^2+（-rB*cos（Q）-mA2（31））^2].^（1/2）;
mL3=[（0-mA3（11））^2+（rB*sin（2*Q）-mA3（21））^2+（-rB*cos（2*Q）-mA3（31））^2].^（1/2）;
mL4=[（0-mA4（11））^2+（rB*sin（3*Q）-mA4（21））^2+（-rB*cos（3*Q）-mA4（31））^2].^（1/2）;
mL5=[（0-mA5（11））^2+（rB*sin（4*Q）-mA5（21））^2+（-rB*cos（4*Q）-mA5（31））^2].^（1/2）;??%1*1

mn1=（mb1-mA1）/mL1;??????????%5個桿的單位方向矢量（在動系B中）=動平臺上五個鉸在動平臺下的坐標mb1-定平臺上五個胡克鉸在動坐標系下的坐標mA1的差值除以五個桿的長度mL1
mn2=（mb2-mA2）/mL2;
mn3=（mb3-mA3）/mL3;
mn4=（mb4-mA4）/mL4;
mn5=（mb5-mA5）/mL5;??%3*1?

An1=ABR*mn1;?%5個桿的單位方向矢量（在定系A中）
An2=ABR*mn2;
An3=ABR*mn3;
An4=ABR*mn4;
An5=ABR*mn5;??%3*1??

cs1=An1（11）;?%求桿與定系x軸的夾角的余弦值
cs2=An2（11）;????????????????????
cs3=An3（11）;
cs4=An4（11）;
cs5=An5（11）;??%1*1