Changed Nominal Z axis values

stretch_camera_testrig/config/testrig_marker_info.yaml

@@ -11,7 +11,7 @@ testrig_aruco_marker_info:
   - - 0.0
     - 0.0
     - -1.0
-    - 0.97551
+    - 0.97195
   - - 0.0
     - 0.0
     - 0.0
@@ -28,7 +28,7 @@ testrig_aruco_marker_info:
   - - 0.0
     - 0.0
     - -1.0
-    - 0.97551
+    - 0.97195
   - - 0.0
     - 0.0
     - 0.0
@@ -45,7 +45,7 @@ testrig_aruco_marker_info:
   - - 0.0
     - 0.0
     - -1.0
-    - 0.97551
+    - 0.97195
   - - 0.0
     - 0.0
     - 0.0
@@ -62,7 +62,7 @@ testrig_aruco_marker_info:
   - - 0.0
     - 0.0
     - -1.0
-    - 0.97551
+    - 0.97195
   - - 0.0
     - 0.0
     - 0.0
@@ -79,7 +79,7 @@ testrig_aruco_marker_info:
   - - 0.0
     - 0.0
     - -1.0
-    - 0.97551
+    - 0.97195
   - - 0.0
     - 0.0
     - 0.0

stretch_camera_testrig/data/results/testrig_results_134322070297_202204281433.yaml

@@ -0,0 +1,67 @@
+Realsense Details:
+  firmware: 05.13.00.50
+  serial: '134322070297'
+  usb: '3.2'
+capture_id: '202204281433'
+lighting_condition:
+  illuminance: null
+null_frames:
+  base_left_marker_pose: 0
+  base_right_marker_pose: 26
+  shoulder_marker_pose: 50
+  wrist_inside_marker_pose: 7
+  wrist_top_marker_pose: 10
+number_samples: 600
+performance_metrics:
+  angle_rotation:
+    base_left_marker_pose:
+      maximum: 5.825776514914376
+      mean: 2.9954012224652917
+      median: 2.946734907433167
+      rmse: 3.078626594110408
+    base_right_marker_pose:
+      maximum: 7.567008009518704
+      mean: 4.50741189429689
+      median: 4.469461271789145
+      rmse: 4.6102176286044765
+    shoulder_marker_pose:
+      maximum: 18.091681718730758
+      mean: 10.459194783228007
+      median: 10.595758488338625
+      rmse: 10.83379138062113
+    wrist_inside_marker_pose:
+      maximum: 31.983934642146934
+      mean: 26.31073325365646
+      median: 26.461641297217252
+      rmse: 26.399583220659725
+    wrist_top_marker_pose:
+      maximum: 22.324392504976935
+      mean: 14.566053360223123
+      median: 14.661843549829827
+      rmse: 14.884930665992593
+  euclidean_error:
+    base_left_marker_pose:
+      maximum: -0.008985985486859138
+      mean: -0.009577033088789532
+      median: -0.00957521465757799
+      rmse: 0.009578776189420272
+    base_right_marker_pose:
+      maximum: -0.006553125706156626
+      mean: -0.007660758286062072
+      median: -0.007668665583789813
+      rmse: 0.007668300536341694
+    shoulder_marker_pose:
+      maximum: 0.00043430227684115863
+      mean: -0.0018141936483635234
+      median: -0.0018266486931424186
+      rmse: 0.0019980920692085295
+    wrist_inside_marker_pose:
+      maximum: -0.0017848569388676516
+      mean: -0.0037750775114444263
+      median: -0.0037610304025108565
+      rmse: 0.003849088485712081
+    wrist_top_marker_pose:
+      maximum: -0.00028974841665929674
+      mean: -0.002435837573032023
+      median: -0.0024146604383572523
+      rmse: 0.0025604756984339387

stretch_camera_testrig/data/results/testrig_results_134322070297_202204281634.yaml

