📄 resolvemotion.m
字号:
% 运动求解程序
function [total,bz]=ResolveMotion
% 声明全局变量
GlobalVariables
for i=1:nBodies
bodyii(i).a(1:3,1:3)=0;
tran1(i).a(1:3,1:3)=0;
joint(i).v(1:3)=0;
joint(i).w(1:3)=0;
end
ppl=0;
flag(1:nBodies)=0;
dtotal(1:3*nBodies*(nBodies+1)) =Ra1a2(0,0); % 维数可能不够大，特乘3
r(1:nBodies*(nBodies+1))=Ra1a2(0,0);
bz(1:allFreeNum)=0;
dd.a1(1:3,1:3)=0; dd.a2(1:3,1:3)=0; dd.a3(1:3,1:3)=0;
for i=1:road(1)
if i==1
begin=road(1)+1; % 每条路的起始位置,0位置
nn=road(2)-road(1)-1; % 每条路物体的个数
else
begin=road(i);
nn=road(i+1)-road(i);
end
for j=begin+1:begin+nn
bnum=road(j); % 现在正处理的物体号
bnum1=iplus(bnum); % 该物体的内接物体
if ~flag(bnum) % 需要作运动学处理
% 向其内接物体局部坐标系转变的变换矩阵
tr=body(bnum).translate1*body(bnum).translate;
if bnum==1
begin1=0;
else
begin1=pos(2*(bnum-1));
end
flag(bnum)=1;
if j==(begin+1) % 假设1号铰的inpo为零
% 求全局变换矩阵 求矩阵d 求每个物体坐标系原点在全局坐标系下的位置r 求每个物体在全局坐标系下的速度求每个铰在全局坐标系下的的角速度
tran1(bnum).a=tr;
bodyii(1).a=tran1(1).a*body(1).ii*(tran1(1).a)';
joint(1).w=joint(1).ww;
joint(1).v=joint(1).vv-joint(1).outpo*tran1(1).a'*DisSymmetryA(joint(1).w)';
% 求r
body(1).r=origin+joint(1).ri(1)*joint(1).kk1(1:3)+joint(1).ri(2)*joint(1).kk2(1:3)+joint(1).ri(3)*joint(1).kk3(1:3)+joint(1).inpo-joint(1).outpo*tran1(1).a';
% dtotal相对坐标的稀疏矩阵
if joint(1).trfree~=0
dtotal(1).a1=joint(1).kk1;
begin1=begin1+1;
if joint(1).trfree~=1
dtotal(begin1+1).a1=joint(1).kk2;
begin1=begin1+1;
if joint(1).trfree==3
dtotal(begin1+1).a1=joint(1).kk3;
begin1=begin1+1;
end
end
end
if joint(1).rofree~=0
p1=joint(1).pp1*body(1).translate1';
p2=joint(1).outpo*tran1(1).a'; % p2是outpo全局坐标系的表示
dtotal(begin1+1)=Ra1a2(p1*DisSymmetryA(p2)',p1);
r(1)=Ra1a2(-p2*DisSymmetryA(joint(1).w)'*DisSymmetryA(joint(1).w)',0);
tm=0;
if joint(1).rofree~=1
dtotal(begin1+2)=Ra1a2(joint(1).pp2*body(1).translate1'*DisSymmetryA(p2),joint(1).pp2*body(1).translate1');
tm=joint(1).pp2*DisSymmetryA(joint(1).pp1*joint(1).q(1))'*joint(1).q(2);
if joint(1).rofree==3
dtotal(begin1+3)=Ra1a2(joint(1).pp3*body(1).translate1'*DisSymmetryA(p2)',joint(1).pp3*body(1).translate1');
tm=tm+joint(1).pp3*DisSymmetryA(joint(1).pp1*joint(1).q(1)+joint(1).pp2*joint(1).q(2))'*joint(1).q(3);
end
tm=tm*body(1).translate1'*DisSymmetryA(p2)';
r(1)=Ra1a2(r(1).a1+tm,tm*body(1).translate1');
end
begin1=begin1+joint(1).rofree;
end % 工作数组是p1,p2,tm1,r1,r2,tm
% 假设0号物体不动，记录和0号物体有关的运动情况
else
tran1(bnum).a=tran1(bnum1).a*tr;
bodyii(bnum).a=tran1(bnum).a*body(bnum).ii*tran1(bnum).a';
joint(bnum).w=joint(bnum1).w+joint(bnum).ww*tran1(bnum1).a';
joint(bnum).v=joint(bnum1).v+joint(bnum).vv*tran1(bnum1).a'-joint(bnum).outpo*tran1(bnum).a'*DisSymmetryA(joint(bnum).w)'+joint(bnum).inpo*tran1(bnum1).a'*DisSymmetryA(joint(bnum1).w)';
p1=joint(bnum).ri(1)*joint(bnum).kk1(1:3)+joint(bnum).ri(2)*joint(bnum).kk2(1:3)+joint(bnum).ri(3)*joint(bnum).kk3(1:3);
joint(bnum).v=joint(bnum).v+p1*tran1(bnum1).a';
body(bnum).r=body(bnum1).r+(p1+joint(bnum).inpo)*tran1(bnum1).a'-joint(bnum).outpo*tran1(bnum).a';
dd=Ra1a2a3(diag(ones(3,1),0),DisSymmetryA(joint(bnum).outpo*tran1(bnum).a')-DisSymmetryA((joint(bnum).inpo+p1)*tran1(bnum1).a'),diag(ones(3,1),0));
% 递推求独立方阵
ll=iplus(bnum);
if ll==1
ll1=0;
else
ll1=pos(2*(ll-1)); % ll1是内接物体在dtotal的开始位置
end
begin1=pos(2*(bnum-1));
for l1=ll1+1:pos(2*ll);
dtotal(begin1+l1-ll1)=Ra1a2(dtotal(l1).a1*dd.a1'+dtotal(l1).a2*dd.a2',dtotal(l1).a2*dd.a3');
end
l1=l1+1; % 我加的.......... C 's for diff Fortran 's do
% 计算独立坐标矩阵d
begin1=begin1+pos(2*ll)-ll1;
if joint(bnum).trfree~=0
dtotal(begin1+1).a1=joint(bnum).kk1*tran1(bnum1).a';
begin1=begin1+1;
if joint(bnum).trfree~=1
dtotal(begin1+1).a1=joint(bnum).kk2*tran1(bnum1).a';
begin1=begin1+1;
if joint(bnum).trfree==3
dtotal(begin1+1).a1=joint(bnum).kk3*tran1(bnum1).a';
begin1=begin1+1;
end
end
end
r(bnum)=Ra1a2(-joint(bnum).outpo*tran1(bnum).a'*DisSymmetryA(joint(bnum).w)'*DisSymmetryA(joint(bnum).w)',0);
if joint(bnum).rofree~=0
p2=joint(bnum).outpo*tran1(bnum).a'; % p2是outpo全局坐标系的表示
dtotal(begin1+1).a2=joint(bnum).pp1*body(bnum).translate1'*tran1(bnum1).a';
dtotal(begin1+1).a1=dtotal(begin1+1).a2*DisSymmetryA(p2)';
r(bnum)=Ra1a2(-p2*DisSymmetryA(joint(bnum).w)'*DisSymmetryA(joint(bnum).w)',0);
tm=0;
if joint(bnum).rofree~=1
dtotal(begin1+2)=Ra1a2(dtotal(begin1+2).a2*DisSymmetryA(p2)',joint(bnum).pp2*body(bnum).translate1'*tran1(bnum1).a');
tm=joint(bnum).pp2*DisSymmetryA(joint(bnum).pp1*joint(bnum).q(1))'*joint(bnum).q(2);
if joint(bnum).rofree==3
dtotal(begin1+3)=Ra1a2(dtotal(begin1+3).a2*DisSymmetryA(p2)',joint(bnum).pp3*body(bnum).translate1'*tran1(bnum1).a');
tm=tm+joint(bnum).pp3*DisSymmetryA((joint(bnum).pp1*joint(bnum).q(1)+joint(bnum).pp2*joint(bnum).q(2)))'*joint(bnum).q(3);
end
r(bnum).a2=tm*body(bnum).translate1'*tran1(bnum1).a'; % 自身不同轴转动引起的转轴速度;
begin1=begin1+joint(bnum).rofree;
end
r(bnum)=Ra1a2(r(bnum).a1+(r(bnum).a2+joint(bnum).ww*tran1(bnum1).a'*DisSymmetryA(joint(bnum1).w)')*DisSymmetryA(joint(bnum).outpo*tran1(bnum).a')',r(bnum).a2+joint(bnum).ww*tran1(bnum1).a'*DisSymmetryA(joint(bnum1).w)');
end % 工作数组是p1,p2,tm1, r1,r2,tm
p1=(joint(bnum).inpo+joint(bnum).kk1*joint(bnum).ri(1)+joint(bnum).kk2*joint(bnum).ri(2)+joint(bnum).kk3*joint(bnum).ri(3))*tran1(bnum1).a'*DisSymmetryA(joint(bnum1).w)';
p2=(p1+2*joint(bnum).vv*tran1(bnum1).a')*DisSymmetryA(joint(bnum1).w)';
r(bnum)=Ra1a2(r(bnum).a1+(dd.a1*r(bnum1).a1')'+(dd.a2*r(bnum1).a2')'+p2,r(bnum).a2+(dd.a3*r(bnum1).a2')');
end
end
end
end
% 组集矩阵A
dtotal1(1:3*(nBodies*(nBodies+1)))=Ra1a2(0,0); % add 3
begin1=0;
for i=1:nBodies
if i==1
if joint(1).trfree~=0
for l1=1:joint(1).trfree;
dtotal1(begin1+l1).a1=dtotal(begin1+l1).a1*(body(1).mm*diag(ones(3,1),0))';
end
begin1=begin1+joint(1).trfree;
end
if joint(1).rofree~=0
for l1=1:joint(1).rofree;
dtotal1(begin1+l1)=Ra1a2(dtotal(begin1+l1).a1*(body(1).mm*diag(ones(3,1),0))',dtotal(begin1+l1).a2*(bodyii(1).a)');
end
begin1=begin1+joint(1).rofree;
end
else
for j=pos(2*(i-1)-1)+1:pos(2*i-1)
bnum=pos(j);
if joint(bnum).trfree~=0
for l1=1:joint(bnum).trfree;
dtotal1(begin1+l1).a1=dtotal(begin1+l1).a1*(body(i).mm*diag(ones(3,1),0))';
end
begin1=begin1+joint(1).trfree;
end
if joint(bnum).rofree~=0
for l1=1:joint(bnum).rofree;
dtotal1(begin1+l1)=Ra1a2(dtotal(begin1+l1).a1*(body(i).mm*diag(ones(3,1),0))',dtotal(begin1+l1).a2*(bodyii(i).a)');
end
begin1=begin1+joint(1).rofree;
end
end
end
end
% A 阵
row1=0;
total(1:allFreeNum,1:allFreeNum)=0;
for i=1:nBodies
iij=0;
if i==1
begin1=nBodies;
nn1=pos2(1)-nBodies;
else
begin1=pos2(i-1);
nn1=pos2(i)-pos2(i-1);
end
col1=row1;
for j=i:nBodies
if j==1
begin2=nBodies;
nn2=pos2(1)-nBodies;
else
begin2=pos2(j-1);
nn2=pos2(j)-pos2(j-1);
end
num1=1;
num2=1;
ll3=0;
ll4=0;
while ((num1<=nn1)&(num2<=nn2))
if pos2(begin1+num1)==pos2(begin2+num2)
ll=pos2(begin1+num1);
if ll==1
ll3=0;
ll4=0;
n1=2*nBodies;
else
ll3=pos(2*(ll-1));
ll4=pos(2*(ll-1));
n1=pos(2*(ll-1)-1);
end
nu1=n1+1;
while (pos(nu1)<i)
ll3=ll3+joint(pos(nu1)).trfree+joint(pos(nu1)).rofree;
nu1=nu1+1;
end
nu2=n1+1;
while (pos(nu2)<j)
ll4=ll4+joint(pos(nu2)).trfree+joint(pos(nu2)).rofree;
nu2=nu2+1;
end
for ll1=1:(joint(i).trfree+joint(i).rofree)
for ll2=1:(joint(j).trfree+joint(j).rofree);
total(row1+ll1,col1+ll2)=total(row1+ll1,col1+ll2)+dtotal(ll3+ll1).a1*dtotal1(ll4+ll2).a1'+dtotal(ll3+ll1).a2*dtotal1(ll4+ll2).a2';
end
end
num1=num1+1;
num2=num2+1;
elseif (pos2(begin1+num1)>pos2(begin2+num2))
num2=num2+1;
else
num1=num1+1;
end
end
col1=col1+joint(j).trfree+joint(j).rofree;
end
row1=row1+joint(i).trfree+joint(i).rofree;
end
for i=1:allFreeNum
for j=1:i
total(i,j)=total(j,i);
end
end
% 有关铰之间的主动力的计算
begin=0;
for i=1:nBodies
if joint(i).trfree~=0
begin=begin+1;
ba(begin)=joint(i).kk1*joint(i).fa';
if joint(i).trfree~=1
begin=begin+1;
ba(begin)=joint(i).kk2*joint(i).fa';
if joint(i).trfree==3
begin=begin+1;
ba(begin)=joint(i).kk3*joint(i).fa';
end
end
end
if joint(i).rofree~=0
for ii=1:joint(i).rofree
begin=begin+1;
ba(begin)=joint(i).ma(joint(i).m(ii));
end
end
end
% 计算mr,fg,和为mr。铰之间的主动力?见读入数据结构，里面好像已经有了呀！
ppl=0;
for i=1:nfor
kk=for1(i).index;
p1=for1(i).po*tran1(kk).a';
ppl=body(kk).gf*DisSymmetryA(p1)';
p2=body(kk).mf*body(kk).translate1';
if kk~=1
p2=p2*tran1(iplus(kk)).a';
end
ppl=ppl+p2;
end
for i=1:nBodies
mr(i).a1=-body(i).mm*r(i).a1+body(i).gf;
mr(i).a2=-r(i).a2*bodyii(i).a'+ppl-joint(i).w*bodyii(i).a'*DisSymmetryA(joint(i).w)';
end
begin1=0;
for i=1:nBodies % i级循环开始
if i==1
begin=nBodies;
nn=pos2(1)-nBodies;
else
begin=pos2(i-1);
nn=pos2(i)-pos2(i-1);
end
for ii=1:nn % ii级循环开始
bnum1=0;
bnum=pos2(begin+ii);
if bnum==1
l=0;
l1=2*nBodies;
else
l=pos(2*(bnum-1));
l1=pos(2*(bnum-1)-1);
end
ll1=l1+1;
while pos(ll1)<i
l=l+joint(pos(ll1)).trfree+joint(pos(ll1)).rofree;
ll1=ll1+1;
end
if joint(i).trfree~=0
bnum1=1;
for tp=1:joint(i).trfree;
bz(begin1+tp)=bz(begin1+tp)+dtotal(l+tp).a1*mr(bnum).a1'+dtotal(l+tp).a2*mr(bnum).a2';
end
l=l+joint(i).trfree;
end
if joint(i).rofree~=0;
bnum1=joint(i).trfree;
for tp=1:joint(i).rofree;
bz(begin1+bnum1+tp)=bz(begin1+bnum1+tp)+dtotal(l+tp).a1*mr(bnum).a1'+dtotal(l+tp).a2*mr(bnum).a2';
end
end
end % ii级循环结束
begin1=begin1+joint(i).trfree+joint(i).rofree;
end % i级循环结束
bz=bz+ba;⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -