TCP Update

bull_boxer454 · November 11, 2020 at 6:30 PM

Hello,

I am looking to make a routine using RAPID programming to modify the TCP of the robot relative to a known point. The robot is an ABB with an IRC5 controller

We use a stationary pointer to verify the TCP of one of our robot tools. Occasionally after maintenance is done to the tool, the tip is off by a couple millimeters. If the tip is not aligned with the pointer, I would like to jog the robot to realign it, then run a TCP update routine to modify the tooldata so that it aligns with the pointer without changing the original robtarget.

I am using the CRobT function with a variable robtarget to get the new robot position and subtracting the pos data from the original point to get the difference. To get the orient data, I am converting to Euler angles and doing the subtraction, then converting back to quaternion. I am having an issue when it comes to modifying the data of the tool. The data in the robtargets are relative to the work object. Is there a way that the calculated difference can be aligned robot tool? Or a way to align the tooldata and wobjdata? I have tried using PoseMult, but didn't get the results that I am looking for. PoseMult is a new function for me, so I may not have been using it correctly

Here is my code for a better description:

!Tip position. Made with tool0 and wObj0 for calculation purposes

MoveJ pTCPcheck10, v50, fine, tool0\WObj:=wobj0;

!This is a separate routine. The above is to show the point for calculation

PROC rUpdateTCP()

CalculationPoint:=CRobT(\Tool:=tool0,\WObj:=wobj0);

!Convert from quaternions to Euler angles

Check_Anglex:=EulerZYX(\X,pTCPcheck10.rot);

Check_Angley:=EulerZYX(\Y,pTCPcheck10.rot);

Check_Anglez:=EulerZYX(\Z,pTCPcheck10.rot);

Calc_Anglex:=EulerZYX(\X,CalculationPoint.rot);

Calc_Angley:=EulerZYX(\Y,CalculationPoint.rot);

Calc_Anglez:=EulerZYX(\Z,CalculationPoint.rot);

!Calculate Angle Difference

AngleDifferencex:=Calc_Anglex-Check_Anglex;

AngleDifferencey:=Calc_Angley-Check_Angley;

AngleDifferencez:=Calc_Anglez-Check_Anglez;

!Calculate position difference

Xdiff:=CalculationPoint.trans.x-pTCPcheck10.trans.x;

Ydiff:=CalculationPoint.trans.y-pTCPcheck10.trans.y;

Zdiff:=CalculationPoint.trans.z-pTCPcheck10.trans.z;

!Calculate Tool Euler Angles

ToolAnglex:=EulerZYX(\X,tMyTool.tframe.rot);

ToolAngley:=EulerZYX(\Y,tMyTool.tframe.rot);

ToolAnglez:=EulerZYX(\Z,tMyTool.tframe.rot);

!Apply angle difference to the tool

UpdatedToolAnglex:=ToolAnglex-AngleDifferencex;

UpdatedToolAngley:=ToolAngley-AngleDifferencey;

UpdatedToolAnglez:=ToolAnglez-AngleDifferencez;

!Make pose out of the difference for frame manipulation

pose_FrameDifference.trans.x:=XDiff;

pose_FrameDifference.trans.y:=YDiff;

pose_FrameDifference.trans.z:=ZDiff;

pose_FrameDifference.rot:=OrientZYX(AngleDifferencez,AngleDifferencey,AngleDifferencex);

!attempted, but did not work

!tMyToolUpdated.tframe:=PoseMult(tMyTool.tframe,pose_FrameDifference);

!Convert Euler angles to quaternions and change the data in the updated tool

tMyToolUpdated.tframe.rot:=OrientZYX(UpdatedToolAnglez,UpdatedToolAngley,UpdatedToolAnglex);

tMyToolUpdated.tframe.trans:=tMyTool.tframe.trans+pose_FrameDifference.trans;

!Copy the load data of the tool

tMyToolUpdated.tload:=tMyTool.tload;

ENDPROC

Lemster68 · November 11, 2020 at 7:07 PM

I am not the original author of these codes, I "borrowed" them. They should be of use to you.

