close?all
clc
lui=0.378;?%質(zhì)心到動(dòng)平臺(tái)胡克鉸ui的距離??
lsi=0.452;?%質(zhì)心到動(dòng)平臺(tái)鉸鏈si的距離
mui=26.15;%擺動(dòng)桿的質(zhì)量??
msi=8.45;?%伸縮桿的質(zhì)量
mB=36.28;%并聯(lián)機(jī)構(gòu)動(dòng)平臺(tái)的質(zhì)量?
mdj=0;%各種機(jī)床參數(shù)賦值（mdj為刀頭質(zhì)量）

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

a=0.025;%動(dòng)平臺(tái)運(yùn)動(dòng)軌跡中的給定參數(shù)
syms?t?f1x?f1z?f2x?f3x?f4x?f5x??%syms函數(shù)：創(chuàng)建符號(hào)變量的捷徑
tt=0:0.01:4;
for?j=1:length（tt）
????t=tt（j）
ms1=0;ms2=0;%對于FEDi、FEmi的疊加運(yùn)算賦初值?
%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;%對歐拉角求一階導(dǎo)數(shù)
%aaq11=（pi/6）*t;bbq11=0;ccq11=0;%對歐拉角求二階導(dǎo)數(shù)
%aaq1=0;bbq1=0;ccq1=-pi/2;%對歐拉角求一階導(dǎo)數(shù)
%aaq11=0;bbq11=0;ccq11=0;%對歐拉角求二階導(dǎo)數(shù)
%aaq1=-pi/18;bbq1=0;ccq1=0;%對歐拉角求一階導(dǎo)數(shù)
aaq1=0;bbq1=0;ccq1=0;%對歐拉角求一階導(dǎo)數(shù)
aaq11=0;bbq11=0;ccq11=0;%對歐拉角求二階導(dǎo)數(shù)
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;?%歐拉轉(zhuǎn)換矩陣（B相對于A的）?3*3
N=inv（ABR）;%歐拉轉(zhuǎn)換矩陣（A相對于B的）后面有用?3*3
X=0.90+0.025*cos（2*t）;%動(dòng)系坐標(biāo)原點(diǎn)在靜坐標(biāo)系下的坐標(biāo)賦值??式40
Y=-0.10+0.025*sin（2*t）;
Z=0.202*cos（mb）*cos（mc）;%動(dòng)系坐標(biāo)原點(diǎn)在靜坐標(biāo)系下的坐標(biāo)賦值?
P=[X;Y;Z];?%3*1
%AaVpB=ABR*[-0.120*cos（2*t）;-0.120*sin（2*t）;0]%動(dòng)平臺(tái)在靜坐標(biāo)下的加速度
%求基本量mni（沿著桿長方向的單位向量）5個(gè)桿的，RPS也要用到
ma1=[0;W;0];???????????%定平臺(tái)上5個(gè)胡克鉸在靜坐標(biāo)系下的坐標(biāo)
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）;?%定平臺(tái)上5個(gè)胡克鉸在靜坐標(biāo)系A(chǔ)下的模
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）;??????????%定平臺(tái)上5個(gè)胡克鉸在動(dòng)坐標(biāo)系下的坐標(biāo)
mA2=N*（ma2-P）;
mA3=N*（ma3-P）;
mA4=N*（ma4-P）;
mA5=N*（ma5-P）;??????????%N=inv（ABR）歐拉轉(zhuǎn)換矩陣（A相對于B的）?3*1

mb1=[0;0;-rB];????????%動(dòng)平臺(tái)上5個(gè)鉸在動(dòng)坐標(biāo)系下的坐標(biāo)
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個(gè)桿的長度
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個(gè)桿的單位方向矢量（在動(dòng)系B中）=動(dòng)平臺(tái)上五個(gè)鉸在動(dòng)平臺(tái)下的坐標(biāo)mb1-定平臺(tái)上五個(gè)胡克鉸在動(dòng)坐標(biāo)系下的坐標(biāo)mA1的差值除以五個(gè)桿的長度mL1
mn2=（mb2-mA2）/mL2;
mn3=（mb3-mA3）/mL3;
mn4=（mb4-mA4）/mL4;
mn5=（mb5-mA5）/mL5;??%3*1?

An1=ABR*mn1;?%5個(gè)桿的單位方向矢量（在定系A(chǔ)中）
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