Y_MA_DecompFrameMatrix
Parameters:
| * | Parameter | Data Type | Description | Default |
|---|---|---|---|---|
| VAR_IN_OUT | ||||
| B | Frame | Yt_MA_Matrix4x4 | 16 values are used to represent a 4x4 matrix |
|
| B | Position | VECTOR | First 6 values are used [X Y Z Rx Ry Rz]. Rx Ry Rz are assumed to be in degrees. |
|
| VAR_INPUT | ||||
| B | Enable | BOOL | The function block will continue to execute every scan while Enable is TRUE and there are no errors. | FALSE |
| VAR_OUTPUT | ||||
| B | Valid | BOOL | The Valid output indicates that the function block is operating normally and the outputs of the function block are valid. | |
| B | Singular | BOOL | TRUE when calculating Euler angles where Ry = +/- pi/2 | |
Notes:
Decomposes a 4x4 frame matrix into a pose vector. Currently assumes a Cartesian space.
INPUT:
Frame [Yt_MA_Matrix4x4] -- 16 values are used to represent a 4x4 matrix. [a11 a12 a13 a14 a21 a22 a23 a24 a31 a32 a33 a34 a41 a42 a43 a44]
OUTPUT:
Position [VECTOR] -- First 6 values are used [X Y Z Rx Ry Rz]. Rx Ry Rz are assumed to be in degrees.
Singular [BOOL] -- TRUE when calculating Euler angles where Ry = +/- pi/2.
General notes:
-- In general, each representation of orientation has two sets of Euler angle solutions (Rx Ry Rz)
-- This code is based on FS/DX robot controllers .
-- A singularity happens at Ry = +/- pi/2, where a rotational DOF is lost. This picks a default solution with Rz = 0 and outputs TRUE to Singular output.
-- Calculated Ry is forced to be between -pi/2 and pi/2 (-90 and 90 degrees). This selects one of the solutions.
-- As a result, range for Rx and Rz will be between -pi and pi (-180 and 180 degrees).
-- Sometimes if a pose is converted from vector to matrix and then back to vector, the resulting pose will be different than the original
-- This is because the code calculated a different equivalent solution.
-- Happens if the input pose vector is outside of solution range of Rx Ry and/or Rz.
-- Results in same orientation mathematically, however, orientation may be wrong for mechanisms with fewer than 6 DOF
-- Problem can be fixed by “teaching” a point using the pose vector will always be in the right format as these values will come from our own calculations.