search

torayeff / fanucpy

Python Interface for FANUC robots
Apache License 2.0
109 stars 29 forks source link

USERFRAME coordinate and GET_CURPOS function issue!!! #22

Closed madinwei closed 1 year ago

madinwei commented 1 year ago

Hello, @torayeff , Hello, I have an inquiry about how the GET_CURPOS work,

as I understand if I set up a specific USERFRAME according to my need, then I just need to send the positions value according to my userframe. however, every time I use robot.move function with a position in my modified userframe it gives me a 'position-is-not-reachable' error. So I moved my robot manually to the 0,0,0 position in my userframe, and used the GET_CURPOS function to retrieve my robot position, but the position retrieved is not the same as my userframe , the other userframe , or toolframe. so on which userframe or coordinate does GET_CURPOS is based on? and in general how i can make robot.move to move the robot on my userframe.

I hope you can give any help or advice, I am still a novice so any help will be great for me.

torayeff commented 1 year ago

@madinwei fanucpy driver uses the USER FRAME=8 and TOOL FRAME=8. Have you tried setting up those?

https://github.com/torayeff/fanucpy/blob/4fe61fcab11ed73aa0eb6daaca57c209ff741c0e/src/fanuc-driver/mappdk_server.kl#L30 https://github.com/torayeff/fanucpy/blob/4fe61fcab11ed73aa0eb6daaca57c209ff741c0e/src/fanuc-driver/mappdk_server.kl#L46

madinwei commented 1 year ago

@torayeff thank you so much for you respond and help.

@madinwei fanucpy driver uses the USER FRAME=8 and TOOL FRAME=8. Have you tried setting up those?

https://github.com/torayeff/fanucpy/blob/4fe61fcab11ed73aa0eb6daaca57c209ff741c0e/src/fanuc-driver/mappdk_server.kl#L30

https://github.com/torayeff/fanucpy/blob/4fe61fcab11ed73aa0eb6daaca57c209ff741c0e/src/fanuc-driver/mappdk_server.kl#L46

Yes, I have changed the user frame in the mappdk_server.kl, however, the position returned from GET_CURPOS is not in the user frame I set. for example: if use this code: ( note : i set the robot to the same position before i start the server, otherwise it will give me a 'position-is-not-reachable' error.)

robot.move(
    "pose",
    vals=[103.190,-31.350,-839.809,61.272,8.277,-144.168],
    velocity=100,
    acceleration=100,
    cnt_val=0,
    linear=False,
)
# get robot state
print("Current poses: ")
cur_pos = robot.get_curpos()
print(f"Current pose: {cur_pos}")

and the output of cur_pos is : 

Current pose: [539.652, 187.344, 826.891, -116.788, -13.564, -93.801]

so the GET_CURPOS is not using the same userframe , or other userframe I check them all and no one has the same position value, both by checking roboguide or changing userframe in mappdk_server.kl and using the GET_CURPOS.

for me I want to learn how to move the robot with my userframe, if GET_CURPOS doesn't work its ok. so if you have any advice or help i will be thankful .

madinwei commented 1 year ago

update: after I go checked the mappdk_cmd.kl file to see what is wrong with GET_CURPOS, I noticed that the first function or ROUTINE is not building, so i swap the GET_CURPOS with GET_CURJPOS function positions in the code. and now the GET_CURJPOS is not working. so I just add a new fake function and now the GET_CURPOS returns the correct position. I don't know why this happened, but here is the error for the first function of mappdk_cmd.kl .

Translating D:\Users\admin\Doucment\My Workcells\WeldPRO5\loadset\mappdk_cmd.kl

KTRANS Version V9.10 (Build 14   4/12/2019)
Copyright (C) FANUC America Corporation, 1985 through 2017.
All Rights Reserved.

D:\Users\admin\Doucment\My Workcells\WeldPRO5\loadset\mappdk_cmd.kl(9)
   9 ROUTINE GET_JUSTFAKECURJPOS(cmd: STRING): STRING
     ^ ERROR
