I need a way to find a taught position's rotation about the Z axis. After doing some reading on transformation matrices, euler angles, gimble lock, and all that sort of thing, I think my best option is to hope that somebody smarter than me has figured out a good way to convert the WPR values into the actual rotation about the Z axis...
Finding rotation around Z axis

TitusLepic 
July 11, 2019 at 11:39 PM 
Thread is marked as Resolved.

 Go to Best Answer

Do you need to do this in the program or would an offline calculator work?
Do you need a general mathematical solution or is it just in a certain range of orientations? In some orientations it will match the r angle from LPOS. If your situation matches this then that would be an easy way.

Interesting.
Personally, I would convert the point to a rotation matrix, then derive the rotation from the x or y rotation vectors. You would have to add special cases to the math in the event that one of those is lined up with the base Z vector.
Online, you could do this in Karel, and maybe (heavy maybe) in tp. Offline would be pretty simple.

I need a general online solution. I'm going to be very close to tool Z being aligned with User X in a lot of these situations so using the R from LPOS won't work.
My application is cutting slots (and in the future, rectangles) into a workpiece with a router. I'd like the operator to only have to teach the center point of the slot and have the robot do the rest programmatically. This is motion I want the robot to take:
L {LPOS} 100mm/s FINE Offset {slot_length/2*sin(zRot),slot_length/2*cos(zRot),0,0,0,0) Tool_Offset {0,0,plunge_depth,0,0,0}
L {LPOS} 100mm/s FINE Offset {slot_length/2*sin(zRot),slot_length/2*cos(zRot),0,0,0,0) Tool_Offset {0,0,plunge_depth,0,0,0}
L {LPOS} 100mm/s FINEIf there's a better/easier way than trying to figure out zRot I'm all ears.

I think I've got your solution.
Make the moves all tool offsets from the starting point. Then your slot, rectangle, or any shape you program is always relative to the tool orientation, without you needing to do any complex math.

I thought about that, the problem with that method is that I can't control the rotation about tool Z when the operator teaches the point, and I need the slot to be parallel to the user XY plane. The tool offset method will only work if the tool X axis is parallel to user XY.

You could train them to correctly orient the tool. Maybe even have an arrow pointing or pictures in an instruction manual.
If they need a diagonal slot or routing on a slanted surface then tool offsets would cover that situation.
If you only want orthagonal then I would suggest creating points with preset orientations for the different faces. Then they could select the from a starting orientation and jog x,y,z to the starting point.

Here's my solution if anyone's interested. I used Karel for sin, cos, and atan but the rest is TP:
1: PR[90:LPOS]=LPOS ;
2: UFRAME[9]=PR[90:LPOS] ;
3: PR[91:MATRIXPOS]=UFRAME[9] ;
4: R[26:TEMP]=PR[91,9:MATRIXPOS] ;
5: IF R[26:TEMP]=1 OR R[26:TEMP]=(1),JMP LBL[1] ;
6: R[26:TEMP]=PR[91,7:MATRIXPOS]/PR[91,8:MATRIXPOS] ;
7: CALL M_ATAN(R[26:TEMP],25) ;
8: PR[92:PR_OFFSET]=LPOS ;
9: PR[92:PR_OFFSET]=PR[92:PR_OFFSET]PR[92:PR_OFFSET] ;
10: CALL M_SIN(R[25:Angle],26) ;
11: PR[92,2:PR_OFFSET]=R[68:Linear Overflow]*R[26:TEMP]*(1)/2 ;
12: CALL M_COS(R[25:Angle],26) ;
13: PR[92,1:PR_OFFSET]=R[68:Linear Overflow]*R[26:TEMP]/2 ;
14: PR[93:TOOL_OFFSET]=PR[93:TOOL_OFFSET]PR[93:TOOL_OFFSET] ;
15: PR[93,3:TOOL_OFFSET]=25.4 ;
16:L PR[90:LPOS] 15mm/sec FINE Offset,PR[92:PR_OFFSET]
: Tool_Offset,PR[93:TOOL_OFFSET] ;
17: PR[92,1:PR_OFFSET]=PR[92,1:PR_OFFSET]*(1) ;
18: PR[92,2:PR_OFFSET]=PR[92,2:PR_OFFSET]*(1) ;
19:L PR[90:LPOS] 30mm/sec FINE Offset,PR[92:PR_OFFSET]
: Tool_Offset,PR[93:TOOL_OFFSET] ;
20:L PR[90:LPOS] 30mm/sec FINE ;
21: LBL[1] ; 
Can you modify the User Frame X and Y values then "move" to the same point that the operator touches up? We've done this for some applications picking parts out of a tray. Store the original User Frame values in a PR to restore later or modify the values of another User Frame that is set to the original at the start of the cut.

Allo Titus
Can you share the Karel to go with this?
I think i can make use of this on my applications.
Thanks

 Best Answer
I made separate TP programs for each trig operation. Below is the program for sin, this just makes for easier readability at the point of use.
And here's the karel code. Hope this helps with your project.
Code
Display MorePROGRAM TRIGFUNC %COMMENT = 'Trig Function' %NOLOCKGROUP %ENVIRONMENT REGOPE %ENVIRONMENT MULTI CONST EN_SIN = 1 EN_COS = 2 EN_TAN = 3 EN_ASIN = 4 EN_ACOS = 5 EN_ATAN = 6 VAR argVal : REAL returnVal : REAL returnReg : INTEGER prmType : INTEGER intVal : INTEGER operation : INTEGER realVal : REAL strVal : STRING [2] STATUS : INTEGER BEGIN get argument GET_TPE_PRM(1,prmType,intVal,argVal,strVal,STATUS) IF prmType = 3 THEN WRITE TPERROR('TRIG ERROR', cr) abort_task('*ALL*', true, true, STATUS) ENDIF IF prmType = 1 THEN argVal = intVal ENDIF get return register GET_TPE_PRM(2,prmType,returnReg,realVal,strVal,STATUS) IF prmType <> 1 THEN WRITE TPERROR('TRIG ERROR') abort_task('*ALL*', true, true, STATUS) ENDIF get operation GET_TPE_PRM(3,prmType,operation,realVal,strVal,STATUS) IF prmType <> 1 THEN WRITE TPERROR('TRIG ERROR') abort_task('*ALL*', true, true, STATUS) ENDIF SELECT operation OF CASE (EN_SIN): returnVal =SIN(argVal) CASE (EN_COS): returnVal = COS(argVal) CASE (EN_TAN): returnVal = TAN(argVal) CASE (EN_ASIN): returnVal = ASIN(argVal) CASE (EN_ACOS): returnVal = ACOS(argVal) CASE (EN_ATAN): returnVal = ASIN(argVal/SQRT(1+argVal*argVal)) ENDSELECT SET_REAL_REG(returnReg,returnVal,STATUS) END TRIGFUNC