Falsche Orientierung aus CRobT()

[MOSHzilla]

Ich habe ein Problem mit der CRobT() Funktion. Ich möchte zur Validierung der Orientierung von einer inertialen Messeinheit, mit dem IRB 1400 ein Muster abfahren und dabei die Winkel der Ausrichtung mit Hilfe eines kontinuierlichen Interruptes in einer Logdatei speichern. Nun aber folgendes Problem: In der Startposition stimmen die Winkel noch, doch nach dem ersten Bewegungsbefehl sind plötzlich die Winkel falsch. Am Ende in der Startposition ist wieder alles korrekt.

Auszug aus der Logdatei:

0.000 90.000 0.000
0.000 90.000 0.000
0.000 90.000 0.000
0.000 90.000 0.000
104.044 89.994 104.037
101.565 89.228 100.807
......
-115.821 88.405 -116.903
-116.214 89.582 -116.498
0.000 90.000 0.000
0.000 90.000 0.000
0.000 90.000 0.000

Hier der Code:
Ausführen der Bewegung

       ConfL\Off;   
   MoveJ pstart, v300, fine, CAM;CONNECT POS_Inter WITH POS_TRAP;
   ITimer 0.1, POS_Inter;   
   WaitTime 3;
   
   FOR i FROM 1 TO steps DO
      ang_x:=ReadNum(dev_x);
      ang_y:=ReadNum(dev_y);
      ang_z:=ReadNum(dev_z);      
      
      MoveL RelTool(pstart,0,0,0\Rx:=ang_x\Ry:=ang_y\Rz:=ang_z),v1000\T:=deltaT,fine,CAM;         
   ENDFOR
        MoveL RelTool(pstart,0,0,0\Rx:=0\Ry:=0\Rz:=0),v1000\T:=deltaT,fine ,CAM;

Interrupt zum schreiben in Logdatei

TRAP POS_TRAP
   act_pos := CRobT(\Tool:=CAM \WObj:=wobj0)

   X_Angle := EulerZYX(\X,act_pos.rot);
   Y_Angle := EulerZYX(\Y,act_pos.rot);
   Z_Angle := EulerZYX(\Z,act_pos.rot);
   
   Open "HOME:"\File:="file_position",dev_pos\Append;
   Write dev_pos, NumToStr(X_Angle,3) + " " + NumToStr(Y_Angle,3) + " " + NumToStr(Z_Angle,3);
   close dev_pos;
ENDTRAP

Ich hoffe es kann mir jemand helfen

[Loipe]

Hallo du

Sehen doch gut aus.Da sollten sich doch die winkel um X und z gegenseitig aufheben.

wie fährt denn der Roboter? du sagst nur das das logging merkwürdig ausschaut?

Gruß Loipe

[MOSHzilla]

Hier sind die Dateien aus denen die Winkel für die Drehung gelesen werden.

X

30
-10
7
-18
27

Y

15
37
-22
30
8

Z

30
10
-22
18
-34

und die werte passen garnicht mit den aus der Logdatei zusammen

[Loipe]

wie fährt denn der Roboter?

Fährt er irgendwie komisch?

die 90° in b sehen da nach aus das beim konsolidieren der homogenen 3x3 Matrix die winkel um x und z ausreißen.

ist aber immernoch die selbe position

[MOSHzilla]

Also der Roboter dreht sich aufjedenfall um die Winkel die aus den Dateien gelesen werden.
Die 90° kommen durch meine definition der Startposition:

VAR pose rotation_cam;

