I've implemented the Karel programs MATRIX and INVERSE in TP. Downside is you have to sacrifice a user frame for it to work. I picked 9.
In the karel programs you would call it like so:
Code
: CALL MATRIX(89,90,91) ;Where PR[91]=PR[89]*PR[90].
The TP Program functions the same way, except for the name.
Code
: CALL MMULT(89,90,91) ;Same deal for inverse. Karel version is called like this:
Code
: CALL INVERSE(89,90) ;Where PR[90]=PR[89]'.
The TP Program functions the same way, except for the name.
Code
: CALL MINV(89,90) ;Here is the TP matrix multiply program:
Code
/PROG MMULT Macro
/ATTR
OWNER = MNEDITOR;
COMMENT = "Matrix Mul in TP";
PROG_SIZE = 2451;
CREATE = DATE 23-11-17 TIME 14:39:10;
MODIFIED = DATE 23-11-17 TIME 14:39:10;
FILE_NAME = FRAME_RO;
VERSION = 0;
LINE_COUNT = 63;
MEMORY_SIZE = 2863;
PROTECT = READ_WRITE;
TCD: STACK_SIZE = 0,
TASK_PRIORITY = 50,
TIME_SLICE = 0,
BUSY_LAMP_OFF = 0,
ABORT_REQUEST = 0,
PAUSE_REQUEST = 0;
DEFAULT_GROUP = 1,*,*,*,*;
CONTROL_CODE = 00000000 00000000;
/APPL
AUTO_SINGULARITY_HEADER;
ENABLE_SINGULARITY_AVOIDANCE : TRUE;
/MN
1: JMP LBL[1] ;
2: !******************************** ;
3: ! ;
4: !Program Name: MMULT ;
5: ! ;
6: !Program Description: Preforms ;
7: ! matrix multiplication of two ;
8: ! PRs, and stores the result in ;
9: ! an additional PR. ;
10: !Caveats: ;
11: ! This requires a free user ;
12: ! frame to do the matrix ;
13: ! conversion. The default in ;
14: ! this program is 9. ;
15: ! ;
16: !Program Arguments: ;
17: ! AR[1]=The 'A' PR index. ;
18: ! AR[2]=The 'B' PR index. ;
19: ! AR[3]=The 'C' PR index. Where ;
20: ! C = A : B ;
21: ! ;
22: !******************************** ;
23: LBL[1:Start] ;
24: ;
25: --eg: Tell the robot to store frames as transform (matrix) rep. ;
26: $PR_CARTREP=(0) ;
27: ;
28: --eg: Convert the passed in PRs to transform rep. ;
29: UFRAME[9:UFrame9]=PR[AR[1]] ;
30: PR[AR[1]]=UFRAME[9:UFrame9] ;
31: UFRAME[9:UFrame9]=PR[AR[2]] ;
32: PR[AR[2]]=UFRAME[9:UFrame9] ;
33: PR[AR[3]]=LPOS-LPOS ;
34: UFRAME[9:UFrame9]=PR[AR[3]] ;
35: PR[AR[3]]=UFRAME[9:UFrame9] ;
36: ;
37: --eg: Preform matrix multiplication ;
38: --eg: Ref:
: http://www.c-jump.com/bcc/common/Talk3/Math/Matrices/W01_0150_matrix_by_matrix_mult.htm ;
39: PR[AR[3],1]=((PR[AR[1],1]*PR[AR[2],1])+(PR[AR[1],4]*PR[AR[2],2])+(PR[AR[1],7]*PR[AR[2],3])) ;
40: PR[AR[3],2]=((PR[AR[1],2]*PR[AR[2],1])+(PR[AR[1],5]*PR[AR[2],2])+(PR[AR[1],8]*PR[AR[2],3])) ;
41: PR[AR[3],3]=((PR[AR[1],3]*PR[AR[2],1])+(PR[AR[1],6]*PR[AR[2],2])+(PR[AR[1],9]*PR[AR[2],3])) ;
42: PR[AR[3],4]=((PR[AR[1],1]*PR[AR[2],4])+(PR[AR[1],4]*PR[AR[2],5])+(PR[AR[1],7]*PR[AR[2],6])) ;
43: PR[AR[3],5]=((PR[AR[1],2]*PR[AR[2],4])+(PR[AR[1],5]*PR[AR[2],5])+(PR[AR[1],8]*PR[AR[2],6])) ;
44: PR[AR[3],6]=((PR[AR[1],3]*PR[AR[2],4])+(PR[AR[1],6]*PR[AR[2],5])+(PR[AR[1],9]*PR[AR[2],6])) ;
45: PR[AR[3],7]=((PR[AR[1],1]*PR[AR[2],7])+(PR[AR[1],4]*PR[AR[2],8])+(PR[AR[1],7]*PR[AR[2],9])) ;
46: PR[AR[3],8]=((PR[AR[1],2]*PR[AR[2],7])+(PR[AR[1],5]*PR[AR[2],8])+(PR[AR[1],8]*PR[AR[2],9])) ;
47: PR[AR[3],9]=((PR[AR[1],3]*PR[AR[2],7])+(PR[AR[1],6]*PR[AR[2],8])+(PR[AR[1],9]*PR[AR[2],9])) ;
48: PR[AR[3],10]=((PR[AR[1],1]*PR[AR[2],10])+(PR[AR[1],4]*PR[AR[2],11])+(PR[AR[1],7]*PR[AR[2],12])+PR[AR[1],10]) ;
49: PR[AR[3],11]=((PR[AR[1],2]*PR[AR[2],10])+(PR[AR[1],5]*PR[AR[2],11])+(PR[AR[1],8]*PR[AR[2],12])+PR[AR[1],11]) ;
50: PR[AR[3],12]=((PR[AR[1],3]*PR[AR[2],10])+(PR[AR[1],6]*PR[AR[2],11])+(PR[AR[1],9]*PR[AR[2],12])+PR[AR[1],12]) ;
51: ;
52: --eg:Tell the robot to store frames as cart rep. ;
53: $PR_CARTREP=(1) ;
54: ;
55: --eg:Set the original PRs and the result back to cartesian rep. ;
56: UFRAME[9:UFrame9]=PR[AR[1]] ;
57: PR[AR[1]]=UFRAME[9:UFrame9] ;
58: UFRAME[9:UFrame9]=PR[AR[2]] ;
59: PR[AR[2]]=UFRAME[9:UFrame9] ;
60: UFRAME[9:UFrame9]=PR[AR[3]] ;
61: PR[AR[3]]=UFRAME[9:UFrame9] ;
62: ;
63: LBL[32766:End of Program] ;
/POS
/ENDAnd here is the TP inverse program:
Code
/PROG MINV Macro
/ATTR
OWNER = MNEDITOR;
COMMENT = "Matrix Inv in TP";
PROG_SIZE = 1905;
CREATE = DATE 23-11-18 TIME 08:46:34;
MODIFIED = DATE 23-11-18 TIME 08:46:34;
FILE_NAME = FRAME_RO;
VERSION = 0;
LINE_COUNT = 58;
MEMORY_SIZE = 2337;
PROTECT = READ_WRITE;
TCD: STACK_SIZE = 0,
TASK_PRIORITY = 50,
TIME_SLICE = 0,
BUSY_LAMP_OFF = 0,
ABORT_REQUEST = 0,
PAUSE_REQUEST = 0;
DEFAULT_GROUP = 1,*,*,*,*;
CONTROL_CODE = 00000000 00000000;
/APPL
AUTO_SINGULARITY_HEADER;
ENABLE_SINGULARITY_AVOIDANCE : TRUE;
/MN
1: JMP LBL[1] ;
2: !******************************** ;
3: ! ;
4: !Program Name: MINV ;
5: ! ;
6: !Program Description: Preforms ;
7: ! matrix inversion of the ;
8: ! passed in PR, and stores the ;
9: ! result in an additional PR. ;
10: !Caveats: ;
11: ! This requires a free user ;
12: ! frame to do the matrix ;
13: ! conversion. The default in ;
14: ! this program is 9. ;
15: ! ;
16: !Program Arguments: ;
17: ! AR[1]=The 'A' PR index. ;
18: ! AR[2]=The 'B' PR index. Where ;
19: ! B = A' ;
20: ! ;
21: !******************************** ;
22: LBL[1:Start] ;
23: ;
24: --eg: Tell the robot to store frames as transform (matrix) rep. ;
25: $PR_CARTREP=(0) ;
26: ;
27: --eg: Convert the passed in PRs to transform rep. ;
28: UFRAME[9:UFrame9]=PR[AR[1]] ;
29: PR[AR[1]]=UFRAME[9:UFrame9] ;
30: PR[AR[2]]=LPOS-LPOS ;
31: UFRAME[9:UFrame9]=PR[AR[2]] ;
32: PR[AR[2]]=UFRAME[9:UFrame9] ;
33: ;
34: --eg: Preform matrix inversion ;
35: --eg: Ref:
: http://www-graphics.stanford.edu/courses/cs248-98-fall/Final/q4.html ;
36: PR[AR[2],1]=(PR[AR[1],1]) ;
37: PR[AR[2],2]=(PR[AR[1],4]) ;
38: PR[AR[2],3]=(PR[AR[1],7]) ;
39: PR[AR[2],4]=(PR[AR[1],2]) ;
40: PR[AR[2],5]=(PR[AR[1],5]) ;
41: PR[AR[2],6]=(PR[AR[1],8]) ;
42: PR[AR[2],7]=(PR[AR[1],3]) ;
43: PR[AR[2],8]=(PR[AR[1],6]) ;
44: PR[AR[2],9]=(PR[AR[1],9]) ;
45: PR[AR[2],10]=(((-1)*PR[AR[1],1]*PR[AR[1],10])-(PR[AR[1],2]*PR[AR[1],11])-(PR[AR[1],3]*PR[AR[1],12])) ;
46: PR[AR[2],11]=(((-1)*PR[AR[1],4]*PR[AR[1],10])-(PR[AR[1],5]*PR[AR[1],11])-(PR[AR[1],6]*PR[AR[1],12])) ;
47: PR[AR[2],12]=(((-1)*PR[AR[1],7]*PR[AR[1],10])-(PR[AR[1],8]*PR[AR[1],11])-(PR[AR[1],9]*PR[AR[1],12])) ;
48: ;
49: --eg:Tell the robot to store frames as cart rep. ;
50: $PR_CARTREP=(1) ;
51: ;
52: --eg:Set the original PRs and the result back to cartesian rep. ;
53: UFRAME[9:UFrame9]=PR[AR[1]] ;
54: PR[AR[1]]=UFRAME[9:UFrame9] ;
55: UFRAME[9:UFrame9]=PR[AR[2]] ;
56: PR[AR[2]]=UFRAME[9:UFrame9] ;
57: ;
58: LBL[32766:End of Program] ;
/POS
/ENDAlso attached is a .zip file of the .TP and .LS files. TP is compiled on V9.30 HandlingTool.
Limitations/Caveats:
- Frame 9 is used to do conversions. If you are using frame 9, modify the top and bottom of each program to use a frame that is free on your system. Also, label it as "RESERVED" or something like that.
- Not sure if it works on JPOS rep points.
- All index arguments must be unique. The programs use the result as a working area for calculations, and would screw up the math.
- Don't do something like this: CALL MINV(90,90)
