function []=zhengwucha (s,ds,il,d_il,Rc,dRc,m,dm,n,dn,d4,Pc,d_Pc)
dd4=0;
s1=s(1);
s2=s(2);
s3=s(3);
s4=s(4);
l1=il(1);
l2=il(2);
l3=il(3);
dl1=d_il(1);
dl2=d_il(2);
dl3=d_il(3);
Pb=[m n 0];
dPb=[dm dn 0];
BT1=[cos(s1) 0 sin(s1) l1*cos(s1);sin(s1) 0 -cos(s1) l1*sin(s1);0 1 0 0;0 0 0 1];
T2=[cos(s2) -sin(s2) 0 l2*cos(s2);sin(s2) cos(s2) 0 l2*sin(s2);0 0 1 0;0 0 0 1];
T3=[cos(s3) 0 sin(s3) 0;sin(s3) 0 -cos(s3) 0;0 1 0 0;0 0 0 1];
TA1=[cos(s4) -sin(s4) 0 0;sin(s4) cos(s4) 0 0;0 0 1 d4;0 0 0 1];
B_T_A1=BT1*T2*T3*TA1;
C_T_B1=[1 0 0 Pb(1);0 1 0 Pb(2);0 0 1 Pb(3);0 0 0 1];
O_T_C=[Rc(1,1) Rc(1,2) Rc(1,3) Pc(1);Rc(2,1) Rc(2,2) Rc(2,3) Pc(2);Rc(3,1) Rc(3,2) Rc(3,3) Pc(3);0 0 0 1];
O_T_A1=O_T_C*C_T_B1*B_T_A1;
disp('解得手的位置如下:');
disp(O_T_A1);

d_f11=dRc(1,1)*(cos(s1)*cos(s2+s3)*cos(s4)+sin(s1)*sin(s4))-Rc(1,1)*(sin(s1)*ds(1)*cos(s2+s3)*cos(s4)+cos(s4)*sin(s2+s3)*(ds(2)+ds(3))*cos(s4)+cos(s1)*cos(s2+s3)*sin(s4)*ds(4)-cos(s1)*ds(1)*sin(s4)-sin(s1)*cos(s4)*ds(4))+dRc(1,2)*(sin(s1)*cos(s2+s3)*cos(s4)-cos(s1)*sin(s4))+Rc(1,2)*(cos(s1)*ds(1)*cos(s2+s3)*cos(s4)-sin(s1)*sin(s2+s3)*(ds(2)+ds(3))*cos(s4)-sin(s1)*cos(s2+s3)*sin(s4)*ds(4)+sin(s1)*ds(1)*sin(s4)-cos(s1)*cos(s4)*ds(4))+dRc(1,3)*sin(s2+s3)*cos(s4)+Rc(1,3)*cos(s2+s3)*(ds(2)+ds(3))*cos(s4)-Rc(1,3)*sin(s2+s3)*sin(s4)*ds(4);

d_f12=dRc(1,1)*(-cos(s1)*cos(s2+s3)*sin(s4)+sin(s1)*cos(s4))+Rc(1,1)*(sin(s1)*ds(1)*cos(s2+s3)*sin(s4)+cos(s1)*sin(s2+s3)*(ds(2)+ds(3))*sin(s4)-cos(s1)*cos(s2+s3)*cos(s4)*ds(4)+cos(s1)*ds(1)*cos(s4)-sin(s1)*sin(s4)*ds(4))-dRc(1,2)*(sin(s1)*cos(s2+s3)*sin(s4)+cos(s1)*cos(s4))-Rc(1,2)*(cos(s1)*ds(1)*cos(s2+s3)*sin(s4)-sin(s1)*sin(s2+s3)*(ds(2)+ds(3))*sin(s4)+sin(s1)*cos(s2+s3)*cos(s4)*ds(4)-sin(s1)*ds(1)*cos(s4)-cos(s1)*sin(s4)*ds(4))-dRc(1,3)*sin(s2+s3)*sin(s4)+Rc(1,3)*cos(s2+s3)*(ds(2)+ds(3))*sin(s4)-Rc(1,3)*sin(s2+s3)*cos(s4)*ds(4);

