No Karel or Vision Tools? Now introducing Matrix Math in TP!

Nation · November 18, 2023 at 7:40 PM Best Answer

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
/END

Display More

And 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
/END

Display More

Also 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)

DS186 · March 3, 2024 at 11:02 PM

Nation,

Thank you very much for sharing! I have a quick question. I couldn't find MATRIX and INVERSE (built-In) functions in the KAREL reference manual (Rev. L). I have only found INV which is probably the INVERSE function you are talking about. What are the exact KAREL functions you are referring to?

Nation · March 4, 2024 at 11:44 PM

Not quite. It is replacing karel programs you call from TP. Not commands within Karel.

Both MATRIX and INVERSE can be found with the Vision Support Tools Option, and is usually included with 3D vision packages.

jorgefernandes · April 5, 2024 at 5:55 PM

Hello. Very nice implementation. Thanks for sharing.

I tried to use the MINV macro in a group with an external axis, which gave me an error INTP-311, saying that the PR it is uninitialized.

Did those functions work with the external axis?

The error occurs on line 53 on MINV macro.

Nation · April 5, 2024 at 9:22 PM

Quote from jorgefernandes

Hello. Very nice implementation. Thanks for sharing.
I tried to use the MINV macro in a group with an external axis, which gave me an error INTP-311, saying that the PR it is uninitialized.
Did those functions work with the external axis?
The error occurs on line 53 on MINV macro.

I haven't tested it on robots with external axis. You may either need to add a group mask, or manually populate the uninitialized element with a zero.

jorgefernandes · April 8, 2024 at 12:33 PM

Quote from Nation

I haven't tested it on robots with external axis. You may either need to add a group mask, or manually populate the uninitialized element with a zero.

I populate the value manually. Thanks

No Karel or Vision Tools? Now introducing Matrix Math in TP!

Advertising from our partners

Anyware Robotics is hiring!

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