initvelocity.m

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

% 仅赋各速度的初始值
function InitVelocity

% 声明全局变量
GlobalVariables

% nv 是有平动的物体个数，nm是有转动的物体个数
nv=0;
nm=0;
im=0;
l=0;   
for i=1:nBodies
	if joint(i).index==4
		nv=nv+1;
        l=l+1;
		wch(l)=joint(i).z(1);
		rch1(nv).a(1)=joint(i).ri(1);
		joint(i).vv(1:3)=0;         % 有必要吗？
		joint(i).vv=joint(i).vv+joint(i).kk1*joint(i).z(1)+joint(i).kk2*joint(i).z(2)+joint(i).kk3*joint(i).z(3);
    elseif joint(i).index==6
		nv=nv+1;
        l=l+3;
		wch(l+(1:3))=joint(i).z(1:3);
		rch1(nv).a(1:3)=joint(i).ri(1:3);
		joint(i).vv(1:3)=0;
		joint(i).vv=joint(i).vv+joint(i).kk1*joint(i).z(1)+joint(i).kk2*joint(i).z(2)+joint(i).kk3*joint(i).z(3);
	end
	if joint(i).rofree~=0
		im=im+1;
		wch(l+(1:joint(i).rofree))=joint(i).q(1:joint(i).rofree);
		l=l+joint(i).rofree;
		% wi 需要先赋值，为增量
        [rch2(im).a,joint(i).pp1,joint(i).pp2,joint(i).pp3,p4]=RotateC(joint(i).m(1),joint(i).m(2),joint(i).m(3),joint(i).wi(1),joint(i).wi(2),joint(i).wi(3));
    end
end
ii=0;
for i=1:nBodies
	if joint(i).rofree==1
		ii=ii+1;
		zhzh(ii).a=joint(i).pp1;
	elseif joint(i).rofree==2   
		zhzh(ii+1).a=joint(i).pp1;
		zhzh(ii+2).a=joint(i).pp2;
        ii=ii+2;     
    elseif joint(i).rofree==3
		zhzh(ii+1).a=joint(i).pp1;
		zhzh(ii+2).a=joint(i).pp2;
		zhzh(ii+3).a=joint(i).pp3;
   		ii=ii+3;        
    end
end
'☆ 仅赋各速度的初始值程序 InitVelocity.m 运行结束 √'