"PROGRAM" expected.

D:\Users\admin\Doucment\My Workcells\WeldPRO5\loadset\mappdk_cmd.kl(9)
   9 ROUTINE GET_JUSTFAKECURJPOS(cmd: STRING): STRING
     ^ ERROR
Id expected.

D:\Users\admin\Doucment\My Workcells\WeldPRO5\loadset\mappdk_cmd.kl(9)
   9 ROUTINE GET_JUSTFAKECURJPOS(cmd: STRING): STRING
             ^ ERROR
";" or new line expected.

D:\Users\admin\Doucment\My Workcells\WeldPRO5\loadset\mappdk_cmd.kl(9)
   9 ROUTINE GET_JUSTFAKECURJPOS(cmd: STRING): STRING
                                ^ ERROR
"BEGIN" expected.

D:\Users\admin\Doucment\My Workcells\WeldPRO5\loadset\mappdk_cmd.kl(9)
   9 ROUTINE GET_JUSTFAKECURJPOS(cmd: STRING): STRING
                                 ^ ERROR
";" or new line expected.

D:\Users\admin\Doucment\My Workcells\WeldPRO5\loadset\mappdk_cmd.kl(9)
   9 ROUTINE GET_JUSTFAKECURJPOS(cmd: STRING): STRING
                                    ^ ERROR
Invalid statement or "ENDxxx" or "UNTIL" expected.

D:\Users\admin\Doucment\My Workcells\WeldPRO5\loadset\mappdk_cmd.kl(9)
   9 ROUTINE GET_JUSTFAKECURJPOS(cmd: STRING): STRING
                                      ^ ERROR
Invalid statement. ";" or new line expected.

D:\Users\admin\Doucment\My Workcells\WeldPRO5\loadset\mappdk_cmd.kl(9)
   9 ROUTINE GET_JUSTFAKECURJPOS(cmd: STRING): STRING
                                            ^ ERROR
Invalid statement. ";" or new line expected.

D:\Users\admin\Doucment\My Workcells\WeldPRO5\loadset\mappdk_cmd.kl(9)
   9 ROUTINE GET_JUSTFAKECURJPOS(cmd: STRING): STRING
                                             ^ ERROR
Invalid statement. ";" or new line expected.

D:\Users\admin\Doucment\My Workcells\WeldPRO5\loadset\mappdk_cmd.kl(9)
   9 ROUTINE GET_JUSTFAKECURJPOS(cmd: STRING): STRING
                                               ^ ERROR
Invalid statement. ";" or new line expected.

D:\Users\admin\Doucment\My Workcells\WeldPRO5\loadset\mappdk_cmd.kl(18)
  18 VAR
     ^ ERROR
Invalid statement or "ENDxxx" or "UNTIL" expected.

D:\Users\admin\Doucment\My Workcells\WeldPRO5\loadset\mappdk_cmd.kl(18)
  18 VAR
     ^ ERROR
Invalid statement. ";" or new line expected.

D:\Users\admin\Doucment\My Workcells\WeldPRO5\loadset\mappdk_cmd.kl(19)
  19     resp:           STRING[254]
             ^ ERROR
Invalid statement. ";" or new line expected.

D:\Users\admin\Doucment\My Workcells\WeldPRO5\loadset\mappdk_cmd.kl(19)
  19     resp:           STRING[254]
                         ^ ERROR
Invalid statement. ";" or new line expected.

D:\Users\admin\Doucment\My Workcells\WeldPRO5\loadset\mappdk_cmd.kl(19)
  19     resp:           STRING[254]
                               ^ ERROR
Invalid statement. ";" or new line expected.

D:\Users\admin\Doucment\My Workcells\WeldPRO5\loadset\mappdk_cmd.kl(19)
  19     resp:           STRING[254]
                                ^ ERROR
Invalid statement. ";" or new line expected.

D:\Users\admin\Doucment\My Workcells\WeldPRO5\loadset\mappdk_cmd.kl(19)
  19     resp:           STRING[254]
                                   ^ ERROR
