motionstation.m

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

% 获得系统当前状态
function MotionStation

% 声明全局变量
GlobalVariables

ii=0;nm=0;iv=0;jj=0;
for i=1:nBodies
	if joint(i).trfree~=0
		iv=iv+1;
		joint(i).ri(1:3)=0;
		for j=1:joint(i).trfree
			ii=ii+1;
			joint(i).z(j)=wch(ii);
			joint(i).ri(j)=rch1(iv).a(j);
		end
		joint(i).vv=joint(i).z(1)*joint(i).kk1+joint(i).z(2)*joint(i).kk2+joint(i).z(3)*joint(i).kk3;
	end
	if joint(i).rofree==1 % 方向余弦矩阵，转轴方向，转动速度大小
        ii=ii+1;    jj=jj+1;        nm=nm+1;
        body(i).translate=rch2(nm).a;
		joint(i).q(1)=wch(ii);
        joint(i).pp1=zhzh(jj).a;	
		joint(i).ww=joint(i).pp1*joint(i).q(1)*body(i).translate1'; 
	elseif joint(i).rofree==2
        joint(i).pp1=zhzh(jj+1).a;        joint(i).pp2=zhzh(jj+2).a;
        nm=nm+1;    jj=jj+2;
        body(i).translate=rch2(nm).a;
		joint(i).q(1:2)=wch(ii+(1:2));		
		joint(i).ww=(joint(i).pp1*joint(i).q(1)+joint(i).pp2*joint(i).q(2))*body(i).translate1';        
        ii=ii+2;
	elseif joint(i).rofree==3
        joint(i).pp1=zhzh(jj+1).a;		  joint(i).pp2=zhzh(jj+2).a;       joint(i).pp3=zhzh(jj+3).a;        
        nm=nm+1;    jj=jj+3;
		body(i).translate=rch2(nm).a;
		joint(i).q(1:3)=wch(ii+(1:3));		
		joint(i).ww=(joint(i).pp1*joint(i).q(1)+joint(i).pp2*joint(i).q(2)+joint(i).pp3*joint(i).q(3))*body(i).translate1';           
        ii=ii+3;
    end
end