rotation_cam.rot := OrientZYX(0,90,0);
pstart:= [[1000,0,400],[rotation_cam.rot.q1,rotation_cam.rot.q2,rotation_cam.rot.q3,rotation_cam.rot.q4],[0,0,0,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];

Oder wie meinst du das?
Kann es vll  auch sein, dass es da Probleme mit der EulerZYX() Funktion gibt und die die Quaternionen falsch umrechnet?

[Hermann]

Denke eher, daß da etwas Verwirrung in den Koordinatensystemen herrscht.

- Bewegung erfolgt per RelTool bzgl. des Tools
- Protokoll der Koordinaten erfolgt im Werkobjekt
- Euler-Koordinaten beziehen sich auf Drehungen die nacheinander erfolgen, d.h. die Drehung um Y erfolgt nach der Drehung um X. Die Winkel kann man sich meist nur in Spezialfällen noch gut vorstellen/verdeutlichen.

[MOSHzilla]

kann mir denn jemand kurz erklären, wie ich es hinkriege, dass er mir die richtige ausrichtung angibt?
ich sitze da jetzt schon ewig vor und bin schon am verzweifeln weil mir die ideen ausgehen

[robiman]

In den Zusammenhang fällt mir auch noch eine Frage ein:
Ist das so???
==>>Euler-Koordinaten beziehen sich auf Drehungen die nacheinander erfolgen, d.h. die Drehung um Y erfolgt nach der Drehung um X.
 d.h. erst Drehung um X dann um Y und zuletzt um Z??


Im Handbuch steht bei Orient
Datentyp: orient
Die aus Eulerschen Winkeln erzeugte Orientierung.
Die Rotationen erfolgen in der nachstehenden Reihenfolge:
• Rotation um die z-Achse,
• Rotation um die neue y-Achse,
• Rotation um die neue x-Achse.
Ist ja auch der umgekehrte Weg oder....

[högi]

Denke eher, daß da etwas Verwirrung in den Koordinatensystemen herrscht.

- Bewegung erfolgt per RelTool bzgl. des Tools

Ich glaube auch das es eher daran liegt, das du mit dem RelTool, in ein völlig anderes Koordinatensystem "umschaltest".

Ich würde es einmal mit Cpos versuchen, oder anstatt RelTool, OffSet benutzen.

Gruß
Högi

[MOSHzilla]

Also CPos kann ich nicht verwenden, weil es mir ja eig nur um die Orientierung geht. Mit dem Offset werde ich morgen früh mal probieren. Danke schonmal für die Vorschläge

[MOSHzilla]

Habe das Drehen jetzt mal folgendermaßen versucht ohne RelTool zu lösen

FOR i FROM 1 TO steps DO
      ang_x:=ReadNum(dev_x);
      ang_y:=ReadNum(dev_y);
      ang_z:=ReadNum(dev_z);   
      
      pstart.rot := OrientZYX(ang_x,90+ang_y,ang_z);
      MoveL pstart, v1000\T:=deltaT, fine, CAM;      
ENDFOR

Allerdings kommt da auch nur Müll raus

0.000 90.000 0.000
0.000 90.000 0.000
0.000 90.000 0.000
-0.000 89.997 0.000
-149.990 89.656 -149.990
-149.996 89.152 -149.996
-149.998 88.648 -149.998
-149.998 88.144 -149.998
-149.999 87.640 -149.999
-149.999 87.136 -149.999
-149.999 86.633 -149.999
-149.999 86.129 -149.999
-149.999 85.625 -149.999
-149.999 85.121 -149.999
-149.999 84.617 -150.000
-150.000 84.113 -150.000
...
...
-179.657 65.911 178.757
178.635 66.605 -179.650
176.927 67.319 -178.063
175.216 68.052 -176.482
173.504 68.803 -174.909
171.790 69.572 -173.344
170.073 70.357 -171.789
168.355 71.157 -170.244
166.632 71.972 -168.712
164.906 72.801 -167.192
163.173 73.643 -165.688
161.433 74.497 -164.201
159.682 75.362 -162.732
157.919 76.238 -161.286
156.138 77.125 -159.865
154.334 78.020 -158.474
152.500 78.924 -157.120
150.628 79.836 -155.811
148.703 80.754 -154.561
146.728 81.677 -153.379
146.001 82.000 -153.000
146.425 82.099 -153.408
147.445 82.343 -154.435
148.471 82.591 -155.461
149.497 82.840 -156.488
150.522 83.092 -157.513
151.547 83.346 -158.539
152.572 83.603 -159.564
153.596 83.861 -160.589
154.620 84.121 -161.613
155.644 84.383 -162.638
156.667 84.647 -163.662
157.690 84.913 -164.686
158.713 85.180 -165.710
159.736 85.449 -166.733
160.758 85.719 -167.757
161.780 85.991 -168.780
162.802 86.264 -169.804
163.824 86.538 -170.828
164.845 86.813 -171.852
165.867 87.089 -172.876
166.888 87.366 -173.900
167.908 87.645 -174.925
168.928 87.924 -175.951
169.948 88.203 -176.977
170.966 88.483 -178.005
171.982 88.764 -179.035
172.995 89.046 179.932
173.999 89.327 178.889
174.975 89.609 177.818
175.910 89.890 176.711
-0.000 90.000 0.000
0.000 90.000 0.000
0.000 90.000 0.000

Start- und Endposition stimmen wieder, und dazwischen verhält sich nur Y zumindest am Anfang korrekt.
Wenn er da falsch rechnet, dürften doch trotzdem keine Sprünge drin sein oder?

Oder hat vll irgendjemand einen anderen Ansatz, wie ich den Roboter um einen Punkt rotieren lassen kann und die richtigen Winkel loggen kann?

Für einen Lösungsansatz wäre ich unendlich Dankbar

[Hermann]

@robiman:
ja kann schon sein, daß ich die Reihenfolge gerade nicht richtig zitiert habe,
das macht der eine oder andere halt auch anders rum,
der Effekt ist der selbe: man kann sich's kaum bildlich vorstellen.

@MOSHzilla:
die Winkel sehen doch gut aus, den Sprung von -179 auf +178 den gibt's praktisch bei jedem Roboter in der Form,
da die Transofrmationswinkel immer in den 'Hauptbereich' umgerechnet werden (d.h. die Drehungen werden betragsmäßig
nie größer 180). Die resultierende Endposition passt dann schon.
Versuch einfach mal die beiden Drehungen vor und nach dem Sprung mit der Hand (Daumen, Zeigefinger u. Mittelfinger)
bzw. einem Koordinatensystem (Knete mit 3 Streichhölzern drin) nachzuvollziehen.

[MOSHzilla]

@MOSHzilla:
die Winkel sehen doch gut aus, den Sprung von -179 auf +178 den gibt's praktisch bei jedem Roboter in der Form,
da die Transofrmationswinkel immer in den 'Hauptbereich' umgerechnet werden (d.h. die Drehungen werden betragsmäßig
nie größer 180). Die resultierende Endposition passt dann schon.
Versuch einfach mal die beiden Drehungen vor und nach dem Sprung mit der Hand (Daumen, Zeigefinger u. Mittelfinger)
bzw. einem Koordinatensystem (Knete mit 3 Streichhölzern drin) nachzuvollziehen.

Das klingt ja noch logisch, aber am Anfang macht er z.B. den Sprung von 0° auf 150°. Und zwischendurch sind auch noch ein paar drin.

ein Bsp.

-173.767 79.616 -170.879
-172.100 81.576 -167.592
-168.947 83.511 -162.824
-162.510 85.390 -154.777
-146.952 87.099 -137.611
-104.474 88.107 -93.527
-55.370 87.401 -42.817
-36.167 85.768 -22.008
-28.570 83.914 -12.805

Und so große Sprünge dürfte es doch garnicht geben, bei einer Fahrzeit pro MoveL von 3s und dem Interrupt alle 100ms oder?

[Hermann]

Geh davon aus, dass die berechneten Winkel stimmen!
Wenn Du es nicht glaubst, dann versuch es mal mit dem Koordinatensystem nachzuvollziehen (ich wiederhole mich), hoffe es glingt Dir.

Oder benutze ein 3D-CAD-Programm und dreh da ein Bauteil um die entsprechenden Koordinaten,
ich wette darauf, dass es bei den zwei fraglichen Winkelkombinationen am Ende jeweils in der selben
(um ein paar Grad verschobenen) Position zu liegen kommt.

Das ist leider die Krux mit den Euler-Winkeln, die reichen nicht aus um eine Drehung eindeutig zu beschreiben.
Es gibt praktisch immer mehrere verschiedene Euler-Winkelkombinationen, die zur selben Endstellung führen.
Das dürfte ein Grund für ABB sein die Quaternionen zu verwenden, die sind da besser.

[Hermann]

Habe jetzt mal die beiden Drehungen
-104.474 88.107 -93.527
-55.370 87.401 -42.817

im CAD nachvollzogen, heraus kommt das Bild im Anhang.
Sieht für mich plausibel aus, obwohl die Zwischenpositionen, die aus den nacheinander erfolgenden Drehungen entstehen völlig unterschiedlich sind.
Edit: Im zweiten Bild sind die Zwischenpositionen eingezeichnet, jeweils von Rot über Cyan und Blau nach grün.