readoriginsdata.m

用于计算多体系统动力学的程序,可用于多体系统动力学的计算和仿镇,也可用于机器人的设计和计算

% 读入数据结构，即给定多体系统信息
function ReadOriginsData(fname)

% 声明全局变量
GlobalVariables
rfid = fopen(fname,'r');
% 物体个数 0号物体的位置
[nBodies,origin(1:3)]= RFile(rfid,2,[1,3]);

% 读入有关物体的性质
for i=1:nBodies      
    % 物体序号
    body(i).index=RFile(rfid,1,1);  
    index=body(i).index;
    % 初始化新物体、连接铰的性质
    NewBodyJoint;           
  
    % 读入新物体相对内接坐标系的初始方位
    [body(index).x(1),body(index).ch(1),body(index).x(2),body(index).ch(2),body(index).x(3),body(index).ch(3)]=RFile(rfid,6,[1,1,1,1,1,1]); 
    
    % 局部坐标系的方位，形成初始变换矩阵,值用角度表示,相对自身原始位置的变换矩阵？ 
    if body(index).x(1)~=0  
		x1=body(index).ch(1)*(pi/180);      
        x2=body(index).ch(2)*(pi/180);		
        x3=body(index).ch(3)*(pi/180);
		[body(i).translate1,p1,p2,p3,p4]=RotateC(body(index).x(1),body(index).x(2),body(index).x(3),x1,x2,x3);
    else 
		body(i).translate1=diag(ones(3,1),0);  
	end
    
	% 物体的转动惯量
	[body(index).ii(1,1:3),body(index).ii(2,1:3),body(index).ii(3,1:3)]=RFile(rfid,3,[3,3,3]);   
    % 物体的质量，质心可能没用。物体的质心在局部坐标系下的位置
	[body(index).mm,body(index).mpoint1(1:3)]=RFile(rfid,2,[1,3]);   
    % 外接物体号
	body(index).outnumber=RFile(rfid,1,1);     
    % 对该物体输入外力、外力矩、内力、内力矩
	[body(index).gf(1:3),body(index).mf(1:3),body(index).fi(1:3),body(index).mi(1:3)]=RFile(rfid,4,[3,3,3,3]);   
	
	% 读入连接该物体的铰的有关性质
    % 铰的标号
	joint(i).number=RFile(rfid,1,1);			 
	index=joint(i).number;
    
 	% 施于该连接铰的铰力和铰力矩 
	[joint(index).fa(1:3),joint(index).ma(1:3)]=RFile(rfid,2,[3,3]);
    % 该铰的内接物体号,铰在内接物体的位置,铰在外接物体的位置
    [joint(index).inbody,joint(index).inpo(1:3),joint(index).outpo(1:3)]=RFile(rfid,3,[1,3,3]);
    
  	% 铰的性质号1-2-3-转动,4-5-6平动
	joint(index).index=RFile(rfid,1,1);			  
	if joint(index).index==1
        % wi()是此刻的方向，m(i)为转轴的方向数，1-x轴，2-y轴，3-z轴,q(i)为转速大小
        joint(index).rofree=1;
        [joint(index).wi(1:3),joint(index).m(1),joint(index).q(1)]=RFile(rfid,3,[3,1,1]);
    elseif joint(index).index==2
        joint(index).rofree=2;
        [joint(index).wi(1:3),joint(index).m(1),joint(index).q(1),joint(index).m(2),joint(index).q(2)]=RFile(rfid,5,[3,1,1,1,1]);       
	elseif joint(index).index==3
        joint(index).rofree=3;
        [joint(index).wi(1:3),joint(index).m(1),joint(index).q(1),joint(index).m(2),joint(index).q(2),joint(index).m(3),joint(index).q(3)]=RFile(rfid,7,[3,1,1,1,1,1,1]);		
    elseif joint(index).index==4
        % ri()是此刻的位置，速度大小,速度方向在其内接物体坐标系下表示
		joint(index).trfree=1;
        [joint(index).ri(1:3),joint(index).z(1),joint(index).kk1(1:3)]=RFile(rfid,3,[3,1,3]);
    elseif joint(index).index==6
		joint(index).trfree=3;
		joint(index).rofree=3;
        [joint(index).ri(1:3),joint(index).wi(1:3),joint(index).z(1),joint(index).kk1(1:3),joint(index).z(2),joint(index).kk2(1:3),joint(index).z(3),joint(index).kk3(1:3),...
         joint(index).m(1),joint(index).q(1),joint(index).m(2),joint(index).q(2),joint(index).m(3),joint(index).q(3)]=RFile(rfid,14,[3,3,1,3,1,3,1,3,1,1,1,1,1,1]);
	end
end % 读入有关物体的性质结束

% 此时刻即初始时刻铰的角度弧度化
for i=1:nBodies
	joint(i).wi=joint(i).wi*pi/180;
end

% 计算系统的自由度
allFreeNum=0;       % 总自由度数
rotFreeNum=0;       % 旋转自由度
transJointsNum=0;   % 平动铰个数
rotJointsNum=0;     % 旋转铰个数
for i=1:nBodies
	allFreeNum=allFreeNum+joint(i).trfree+joint(i).rofree;
	if joint(i).trfree~=0
		transJointsNum=transJointsNum+1;
	end
   	rotFreeNum=rotFreeNum+joint(i).rofree;
	if joint(i).rofree~=0
		rotJointsNum=rotJointsNum+1;
    end
end

% 如果有力元 ,1-有力元,0-没有力元
nef=RFile(rfid,1,1);
ef=0;
if nef~=0
	for i=1:nef
        % 力的大小 内接和外接物体的个数 内接物体的位置 外接物体的位置
        [ef(i).f(1:3),ef(i).ek,ef(i).eek,ef(i).cek(1:3),ef(i).ceek(1:3)]=RFile(rfid,5,[3,1,3,3,3]); 
	end
end

% 外力力矩和力矩大小
% 外力的个数
nfor=RFile(rfid,1,1);
if nfor~=0
	for i=1:nfor
        [for1(i).index,for1(i).dx(1:3),for1(i).po(1:3)]=RFile(rfid,3,[1,3,3]);
		kk1=for1(i).index;
		body(kk1).gf=body(kk1).gf+for1(i).dx;
	end
end

% pos记录b0到bi物体的路上所有元素,pos(1)...pos(n)记录最后一元素在数组中的位置,其后为各物体序号.
% po记录第i物体在总体矩阵ttoal的的位置 % 如1-2个平动1个转动,2-3个平动2个转动,则po(1)=3,po(2)=8 % po(i) 记录第i物体自由度的最末位置 ?
po(nBodies)=0;		
l=0;
for i=1:nBodies
	l=joint(i).trfree+joint(i).rofree+l;
    po(i)=l;
end
fclose(rfid);
'☆ 读入数据结构，即给定多体系统信息程序 ReadOriginsData.m 运行结束 √'