MATRIX_TO_EULER (189)
MATRIX_TO_EULER (189)
ID 189
|
Application: |
PLC |
SIM |
Group: DATA CONVERSION
Short description:
Function that calculates the Euler angles from the elements of the transformation matrix.
Inputs (9):
|
no. |
type |
impulse |
mark |
notes |
|
1 |
R |
no |
PR11 | [1,1] element of the transformation matrix |
| 2 | R | no | PR12 | [1,2] element of the transformation matrix |
| 3 | R | no | PR13 | [1,3] element of the transformation matrix |
| 4 | R | no | PR21 | [2,1] element of the transformation matrix |
| 5 | R | no | PR22 | [2,2] element of the transformation matrix |
| 6 | R | no | PR23 | [2,3] element of the transformation matrix |
| 7 | R | no | PR31 | [3,1] element of the transformation matrix |
| 8 | R | no | PR32 | [3,2] element of the transformation matrix |
| 9 | R | no | PR33 | [3,3] element of the transformation matrix |
Outputs (4):
|
no. |
type |
impulse |
mark |
notes |
|
1 |
R |
no |
rot_x |
First Euler angle [Rad] - X axis orientation |
| 2 | R | no | rot_y | Second Euler angle [Rad] - Y axis orientation |
| 3 | R | no | rot_z | Third Euler angle [Rad] - Z axis orientation |
| 4 | B | no | error | Ambiguous solution |
Settings: none
Operation:
MATRIX_TO_EULER function calculates the Euler angles from the elements of the transformation matrix.
Input matrix:
[ PR11 PR12 PR13 ]
Wej = [ PR21 PR22 PR23 ]
[ PR31 PR32 PR33 ]
Euler angles are obtained by using the function atan2:
when Ry = -PI/2:PI/2
Rz = atan2((PR21),(PR11))
Ry = atan2((-PR31),(sqrt((PR32*PR32)+(PR33*PR33))))
Rx = atan2((PR32),(PR33))
when Ry = PI/2:3PI/2
Rz = atan2((-PR21),(-PR11))
Ry = atan2((-PR31),(-sqrt((PR32*PR32)+(PR33*PR33))))
Rx = atan2((-PR32),(-PR33))
Error handling (error with an ambiguous position):
When Ry is -PI / 2 or PI / 2, the robot is in an ambiguous position. 0 values are recorded at the Rz, Ry and Rx outputs.
More theorethical information: Wikipedia
R registers and M markers used: none