- SRS conversion modified

- SRS conversion tested

init_Industrierobotik.txt

 {"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

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 {

src/main/java/ipos/project/Functionality/SRSConversion/Test plan.txt

+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

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());
     }
 }

testdata_raw_indfro.txt

-{"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": "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
-{"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