混联机器人运动学测试

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=(q4)^2+(q3)^2-(2*(((q4)^2-(q3)^2)/(4*b))^2+2*(a^2+b^2));
k2=-4*a*b;
k3=2*(((q4)^2-(q3)^2)/(4*b))^2
k6=-1*(k1+k3)/k2;
k4=-1*(k6^2)/3-1;
k5=-1*(k6/3)^3+(k6^3)/9+k1/k2+k6/3;
kk=(k5/2)^2+(k4/3)^3
k7=-1*k5/2-sqrt(kk);
k8=-1*k5/2+sqrt(kk);
ww=complex(-0.5,sqrt(3)/2);
t1=k7^(1/3)+k8^(1/3);
t2=((k7)^(1/3))*ww+((k8)^(1/3))*ww*ww;
t3=((k7)^(1/3))*ww*ww+((k8)^(1/3))*ww;
t01=t1-k6/3;
t02=t2-k6/3
t03=t3-k6/3
ryy1=sign(q4-q3)*(acos(t01)/con);
xpw1=(zpw+zw-et*cos(ry)*cos(rx))*tan(ry)+et*cos(rx)*sin(ry)-xw
ypw1=-1*et*sin(ry)-yw