多体动力学源程序[Matlab]

keybox

多体动力学源程序[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@sohu.com,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@sohu.com,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)
%
%     注意：求解前应当作装配分析，以选取适当的初值
% 参考书目 ：
%       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_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@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;

406676998

你太牛了,佩服

zhaowu

很好

zhaowu

很好

ruoti

请问楼主,有没有求解动力方程的中心差分法的程序啊,先谢了哈

linuaries

多谢楼主分享

xdmonkey

感谢楼主

keybox

交流一下。
好久没有做这个了，还是想把它做完……
很希望有人合作……

keybox

做过这类微分代数方程组求解的，交流一下。
好久没有做这个了，还是想把它做完……
很希望有人合作……

heeric

这个要顶 lz不错哈

liangrc

强人，学习了

leoliang

强人啊！赞一个！不得不！

chaoren360

我现在也做多体动力学的仿真，有问题还得多向牛人请教啊