TASKPERS tooldata Tooldata_1:=[TRUE,[[80,20,20],[1,0,0,0]],[1,[0,0,1],[1,0,0,0],0,0,0]];TASKPERS tooldata toolNew:=[TRUE,[[79.9999,19.9999,0],[1,0,0,0]],[0.001,[0,0,0.001],[1,0,0,0],0,0,0]];CONST jointtarget j10:=[[35.1052,69.519,1.50398E-5,-42.1947,0,0],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]];CONST jointtarget j20:=[[49.2684,75.0454,1.47516E-5,80.364,0,0],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]];   
  PROC main()      toolNew:=tool0;        !MoveAbsJ j10\NoEOffs,v1000,fine,tool0\WObj:=wobj0;      ! 记录位置1      !MoveAbsJ j20\NoEOffs,v1000,fine,tool0\WObj:=wobj0;      ! 记录位置2      ScaraTCPCalib j10,j20,toolNew;      stop;           ENDPROC    
  PROC ScaraTCPCalib(jointtarget j1,jointtarget j2,inout tooldata tool)        VAR num theta1;        VAR num theta2;        VAR dnum A{2,2};        VAR dnum B{2};        VAR dnum result{2};        VAR robtarget p1;        VAR robtarget p2;                p1:=CalcRobT(j1,tool0\WObj:=wobj0);        p2:=CalcRobT(j2,tool0\WObj:=wobj0);           ! 将joint值转为对应的tool0的位姿                    theta1:=EulerZYX(\z,p1.rot);        theta2:=eulerzyx(\z,p2.rot);                  A:=[[numtodnum(cos(theta1)-cos(theta2)),numtodnum(sin(theta2)-sin(theta1))],            [numtodnum(sin(theta1)-sin(theta2)),numtodnum(cos(theta1)-cos(theta2))]];        B:=[numtodnum(p2.trans.x-p1.trans.x),numtodnum(p2.trans.y-p1.trans.y)];        MatrixSolve A,B,result;        tool.tframe.trans.x:=dnumtonum(result{1});        tool.tframe.trans.y:=-dnumtonum(result{2});        ! 实际tool0的y方向与上图坐标系示意中的y方向相反，y此处取反ENDPROC