并联机器人动力学matlab求解

% ----------------------------------------
% ---delta机器人 位置逆解程序 走门式轨迹
% ----------------------------------------


ee=0.125;   % ee: 静平台与动平台的半径差值
l1=0.375;    % l1 大臂长度
l2=0.95;    % l2 小臂长度
HH=0.78;    % HH: 从静平台原点到工作空间顶部的高度差，
hh=0.25;    % hh：工作空间底部到顶部的高度差 
h=0.25; 
b=0.500; 
n=199;  %假设插补点的数目为600，后面暂时用599+1
Z=[0;0;1];
g=10;
p1=3.86 ;  %杆1的密度
p2=0.105 ;  %杆2的密度
mb=0.282;
md=0.986;  %动平台质量
me=p2*l2;  %从动臂等效质量
IA1=0.000998;
IA2=p1*l1^3/12;
IA3=mb*l1^2;
IA4=2*p2*l2*l1^2/3;
IA=IA1+IA2+IA3+IA4;
m=md+me;
mA=p1*l1+mb;
rA=l1*((0.5*p1*l1+mb)/(p1*l1+mb));
% 假设水平方向最大加速度是垂直方向最大加速度的两倍
amaxxy=100; amaxz=amaxxy/2;

%=========================================================
% Part 1 计算轨迹点
%=========================================================

% 加速度足够大，
T1=sqrt(5.7735*(h)/amaxz);
T2=sqrt(5.7735*(b)/amaxxy);


Ttotal=T1+T2;
ax=[];ay=[];az=[];vx=[];vy=[];vz=[];rx=[];ry=[];rz=[];
dt=Ttotal./n;
ax=[];ay=[];az=[];vx=[];vy=[];vz=[];rx=[];ry=[];rz=[];

for k=1:n+1
    tt=(k-1).*dt;
    % 计算Z方向的采样点坐标
    if tt>=0&&tt<=T1
        t=tt;T=T1;amax=amaxz;
        [a,v,s1]=motionrule1(amax,t,T);
        a_z=a;v_z=v;r_z=s1-(HH+hh/2);
    elseif tt>=T2&&tt<=T1+T2
        t=tt-T2;T=T1;amax=amaxz;
        [a,v,s1]=motionrule1(amax,t,T);
        a_z=-a;v_z=-v;r_z=-s1-(HH+hh/2-h);
    else
        a_z=0;v_z=0;r_z=-(HH+hh/2-h);
    end