@@ -0,0 +1,67 @@
+Realsense Details:
+  firmware: 05.13.00.50
+  serial: '134322070297'
+  usb: '3.2'
+capture_id: '202204281634'
+lighting_condition:
+  illuminance: null
+null_frames:
+  base_left_marker_pose: 0
+  base_right_marker_pose: 95
+  shoulder_marker_pose: 289
+  wrist_inside_marker_pose: 15
+  wrist_top_marker_pose: 3
+number_samples: 600
+performance_metrics:
+  angle_rotation:
+    base_left_marker_pose:
+      maximum: 7.883329791794328
+      mean: 3.333362385585352
+      median: 3.1888204675914764
+      rmse: 3.528896862288492
+    base_right_marker_pose:
+      maximum: 12.735867335526736
+      mean: 7.616777191598444
+      median: 7.699020844123073
+      rmse: 7.861219384542112
+    shoulder_marker_pose:
+      maximum: 27.306679457568823
+      mean: 20.449430883324162
+      median: 20.628420193173156
+      rmse: 20.628435996037233
+    wrist_inside_marker_pose:
+      maximum: 29.23617249110053
+      mean: 23.128910857772752
+      median: 23.255818372351502
+      rmse: 23.230346917342548
+    wrist_top_marker_pose:
+      maximum: 25.371802060809532
+      mean: 18.688207615257518
+      median: 18.728240832467602
+      rmse: 18.853912306903087
+  euclidean_error:
+    base_left_marker_pose:
+      maximum: -0.002290839411512313
+      mean: -0.0037081369247007646
+      median: -0.003719444131558114
+      rmse: 0.0037363307582611556
+    base_right_marker_pose:
+      maximum: -0.00042089050574423137
+      mean: -0.0023382929359848687
+      median: -0.00221527329632476
+      rmse: 0.0025078253699411953
+    shoulder_marker_pose:
+      maximum: 0.007758186739723727
+      mean: 0.005496741248451533
+      median: 0.0057369441134687005
+      rmse: 0.005645993132779887
+    wrist_inside_marker_pose:
+      maximum: 0.004220067988259468
+      mean: 0.002450121622343215
+      median: 0.002474100441822502
+      rmse: 0.0025470915277097613
+    wrist_top_marker_pose:
+      maximum: 0.0027556869442825205
+      mean: 0.00039448586391564207
+      median: 0.00043695966301537936
+      rmse: 0.0009758048021871322

stretch_camera_testrig/data/results/testrig_results_134322070297_202204281730.yaml

@@ -0,0 +1,67 @@
+Realsense Details:
+  firmware: 05.13.00.50
+  serial: '134322070297'
+  usb: '3.2'
+capture_id: '202204281730'
+lighting_condition:
+  illuminance: null
+null_frames:
+  base_left_marker_pose: 1
+  base_right_marker_pose: 17
+  shoulder_marker_pose: 384
+  wrist_inside_marker_pose: 1
+  wrist_top_marker_pose: 0
+number_samples: 600
+performance_metrics:
+  angle_rotation:
+    base_left_marker_pose:
+      maximum: 9.952229122964782
+      mean: 4.392090954368037
+      median: 4.318868910200014
+      rmse: 4.5761145629699564
+    base_right_marker_pose:
+      maximum: 12.96113467859752
+      mean: 8.465560781173895
+      median: 8.44145156397168
+      rmse: 8.618791683889652
+    shoulder_marker_pose:
+      maximum: 28.34555248546537
+      mean: 22.37736375764678
+      median: 22.45304047638072
+      rmse: 22.48626445560198
+    wrist_inside_marker_pose:
+      maximum: 26.882938633504565
+      mean: 21.40152660944474
+      median: 21.461767960613614
+      rmse: 21.490331628283197
+    wrist_top_marker_pose:
+      maximum: 29.20380524192317
+      mean: 21.75256364974819
+      median: 21.88029029125677
+      rmse: 21.914232302114378
+  euclidean_error:
+    base_left_marker_pose:
+      maximum: -0.002174108727677204
+      mean: -0.0034172115729892505
+      median: -0.0034401631854353854
+      rmse: 0.0034326908594153956
+    base_right_marker_pose:
+      maximum: 0.000802316639218037
+      mean: -0.0013320679583119423
+      median: -0.001326506962837537
+      rmse: 0.001489348429484937
+    shoulder_marker_pose:
+      maximum: 0.0074767636327430775
+      mean: 0.0061658390587243095
+      median: 0.0061892502909408065
+      rmse: 0.0061868039560887805
+    wrist_inside_marker_pose:
+      maximum: 0.004113446341910576
+      mean: 0.0025642961442791175
+      median: 0.0025637519581322987
+      rmse: 0.0026241001550440573
+    wrist_top_marker_pose:
+      maximum: 0.002018960086641153
+      mean: -0.0004237816408453247
+      median: -0.00040472782750233804
+      rmse: 0.0008307344576854903

