Auto Tooldata Adjustment

hookem245

I am attempting to write a calibration program that will automatically adjust tool data in the event of a crash which has caused the tool to partially move. This is for a vendor who doesn't know how to teach a tool center point. The intent is to tweak the tool data after a crash instead of reteaching hundreds of points. The method to do so uses a calibration rack with three pointers. The EOAT also has three pointers and the calibration program has two taught points. The points are with the pointers precisely aligned before and after the crash. The program attempts to calculate the delta between the two points and then applies this to the tool data.

I have had success when I only offset the tool in x, y, z, but when I rotate I run into trouble. Here is my current code:

PROC Calibration()

TrayGripper.tframe.trans.x:=50;

TrayGripper.tframe.trans.y:=0;

TrayGripper.tframe.trans.z:=100;

TrayGripper.tframe.rot.q1:=1;

TrayGripper.tframe.rot.q2:=0;

TrayGripper.tframe.rot.q3:=0;

TrayGripper.tframe.rot.q4:=0;

! The following is the original position of the EOAT

! at the alignment fixture during install. This

! should only be taught once, during original

! commisioning.

! MoveJ p10, v1000, z50, TrayGripper;

! Create pose10 from position and

! orientation data of robtargets p20.

pose10.trans:=p10.trans;

pose10.rot:=p10.rot;

! The following is the new position of the EOAT

! at the alignment fixture after it has moved. This

! should only be taught if something (i.e. a crash)

! has caused the EOAT to move with repect to the

! robot flange.

MoveJ p20, v1000, z50, TrayGripper;

! Create pose20 from position and

! orientation data of robtargets p20.

pose20.trans:=p20.trans;

pose20.rot:=p20.rot;

! Calculate pose30 using matrix math.

pose30:=PoseMult(pose10,PoseInv(pose20));

! Find the deltas in x, y, and z

pose30.trans.x:=pose10.trans.x-pose20.trans.x;

pose30.trans.y:=pose10.trans.y-pose20.trans.y;

pose30.trans.z:=pose10.trans.z-pose20.trans.z;

! compensate for rack orientation relative to tool orientation

pose40.trans.x:=pose30.trans.z*(-1);

pose40.trans.y:=pose30.trans.x;

pose40.trans.z:=pose30.trans.y*(-1);

pose40.rot.q1:=pose30.rot.q1;

pose40.rot.q2:=pose30.rot.q4;

pose40.rot.q3:=pose30.rot.q2;

pose40.rot.q4:=pose30.rot.q3*(-1);

GTrayToolPose.trans := TrayGripper.tframe.trans;

GTrayToolPose.rot := TrayGripper.tframe.rot;

GTrayToolPose:=PoseMult(pose40,GTrayToolPose);

TrayGripper.tframe.trans:=GTrayToolPose.trans;

TrayGripper.tframe.rot:=GTrayToolPose.rot;

ENDPROC

This code "sort of" works. If the only difference between the two taught points is in x, y, z and none in w, p, and r it works. Also if there ARE deltas in w, p, and r the tool orientation is correct after running this program, but the tool isn't offsetting in x, y, and z correctly.

What am I missing!?! Please help!

-Patrick

xerim

What is your "calibration" rack? Is it something like Bullseye that can trigger an I/O when the tool crosses a laser? If so, you could write a proc that triggers after an event such as a collision to redefine the TCP. One thing I noticed from your code that calculates the delta from two points is the zone size for your MoveJ commands is fairly large (z50), I would change it to a fine point to ensure you are in the same position every time you calculate new tooldata. I also wonder if your results would be better if you programmed this using RelTool() instead of pose.