Code

  PROC UTL_toolAutoUpd(
    PERS tooldata Tool,
    robtarget Ref_Point,
    robtarget Cur_Point)

    VAR pose ppnt:=[[0,0,0],[1,0,0,0]];
    VAR pose ppntnew:=[[0,0,0],[1,0,0,0]];
    VAR pose pa:=[[0,0,0],[1,0,0,0]];

    ppnt.trans:=Ref_Point.trans;
    ppnt.rot:=Ref_Point.rot;
    ppntnew.trans:=Cur_Point.trans;
    ppntnew.rot:=Cur_Point.rot;
    pa:=PoseMult(ppntnew,PoseInv(Tool.tframe));
    Tool.tframe:=PoseMult(PoseInv(pa),ppnt);
  ENDPROC

  PROC Glue_TCP_Check()
    CONST string astTCPData{3}:=["The following adjustments","to the TCP will be made:"," "];
    CONST string astJogTooFar{6}:=["Your attempt at updating the TCP","has failed. ","    ","Jogging the robot more than 12mm","in any direction is not allowed.","    "];
    CONST string astTCPLocPrompt{6}:=["Pointer location has been modified.","Pointer location and present","tooldata have been saved.","Tool corrections will be made based on","this tool and pointer location.","Do not modify pointer location again."];
    CONST errnum erJoggedTooFar:=99;
    VAR robtarget rTool0Ref;
    VAR robtarget Before;
    VAR robtarget After;
    VAR robtarget Difference;
    VAR num nAnswer;
    VAR num nDummy;
    VAR pos posOldTool;

    ! Manually check and possibly make 
    ! adjustments to the Tool Center Point
    posOldTool:=tGlue.tframe.trans;
    FromParking;
    MoveJ Offs(Glue_On_Pointer,0,0,300),v400,z50,tGlue;
    MoveL Offs(Glue_On_Pointer,0,0,300),v400,z50,tGlue;
    MoveL Offs(Glue_On_Pointer,0,0,50),v100,z5,tGlue;
    MoveL Glue_On_Pointer,v50,fine,tGlue;
    IF VectMagn((Glue_On_Pointer.trans-poStoredFrame1.trans))>0.25 THEN
      poStoredFrame1.trans:=Glue_On_Pointer.trans;
      poStoredFrame1.rot:=Glue_On_Pointer.rot;
      TPWriteInfo astTCPLocPrompt;
      TPReadFK nDummy,"Setup is done.","","","","OK","";
    ENDIF
    WaitTime\InPos,0.3;
    Before:=CRobT();
    StopMove;
    StorePath;
    Stop;
    !Jog the robot to the TCP locator,
    !then restart the program.
    StartMove;
    RestoPath;
    After:=CRobT();
    WaitTime\InPos,0.3;
    Difference.trans:=(Before.trans)-(After.trans);
    IF (Abs(Difference.trans.x)>12) OR (Abs(Difference.trans.y)>12) OR (Abs(Difference.trans.z)>12) RAISE erJoggedTooFar;
    rTool0Ref:=CRobT(\Tool:=tool0);
    tGlue.tframe:=PoseMult((PoseInv([rTool0Ref.trans,rTool0Ref.rot])),poStoredFrame1);
    TPWriteInfo astTCPData;
    TPWrite "X = "\Num:=Round((posOldTool.x-tGlue.tframe.trans.x)\Dec:=2);
    TPWrite "Y = "\Num:=Round((posOldTool.y-tGlue.tframe.trans.y)\Dec:=2);
    TPWrite "Z = "\Num:=Round((posOldTool.z-tGlue.tframe.trans.z)\Dec:=2);
    TPReadFK nAnswer,"","","","","Accept","Reject";
    IF nAnswer=5 tGlue.tframe.trans:=posOldTool;
    MoveL Offs(Glue_On_Pointer,0,0,50),v100,fine,tGlue;
    ToParking;
  ERROR
    IF ERRNO=erJoggedTooFar THEN
      TPWriteInfo astJogTooFar;
      TPReadFK nDummy,"","","","","OK","";
    ENDIF
    MoveL Offs(Glue_On_Pointer,0,0,50),v100,z5,tGlue;
    ToParking;
  ENDPROC

Display More

hermann · November 11, 2020 at 7:41 PM

First thing I see, is that you use Tool0 and wobj0 for calculations, Think this is absolutely not leading to the goal.

For aligning wobj and tool you can try to align the workobject to the acual position (real tcp in wobj0), then you should have the distance in tool coordinates:

Code

actPoint:=CRobT(\Tool:=tMyTool,\WObj:=wobj0);
Wobj1:=wobj0;
Wobj1.uframe.trans:=actPoint.trans;
Wobj1.uframe.rot:=actPoint.rot;
! here the actual position in the aligned wobj
newPoint:=CRobT(\Tool:=tMyTool,\WObj:=wobj1);

bull_boxer454 · November 11, 2020 at 7:52 PM

Thanks Lemster68! This has shown me what I need to do

Dodo41 · January 29, 2024 at 10:27 PM

.

TCP Update

Advertising from our partners

How to see your Job Posting (search or recruit) here in Robot-Forum.com

Similar Threads

ORANGE KUKA SCREEN

SC1 Not Synchronized Fault

Fanuc robot software version?

How to quickly/conveniently update a points coordinate inside a DAT file

IRC5 loses rev counters, date/time?

How to Change IRC 5 Main Drive Unit

Revolution counters update problem

Tags

Users Viewing This Thread