Help simultus

MATRIX_TO_EULER (189)

ID 189

było:

Zastosowanie:

PLC

SIM

Grupa: DATA CONVERSION

Opis skrótowy:

Funkcja obliczająca kąty Eulera na podstawie elementów macierzy transformacji.

wejścia (9 sztuk):

nr	typ	impuls	oznaczenie	uwagi
1	R	nie	PR11	Element [1,1] macierzy transformacji
2	R	nie	PR12	Element [1,2] macierzy transformacji
3	R	nie	PR13	Element [1,3] macierzy transformacji
4	R	nie	PR21	Element [2,1] macierzy transformacji
5	R	nie	PR22	Element [2,2] macierzy transformacji
6	R	nie	PR23	Element [2,3] macierzy transformacji
7	R	nie	PR31	Element [3,1] macierzy transformacji
8	R	nie	PR32	Element [3,2] macierzy transformacji
9	R	nie	PR33	Element [3,3] macierzy transformacji

było: wyjścia (4 sztuki):

nr	typ	impuls	oznaczenie	uwagi
1	R	nie	Rz	Pierwszy kąt Eulera - orientacja w układzie współrzędnych nr 6 względem układu współrzędnych podstawy robota
2	R	nie	Ry	Drugi kąt Eulera - orientacja w układzie współrzędnych nr 6 względem układu współrzędnych podstawy robota
3	R	nie	Rx	Trzeci kąt Eulera - orientacja w układzie współrzędnych nr 6 względem układu współrzędnych podstawy robota
4	B	nie	error	Rozwiązanie niejednoznaczne

wyjścia (4 sztuki):

nr	typ	impuls	oznaczenie	uwagi
1	R	nie	rot_x	Pierwszy kąt Eulera [Rad] - orientacja w osi X
2	R	nie	rot_y	Drugi kąt Eulera [Rad] - orientacja w osi Y
3	R	nie	rot_z	Trzeci kąt Eulera [Rad] - orientacja w osi Z
4	B	nie	error	Rozwiązanie niejednoznaczne

ustawienia: brak

opis działania:

Funkcja MATRIX_TO_EULER oblicza kąty Eulera na podstawie elementów macierzy transformacji.

Macierz wejściowa:
            [ PR11    PR12    PR13 ]
   Wej = [ PR21    PR22    PR23 ]
            [ PR31    PR32    PR33 ]

Kąty Eulera uzyskuje się wykorzystując funkcję atan2:

gdy Ry = -PI/2:PI/2
Rz = atan2((PR21),(PR11))
Ry = atan2((-PR31),(sqrt((PR32*PR32)+(PR33*PR33))))
Rx = atan2((PR32),(PR33))

gdy Ry = PI/2:3PI/2
Rz = atan2((-PR21),(-PR11))
Ry = atan2((-PR31),(-sqrt((PR32*PR32)+(PR33*PR33))))
Rx = atan2((-PR32),(-PR33))

Obsługa błędu error (błąd o niejednoznacznej pozycji):

Gdy kąt Ry wynosi -PI/2 lub PI/2, robot znajduje się w pozycji niejednoznacznej. Na wyjściach Rz, Ry oraz Rx zapisywane są wartości 0.

więcej informacji teoretycznych:
Wikipedia

Używane rejestry R i znaczniki M: brak