d_f13=dRc(1,1)*cos(s1)*sin(s2+s3)-Rc(1,1)*sin(s1)*ds(1)*sin(s2+s3)+Rc(1,1)*cos(s1)*cos(s2+s3)*(ds(2)+ds(3))+dRc(1,2)*sin(s1)*sin(s2+s3)+Rc(1,2)*cos(s1)*ds(1)*sin(s2+s3)+Rc(1,2)*sin(s1)*cos(s2+s3)*(ds(2)+ds(3))-dRc(1,3)*cos(s2+s3)+Rc(1,3)*sin(s2+s3)*(ds(2)+ds(3));
 
d_f14=dRc(1,1)*(d4*cos(s1)*sin(s2+s3)+l2*cos(s1)*cos(s2)+l1*cos(s1)+Pb(1))+Rc(1,1)*(dd4*cos(s1)*sin(s2+s3)-d4*sin(s1)*ds(1)*sin(s2+s3)+d4*cos(s1)*cos(s2+s3)*(ds(2)+ds(3))+dl2*cos(s1)*cos(s2)-l2*sin(s1)*ds(1)*cos(s2)-l2*cos(s1)*sin(s2)*ds(2)+dl1*cos(s1)-l1*sin(s1)*ds(1)+dPb(1))+dRc(1,2)*(sin(s1)*sin(s2+s3)*d4+sin(s1)*l2*cos(s2)+l1*sin(s1)+Pb(2))+Rc(1,2)*(cos(s1)*ds(1)*sin(s2+s3)*d4+sin(s1)*cos(s2+s3)*(ds(2)+ds(3))*d4+sin(s1)*sin(s2+s3)*dd4+cos(s1)*ds(1)*l2*cos(s2)+sin(s1)*dl2*cos(s2)-sin(s1)*l2*sin(s2)*ds(2)+dl1*sin(s1)+l1*cos(s1)*ds(1)+dPb(2))+dRc(1,3)*(-cos(s2+s3)*d4+l2*sin(s2)+Pb(3))+Rc(1,3)*(sin(s2+s3)*(ds(2)+ds(3))*d4-cos(s2+s3)*dd4+dl2*sin(s2)+l2*cos(s2)*ds(2)+dPb(3))+d_Pc(1);

d_f21=dRc(2,1)*(cos(s1)*cos(s2+s3)*cos(s4)+sin(s1)*sin(s4))-Rc(2,1)*(sin(s1)*ds(1)*cos(s2+s3)*cos(s4)+cos(s1)*sin(s2+s3)*(ds(2)+ds(3))*cos(s4)+cos(s1)*cos(s2+s3)*sin(s4)*ds(4)-cos(s1)*ds(1)*sin(s4)-sin(s1)*cos(s4)*ds(4))+dRc(2,2)*(sin(s1)*cos(s2+s3)*cos(s4)-cos(s1)*sin(s4))+Rc(2,2)*(cos(s1)*ds(1)*cos(s2+s3)*cos(s4)-sin(s1)*sin(s2+s3)*(ds(2)+ds(3))*cos(s4)-sin(s1)*cos(s2+s3)*sin(s4)*ds(4)+sin(s1)*ds(1)*sin(s4)-cos(s1)*cos(s4)*ds(4))+dRc(2,3)*sin(s2+s3)*cos(s4)+Rc(2,3)*cos(s2+s3)*(ds(2)+ds(3))*cos(s4)-Rc(2,3)*sin(s2+s3)*sin(s4)*ds(4);

d_f22=dRc(2,1)*(-cos(s1)*cos(s2+s3)*sin(s4)+sin(s1)*cos(s4))+Rc(2,1)*(sin(s1)*ds(1)*cos(s2+s3)*sin(s4)+cos(s1)*sin(s2+s3)*(ds(2)+ds(3))*sin(s4)-cos(s1)*cos(s2+s3)*cos(s4)*ds(4)+cos(s1)*ds(1)*cos(s4)-sin(s1)*sin(s4)*ds(4))-dRc(2,2)*(sin(s1)*cos(s2+s3)*sin(s4)+cos(s1)*cos(s4))-Rc(2,2)*(cos(s1)*ds(1)*cos(s2+s3)*sin(s4)-sin(s1)*sin(s2+s3)*(ds(2)+ds(3))*sin(s4)+sin(s1)*cos(s2+s3)*cos(s4)*ds(4)-sin(s1)*ds(1)*cos(s4)-cos(s1)*sin(s4)*ds(4))-dRc(2,3)*sin(s2+s3)*sin(s4)-Rc(2,3)*cos(s2+s3)*(ds(2)+ds(3))*sin(s4)-Rc(2,3)*sin(s2+s3)*cos(s4)*ds(4);

