diff --git a/init_Industrierobotik.txt b/init_Industrierobotik.txt
index 42e2dbaa4089bf0a186643727f07ca77dbf5998c..ae7321ce8054e2b0953282a6e2813b0bc3d36564 100644
--- a/init_Industrierobotik.txt
+++ b/init_Industrierobotik.txt
@@ -1,5 +1,5 @@
{"frames": [{"id": "cobot1_door_zone", "space": [{"position": {"refSystemId": "ROOT", "point": {"x": 2.0,"y": 0.0,"z": 0.0}, "accuracy": 0.0}, "orientation": {"x": 1.0,"y": 0.5,"z": 1.0, "w": 1.5}, "x": 6.0, "y": 4.0, "z":6.0}]}, {"id": "cobot1_window_zone", "space": [{"position": {"refSystemId": "ROOT", "point": {"x": 2.0,"y": 4.0,"z": 0.0}, "accuracy": 0.0}, "orientation": {"x": 1.0,"y": 0.5,"z": 1.0, "w": 1.5}, "x": 6.0, "y": 4.0, "z":6.0}]}, {"id": "robolab_east", "space": [{"position": {"refSystemId": "ROOT", "point": {"x": 4.0,"y": 2.0,"z": 0.0}, "accuracy": 0.0}, "orientation": {"x": 1.0,"y": 0.5,"z": 1.0, "w": 1.5}, "x": 4.0, "y": 4.0, "z":4.0}]}, {"id": "robolab_west", "space": [{"position": {"refSystemId": "ROOT", "point": {"x": -4.0,"y": 2.0,"z": 0.0}, "accuracy": 0.0}, "orientation": {"x": 1.0,"y": 0.5,"z": 1.0, "w": 1.5}, "x": 8.0, "y": 10.0, "z":6.0}]}, {"id": "robolab_armarea", "space": [{"position": {"refSystemId": "CETI_ROBOTARM_CELL", "point": {"x": 0.0,"y": 0.0,"z": 0.0}, "accuracy": 0.0}, "orientation": {"x": 1.0,"y": 0.5,"z": 1.0, "w": 1.5}, "x": 8.0, "y": 10.0, "z":6.0}]} ]}
-{"refSystems": [{"id": "ROOT"}, {"id": "CETI_ROOT", "position": {"refSystemId": "ROOT", "point": {"x": 3.05,"y": 2.08,"z": 0.0}, "accuracy": 1.0}, "orientation": {"x": 0.0,"y": 0.0,"z": 0.0, "w": 0.0}}, {"id": "CETI_ROBOLAB", "position": {"refSystemId": "CETI_ROOT", "point": {"x": 1.0,"y": 1.0,"z": 0.0}, "accuracy": 1.0}, "orientation": {"x": 0.0,"y": 0.0,"z": 0.0, "w": 0.0}}, {"id": "CETI_OFFICE", "position": {"refSystemId": "CETI_ROOT", "point": {"x": 2.0,"y": 2.0,"z": 0.0}, "accuracy": 1.0}, "orientation": {"x": 0.0,"y": 0.0,"z": 0.0, "w": 0.0}}, {"id": "TL_TESTHALL", "position": {"refSystemId": "ROOT", "point": {"x": 40.0,"y": 60.0,"z": 0.0}, "accuracy": 1.0}, "orientation": {"x": 0.0,"y": 0.0,"z": 0.0, "w": 0.0}}, {"id": "CETI_ROBOTARM_CELL", "position": {"refSystemId": "CETI_ROBOLAB", "point": {"x": 2.0,"y": 2.0,"z": 0.0}, "accuracy": 1.0}, "orientation": {"x": 0.0,"y": 0.0,"z": 0.0, "w": 0.0}}]}
+{"refSystems": [{"id": "ROOT"}, {"id": "CETI_ROOT", "position": {"refSystemId": "ROOT", "point": {"x": 0.5,"y": 0.75,"z": 1.0}, "accuracy": 1.0}, "orientation": {"x": 0.7071067811865477,"y": 0.0,"z": 0.0, "w": 0.7071067811865477}}, {"id": "CETI_ROBOLAB", "position": {"refSystemId": "CETI_ROOT", "point": {"x": 0.2,"y": 0.2,"z": 0.2}, "accuracy": 1.0}, "orientation": {"x": 0.4082481001180531,"y": 0.4082481001180531,"z": 0.4082481001180531, "w": 0.7071067811865477}}, {"id": "CETI_OFFICE", "position": {"refSystemId": "CETI_ROOT", "point": {"x": 2.0,"y": 2.0,"z": 0.0}, "accuracy": 1.0}, "orientation": {"x": 0.0,"y": 0.0,"z": 0.0, "w": 0.0}}, {"id": "TL_TESTHALL", "position": {"refSystemId": "ROOT", "point": {"x": 40.0,"y": 60.0,"z": 0.0}, "accuracy": 1.0}, "orientation": {"x": 0.0,"y": 0.0,"z": 0.21643961393810285, "w": 0.9762960071199334}}, {"id": "CETI_ROBOTARM_CELL", "position": {"refSystemId": "CETI_ROBOLAB", "point": {"x": 0.0,"y": 1.2,"z": 2.2}, "accuracy": 1.0}, "orientation": {"x": 0.2705992818596786,"y": 0.2705992818596786,"z": 0.0, "w": 0.9238795325112868}}]}
{"objectConfigs": [{"agentId": "Employee_1", "sensorId": "UWB_2", "agentType": "HUMAN", "sensorType": "UWB"}, {"agentId": "turtlebot", "sensorId": "UWB_1", "agentType": "ROBOT", "sensorType": "UWB"}, {"agentId": "turtlebot", "sensorId": "98:CD:AC:26:2D:18", "agentType": "ROBOT", "sensorType": "RFID_SCANNER"}, {"agentId": "turtlebot", "sensorId": "handy1", "agentType": "ROBOT", "sensorType": "IMU"}]}
{"pois": [{"id": "UWB_BEACON_1", "description": "Beacon_Position", "data": {"type" : "UWB"}, "position": {"refSystemId": "ROOT", "point": {"x": 3.1,"y": 0.5,"z": 0.0}, "accuracy": 1.0}, "orientation": {"x": 0.0,"y": 0.0,"z": 0.0, "w": 0.0} }, {"id": "UWB_BEACON_2", "description": "Beacon_Position", "data": {"type" : "UWB"}, "position": {"refSystemId": "ROOT", "point": {"x": -0.3,"y": 4.5,"z": 0.0}, "accuracy": 1.0}, "orientation": {"x": 0.0,"y": 0.0,"z": 0.0, "w": 0.0}}, {"id": "83221710", "description": "RFID_TAG", "data": {"type" : "NFC"}, "position": {"refSystemId": "ROOT", "point": {"x": 2.1,"y": 0.4,"z": 0.5}, "accuracy": 0.1}, "orientation": {"x": 0.0,"y": 0.0,"z": 0.0, "w": 0.0} }, {"id": "31762128", "description": "RFID_TAG", "data": {"type" : "NFC"}, "position": {"refSystemId": "ROOT", "point": {"x": 2.2,"y": 3.4,"z": 0.5}, "accuracy": 0.1}, "orientation": {"x": 0.0,"y": 0.0,"z": 0.0, "w": 0.0} }, {"id": "885", "description": "Beacon_Position", "data": {"type" : "UWB"}, "position": {"refSystemId": "ROOT", "point": {"x": 0,"y": 0,"z": 0.97}, "accuracy": 0.01}, "orientation": {"x": 0.0,"y": 0.0,"z": 0.0, "w": 0.0} }, {"id": "2934", "description": "Beacon_Position", "data": {"type" : "UWB"}, "position": {"refSystemId": "ROOT", "point": {"x": 4.24,"y": 1.33,"z": 1.0}, "accuracy": 0.01}, "orientation": {"x": 0.0,"y": 0.0,"z": 0.0, "w": 0.0} }, {"id": "3383", "description": "Beacon_Position", "data": {"type" : "UWB"}, "position": {"refSystemId": "ROOT", "point": {"x": 1.25,"y": 5.89,"z": 1.06}, "accuracy": 0.01}, "orientation": {"x": 0.0,"y": 0.0,"z": 0.0, "w": 0.0} }, {"id": "1107", "description": "Beacon_Position", "data": {"type" : "UWB"}, "position": {"refSystemId": "ROOT", "point": {"x": 4.24,"y": 5.83,"z": 1.05}, "accuracy": 0.01}, "orientation": {"x": 0.0,"y": 0.0,"z": 0.0, "w": 0.0} }]}
{"monitoringRequests": [{"frameIds": ["cobot1_door_zone", "cobot1_window_zone"], "monitoringTaskId": "RobolabMonitoringCeti", "refSystemId": "CETI_ROBOTARM_CELL", "serializationType": "protobuf"}, {"frameIds": ["cobot1_door_zone", "cobot1_window_zone"], "monitoringTaskId": "RobolabMonitoringFrontend", "refSystemId": "CETI_ROBOTARM_CELL", "serializationType": "json"}]}
\ No newline at end of file
diff --git a/src/main/java/ipos/project/Functionality/SRSConversion/SRSConversion.java b/src/main/java/ipos/project/Functionality/SRSConversion/SRSConversion.java
index 429f0c0501b0d552b59b580984f1e7c882b2c713..70de57a17d69ed7a7c63d71479eb377961f44c20 100644
--- a/src/main/java/ipos/project/Functionality/SRSConversion/SRSConversion.java
+++ b/src/main/java/ipos/project/Functionality/SRSConversion/SRSConversion.java
@@ -42,10 +42,11 @@ public class SRSConversion {
if (pla_ref_id.equals(ref_id)) {
//System.out.println("inversed Quaterion_1: " + quat_1.inverse().getX() + ", "+ quat_1.inverse().getY() + ", "+ quat_1.inverse().getZ() + ", " + quat_1.inverse().getW());
LOG.info("INDFRO-DEBUG: Transforming placing-SRS " + pla_ref_id + " to SRS " + ref.getOrigin().getPosition().getReferenceSystem().getId());
- quat_1.mulLeft(quat_2);
+ quat_1.mulLeft(quat_2.inverse());
//System.out.println("phase 1 transfrom: " + p1.getX() + ", "+ p1.getY() + ", "+ p1.getZ());
- p1.transform(quat_2);
+ p1.rotate_quaternion(quat_2);
+ //System.out.println("phase 1 transfrom: " + p1.getX() + ", "+ p1.getY() + ", "+ p1.getZ());
p1.add(ref_org);
pos.setReferenceSystem(ref.getOrigin().getPosition().getReferenceSystem());
@@ -53,11 +54,9 @@ public class SRSConversion {
else if(pla_ref_id.equals(ref.getOrigin().getPosition().getReferenceSystem().getId())) {
LOG.info("INDFRO-DEBUG: Transforming placing-SRS " + pla_ref_id + " to SRS " + ref_id);
//System.out.println("inversed Quaterion_1: " + quat_1.inverse().getX() + ", "+ quat_1.inverse().getY() + ", "+ quat_1.inverse().getZ() + ", " + quat_1.inverse().getW());
- quat_1.mulLeft(quat_2.inverse());
-
- //System.out.println("phase 1 transfrom: " + p1.getX() + ", "+ p1.getY() + ", "+ p1.getZ());
- p1.transform(quat_2.inverse());
+ quat_1.mulLeft(quat_2);
p1.substract(ref_org);
+ p1.rotate_quaternion(quat_2.inverse());
pos.setReferenceSystem(ref);
}
else {
diff --git a/src/main/java/ipos/project/Functionality/SRSConversion/Test plan.txt b/src/main/java/ipos/project/Functionality/SRSConversion/Test plan.txt
new file mode 100644
index 0000000000000000000000000000000000000000..62b39ad134a7ba27841897ef4ee4a9ad0cb2a71b
--- /dev/null
+++ b/src/main/java/ipos/project/Functionality/SRSConversion/Test plan.txt
@@ -0,0 +1,31 @@
+Test rotation based on quaternion:
+
+quaternion calculator: https://www.vcalc.com/equation/?uuid=fed09682-75fb-11e6-9770-bc764e2038f2
+
+Definition of reference systems:
+ def CITI_ROOT against ROOT (0.5, 0.75, 1) rot: (0.7071067811865477,0.7071067811865474,0.0,0.0) //Rotate 90 degrees around x axis.
+ def CITI_ROBOLAB against CETI_ROOT (0.2,0.2,0.2) rot: (0.4082481001180531,0.4082481001180531,0.4082481001180531,0.7071067811865477) //Rotation 90 degrees around (0,0,1)
+ def CETI_ROBOTARM_CELL against CITI_ROBOLAB (0.0, 1.2, 2.2) rot: (0.2705992818596786,0.2705992818596786,0.0,0.9238795325112868)
+ def TL_TESTHALL against ROOT (40.0,60.0,0) rot (0.0,0.0,0.21643961393810285,0.9762960071199334)
+
+
+1. CITI_ROOT -> ROOT -> CITI_ROOT -> CITI_ROBOLAB -> CETI_ROBOTARM_CELL
+
+ original placing: (1,1,1) rot:(1,0,0,0) ref: CETI_ROOT
+
+ change to ref: ROOT
+ expected result:(1.5, 1.75, 0) rot: (0.7071067811865474,0.0,0.0,0.7071067811865477) ref: ROOT
+
+ change back to ref: CETI_ROOT
+ expected result: (1,1,1) rot:(1,0,0,0) ref: CETI_ROOT
+
+ change to ref: CITI_ROBOLAB
+ expected result: (0.8, 0.8, 0.8) rot: (0.7071067811865477, 0.4082481001180531, -0.4082481001180531, w: -0.4082481001180531)
+
+ change to ref: CETI_ROBOTARM_CELL
+ expected result: (-0.07573951,0.47573978,-1.5899485) rot:(0.4323382,0.37717205,-0.45804298,-0.6789863)
+
+
+2. CETI_ROBOTARM_CELL -> CITI_ROBOLAB -> CITI_ROOT -> ROOT -> CITI_ROOT -> CITI_ROBOLAB -> CETI_ROBOTARM_CELL
+
+ original placing: (-0.07573951,0.47573978,-1.5899485) rot:(0.4323382,0.37717205,-0.45804298,-0.6789863) ref: CETI_ROBOTARM_CELL
\ No newline at end of file
diff --git a/src/main/java/ipos/project/Functionality/SRSConversion/Vector_math.java b/src/main/java/ipos/project/Functionality/SRSConversion/Vector_math.java
index d43b7d7bb827197bb6d57d58d7f65275bcb5f956..5d5b0a35a987a001eed6c078344dba5156514de7 100644
--- a/src/main/java/ipos/project/Functionality/SRSConversion/Vector_math.java
+++ b/src/main/java/ipos/project/Functionality/SRSConversion/Vector_math.java
@@ -55,15 +55,42 @@ public final class Vector_math {
y -= v.y;
z -= v.z;
}
- public void transform (Quaternion_math q) {
- tmp2.set(q);
- tmp2.conjugate();
- tmp1 = new Quaternion_math(x, y, z, 0);
- tmp2.mulLeft(tmp1);
- //tmp2.mulLeft(tmp2);
+ public float dot (Vector_math v){
+ float dotProduct;
+ dotProduct = x * v.x + y * v.y +z * v.z;
+ return dotProduct;
+ }
+ public void cross (Vector_math v){
+ float xt = x;
+ float yt = y;
+ float zt = z;
+ x = yt * v.z - zt * v.y;
+ y = zt * v.x - xt * v.z;
+ z = xt * v.y - yt * v.x;
+ }
+ public void rotate_quaternion (Quaternion_math q) {
+ // Extract the scalar part of the quaternion
+ Vector_math u = new Vector_math(q.getX(),q.getY(),q.getZ());
+
+ Vector_math temp1 = new Vector_math(x,y,z);
+ Vector_math temp2 = new Vector_math(q.getX(),q.getY(),q.getZ());
+
+ float s = q.getW();
+ //System.out.println("s: " + s);
+ //System.out.println("Quaternion Vector: " + u.getX() + ", "+ u.getY() + ", "+ u.getZ());
- x = tmp2.getX();
- y = tmp2.getY();
- z = tmp2.getZ();
+ // Do the math
+ temp2.multiply(2.0f * this.dot(u));
+ temp1.cross(u);
+ temp1.multiply(2.0f * s);
+ //System.out.println("temp1: " + temp1.getX() + ", "+ temp1.getY() + ", "+ temp1.getZ());
+ //System.out.println("temp2: " + temp2.getX() + ", "+ temp2.getY() + ", "+ temp2.getZ());
+ //System.out.println("phase 1 transfrom: " + this.getX() + ", "+ this.getY() + ", "+ this.getZ());
+ this.multiply(s * s - u.dot(u));
+ //System.out.println("phase 2 transfrom: " + this.getX() + ", "+ this.getY() + ", "+ this.getZ());
+ this.add(temp1);
+ //System.out.println("phase 3 transfrom: " + this.getX() + ", "+ this.getY() + ", "+ this.getZ());
+ this.add(temp2);
+ //System.out.println("phase 4 transfrom: " + this.getX() + ", "+ this.getY() + ", "+ this.getZ());
}
}
diff --git a/testdata_raw_indfro.txt b/testdata_raw_indfro.txt
index f0997c5c975ce9b0d760f49228a213c64cbfff82..21e970201e6e02772da033b5758b2eb25eab28e6 100644
--- a/testdata_raw_indfro.txt
+++ b/testdata_raw_indfro.txt
@@ -1,7 +1,2 @@
-{"sensorPositionEvent": [{"sensorId": "UWB_1", "position": {"refSystemId": "ROOT", "point": {"x": 3.0,"y": 1.5,"z": 3.0}, "accuracy": 10.0}, "orientation": {"x": 1.0,"y": 0.5,"z": 1.0, "w": 1.5}, "lastPosUpdate": "2021-10-14T19:32:20+00:00"}]}
-{"sensorPositionEvent": [{"sensorId": "UWB_1", "position": {"refSystemId": "CETI_ROBOTARM_CELL", "point": {"x": 1.0,"y": 0.5,"z": 2.0}, "accuracy": 10.0}, "orientation": {"x": 1.0,"y": 0.5,"z": 1.0, "w": 1.5}, "lastPosUpdate": "2021-10-14T19:32:21+00:00"}]}
-{"sensorPositionEvent": [{"sensorId": "UWB_1", "position": {"refSystemId": "CETI_ROOT", "point": {"x": 3.0,"y": 4.5,"z": 3.0}, "accuracy": 10.0}, "orientation": {"x": 1.0,"y": 0.5,"z": 1.0, "w": 1.5}, "lastPosUpdate": "2021-10-14T19:32:22+00:00"}]}
-{"sensorPositionEvent": [{"sensorId": "UWB_1", "position": {"refSystemId": "ROOT", "point": {"x": -3.0,"y": 4.5,"z": 3.0}, "accuracy": 10.0}, "orientation": {"x": 1.0,"y": 0.5,"z": 1.0, "w": 1.5}, "lastPosUpdate": "2021-10-14T19:32:23+00:00"}]}
-{"sensorPositionEvent": [{"sensorId": "UWB_1", "position": {"refSystemId": "ROOT", "point": {"x": 3.0,"y": 1.5,"z": 3.0}, "accuracy": 10.0}, "orientation": {"x": 1.0,"y": 0.5,"z": 1.0, "w": 1.5}, "lastPosUpdate": "2021-10-14T19:32:24+00:00"}]}
-{"nfcRawDataEvent" : [ {"timestamp" : "2021-10-14T19:33:25+00:00", "tagId": "83221710", "type": "RFID", "scannerId": "98:CD:AC:26:2D:18"} ] }
-{"sensorPositionEvent": [{"sensorId": "UWB_1", "position": {"refSystemId": "ROOT", "point": {"x": 3.0,"y": 1.5,"z": 3.0}, "accuracy": 10.0}, "orientation": {"x": 1.0,"y": 0.5,"z": 1.0, "w": 1.5}, "lastPosUpdate": "2021-10-14T19:34:25+00:00"}]}
\ No newline at end of file
+{"sensorPositionEvent": [{"sensorId": "UWB_1", "position": {"refSystemId": "CETI_ROOT", "point": {"x": 1.0,"y": 1.0,"z": 1.0}, "accuracy": 10.0}, "orientation": {"x": 1.0,"y": 0.0,"z": 0.0, "w": 0.0}, "lastPosUpdate": "2021-10-14T19:32:20+00:00"}]}
+{"sensorPositionEvent": [{"sensorId": "UWB_1", "position": {"refSystemId": "CETI_ROBOTARM_CELL", "point": {"x": -0.07573951,"y": 0.47573978,"z": -1.5899485}, "accuracy": 10.0}, "orientation": {"x": 0.4323382,"y": 0.37717205,"z": -0.45804298, "w": -0.6789863}, "lastPosUpdate": "2021-10-14T19:32:20+00:00"}]}
\ No newline at end of file