delta机械手运动学反解代码.docxVIP

%delta机械手运动学正解代码syms x y zLa=0.2;Lb=0.6;R=0.348;r= 0.1068;o1=input(请输入弧度制角o1=);o2=input(请输入弧度制角o1=);o3=input(请输入弧度制角o3=);eq1=(R-r+La*cos(o1)-x)^2+y^2+(La*sin(o1)+z)^2-Lb^2;eq2=(-1/2*(R-r+La*cos(o2))-x)^2+(1/2*sqrt(3)*(R-r+La*cos(o2))-y)^2+(La*sin(o2)+z)^2-Lb^2;eq3=(-1/2*(R-r+La*cos(o3))-x)^2+(-1/2*sqrt(3)*(R-r+La*cos(o3))-y)^2+(La*sin(o3)+z)^2-Lb^2;[x,y,z]=solve(eq1,eq2,eq3);x=vpa(x,5);y=vpa(y,5);z=vpa(z,5);for i=1:2 z(i); if z(i)0 ans=[x(i) y(i) z(i)]; else disp() endendans=vpa(ans,5);if ans(1).^2+ans(2).^2+ans(3).^210000000 ans=[ans(1) ans(2) ans(3)];else disp(no answer!)endans=vpa(ans,5)%注解：坐标位置的控制是精度控制之一%运动学反解代码syms A1 A2 A3 B C1 C2 C3R=0.348;r=0.1068;La=0.200;Lb=0.600;x=input(请输入分量坐标x=);y=input(请输入分量运动y=);z=input(请输入分量运动z=);if z0if x.^2+y.^2+z.^2=100000disp(The program has not found solution!)elseA1=(La-R+r)^2+2*(x*cos(0)+y*sin(0))*(La-R+r)+(x^2+y^2+z^2-Lb^2);A2=(La-R+r)^2+2*(x*cos(2*pi/3)+y*sin(2*pi/3))*(La-R+r)+(x^2+y^2+z^2-Lb^2);A3=(La-R+r)^2+2*(x*cos(4*pi/3)+y*sin(4*pi/3))*(La-R+r)+(x^2+y^2+z^2-Lb^2);B=4*La*z;C1=(La+R-r)^2-2*(La+R-r)^2*(x*cos(0)+y*sin(0))+(x^2+y^2+z^2-Lb^2);C2=(La+R-r)^2-2*(La+R-r)^2*(x*cos(2*pi/3)+y*sin(2*pi/3))+(x^2+y^2+z^2-Lb^2);C3=(La+R-r)^2-2*(La+R-r)^2*(x*cos(4*pi/3)+y*sin(4*pi/3))+(x^2+y^2+z^2-Lb^2);t11=(-B-sqrt(B^2-4*A1*C1));t11=(-B-sqrt(B^2-4*A1*C1))/(2*A1);t12=(-B+sqrt(B^2-4*A1*C1))/(2*A1);t21=(-B-sqrt(B^2-4*A2*C2))/(2*A2);t22=(-B+sqrt(B^2-4*A2*C2))/(2*A2);t31=(-B-sqrt(B^2-4*A3*C3))/(2*A3);t32=(-B+sqrt(B^2-4*A3*C3))/(2*A3);o11=2*atan(t11);o12=2*atan(t12);o21=2*atan(t21);o22=2*atan(t22);o31=2*atan(t31);o32=2*atan(t32);endenda=[o11 o12 ];b=[o21 o22];c=[o31 o32];for i=1:2 if abs(a(i))pi/2x=o(i) else disp( ) endendfor i=1:2 if abs(b(i))pi/2y=o(i) else disp( ) endendfor i=1:2 if abs(c(i))pi/2z=o(i) else disp( ) endend%数值计算法求积分方程的特解和t=0.05;t=0:dt:4;Ft=exp(-abs(sin(t)));%Ft数组与自变量数组有相同数目的元素，Ft数组的元素值就是t数组相应位置的函数值f（t）Sx=dt*cumtrapz(Ft);%plot(t,Ft,*r,MarkerS