Invalid statement. ";" or new line expected.

D:\Users\admin\Doucment\My Workcells\WeldPRO5\loadset\mappdk_cmd.kl(20)
  20     out:            STRING[254]
            ^ ERROR
Invalid statement. ";" or new line expected.

D:\Users\admin\Doucment\My Workcells\WeldPRO5\loadset\mappdk_cmd.kl(20)
  20     out:            STRING[254]
                         ^ ERROR
Invalid statement. ";" or new line expected.

D:\Users\admin\Doucment\My Workcells\WeldPRO5\loadset\mappdk_cmd.kl(20)
  20     out:            STRING[254]
                               ^ ERROR
Invalid statement. ";" or new line expected.

D:\Users\admin\Doucment\My Workcells\WeldPRO5\loadset\mappdk_cmd.kl(20)
  20     out:            STRING[254]
                                ^ ERROR
Invalid statement. ";" or new line expected.

D:\Users\admin\Doucment\My Workcells\WeldPRO5\loadset\mappdk_cmd.kl(20)
  20     out:            STRING[254]
                                   ^ ERROR
Invalid statement. ";" or new line expected.

D:\Users\admin\Doucment\My Workcells\WeldPRO5\loadset\mappdk_cmd.kl(21)
  21     jpos:           JOINTPOS
             ^ ERROR
Invalid statement. ";" or new line expected.

D:\Users\admin\Doucment\My Workcells\WeldPRO5\loadset\mappdk_cmd.kl(21)
  21     jpos:           JOINTPOS
                         ^ ERROR
Invalid statement. ";" or new line expected.

D:\Users\admin\Doucment\My Workcells\WeldPRO5\loadset\mappdk_cmd.kl(22)
  22     joint_vals:     ARRAY[9] OF REAL
                   ^ ERROR
Invalid statement. ";" or new line expected.

D:\Users\admin\Doucment\My Workcells\WeldPRO5\loadset\mappdk_cmd.kl(22)
  22     joint_vals:     ARRAY[9] OF REAL
                         ^ ERROR
Invalid statement. ";" or new line expected.

D:\Users\admin\Doucment\My Workcells\WeldPRO5\loadset\mappdk_cmd.kl(22)
  22     joint_vals:     ARRAY[9] OF REAL
                              ^ ERROR
Too many syntax errors encountered.

===============Translation not successful===============

however regarding using robot.move with my userframe still is giving me the 'position-is-not-reachable' error if i use as pose but the joint is working fine.

torayeff commented 1 year ago

@madinwei there are some issues:

  1. What are you trying to compile? From the above logs, it seems that you are trying to compile mappdk_cmd.kl. After making any changes to any of the .kl files, the only files that require compiling are mappdk_server.kl and mappdk_logger.kl. Can you please try doing this?
  2. If you are changing the following line https://github.com/torayeff/fanucpy/blob/4fe61fcab11ed73aa0eb6daaca57c209ff741c0e/src/fanuc-driver/mappdk_server.kl#LL30C5-L31C16 then you need to re-compile again (I assume you already do it)
  3. By default CURPOS KAREL routine returns the current position of the tool center point (TCP) relative to the current value of the system variable $UFRAME for group 1. Is it also the case with your setup?

Please let me know if you still have issues. I am happy to help you with this issue.

madinwei commented 1 year ago
  1. What are you trying to compile? From the above logs, it seems that you are trying to compile mappdk_cmd.kl. After making any changes to any of the .kl files, the only files that require compiling are mappdk_server.kl and mappdk_logger.kl. Can you please try doing this?

I was recompiling all the files, I see so I was not supposed to recompile the mappdk_cmd.kl.

  1. If you are changing the following line https://github.com/torayeff/fanucpy/blob/4fe61fcab11ed73aa0eb6daaca57c209ff741c0e/src/fanuc-driver/mappdk_server.kl#LL30C5-L31C16 then you need to re-compile again (I assume you already do it)

  2. By default CURPOS KAREL routine returns the current position of the tool center point (TCP) relative to the current value of the system variable $UFRAME for group 1. Is it also the case with your setup?

