混联机器人运动学测试

clc;
clear;
con=pi/180;
rxx2=30;
ryy2=10;
a=57;b=120;rx=rxx2*con;ry=ryy2*con;xpw=6;ypw=8;zpw=-10;zw=265;et=10;
aa=(zpw+zw-et*cos(ry)*cos(rx))*tan(ry)-a*sin(ry)*sin(rx);
bb=-1*a*cos(rx)+b;
cc=(zpw+zw-et*cos(ry)*cos(rx))-a*cos(ry)*sin(rx);
q1=sqrt(aa^2+bb^2+cc^2);

aa2=(zpw+zw-et*cos(ry)*cos(rx))*tan(ry)+a*sin(ry)*sin(rx);
bb2=a*cos(rx)-b;
cc2=(zpw+zw-et*cos(ry)*cos(rx))+a*cos(ry)*sin(rx);
q2=sqrt(aa2^2+bb2^2+cc2^2);

aa3=(zpw+zw-et*cos(ry)*cos(rx))*tan(ry)+a*cos(ry)-b;
bb3=zpw+zw-et*cos(ry)*cos(rx)-a*sin(ry);
q3=sqrt(aa3^2+bb3^2);

aa4=(zpw+zw-et*cos(ry)*cos(rx))*tan(ry)-a*cos(ry)+b;
bb4=zpw+zw-et*cos(ry)*cos(rx)+a*sin(ry);
q4=sqrt(aa4^2+bb4^2);

xw=(zpw+zw-et*cos(ry)*cos(rx))*tan(ry)+et*cos(rx)*sin(ry)-xpw
yw=-1*et*sin(ry)-ypw

%%
%正解
k1&#