多体动力学源程序[matlab]

m
t(m) = tspan(m);
TrigerTimer(t(m)); % 保持系统时间同步更新
if m == 1
% ## 初始化 ##
% Book_B <<计算多体系统动力学>> Version 1 洪嘉振
% Book_C <<数值计算方法>> Version 2 徐涛
% Corrector
y0 = y0 + h/24*(9*F(:,4) + 19*F(:,3) - 5*F(:,2) + 1*F(:,1));
F(:,4) = feval(odefun,t,y0);
yout=[yout,y0];
end
function [t , q , qdot , qdotdot] = SolverKine(FunCstEq , tspan ,q0 ,SolverOpt)
q;break;
ቤተ መጻሕፍቲ ባይዱend
q0 = q+h; % 确定下一点
drawnow;
% % ##for test##
% qArray = [qArray,q]
% fqErrArray = [fqErrArray,norm(fq)]
% 方法二 Book_B212 较好。如下：
dt = t(m) - t(m-1);
q0 = q1 + dt*qdot1 + 1/2*dt^2*qdotdot1;
end
q = [q,q1];
qdot = [qdot,qdot1];
% 参考书目 ：
% Book_A <<机械系统的计算机辅助运动学和动力学>> Version 1 E.J.Haug
% Book_B <<计算多提系统动力学>> Version 1 洪嘉振
function [tout , yout] = SolverDyna(odefun , tspan , y0 ,SolverOpt)
%
% $ 动力学求解器 $
% [q,q_dot,q_dotdot]=SolverDyna(FUN,TSPAN,X0,OPTIONS)
%
% 注意：求解前应当作装配分析，以选取适当的初值
% Book_B <<数值计算方法>> Version 2 徐涛
%
% Supported by
% sunshengli@,2005-11-01
%
tout=[tspan];
% RK4起步
F = feval( odefun,tspan(n-4:n-1),yout(:,n-4:n-1) );
% Predictor
p = y0 + h/24*(55*F(:,4) - 59*F(:,3) + 37*F(:,2) - 9*F(:,1));
F = [F(:,2) F(:,3) F(:,4) feval(odefun,t,p)];
q = q0;
% 给定构型初值，计算约束方程及Jacobian
[f,fq,v] = feval(FunCstEq , 1 , q ,[] ); % 注意:Iswitch==1求解位姿及速度，不需要速度信息，可将q_dot置空
% 计算步长。参见Book_C ：P277 ，Book_A ：P100
% qArray = [];
% fqErrArray = [];
% hErrArray = [];
% 设置判敛控制参数
epsilon_e = SolverOpt.epsilon_e;
epsilon_s = SolverOpt.epsilon_s;
% 求解位姿
while(1)
%
% 参考书目 ：
% Book_A <<机械系统的计算机辅助运动学和动力学>> Version 1 E.J.Haug
% Book_B <<计算多提系统动力学>> Version 1 洪嘉振
[f,fq,v] = feval(FunCstEq , 1 , q0 ,[] ); % 注意:Iswitch==1求解位姿及速度，不需要速度信息，可将q_dot置空
qdot0 = fq\v; % 求解速度
%
% Supported by
% sunshengli@,2005-10-31
%
q = [];
qdot = [];
qdotdot = [];
% ## Solver ： 时间迭代求解（非线性）运动学方程 ##
for m=1:length(tspan)
多体动力学源程序[Matlab]
E.J.Haug <<机械系统的计算机辅助运动学和动力学>> 一书中的例题及习题主要用DADS完成。前段时间我学习该书，苦于没有DADS，所以只得自己硬着头皮尝试自己做程序。平面运动学及动力学部分已初步完成，正在整理，并进行空间运动学及动力学部分学习。现摘录部分源程序[Matlab]如下,欢迎各位不吝指教。
k1=hh*feval(odefun,t,y0);
k2=hh*feval(odefun ,t+hh/2,y0+0.5*k1);
k3=hh*feval(odefun ,t+hh/2,y0+0.5*k2);
k4=hh*feval(odefun ,t+hh,y0+k3);
% Book_C <<数值计算方法>> Version 2 徐涛
%
% Supported by
% sunshengli@,2005-10-31
%
% % ##for test##
%
% $ 运动学求解 $
% [q,q_dot,q_dotdot]=SolverKine(FUN,TSPAN,X0,OPTIONS)
%
% 注意：求解前应当作装配分析，以选取适当的初值
% 参考书目 ：
% Book_A <<机械系统的计算机辅助运动学和动力学>> Version 1 E.J.Haug
y0=y0+(k1+2*k2+2*k3+k4)/6;
yout=[yout;y0'];
% y0 is qdotdot
% 求解逆向动力学
end
function [tout,yout] = odeAdamsPC(odefun,tspan,y0,SolverOpt)
qdotdot = [qdotdot,qdotdot1];
end
function [q1,qdot1,qdotdot1] = SolverNR(FunCstEq , q0 , SolverOpt)
%
% $ Newton-Raphson法求解非线性代数方程 $
% [q,q_dot,q_dotdot]=SOLVERNR(FUN,X0,OPTIONS)
if length(tspan)<=4
[tstart,ystart] = odeRK4(odefun,tspan,y0);
tout=[tstart];
else
[tstart,ystart] = odeRK4(odefun,tspan(1:4),y0);
h = -fq\f;
% 判敛条件：||PHIq(q,t)||<epsilon_e 或者 ||qk1-qk||<epsilon_s
% if ((norm(fq)<epsilon_e)| (norm(h)<epsilon_s) )
if ((norm(f)<epsilon_e)| (norm(h)<epsilon_s) )
qdotdot1 = fq\y;�
end
yout = [ystart];
% 4th order Adams Predictor-Correctot Methods
y0 = ystart(:,4);
for n=5:length(tspan)
t = tspan(n);
h = tspan(n)-tspan(n-1);
% Book_C <<数值计算方法>> Version 2 徐涛
%
% Supported by
% sunshengli@,2005-10-31
%
% tspan 行向量初值
[f,fq,v,y] = feval(FunCstEq , 2 , q0 ,qdot0 ); % 求加速度右项时用到速度信息
qdotdot0 = fq\y; % 求解加速度
q1 = q0; qdot1 = qdot0; qdotdot1 = qdotdot0; % 返回结果
% y0 列向量初值
% 输出为行向量形式
% RK4
tout=[tspan];
for n=1:length(tspan)
n
t=tspan(n);