[/size][/backcolor][/color]
clear;
clc;
global pi m11 m12 l11 l12 m21 m22 l21 l22 m31 m32 l31 l32 m4 g d1 d2 d3 fa a21 a23
m11=1;m12=0.1;l11=1;l12=1.044;m21=1;m22=0.2;l21=1;l22=0.938;m31=1;m32=0.3;l31=1;l32=0.917;m4=1;%参数输入
g=9.8;d1=0.7;d2=0.5;d3=0.3;a21=0.1;a23=0.2;pi=3.1415926;fa=(2/3)*pi;%参数输入
syms w1 w2 w3
q1=acos((l11.^2-l12.^2+(w1).^2+(w2).^2+(w3-d1).^2)./(2*l11*((w1).^2+(w2).^2).^(1/2)))+atan(w2./w1);
q2=atan((w2-l11*sin(q1))./(w1-l11*cos(q1)));
q3=atan(sqrt((w1-l11*cos(q1)).^2+(w2-l11*sin(q1)).^2))./(w3-d1);
q7=-acos(((w1).^2+(w2).^2+(w3-d3-a21).^2+(l31-a23).^2-l32.^2)./(2*(l31-a23)*sqrt((w1).^2+(w2).^2)))-atan(w2./w1);
q8=atan((w2+a23*sin(q7)-l31*sin(q7))./(w1+a23*cos(q7)-l31*cos(q7)));