Skip to content
Snippets Groups Projects
Commit 09b93333 authored by Hailong Zhu's avatar Hailong Zhu
Browse files

- SRS conversion modified 

- SRS conversion tested
parent 8f8ca56a
Branches
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
init_Industrierobotik.txt
+ 1
− 1
View file @ 09b93333
{"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
+ 5
− 6
View file @ 09b93333
......@@ -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 0 → 100644
+ 31
− 0
View file @ 09b93333
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
+ 36
− 9
View file @ 09b93333
......@@ -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
+ 2
− 7
View file @ 09b93333
{"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
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment