delta机械手运动学反解代码

1、%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)-xF2+yA2+(La*si n( o1)+z)A2-LZ2; eq2=(-1/2*(R-r+La*cos(o2)-x)A2+(1/2*sqrt(3)*(R-r+La*cos(o2)-yF2+(La*si n( o2)+z)A2-LZ2; eq3

2、=(-1/2*(R-r+La*cos(o3)-x)A2+(-1/2*sqrt(3)*(R-r+La*cos(o3)-y)A2+(La*si n( o3)+z)A2-LZ2; x,y,z=solve(eq1,eq2,eq3);x=vpa(x,5);y=vpa(y,5);z=vpa(z,5);for i=1:2z(i);if z(i)<0ans=x(i) y(i) z(i);elsedisp('')endendans=vpa(ans,5);if ans(1).A2+ans(2).A2+ans(3).A2<10000000ans=ans(1) ans(2) ans(3);

3、elsedisp('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 z<0if x.A2+y.A2+z.A2>= 100000 disp('The program has not found solu

4、tion!elseA1=(La-R+r)A2+2*(x*cos(0)+y*s in (0)*(La-R+r)+(xA2+yA2+zA2-LbA2);A2=(La-R+r)A2+2*(x*cos(2*pi/3)+y*si n(2*pi/3)*(La-R+r)+(xA2+yA2+zA2-LbA2);A3=(La-R+r)A2+2*(x*cos(4*pi/3)+y*si n(4*pi/3)*(La-R+r)+(xA2+yA2+zA2-LbA2);B=4*La*z;8= (La+R-r)A2-2*(La+R-r)A2*(x*cos(0)+y*si n( 0)+仪人2+人2+乙人2丄匕人2);C2=(L

5、a+R-r)A2-2*(La+R-r)A2*(x*cos(2*pi/3)+y*si n(2*pi/3)+(xA2+yA2+zA2-LbA2);C3=(La+R-r)A2-2*(La+R-r)A2*(x*cos(4*pi/3)+y*si n(4*pi/3)+(xA2+yA2+zA2-LbA2); t1 仁(-B-sqrt(BA2-4*A1*C1);t11=(-B-sqrt(BA2-4*A1*C1)/(2*A1);t12=(-B+sqrt(BA2-4*A1*C1)/(2*A1);t21=(-B-sqrt(BA2-4*A2*C2)/(2*A2);t22=(-B+sqrt(BA2-4*A2*C2)/(

6、2*A2);t31=(-B-sqrt(BA2-4*A3*C3)/(2*A3);t32=(-B+sqrt(BA2-4*A3*C3)/(2*A3);o11=2*ata n(t11);o12=2*ata n(t12);o21=2*ata n( t21);o22=2*ata n(t22);o31=2*ata n(t31);o32=2*ata n(t32);endenda=o11 o12 ;b=o21 o22;c=o31 o32;for i=1:2if abs(a(i)vpi/2x=o(i)elsedisp('')endendfor i=1:2if abs(b(i)vpi/2 y=o(i

7、) else disp('') endendfor i=1:2if abs(c(i)vpi/2 z=o(i) else disp('') endend%数值计算法求积分方程的特解ft = e-|sin?：t)| 和 s x = 04f(t) dt» syms t kft-eap '：-abs)；» 5x=int(ft；0, i)Warning! Explicit int ?gral could, rot be found-sx =ini (txp(-abs (sin(t) t = 04)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','MarkerSize',4) %Sx数组与自变量有相同数目的元素，Sx数组的元素给出与t数组采样点对应的积分值 hold onplot(t,Sx