声振论坛

 找回密码
 我要加入

QQ登录

只需一步,快速开始

查看: 1837|回复: 1

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

[复制链接]
发表于 2005-11-14 07:23 | 显示全部楼层 |阅读模式

马上注册,结交更多好友,享用更多功能,让你轻松玩转社区。

您需要 登录 才可以下载或查看,没有账号?我要加入

x
[转帖]多体动力学源程序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 编辑 ]

评分

1

查看全部评分

回复
分享到:

使用道具 举报

发表于 2005-11-26 15:10 | 显示全部楼层
真是急中生智哦
您需要登录后才可以回帖 登录 | 我要加入

本版积分规则

QQ|小黑屋|Archiver|手机版|联系我们|声振论坛

GMT+8, 2024-11-14 10:31 , Processed in 0.075215 second(s), 21 queries , Gzip On.

Powered by Discuz! X3.4

Copyright © 2001-2021, Tencent Cloud.

快速回复 返回顶部 返回列表