stretch_camera_testrig/data/results/testrig_results_134322070297_202204281734.yaml

@@ -0,0 +1,67 @@
+Realsense Details:
+  firmware: 05.13.00.50
+  serial: '134322070297'
+  usb: '3.2'
+capture_id: '202204281734'
+lighting_condition:
+  illuminance: null
+null_frames:
+  base_left_marker_pose: 0
+  base_right_marker_pose: 7
+  shoulder_marker_pose: 309
+  wrist_inside_marker_pose: 0
+  wrist_top_marker_pose: 0
+number_samples: 600
+performance_metrics:
+  angle_rotation:
+    base_left_marker_pose:
+      maximum: 7.427453461222183
+      mean: 3.1513410620287035
+      median: 2.9981604897763448
+      rmse: 3.321170862012057
+    base_right_marker_pose:
+      maximum: 9.09563824850721
+      mean: 5.4257448299606645
+      median: 5.499135045560987
+      rmse: 5.559805299677735
+    shoulder_marker_pose:
+      maximum: 28.80127671768137
+      mean: 21.270674755230054
+      median: 21.249238800714558
+      rmse: 21.452505717904476
+    wrist_inside_marker_pose:
+      maximum: 29.154786204846257
+      mean: 21.377886532267407
+      median: 21.415372407411247
+      rmse: 21.542694487800304
+    wrist_top_marker_pose:
+      maximum: 26.232564121266705
+      mean: 18.761796974956898
+      median: 18.847158226824632
+      rmse: 19.035738641516282
+  euclidean_error:
+    base_left_marker_pose:
+      maximum: -0.007737422444664244
+      mean: -0.008585105153257156
+      median: -0.008595787873107663
+      rmse: 0.008590298916510185
+    base_right_marker_pose:
+      maximum: -0.0040035336708685865
+      mean: -0.0059731576898334904
+      median: -0.005962729815138901
+      rmse: 0.006009988966125487
+    shoulder_marker_pose:
+      maximum: 0.0013342860981623295
+      mean: -0.00047329907243346314
+      median: -0.00041251555369214987
+      rmse: 0.0008211188308446646
+    wrist_inside_marker_pose:
+      maximum: -0.00022003016295701894
+      mean: -0.0018687907170104382
+      median: -0.0018339301246591555
+      rmse: 0.001989832714490403
+    wrist_top_marker_pose:
+      maximum: -0.0013734917491508014
+      mean: -0.0038327071228253275
+      median: -0.003793405493255908
+      rmse: 0.003958746299079997

stretch_camera_testrig/launch/testrig_collect_data.launch

@@ -14,7 +14,7 @@
   </node>
 
   <!-- REALSENSE D435i Model Load -->
-  <param name="robot_description" command="$(find xacro)/xacro --inorder '$(find realsense2_description)/urdf/test_d435i_camera.urdf.xacro' use_nominal_extrinsics:=true" />
+  <param name="robot_description" command="$(find xacro)/xacro --inorder '$(find realsense2_description)/urdf/test_d435i_camera.urdf.xacro' use_nominal_extrinsics:=false" />
 
   <node
       name="robot_state_publisher"

stretch_camera_testrig/nodes/testrig_analyze_data.py

@@ -175,7 +175,7 @@ class TestRig_Analyze:
         for key in error_dict.keys():
             for i in range(Num_samples):
                 if type(data_dict[key][i]) != type(None):
-                    error_dict[key].append(self.euclidean_error(nominal_poses_dict[key], data_dict[key][i]))
+                    error_dict[key].append(self.euclidean_error(data_dict[key][i], nominal_poses_dict[key]))
                 else:
                     error_dict[key].append(None)
         return error_dict
@@ -211,7 +211,8 @@ class TestRig_Analyze:
         p2 = r2[:3, 3]
         dist1 = np.linalg.norm(p1)
         dist2 = np.linalg.norm(p2)
-        dist = abs(dist1 - dist2)
+        dist = dist1 - dist2
+        # dist = abs(dist)
         #     dist = np.linalg.norm(p1-p2)
         return float(dist)