Fanuc- Pos2Joint

I do this in karel in this post.

I never was able to get POS2JOINT to work. Casting the variables worked though.

I got it working for me at some point.

The reason I needed it was to check to see if a linear move was going to have a config or singularity issue.

my code:

This should be called from a TP program as:

: CALL K_POS2JNT( 1, 2, 3, 4) ;

Where PR[1] will have the joint result
PR[2] is the position register with the caretsian point to convert
3 is the number of the UFRAME
4 is the number of the UTOOL

PROGRAM K_POS2JNT
-- translator directive
%COMMENT = 'Cartesian to Joint Coord Conversion'

-- program vars
VAR
result : INTEGER
status : INTEGER
data_type : INTEGER
int_value : INTEGER
real_value : REAL
str_value : STRING[10]
dummyConfig : CONFIG
dummyArray : ARRAY[1] OF REAL

currentJPOS : JOINTPOS
cartPos : POSITION
UF_Ref : POSITION
UT_Ref :POSITION
jointResult : JOINTPOS
resultPR : INTEGER
cartPosPR : INTEGER
UF_Number : INTEGER
UT_Number : INTEGER

BEGIN

-- Retrieve TP Arguments
GET_TPE_PRM(1, data_type, resultPR, real_value, str_value, result)
GET_TPE_PRM(2, data_type, cartPosPR, real_value, str_value, result)
GET_TPE_PRM(3, data_type, UF_Number, real_value, str_value, result)
GET_TPE_PRM(4, data_type, UT_Number, real_value, str_value, result)

-- Get Robot's current JPOS
currentJPOS = CURJPOS(0,0)

-- Get Cartesian Position to Convert
cartPos = GET_POS_REG(cartPosPR,result)
UF_Ref = $MNUFRAME[1, UF_Number]
UT_Ref = $MNUTOOL[1, UT_Number]

-- Call Built-In Conversion Function
POS2JOINT(currentJPOS,cartPos,UF_Ref,UT_Ref,0,dummyConfig,dummyArray,jointResult,status)

-- Return Result to TP Position Registers
SET_JPOS_REG(resultPR, jointResult, result)

END K_POS2JNT

Big Thanks also from me to Davvoo22.

Your program was not working in the beginning (Anyhow the GET_TPE_PRM command was not working). I had to do some minor modifications. I will post the program in case that other people have the same issues.

I just recognized that i still have a problem if the UserFrame is 0.

Do someone have any idea ?

Greetings

----

PROGRAM K_POS2JNT

-- translator directive

%COMMENT = 'Cartesian to Joint Coord Conversion'

-- program vars

VAR

result : INTEGER

status : INTEGER

data_type : INTEGER

int_value : INTEGER

real_value : REAL

str_value : STRING[10]

dummyConfig : CONFIG

dummyArray : ARRAY[1] OF REAL

currentJPOS : JOINTPOS

cartPos : POSITION

UF_Ref : POSITION

UT_Ref :POSITION

jointResult : JOINTPOS

resultPR : INTEGER

cartPosPR : INTEGER

UF_Number : INTEGER

UT_Number : INTEGER

BEGIN

-- Retrieve TP Arguments

-- GET_TPE_PRM(1, data_type, resultPR, real_value, str_value, result)
-- GET_TPE_PRM(2, data_type, cartPosPR, real_value, str_value, result)
-- GET_TPE_PRM(3, data_type, UF_Number, real_value, str_value, result)
-- GET_TPE_PRM(4, data_type, UT_Number, real_value, str_value, result)

resultPR = 1

cartPosPR = 2

UF_Number = $MNUFRAMENUM[1] -- get the actual UserFrame

UT_Number = $MNUTOOLNUM[1] -- ge the actual ToolFrame

WRITE('UF_Number: ', UF_Number)

WRITE('UT_Number: ', UT_Number, CR)

-- Get Robot's current JPOS

currentJPOS = CURJPOS(0,0)

-- Get Cartesian Position to Convert

cartPos = GET_POS_REG(cartPosPR,result)

UF_Ref = $MNUFRAME[1, UF_Number]

UT_Ref = $MNUTOOL[1, UT_Number]

-- Call Built-In Conversion Function

POS2JOINT(currentJPOS,cartPos,UF_Ref,UT_Ref,0,dummyConfig,dummyArray,jointResult,status)

-- Return Result to TP Position Registers

SET_JPOS_REG(resultPR, jointResult, result)

END K_POS2JNT

Thanks Davvoo22, the program is working for me.

I observed that it does not work in some case.

For exemple when :

PR[2] = [360.0, 0.0, 280.0, 180.0, -90.0, 0.0]

In this case POS2JOINT returns status = 15023 (=> MOTN-023 In singularity, The position is near a singularity point)

Do you know how to solve this? Is there another built-in function or method to be able to convert any cartesian position to joint?

HI CPh,

At the moment i am on vacations and can only check it if i am back in the company (25.).

Did you include the Values to PR[2] by editing or are they come from the robot ? I guess it could be a problem with the config of the position in this case.

Greetings

Wiggi

Hi Wiggi,

Sorry, for my late reply..

If I understood well, yes I have written the values manualy in PR[2].

Today I tried to record them with the teach pendant (Enable Singularity Avoidance : TRUE)

I made a simple TPE program

L PR[2] 100mm/sec FINE

Then I opened the PR DATA menu, selected PR[2] and pressed on RECORD.

The result is the same, I get the error 15023.

(btw, PR[2] = [360.0, 0.0, 280.0, 180.0, -90.0, 0.0] (NUT000) is in fact == to PR[2] = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] in JNT coordinates)

I guess there is nothing we/Fanuc can do to make the conversion work in case of a singularity, there is indeed an infinity of JOINT (J4,J6) values possible for this cartesian position.

The program is working well for other positions :

e.g. PR[2] = [360.0, 0.0, 280.0, 180.0, -85.0, 0.0] (NUT000)

Enjoy your vacations!

Hey Wiggi.. Yeah the problem you had was possibly because a lot of the time I assume that people will sent a register that is setup as an integer.. But sometimes those registers can turn into pesky floats if you aren't careful.

You can check it on the routine:

data_type indicates the data type for the parameter, as follows:

1 : INTEGER

2: REAL

3: STRING

To make the code more robust you should probably change it to this

GET_TPE_PRM(3, data_type, Int_value, real_value, str_value, result)

IF data_type = 2 THEN

UF_Number = ROUND(real_value)

ELSE

UF_Number = int_value

ENDIF

But again your way is fine as long as you have the right frames active when you call the function.

I'm afraid I don't know about the frame 0. Seems strange that it wouldn't work. I would suggest you just pick a random empty frame and use that instead.

CPh

I never really meant this function for other people when I wrote it so actually only spent about 2 hours writing this routine. Hence it doesn't check for any errors etc.

For my application I got it working with the parameters as they were. If I was you though I imagine that you will need to tweak these parameters

POS2JOINT (in_pos, uframe, utool, config_ref, wjnt_cfg, ext_ang, out_jnt, and status).

I would potentially try using a cur_pos call and getting the current config you are in to replace my dummy variable I used.

Also possible that it simply cannot be done. The singularity avoidance deliberately drives around the singularities and doesn't follow the correct path.