denghaili86
2010-01-21, 12:54
syms WX a a1 a2 a3 a4 a5 a6 a7 a8 a9 a10 a11 b c d e Q T E Qx Qy Qz R T0 B1 D1 F1 H1 S1 U1 U2 U3 U4 BX DX FX HX SX L1 L2 L3 L4 T1 T2 T3 T4 m g Z x1 x2 x3;
WX=[0 -b a;b 0 -c;-a c 0]
R=WX*d+WX*WX*(1-e)+[1 0 0;0 1 0;0 0 1]
Q=[Qx;Qy;Qz]
T=[R Q;0 0 0 1]
E=T.' %求转置矩阵
T0=E*T
B1=R*[1;1;0]+Q
D1=R*[-1;1;0]+Q
F1=R*[-1;-1;0]+Q
H1=R*[1;-1;0]+Q
S1=R*[0;0;-2]+Q %
U1=([10;10;0]-[1;1;0])/L1
U2=([-10;10;0]-[-1;1;0])/L2
U3=([-10;-10;0]-[-1;-1;0])/L3
U4=([10;-10;0]-[1;-1;0])/L4
BX=[0 0 1;0 0 -1;-1 1 0]
DX=[0 0 1;0 0 1;-1 -1 0]
FX=[0 0 -1;0 0 1;1 -1 0]
HX=[0 0 -1;0 0 1;1 1 0]
SX=[0 2 0;-2 0 0;0 0 0]
Z=[0;0;1]
a1=simplify(36-[1;1;0;1]'*T0*[1;1;0;1]-2*[10;10;0;1]'*T*[1;1;0;1]+[10;10;0;1]'*[10;10;0;1]);
a2=simplify(36-[-1;1;0;1]'*T0*[-1;1;0;1]-2*[-10;10;0;1]'*T*[-1;1;0;1]+[-10;10;0;1]'*[-10;10;0;1]);
a3=simplify(36-[-1;-1;0;1]'*T0*[-1;-1;0;1]-2*[-10;-10;0;1]'*T*[-1;-1;0;1]+[-10;-10;0;1]'*[-10;-10;0;1]);
a4=simplify(36-[1;-1;0;1]'*T0*[-1;-1;0;1]-2*[10;-10;0;1]'*T*[-1;-1;0;1]+[10;-10;0;1]'*[10;-10;0;1]);
a5=simplify(T1*U1+T2*U2+T3*U3+T4*U4-m*g*Z-[0;0;0]);
a6=simplify(T1*BX*U1+T2*DX*U2+T3*FX*U3+T4*HX*U4-SX*m*g*Z-[0;0;0]);
a7=simplify(a-sin(x1));
a8=simplify(c-cos(x1));
a9=simplify(b-cos(x2));
a10=simplify(d-sin(x3));
a11=simplify(e-cos(x3));
[x1,x2,x3,Qx,Qy,Qz,T1,T2,T3,T4]=solve(a1,a2,a3,a4,a5,a6,a7,a8,a9,a10,a11);
s=[x1,x2,x3,Qx,Qy,Qz,T1,T2,T3,T4]
WX=[0 -b a;b 0 -c;-a c 0]
R=WX*d+WX*WX*(1-e)+[1 0 0;0 1 0;0 0 1]
Q=[Qx;Qy;Qz]
T=[R Q;0 0 0 1]
E=T.' %求转置矩阵
T0=E*T
B1=R*[1;1;0]+Q
D1=R*[-1;1;0]+Q
F1=R*[-1;-1;0]+Q
H1=R*[1;-1;0]+Q
S1=R*[0;0;-2]+Q %
U1=([10;10;0]-[1;1;0])/L1
U2=([-10;10;0]-[-1;1;0])/L2
U3=([-10;-10;0]-[-1;-1;0])/L3
U4=([10;-10;0]-[1;-1;0])/L4
BX=[0 0 1;0 0 -1;-1 1 0]
DX=[0 0 1;0 0 1;-1 -1 0]
FX=[0 0 -1;0 0 1;1 -1 0]
HX=[0 0 -1;0 0 1;1 1 0]
SX=[0 2 0;-2 0 0;0 0 0]
Z=[0;0;1]
a1=simplify(36-[1;1;0;1]'*T0*[1;1;0;1]-2*[10;10;0;1]'*T*[1;1;0;1]+[10;10;0;1]'*[10;10;0;1]);
a2=simplify(36-[-1;1;0;1]'*T0*[-1;1;0;1]-2*[-10;10;0;1]'*T*[-1;1;0;1]+[-10;10;0;1]'*[-10;10;0;1]);
a3=simplify(36-[-1;-1;0;1]'*T0*[-1;-1;0;1]-2*[-10;-10;0;1]'*T*[-1;-1;0;1]+[-10;-10;0;1]'*[-10;-10;0;1]);
a4=simplify(36-[1;-1;0;1]'*T0*[-1;-1;0;1]-2*[10;-10;0;1]'*T*[-1;-1;0;1]+[10;-10;0;1]'*[10;-10;0;1]);
a5=simplify(T1*U1+T2*U2+T3*U3+T4*U4-m*g*Z-[0;0;0]);
a6=simplify(T1*BX*U1+T2*DX*U2+T3*FX*U3+T4*HX*U4-SX*m*g*Z-[0;0;0]);
a7=simplify(a-sin(x1));
a8=simplify(c-cos(x1));
a9=simplify(b-cos(x2));
a10=simplify(d-sin(x3));
a11=simplify(e-cos(x3));
[x1,x2,x3,Qx,Qy,Qz,T1,T2,T3,T4]=solve(a1,a2,a3,a4,a5,a6,a7,a8,a9,a10,a11);
s=[x1,x2,x3,Qx,Qy,Qz,T1,T2,T3,T4]