Convert Cartesian to Joint, without moving to the position.

  • Lets say I have a PR[1] represented as a Cartesian position.
    PR[1] happens to be a calculated position in a user frame, with a utool of a position on a "parts tray" with some 250 locations.
    So... depending on the row and column of the part I want to pick at that location... I calculate PR[1].


    Now, I want to know what the Joint angles are of PR[1] without moving to the position first. Specifically, what is the J1 angle.
    Why... I have a fifth axis mounted camera on a LR Mate for cable management reasons.


    Anybody know how to convert a Cartesian position to a Joint position without moving there.


    1. This robot system is an LRMate 200iD V9.1 Handling Tool, R30iB Mate Plus.
    2. Yes... I can buy the KAREL Package (irMath?) but I can't figure out what instructions I would program in KAREL or how I would call it from a TP program.
    What would the Karel program or IrMath call look like? IrMath does have a call to go from Joint TO Cartesian, but not the other way around it seems.
    3. Yes... It can be done using trigonometry, but this would be horribly complicated in my application.
    4. Yes... this would be done online (in the robot).


    What would the KAREL program look like? Assume UFRAME 5, Utool 3, PR[10]


    Cheers!

    Edited once, last by bwlees ().

  • AD
  • HI


    Assuming that is a traditional palletizer,I will assume that the TCP is 0,0,something and that axis 6 faces down


    Using trig you will define a rectangle triangle that has a vertices on 0,0 and the other on x,y. Therefore you can find the angle of the arm based on trigonometry You need the math package

    somar


  • If all you need is the J1 angle, it could be done with the iRMath suite. If you need any other angle, then Karel would be required, unless you want to write an IK solver in TP.


    The J1 math could be done like this:
    [list type=decimal]

    • Get the uframe of the position.

    • Get position itself.

    • Matrix multiply the user frame by the position to convert the position to world.

    • Get user tool of that position. Invert it with the matrix invert tool.

    • Multiply the invert of the utool by the found world position to get the faceplate world position.

    • Multiply the faceplate world position by a matrix containing the -Z distance to get the wrist center point.

    • Now you have the wrist center point of the position. Take the X and Y position, run them through ATAN2, and that will be your J1 position in radians.

    • Convert to degrees.

    [/list]
    Simple right?


    If you need any other angle, you would have to buy the Karel package. Last I checked it was about $500.
    Since this is a 6 axis robot, the math for converting from position to joint gets significantly more complicated. The nice thing with Karel is that you can cast a Cartesian position onto a Joint position variable, and it does all the complex conversion math for you behind the scenes.


    Karel program would look something like this:
    [list type=decimal]

    • Get passed in PR index, Utool num, and Uframe num from the user, who is calling the program from a TP program. (An example of this can be found here.)

    • Store PR into xwzwpr variable.

    • Populate $UFRAME and $UTOOL with the passed in uframe and utool, (which are stored in $MNUTOOL and $MNUFRAME repectively) because this is what Karel uses to do cart to joint conversions.

    • Cast your cart position unto a jpos var. Karel does the conversion behind the scenes.

    • Write your angles to a PR or some registers so you can access them from TP.

    [/list]


    Slightly simpler, but still a bit of work.

    Check out the Fanuc position converter I wrote here!

  • I dont see the example here. When I click on the link, I get a post about string calls.


    The example was for the specific line of pulling in data passed to karel via TP arguments. It would be done in a similar manner as argument 2, where the user passes in a number.

    Check out the Fanuc position converter I wrote here!

  • Nation: Isn't it actually the inverse of the User Frame???? (Step 3 in your procedure??)


    Since the PR[] is the position of the robot is user coordinates:


    To calculate the X,Y position of the wrist center in world coordinates one must


    pre-multiplied the PR[] by the inverse user frame transformation


    then post multiply the result by the inverse of the tool frame


    finally post multiply by the transformation in -Z to get the position of the wrist center


    This is the general mathematical solution


    The math can be simplified, but need to know more information:


    Tool Frame data, User Frame data, a sample of PRs


    Is not a practical solution but here is a trick:
    Disable the group motion (to put the robot in machine look)
    Then command the robot to the desired PR
    wait until the motion command is completed (robot does not actually move)


    Then just read the joint value


    All this can be done in a program by reading and writing the robot variables
    I don't have the manual with me but if you check on the forum you can find the variables


  • Nation: Isn't it actually the inverse of the User Frame???? (Step 3 in your procedure??)


    No, its the normal matrix, since you want the vector from base to the point, but you have the vector from the base to the uframe origin, and the vector from uframe origin to the point, you just multiply them together to get the position in world.


    Here is a snippet of code I've been writing in c# to do offline cart to joint conversion that I based my instructions on:
    Matrix<double> facePlateInverse = Matrix<double>.Build.DenseIdentity(4, 4);
    facePlateInverse[2, 3] = -facePlateThickness;
    Matrix<double> wristCenter = uFrames[uframe] * commandedPostion * uTools[utool].Inverse() * facePlateInverse;




    It is pretty hacky. $MCR_GRP[1].$MACHINELOCK is the var if you are interested in this solution.

    Check out the Fanuc position converter I wrote here!

  • No idea what I'm doing in KAREL... but here is my attempt.


    Would this work or am I missing code to set the Uframe on Utool?


    Let’s assume PR[1] is a cartesian position in UTOOL 0, WORLD.



    PROGRAM CONVERT_TO_JOINT -- Syntax ok


    %nolockgroup -- no idea what this does
    %comment = 'Convert XYZ to JOINT -- ??
    %alphabetize -- ?? Why


    VAR
    xyz :XYZWPR
    jpos :JOINTPOS
    status :INTEGER


    BEGIN


    xyz= GET_POS_REG(1, status) -- corresponds to getting PR[1] stored into “xyz”
    jpos=xyz -- convert ‘xyz’ to JOINTPOS
    SET_JPOS_REG(2,jpos,status) -- set PR[2] as the joint position of jpos


    END CONVERT_TO_JOINT




    I think I'm missing some statements... but I donno.

  • I took your code and compiled it in roboguide. It works, but I added some stuff to it. I used different PRs, as PR1 is defined in the robot I was working with.


    Check out the Fanuc position converter I wrote here!

  • Hello,


    I tried to call this Karel code from a TP program and I get the error :

    MOTN-018 Position not reachable (G:0)

    CNVT_TO_JNT LINE 27 T2 PAUSED


    I am simulating on RoboGuide

    Robot software version: V9.30

    Robot Model: LR Mate 200iD/4S

    Robot Options: R507, R512, R663, R632, R792, R648


    I think I need something else to make this code work, do you know what is missing?

  • Hello, indeed it's not working when the position is not reachable or in singularity !

    I get MOTN-023 with the following coordinates [360.0, 0.0, 280.0, 180.0, -90.0, 0.0]


    So I have the same problem with the code given in this thread


    Do you know how to make it work with any reachable position even if it's in singularity ?


    By the way what is the difference between POS2JOINT built-in routine and your simple conversion (x=y) ?

Create an account or sign in to comment

You need to be a member in order to leave a comment

Create an account
Sign up for a new account in our community. It's easy!
Register a new account
Sign in
Already have an account? Sign in here.
Sign in Now