x_value of wrist root point

  • flighttothemoon
  • Thread is marked as Resolved.

    I am coding post-processor for KUKA KR 3. Now i am stuck at S(status) value in POS position. S value is formed with 3 bits (bit 2, bit 1, bit 0).
    According to expert programming manual:


    "The basic/overhead areas can be visualized in Cartesian terms. To do this, the following
    terms are defined:
    Wrist root point: Intersection of the wrist axes
    A1 coordinate system: If axis 1 is at 0_, it is identical to the $ROBROOT coordinate
    system. For values not equal to 0_, it moves with axis 1.


    The basic/overhead areas can thus be defined as follows:
    -If the x--value of the wrist root point, expressed in the A1 coordinate system, is positive,
    the robot is in the basic area.
    -If the x--value of the wrist root point, expressed in the A1 coordinate system, is negative,
    the robot is in the overhead area."



    But how can i find value of that wrist root point?

    Well, this should be something that can be done with standard 3D transformations. I'm still trying to learn that math myself, but there are some discussions in the forum archives (both in this sub-forum and in the Math And Kinematics sub-forum) that you will probably find helpful. I know there's a formula in the archives for converting E6POS data to a 4x4 matrix for performing transforms on.


    If I were doing this in KRL, my first thought would be to create a tool with X and Y values of 0, and a negative Z value equal to the distance between Tool Zero and the axis of A5. That would give me the position of the wrist root in Cartesian space (although only in the active Base, not necessarily in $ROBROOT). The Geometric Operator could also be used to determine this value on the fly mathematically.

    Ah. After looking at the manual, I think I see what's going on.


    The A1 coordinate system is essentially the same as $ROBROOT, but rotates with A1.


    The X-value of the Wrist Root point can only be negative in the A1 coordinate system if the robot is reaching "over it's shoulder", so to speak. There are some illustrations of this in the Expert Manual in the section on singularities.


    Basically, if A2 were to be placed at -90 (straight up), and A3 were rotated through its entire range, the X value of the Wrist Root position would be negative for any value of A3 less than 0, and positive for any value of A3 greater than 0. I'm not sure offhand what it would be if A3 was exactly 0....


    To think of it another way: set A1 to 0deg. Imagine a vertical 2-dimensional plane passing through the origin of $ROBROOT, aligned to be orthogonal to the X axis of $ROBROOT. This plane would rotate with A1. The X value of the Wrist Root position would always be positive if the Wrist Root is in "front" of this plane, and negative if the Wrist Root moves "behind" it.

    Quote from SkyeFire

    Basically, if A2 were to be placed at -90 (straight up), and A3 were rotated through its entire range, the X value of the Wrist Root position would be negative for any value of A3 less than 0, and positive for any value of A3 greater than 0. I'm not sure offhand what it would be if A3 was exactly 0....


    Don't forget about the distance A2 is offset from A1 and the distance A4 is offset from A3. Take the Series 2000 robots for example (KR 150-2 through KR 240-2). The A2 to A1 offset is 350mm and the A4 to A3 offset is -55mm. This means when A2 is at -90, A3 would have to be further negative than ~-20 degrees to cross the "A1 plane". So in this case even if A2 is -90 and A3 is -10, Bit 0 of Status will still be 0.


    The distance A4 is offset from A3 also affects the transition point of Bit 1 of the Status. Thankfully for flighttothemoon, the KR3 has no offset between A4 and A3. But it still has the A2 to A1 offset (100mm) to deal with for Bit 0.

    For a KR3 the Status Bit 0 should be:


    Bit 0 = ((100+ COS (A2) * 265 + COS (A2 + A3) * 270) <= 0)


    Where A2 is the angle of axis 2 in radians and A3 is the angle of axis 3 in radians. The only concern would be whether it is supposed to < 0 or <= 0.


    I guess the big headache for you since you are doing the post would be doing the reverse kinematic to get A2 and A3.

    Hello,


    For beginners the easiest way of understanding homogenous coordination Transformations without to much mathematics involved is the following notation:


    Given a Frame B subject to Frame A is written


    Code
    A
 T 
  B


    Assume we have a second transformation T describing Frame C subject to frame B.


    Code
    B
 T
  C


    Hence the description of Frame C subject to Frame A is given by


    Code
    A     A     B
 T  =  T  *  T 
  C     B     C


    (that is simply ommitting the diagonal B's).


    Moreover the inverse of a homogenous coordination Transformation say A subject to B is done by simply exchanging A and B:

    Code
     A           B
[ T ]^(-1) =  T
   B           A


    Therefore the wrist point in $WORLD may be calculated in KRL by means of


    Code
    WpInWorld = $BASE:$POS_ACT:INV_POS($TOOL):INV_POS($TFLWP)

    which is identical to the following homogenous matrix calculations:

    Code
    WORLD          WORLD       BASE        FL                WP
      T      =      T    *     T    * [   T    ]^(-1) * [  T  ]^(-1)
        WP           BASE        TCP        TCP             FL 
               <------->   <------->  <------------->   <---------->
                 = $BASE    =$POS_ACT    =$TOOL^(-1)     = $TFLWP^(-1)

    If the result is needed in the robot base simply apply


    Code
    WpInRobroot = INV_POS($ROBROOT):WpInWorld


    equals


    Code
    ROBROOT       WORLD                 WORLD    ROBROOT         WORLD
       T   = [     T       ]^(-1) *      T =        T      *      T
        WP          ROBROOT               WP         WORLD         WP
             <------------------>
                  =$ROBROOT^(-1)


    Hope this is still is possible to read. Writing formulae in a simple editor is not that easy :zwink:


    Fubini

    Edited once, last by Fubini ().

    Hello Fubini,



    Again a "Newbie" who is a german expert… Welcome Fubini!


    Thank you for your effort to write the formulas. The way to explain homogenous transformations was new for me. It is very useful.


    Some little remarks:


    1) You did not mention it, but I think that not all readers know $TFLWP (I didn't know this variable either). It is the offset between flange point and wrist point, this variable is defined in the machine.dat.


    2) The function Inv_Pos is not documented… but you can also use INV_FRAME (source code given in klue_weg.src).


    3)

    Quote

    WpInWorld = $BASE:$POS_ACT*INV_POS($TOOL):INV_POS($TFLWP)

    should be WpInWorld = $BASE:$POS_ACT:INV_POS($TOOL):INV_POS($TFLWP), of course.



    Thank you again,
    Tilman/France

    Quote from MeanRobot

    Don't forget about the distance A2 is offset from A1 and the distance A4 is offset from A3. Take the Series 2000 robots for example (KR 150-2 through KR 240-2). The A2 to A1 offset is 350mm and the A4 to A3 offset is -55mm. This means when A2 is at -90, A3 would have to be further negative than ~-20 degrees to cross the "A1 plane". So in this case even if A2 is -90 and A3 is -10, Bit 0 of Status will still be 0.


    The distance A4 is offset from A3 also affects the transition point of Bit 1 of the Status. Thankfully for flighttothemoon, the KR3 has no offset between A4 and A3. But it still has the A2 to A1 offset (100mm) to deal with for Bit 0.


    :wallbash: MeanRobot is entirely correct. I completely forgot to take that into account in my description. :party1:


    My off-the-cuff example would only work if A2 were rotated to place A3 directly above A1, rather than simply putting A2 at -90deg.

    Edited once, last by SkyeFire ().

    Fubini, that was an excellent explanation, thank you.


    So, $BASE : $POS_ACT gives us, in effect, $POS_ACT in World, since $Base is defined relative to World and $POS_ACT is always relative to $BASE.


    INV_POS ($TOOL) gives us the 6DOF vector from $POS_ACT to the "nullframe" tool.


    And INV_POS ($TFLWP) gives us the 6DOF vector from the "nullframe" tool to the wrist point.


    So, in effect, WpInWorld = $BASE:$POS_ACT:INV_POS($TOOL):INV_POS($TFLWP) works "forward" to find $POS_ACT in $WORLD, then works "backwards" from there along the $TOOL and $TFLWP chain to determine the position of $TFLWP's origin relative to World.


    And then, using INV_POS ($ROBROOT) gives us the current 6DOF vector from $ROBROOT to World, which allows us to convert the World position of $TFLWP to $TFLWP's position relative to $ROBROOT, for those robots whose $ROBROOT and $WORLD are not coincident (robots mounted on external axes like a KL1500, for example). The X value of which is, of course, the critical measurement needed to determine this bit of the S variable.


    Whew! Now my brain is tired. :sleeping_face:


    Still, it's always fun to be reminded of just how powerful the combination of those two simple-seeming tools (the Geometric Operator and INV_POS) can be for doing 6DOF mathematics.

    Edited once, last by SkyeFire ().

    Hello,


    Tilman: Thank you for the nice welcome. It took a little while but now I am arrived :icon_wink:


    SkyeFire: A small correction for

    Quote

    robots mounted on external axes like a KL1500


    In this case $ROBROOT has to be replaced by $ROBROOT_C since $ROBROOT is only defined for robots not mounted on another kinematic.

    Really? :icon_eek: I will have to make a note of that for future reference. I take it then that $ROBROOT only changes in a "static" fashion, for systems where (for whatever reason) $WORLD is required to be different from $ROBROOT.


    Am I correct in assuming that $ROBROOT_C is updated dynamically?

    Hello SkyeFire,


    you are perfectly right. If the robot is mounted on an external kinematic $ROBROOT_C gives the current position of the robot base in $WORLD.


    $ROBROOT (see $machine.dat) is used e.g. for "stand-alone"-Robots mounted on the ceiling. In this case $ROBROOT.B or $ROBROOT.C should be 180°.


    If your robot is mounted on an KL $ERSYSROOT (see $machine.dat) is used to tell the controller where the KL is. The robot then is always attached to the flange-system of the KL and $ROBROOT becomes obsolete. If you come around a robot with KL try Disply->Variable->Single and enter $ROBROOT. As result you should be able to see $ROBROOT = {FRAME: } (that is the components of $ROBROOT are not valid). On the other hand $ROBROOT_C should tell you the position of the robot base in $WORLD and the Value of $ROBROOT_C should change according to the KL-movement.


    If your robot is not mounted on an KL its the other way round: $ERSYSROOT should read $ERSYSROOT = {FRAME: } and $ROBROOT shows the fixed value of the robots base system.


    Fubini

    Thank you. But i still dont get how to compute either bit 0 should be "1" or "0". :icon_mrgreen: In the CAD software, i am coding post-processor for, i have values for each of the six axes. How can i compute "x--value of the wrist root point, expressed in the A1 coordinate system" from those axis values?

    flighttothemoon,


    If you already have values for each of the six axes (A1, A2, A3, A4, A5, A6), then I already gave you your answer in my earlier post.


    Yes, but could you please also explain how you got this formula? What is the logic behind it? :icon_smile:

    Please reread the earlier posts by SkyeFire and I for the explanation of the equation itself.


    The numbers all come from the KR3 data sheet. As previously mentioned, the 100 is the A2 to A1 offset for the KR3. The 265 is the distance from A2 to A3 for a KR3. And the 270 is the distance from A3 to A5 (the wrist root) for a KR3.

    Just to amplify a bit: 100mm is the offset along the X axis of the A1 reference frame between A1 and A2. For the purposes of this calculation, the Y and Z values would be irrelevant, since we're only interested in the wrist's position relative to the YZ plane of the A1 frame. Of course, the X offset could be negative for some robot models, although I can't recall ever seeing a robot arranged that way in real life.


    So, for a more general case of MeanRobot's formula:
    Bit 0 = ((dA1A2 + (COS(A2) * dA2A3) + (COS(A2+A3) * dA3A5) <=0)


    Where A2 and A3 are the joint angles of Axis 2 and Axis 3, respectively, and dAxAy is the distance between Axis X and Axis Y (for most wrist models, the distance between A3 and the wrist should be the same as the distance between A3 and A5). This of course assumes a fairly standard kinematic configuration for a six-axis robot.

    Hi Everyone!!!


    I am coming back with a very old topic as I was going through the old topics for my current work. I have question, if I write something like below:


    ACT_POS_CONV=$POS_ACT
    ACT_POS_WORLD=$WORLD:$POS_ACT


    then, ACT_POS_CONV is the current position of the robot in the current base assigned and ACT_POS_WORLD is the current position of the robot in World Coordinate system.


    Am I correct?


    Thanks

    No unfortunatly not. $WORLD always is the given fixed initial coordinate frame and therefore equals $NULLFRAME = {x 0, y 0, z 0, a 0, b 0, c 0). A shift of the base frame is denoted in $BASE . To convert a $POS_ACT position into the $WORLD Frame you would have to apply $BASE:$POS_ACT for a advance run evaluation and $BASE_C:$POS_ACT for a main run evaluation.


    WORLD WORLD BASE
    T = T * T
    TCP BASE : TCP
    <----------> <--------->
    = $BASE =$POS_ACT


    Fubini

    Edited once, last by Fubini ().

Advertising from our partners