2&3 yes I change this file, but before I change the CURPOS function from the beginning of the mappdk_cmd file, it was not returning the current pos with my user frame. now it's working.

Please let me know if you still have issues. I am happy to help you with this issue.

@torayeff , thank you so much you provide me with a lot of help. right now, my issue is when I use my Python code to move the robot, it most of the time returns the 'position-is-not-reachable' error. I have set up a 3d camera that returns pointcloud and it detects my object. and I have set up my userframe according to my camera and its pointcloud. and I put my object within the range of the robot arm. My understanding is that even though the object is in range of the robot but some of the joints have reached their limits, so it can't go in that direction. so how I could avoid this problem? since I have to use the 'pose' not the 'joint' in robo.move because my pointcloud can return the coordinate of the object. so I have the X, Y, and Z values and not the W, P, and R values. Is there a way I can set the best value or make the robot choose automatically?

torayeff commented 1 year ago

@madinwei since

I have to use the 'pose' not the 'joint' in robo.move because my pointcloud can return the coordinate of the object. so I have the X, Y, and Z values and not the W, P, and R values.

From the above, I understand that you only provide X, Y, and Z to reach the object, right? If this is the case, then I think the problem in your case is reaching points from different orientations. Suppose this example where a cup is located on top of a table. You can reach this cup from different positions but probably not directly from the bottom. So one way will be to set up to your W, P, and R values directly above the point. But I would recommend you use some pose estimation methods. For example, there is a good thread on how to convert between different reference frames: https://forum.opencv.org/t/eye-to-hand-calibration/5690/

There are also some helper functions here: https://github.com/torayeff/fanucpy/blob/main/src/fanucpy/Transformations.py

So answering the following question:

Is there a way I can set the best value or make the robot choose automatically?

It depends; if all of your objects are reachable from the top, then make W, P, and R perpendicular to that, as if you are always reaching from the top.

madinwei commented 1 year ago

@torayeff thank you, since i have solved the original issue I will close this issue, but i will keep discussing with you ^_^

From the above, I understand that you only provide X, Y, and Z to reach the object, right? If this is the case, then I think the problem in your case is reaching points from different orientations. Suppose this example where a cup is located on top of a table. You can reach this cup from different positions but probably not directly from the bottom. So one way will be to set up to your W, P, and R values directly above the point. But I would recommend you use some pose estimation methods. For example, there is a good thread on how to convert between different reference frames: https://forum.opencv.org/t/eye-to-hand-calibration/5690/

Yes, I have a 3d object detection which can detect the object and return its x,y, and z value for the pointcloud produced by the camera. and exactly as you said I need to use pose estimation method. which I am a novice on it.

It depends; if all of your objects are reachable from the top, then make W, P, and R perpendicular to that, as if you are always reaching from the top.

I understand, but since I have not figured that out, I want to send the XYZ values and make the robot choose the WPR values that will not give an error. is that possible? because almost all tests I do with my Python code return the 'position-is-not-reachable' error.

torayeff commented 1 year ago

I do not quite understand how it might be possible to "make the robot choose the WPR values that will not give an error". Why don't you try to reach any of the points manually, record only WPR values, and use those values whenever you try to reach new points again?

madinwei commented 1 year ago

I do not quite understand how it might be possible to "make the robot choose the WPR values that will not give an error". Why don't you try to reach any of the points manually, record only WPR values, and use those values whenever you try to reach new points again?

You are a very nice and kind person for having the patience to putting up with me ^_^ 👍 I understand I am using object detection to get the coordinates so it will be automatic, but using a manually input of WPR values will go against it. I see, I can have a set of record WPR, and if a new point is near one of the recorded points I will just use its WPR. I will try it.

However, I used the CURPOS to get the current WPR, but not every time the robot can go from the current position to the new position with the same WPR. because the joint may have reached its limit.