d_f23=dRc(2,1)*cos(s1)*sin(s2+s3)-Rc(2,1)*sin(s1)*ds(1)*sin(s2+s3)+Rc(2,1)*cos(s1)*cos(s2+s3)*(ds(2)+ds(3))+dRc(2,2)*sin(s1)*sin(s2+s3)+Rc(2,2)*cos(s1)*ds(1)*sin(s2+s3)+Rc(2,2)*sin(s1)*cos(s2+s3)*(ds(2)+ds(3))-dRc(2,3)*cos(s2+s3)+Rc(2,3)*sin(s2+s3)*(ds(2)+ds(3));

d_f24=dRc(2,1)*(d4*cos(s1)*sin(s2+s3)+l2*cos(s1)*cos(s2)+l1*cos(s1)+Pb(1))+Rc(2,1)*(dd4*cos(s1)*sin(s2+s3)-d4*sin(s1)*ds(1)*sin(s2+s3)+d4*cos(s1)*cos(s2+s3)*(ds(2)+ds(3))+dl2*cos(s1)*cos(s2)-l2*sin(s1)*ds(1)*cos(s2)-l2*cos(s1)*sin(s2)*ds(2)+dl1*cos(s1)-l1*sin(s1)*ds(1)+dPb(1))+dRc(2,2)*(sin(s1)*sin(s2+s3)*d4+sin(s1)*l2*cos(s2)+l1*sin(s1)+Pb(2))+Rc(2,2)*(cos(s1)*ds(1)*sin(s2+s3)*d4+sin(s1)*cos(s2+s3)*(ds(2)+ds(3))*d4+sin(s1)*sin(s2+s3)*dd4+cos(s1)*ds(1)*l2*cos(s2)+sin(s1)*dl2*cos(s2)-sin(s1)*l2*sin(s2)*ds(2)+dl1*sin(s1)+l1*cos(s1)*ds(1)+dPb(2))+dRc(2,3)*(-cos(s2+s3)*d4+l2*sin(s2)+Pb(3))+Rc(2,3)*(sin(s2+s3)*(ds(2)+ds(3))*d4-cos(s2+s3)*dd4+dl2*sin(s2)+l2*cos(s2)*ds(2)+dPb(3))+d_Pc(2);

d_f31=dRc(3,1)*(cos(s1)*cos(s2+s3)*cos(s4)+sin(s1)*sin(s4))-Rc(3,1)*(sin(s1)*ds(1)*cos(s2+s3)*cos(s4)+cos(s1)*sin(s2+s3)*(ds(2)+ds(3))*cos(s4)+cos(s1)*cos(s2+s3)*sin(s4)*ds(4)-cos(s1)*ds(1)*sin(s4)-sin(s1)*cos(s4)*ds(4))+dRc(3,2)*(sin(s1)*cos(s2+s3)*cos(s4)-cos(s1)*sin(s4))+Rc(3,2)*(cos(s1)*ds(1)*cos(s2+s3)*cos(s4)-sin(s1)*sin(s2+s3)*(ds(2)+ds(3))*cos(s4)-sin(s1)*cos(s2+s3)*sin(s4)*ds(4)+sin(s1)*ds(1)*sin(s4)-cos(s1)*cos(s4)*ds(4))+dRc(3,3)*sin(s2+s3)*cos(s4)+Rc(3,3)*cos(s2+s3)*(ds(2)+ds(3))*cos(s4)-Rc(3,3)*sin(s2+s3)*sin(s4)*ds(4);

d_f32=dRc(3,1)*(-cos(s1)*cos(s2+s3)*sin(s4)+sin(s1)*cos(s4))+Rc(3,1)*(sin(s1)*ds(1)*cos(s2+s3)*sin(s4)+cos(s1)*sin(s2+s3)*(ds(2)+ds(3))*sin(s4)-cos(s1)*cos(s2+s3)*cos(s4)*ds(4)+cos(s1)*ds(1)*cos(s4)-sin(s1)*sin(s4)*ds(4))-dRc(3,2)*(sin(s1)*cos(s2+s3)*sin(s4)+cos(s1)*cos(s4))-Rc(3,2)*(cos(s1)*ds(1)*cos(s2+s3)*sin(s4)-sin(s1)*sin(s2+s3)*(ds(2)+ds(3))*sin(s4)+sin(s1)*cos(s2+s3)*cos(s4)*ds(4)-sin(s1)*ds(1)*cos(s4)-cos(s1)*sin(s4)*ds(4))-dRc(3,3)*sin(s2+s3)*sin(s4)-Rc(3,3)*cos(s2+s3)*(ds(2)+ds(3))*sin(s4)-Rc(3,3)*sin(s2+s3)*cos(s4)*ds(4);

