Skip to content

Commit f42a25f

Browse files
madhephaestusmadhephaestus
committed
Robust unit testing across the range of euler calculations
1 parent 12b130f commit f42a25f

File tree

2 files changed

+82
-44
lines changed

2 files changed

+82
-44
lines changed

‎src/main/java/com/neuronrobotics/sdk/addons/kinematics/math/RotationNR.java

Lines changed: 26 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -19,8 +19,8 @@ public class RotationNR {
1919
/** The rotation matrix. */
2020
//double[][] rotationMatrix = ;
2121
private Rotation storage=new Rotation(1,0,0,0,false);
22-
private RotationOrder order = RotationOrder.ZXZ;
23-
private RotationConvention convention = RotationConvention.VECTOR_OPERATOR;
22+
private static RotationOrder order = RotationOrder.ZXZ;
23+
private static RotationConvention convention = RotationConvention.FRAME_TRANSFORM;
2424
/**
2525
* Null constructor forms a.
2626
*/
@@ -56,7 +56,11 @@ public RotationNR(double tilt, double azumeth, double elevation) {
5656
}
5757

5858
private void loadFromAngles(double tilt, double azumeth, double elevation) {
59-
storage = new Rotation(order, convention, azumeth, elevation, tilt);
59+
storage = new Rotation(getOrder(), getConvention(),
60+
Math.toRadians(azumeth),
61+
Math.toRadians(elevation),
62+
Math.toRadians(tilt)
63+
);
6064
}
6165

6266
/**
@@ -315,7 +319,7 @@ public static boolean bound(double low, double high, double n) {
315319
*/
316320
public double getRotationTilt() {
317321

318-
return storage.getAngles(order, convention)[1];
322+
return storage.getAngles(getOrder(), getConvention())[2];
319323

320324
}
321325

@@ -326,7 +330,7 @@ public double getRotationTilt() {
326330
*/
327331
public double getRotationElevation() {
328332

329-
return storage.getAngles(order, convention)[1];
333+
return storage.getAngles(getOrder(), getConvention())[1];
330334
}
331335

332336
/**
@@ -336,7 +340,7 @@ public double getRotationElevation() {
336340
*/
337341
public double getRotationAzimuth() {
338342

339-
return storage.getAngles(order, convention)[0];
343+
return storage.getAngles(getOrder(), getConvention())[0];
340344
}
341345

342346
/**
@@ -375,4 +379,20 @@ public double getRotationMatrix2QuaturnionZ() {
375379
return storage.getQ3();
376380
}
377381

382+
public static RotationOrder getOrder() {
383+
return order;
384+
}
385+
386+
public static void setOrder(RotationOrder o) {
387+
order = o;
388+
}
389+
390+
public static RotationConvention getConvention() {
391+
return convention;
392+
}
393+
394+
public static void setConvention(RotationConvention convention) {
395+
RotationNR.convention = convention;
396+
}
397+
378398
}

‎test/java/src/junit/test/neuronrobotics/utilities/RotationNRTest.java

Lines changed: 56 additions & 38 deletions
Original file line numberDiff line numberDiff line change
@@ -2,6 +2,8 @@
22

33
import static org.junit.Assert.*;
44

5+
import org.apache.commons.math3.geometry.euclidean.threed.RotationConvention;
6+
import org.apache.commons.math3.geometry.euclidean.threed.RotationOrder;
57
import org.junit.Test;
68

79
import com.neuronrobotics.sdk.addons.kinematics.math.RotationNR;
@@ -19,50 +21,66 @@ public class RotationNRTest {
1921
@Test
2022
public void test() {
2123
int failCount = 0;
22-
int iterations = 100;
23-
for (int i = 0; i < iterations; i++) {
24-
double tilt = Math.toRadians((Math.random() *360.0) -180);
25-
double elevation = Math.toRadians((Math.random() * 360.0) -180 );
26-
double azumus = Math.toRadians((Math.random() * 360.0) -180 );
27-
RotationNR rotTest = new RotationNR(Math.toDegrees(tilt), Math.toDegrees(azumus),
28-
Math.toDegrees(elevation));
29-
System.out.println("\n\nTest #" + i);
30-
System.out.println("Testing Az=" + Math.toDegrees(azumus) + " El=" + Math.toDegrees(elevation) + " Tl="
31-
+ Math.toDegrees(tilt));
32-
System.out.println("Got Az=" + Math.toDegrees(rotTest.getRotationAzimuth()) + " El="
33-
+ Math.toDegrees(rotTest.getRotationElevation()) + " Tl="
34-
+ Math.toDegrees(rotTest.getRotationTilt()));
35-
36-
if (!RotationNR.bound(tilt - .01, tilt + .01, rotTest.getRotationTilt())) {
37-
failCount++;
38-
System.err.println("Rotation Tilt is not consistant. expected " + Math.toDegrees(tilt) + " got "
39-
+ Math.toDegrees(rotTest.getRotationTilt())+
40-
" \t\tOff By "+(Math.toDegrees(tilt) - Math.toDegrees(rotTest.getRotationTilt()) )
41-
);
42-
}
43-
if (!RotationNR.bound(elevation - .01, elevation + .01, rotTest.getRotationElevation())) {
44-
failCount++;
45-
System.err.println("Rotation Elevation is not consistant. expected " + Math.toDegrees(elevation)
46-
+ " got " + Math.toDegrees(rotTest.getRotationElevation())+
47-
" \t\tOff By "+(Math.toDegrees(elevation) + Math.toDegrees(rotTest.getRotationElevation()) )
48-
49-
);
50-
}
51-
if (!RotationNR.bound(azumus - .01, azumus + .01, rotTest.getRotationAzimuth())) {
52-
failCount++;
53-
System.err.println("Rotation Tilt is not consistant. expected " + Math.toDegrees(azumus) + " got "
54-
+ Math.toDegrees(rotTest.getRotationAzimuth())+
55-
" \t\tOff By "+(Math.toDegrees(azumus) - Math.toDegrees(rotTest.getRotationAzimuth()) )
24+
int iterations = 10;
25+
RotationOrder[] list = { RotationOrder.XYZ,
26+
RotationOrder.XZY, RotationOrder.YXZ, RotationOrder.YZX,
27+
RotationOrder.ZXY, RotationOrder.ZYX, RotationOrder.XYX, RotationOrder.XZX, RotationOrder.YXY,
28+
RotationOrder.YZY, RotationOrder.ZXZ, RotationOrder.ZYZ
29+
30+
};
31+
RotationConvention[] conventions = { RotationConvention.FRAME_TRANSFORM, RotationConvention.VECTOR_OPERATOR };
32+
for (RotationConvention conv : conventions) {
33+
RotationNR.setConvention(conv);
34+
System.out.println("\n\nUsing convention " + conv.toString());
35+
for (RotationOrder ro : list) {
36+
RotationNR.setOrder(ro);
37+
System.out.println("\n\nUsing rotationOrder " + ro.toString());
38+
failCount = 0;
39+
for (int i = 0; i < iterations; i++) {
40+
double tilt = Math.toRadians((Math.random() * 359) - 179.5);
41+
double elevation = Math.toRadians((Math.random() * 359) - 179.5);
42+
double azumus = Math.toRadians((Math.random() * 359) - 179.5);
43+
RotationNR rotTest = new RotationNR(Math.toDegrees(tilt), Math.toDegrees(azumus),
44+
Math.toDegrees(elevation));
45+
System.out.println("\n\nTest #" + i);
46+
System.out.println("Testing Az=" + Math.toDegrees(azumus) + " El=" + Math.toDegrees(elevation)
47+
+ " Tl=" + Math.toDegrees(tilt));
48+
System.out.println("Got Az=" + Math.toDegrees(rotTest.getRotationAzimuth()) + " El="
49+
+ Math.toDegrees(rotTest.getRotationElevation()) + " Tl="
50+
+ Math.toDegrees(rotTest.getRotationTilt()));
51+
52+
if (!RotationNR.bound(tilt - .01, tilt + .01, rotTest.getRotationTilt())) {
53+
failCount++;
54+
System.err.println("Rotation Tilt is not consistant. expected " + Math.toDegrees(tilt) + " got "
55+
+ Math.toDegrees(rotTest.getRotationTilt()) + " \t\tOff By "
56+
+ (Math.toDegrees(tilt) - Math.toDegrees(rotTest.getRotationTilt())));
57+
}
58+
if (!RotationNR.bound(elevation - .01, elevation + .01, rotTest.getRotationElevation())) {
59+
failCount++;
60+
System.err.println("Rotation Elevation is not consistant. expected " + Math.toDegrees(elevation)
61+
+ " got " + Math.toDegrees(rotTest.getRotationElevation()) + " \t\tOff By "
62+
+ (Math.toDegrees(elevation) + Math.toDegrees(rotTest.getRotationElevation()))
63+
5664
);
65+
}
66+
if (!RotationNR.bound(azumus - .01, azumus + .01, rotTest.getRotationAzimuth())) {
67+
failCount++;
68+
System.err.println("Rotation azumus is not consistant. expected " + Math.toDegrees(azumus)
69+
+ " got " + Math.toDegrees(rotTest.getRotationAzimuth()) + " \t\tOff By "
70+
+ (Math.toDegrees(azumus) - Math.toDegrees(rotTest.getRotationAzimuth())));
71+
}
72+
ThreadUtil.wait(20);
73+
}
74+
if (failCount < 1) {
75+
System.out.println("Orentation " + ro.toString() + " worked ina all cases");
76+
return;
77+
}
5778
}
58-
ThreadUtil.wait(20);
5979
}
60-
6180
if (failCount > 1) {
62-
fail("Rotation failed " + failCount + " times of "+iterations*3);
81+
fail("Rotation failed " + failCount + " times of " + ((iterations * 3 * list.length) - 0));
6382

6483
}
65-
6684
}
6785

6886
}

0 commit comments

Comments
 (0)