多体动力学源程序[matlab]

多体动力学源程序[Matlab]
E.J.Haug <<机械系统的计算机辅助运动学和动力学>> 一书中的例题及习题主要用DADS完成。前段时间我学习该书，苦于没有DADS，所以只得自己硬着头皮尝试自己做程序。平面运动学及动力学部分已初步完成，正在整理，并进行空间运动学及动力学部分学习。现摘录部分源程序[Matlab]如下,欢迎各位不吝指教。

function [tout , yout] = SolverDyna(odefun , tspan , y0 ,SolverOpt)
%
% $ 动力学求解器 $
% [q,q_dot,q_dotdot]=SolverDyna(FUN,TSPAN,X0,OPTIONS)
%
% 注意：求解前应当作装配分析，以选取适当的初值
% 参考书目 ：
% Book_A <<机械系统的计算机辅助运动学和动力学>> Version 1 E.J.Haug
% Book_B <<计算多提系统动力学>> Version 1 洪嘉振
% Book_C <<数值计算方法>> Version 2 徐涛
%
% Supported by
% sunshengli@https://www.doczj.com/doc/093573739.html,,2005-10-31
%
% tspan 行向量初值
% y0 列向量初值
% 输出为行向量形式

% RK4
tout=[tspan];
for n=1:length(tspan)
n
t=tspan(n);
if n==1
yout=y0';
continue;
end
hh=tspan(n)-tspan(n-1);
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);
y0=y0+(k1+2*k2+2*k3+k4)/6;
yout=[yout;y0'];

% y0 is qdotdot
% 求解逆向动力学

end

function [tout,yout] = odeAdamsPC(odefun,tspan,y0,SolverOpt)
%
% [TOUT,YOUT] = ODEADAMSPC(ODEFUN,TSPAN,Y0) , 4th order Adams
% Predictor-Corrector Method
%
% 参考书目
% Book_A <<数值方法（MATLAB版）>> Version 3 John H.Mathews
% Book_B <<数值计算方法>> Version 2 徐涛
%
% Supported by
% sunshengli@https://www.doczj.com/doc/093573739.html,,2005-11-01
%

tout=[tspan];

% RK4起步
if length(tspan)<=4
[tstart,ystart] = odeRK4(odefun,tspan,y0);
tout=[tstart];
else
[tstart,ystart] = odeRK4(odefun,tspan(1:4),y0);
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);
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)];
% 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,q_dot,q_dotdot]=SolverKine(FUN,TSPAN,X0,OPTIONS)
%
% 注意：求解前应当作装配分析，以选取适当的初值
% 参考书目 ：
% Boo

k_A <<机械系统的计算机辅助运动学和动力学>> Version 1 E.J.Haug
% Book_B <<计算多体系统动力学>> Version 1 洪嘉振
% Book_C <<数值计算方法>> Version 2 徐涛
%
% Supported by
% sunshengli@https://www.doczj.com/doc/093573739.html,,2005-10-31
%

q = [];
qdot = [];
qdotdot = [];

% ## Solver ： 时间迭代求解（非线性）运动学方程 ##
for m=1:length(tspan)
m
t(m) = tspan(m);
TrigerTimer(t(m)); % 保持系统时间同步更新

if m == 1
% ## 初始化 ##
[f,fq,v] = feval(FunCstEq , 1 , q0 ,[] ); % 注意:Iswitch==1求解位姿及速度，不需要速度信息，可将q_dot置空
qdot0 = fq\v; % 求解速度
[f,fq,v,y] = feval(FunCstEq , 2 , q0 ,qdot0 ); % 求加速度右项时用到速度信息
qdotdot0 = fq\y; % 求解加速度
q1 = q0; qdot1 = qdot0; qdotdot1 = qdotdot0; % 返回结果
else
% ## 牛顿-拉夫森法求解非线性代数方程 ##
[q1,qdot1,qdotdot1] = SolverNR(FunCstEq , q0 , SolverOpt);

% 确定下一步迭代初值
% 方法一 ： q0 = q1;
% 方法二 Book_B212 较好。如下：
dt = t(m) - t(m-1);
q0 = q1 + dt*qdot1 + 1/2*dt^2*qdotdot1;
end

q = [q,q1];
qdot = [qdot,qdot1];
qdotdot = [qdotdot,qdotdot1];
end

function [q1,qdot1,qdotdot1] = SolverNR(FunCstEq , q0 , SolverOpt)
%
% $ Newton-Raphson法求解非线性代数方程 $
% [q,q_dot,q_dotdot]=SOLVERNR(FUN,X0,OPTIONS)
%
% 参考书目 ：
% Book_A <<机械系统的计算机辅助运动学和动力学>> Version 1 E.J.Haug
% Book_B <<计算多提系统动力学>> Version 1 洪嘉振
% Book_C <<数值计算方法>> Version 2 徐涛
%
% Supported by
% sunshengli@https://www.doczj.com/doc/093573739.html,,2005-10-31
%

% % ##for test##
% qArray = [];
% fqErrArray = [];
% hErrArray = [];

% 设置判敛控制参数
epsilon_e = SolverOpt.epsilon_e;
epsilon_s = SolverOpt.epsilon_s;

% 求解位姿
while(1)
q = q0;

% 给定构型初值，计算约束方程及Jacobian
[f,fq,v] = feval(FunCstEq , 1 , q ,[] ); % 注意:Iswitch==1求解位姿及速度，不需要速度信息，可将q_dot置空

% 计算步长。参见Book_C ：P277 ，Book_A ：P100
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) )
q;break;
end

q0 = q+h; % 确定下一点

drawnow;

% % ##for test##
% qArray = [qArray,q]
% fqErrArray = [fqErrArray,norm(fq)]
% hErrArray = [hErrArray,norm(h)]
end
q1 = q; % 位姿

% 求解速度
qdot1 = fq\v;

% 求解加速度
[f,fq,v,y] = feval(FunCstEq , 2 , q1 ,qdot1 ); % 注意：这里求加速度右项时用到速度信息
qdotdot1 = fq\y;