Checking W,P,R given a range / tolerance

    • New

    Hello,

    Is there an equation or set of if statements I can use to check if W,P,R values are in a given range? For example, if the planned position W = 180, and tolerance is 1, then acceptable values are -179, 180, 179. Similarly, planned position W = 0, then acceptable values are -1,0,1.

    I've tried normalizing the value and using modulo division, but I have issues at the worst case values or when planned W is positive and actual W is negative and vice versa.

    Attached is an image of my program where I try normalizing the value based on the tolerance, but I found a situation where it does not work. (AR[1] = Planned Position & AR[2] = Tolerance)

    • New
    Quote from Fubini

    And this relates to what robot brand? This subforum is for general discussion independently of specific brand.

    I am working with a Fanuc Robot, but this question is not specific to Fanuc. It pertains to the math where the yaw, pitch, and roll values can be any value between -180 to 180 and -180=180. This is common in many robots and vision systems.

    Edited once, last by EndAffector (February 25, 2025 at 5:08 PM).

    • New

    Ok. So W,P,R are Euler Angles The naming W,P,R is not standard. For KUKA e.g. they are called A,B and C. But now I understand. You have to calculate the respective rotation matrix and and use matrix multiplication to take differences. So given two rotation matrices X and Y their difference is X * INVERSE(Y) For rotation matrices inverse is the same as transponing the matrix since they are orthonormal. The result can then be calculated back into Euler angles if you want difference expressed in angles again. In Kuka bots this is builtin functionality by X:inv_pos(Y) if X and Y are declared as frames with translational components set to zero. Maybe also in Fanuc something similar exists.

    Exact formula depends on the convention w,p,r of Fanuc follows. Something I do not know. For Kuka A,B,C are ZYX intrinsic Euler angles.

    • New

    I do not understand your problem. Where do you get your angle values from?

    Normally the values are correct and there is no need to check for tolerance.


    You may check and compare the values from ABB/KUKA and FANUC

    The rotation sequence is ZY'X'' (intrinsic) and for FANUC XYZ (extrinsic)

    ATAN2 limits the angle range to +/- 180 deg

    SIN limits the angle range to +/- 90 deg

    Check HO.PDF (mit.edu) bottom of page 1

    • New

    Thank you for the suggestions!

    Fanuc is pretty bare without the math or KAREL options. I was able to solve this issue by using the formula below:

    I check the Rx, Ry, and Rz (W,P,R in Fanuc or 4,5,6) individually by finding the absolute value of the difference between point components and checking against a passed tolerance to the program.


    ABS((pos1(R<n>) - pos2(R<n>) + 540) % 360 -180), where R<n> is substituted for the value of Rx, Ry, Rz of each check.

    So to check for Rx the logic becomes:

    Difference = ABS((pos1(Rx) - pos2(Rx) + 540) % 360 -180)

    Rx_InRange = (Difference <= Tolerance)

    If all 3 return true, then the orientation is in range of the given position.

    • New

    MOM, to add more context (which should have been in my original post) I am using this check to compare the current robot pose to a set of pick positions. The robot is double tooled (Grippers 1 & 2) and it can be in a station with different poses. The station is very confined so the homing procedure needs to be precise.

    By using a linear distance function, my homing routine detects if the robot is within 100mm of the station pick position. If this returns true, I compare the current pose of the robot to the different poses inside the station so that I can determine which offsets to use.

Advertising from our partners