d_f33=dRc(3,1)*cos(s1)*sin(s2+s3)-Rc(3,1)*sin(s1)*ds(1)*sin(s2+s3)+Rc(3,1)*cos(s1)*cos(s2+s3)*(ds(2)+ds(3))+dRc(3,2)*sin(s1)*sin(s2+s3)+Rc(3,2)*cos(s1)*ds(1)*sin(s2+s3)+Rc(3,2)*sin(s1)*cos(s2+s3)*(ds(2)+ds(3))-dRc(3,3)*cos(s2+s3)+Rc(3,3)*sin(s2+s3)*(ds(2)+ds(3));

d_f34=dRc(3,1)*(d4*cos(s1)*sin(s2+s3)+l2*cos(s1)*cos(s2)+l1*cos(s1)+Pb(1))+Rc(3,1)*(dd4*cos(s1)*sin(s2+s3)-d4*sin(s1)*ds(1)*sin(s2+s3)+d4*cos(s1)*cos(s2+s3)*(ds(2)+ds(3))+dl2*cos(s1)*cos(s2)-l2*sin(s1)*ds(1)*cos(s2)-l2*cos(s1)*sin(s2)*ds(2)+dl1*cos(s1)-l1*sin(s1)*ds(1)+dPb(1))+dRc(3,2)*(sin(s1)*sin(s2+s3)*d4+sin(s1)*l2*cos(s2)+l1*sin(s1)+Pb(2))+Rc(3,2)*(cos(s1)*ds(1)*sin(s2+s3)*d4+sin(s1)*cos(s2+s3)*(ds(2)+ds(3))*d4+sin(s1)*sin(s2+s3)*dd4+cos(s1)*ds(1)*l2*cos(s2)+sin(s1)*dl2*cos(s2)-sin(s1)*l2*sin(s2)*ds(2)+dl1*sin(s1)+l1*cos(s1)*ds(1)+dPb(2))+dRc(3,3)*(-cos(s2+s3)*d4+l2*sin(s2)+Pb(3))+Rc(3,3)*(sin(s2+s3)*(ds(2)+ds(3))*d4-cos(s2+s3)*dd4+dl2*sin(s2)+l2*cos(s2)*ds(2)+dPb(3))+d_Pc(3);

disp('机械臂正运动学误差d_F=');
d_F=[d_f11,d_f12,d_f13,d_f14;d_f21,d_f22,d_f23,d_f24;d_f31,d_f32,d_f33,d_f34]

function []=zhengwucha (s,il,Rc,m,n,d4,Pc)
s1=s(1);
s2=s(2);
s3=s(3);
s4=s(4);
l1=il(1);
l2=il(2);
l3=il(3);
Pb=[m n 0];
BT1=[cos(s1) 0 sin(s1) l1*cos(s1);sin(s1) 0 -cos(s1) l1*sin(s1);0 1 0 0;0 0 0 1];
T2=[cos(s2) -sin(s2) 0 l2*cos(s2);sin(s2) cos(s2) 0 l2*sin(s2);0 0 1 0;0 0 0 1];
T3=[cos(s3) 0 sin(s3) 0;sin(s3) 0 -cos(s3) 0;0 1 0 0;0 0 0 1];
TA1=[cos(s4) -sin(s4) 0 0;sin(s4) cos(s4) 0 0;0 0 1 d4;0 0 0 1];
B_T_A1=BT1*T2*T3*TA1;
C_T_B1=[1 0 0 Pb(1);0 1 0 Pb(2);0 0 1 Pb(3);0 0 0 1];
O_T_C=[Rc(1,1) Rc(1,2) Rc(1,3) Pc(1);Rc(2,1) Rc(2,2) Rc(2,3) Pc(2);Rc(3,1) Rc(3,2) Rc(3,3) Pc(3);0 0 0 1];
O_T_A1=O_T_C*C_T_B1*B_T_A1;
disp('解得手的位置如下:');
disp(O_T_A1);

只做这个也行

我做了GUI 但是回调函数SIN 和COS 不行

直接在回调函数下用sin就可以了