[转帖]多体动力学源程序

christy

[转帖]多体动力学源程序E.J.Haug <<机械系统的计算机辅助运动学和动力学>> 一书中的例题及习题主要用DADS完成。前段时间我学习该书，苦于没有DADS，所以只得自己硬着头皮尝试自己做程序。平面运动学及动力学部分已初步完成，正在整理，并进行空间运动学及动力学部分学习。现摘录部分源程序[Matlab]如下：
function [t , q , qdot , qdotdot] = SolverKine(FunCstEq , tspan ,q0 ,SolverOpt)
% $ 运动学求解 $
% [q,q_dot,q_dotdot]=SolverKine(FUN,TSPAN,X0,OPTIONS)
%
% 注意：求解前应当作装配分析，以选取适当的初值
% 参考书目 ：
% Book_A <<机械系统的计算机辅助运动学和动力学>> Version 1 E.J.Haug
% Book_B <<计算多体系统动力学>> Version 1 洪嘉振
% Book_C <<数值计算方法>> Version 2 徐涛
%
% Supported by  sunshengli@sohu.com,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_B:P212 较好。如下：
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@sohu.com,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;

[ 本帖最后由 ChaChing 于 2010-7-25 23:08 编辑 ]

superliu

真是急中生智哦