Nominal Poses updated Rot Matrix

stretch_camera_testrig/.idea/workspace.xml

@@ -0,0 +1,42 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<project version="4">
+  <component name="ChangeListManager">
+    <list default="true" id="1af2d8e2-435e-4ad7-807c-be3ee3f35fe6" name="Changes" comment="">
+      <change beforePath="$PROJECT_DIR$/data/Testrig_Data_Analyse.ipynb" beforeDir="false" afterPath="$PROJECT_DIR$/data/Testrig_Data_Analyse.ipynb" afterDir="false" />
+      <change beforePath="$PROJECT_DIR$/rviz/rviz.rviz" beforeDir="false" afterPath="$PROJECT_DIR$/rviz/rviz.rviz" afterDir="false" />
+    </list>
+    <option name="SHOW_DIALOG" value="false" />
+    <option name="HIGHLIGHT_CONFLICTS" value="true" />
+    <option name="HIGHLIGHT_NON_ACTIVE_CHANGELIST" value="false" />
+    <option name="LAST_RESOLUTION" value="IGNORE" />
+  </component>
+  <component name="Git.Settings">
+    <option name="RECENT_GIT_ROOT_PATH" value="$PROJECT_DIR$/.." />
+  </component>
+  <component name="MarkdownSettingsMigration">
+    <option name="stateVersion" value="1" />
+  </component>
+  <component name="ProjectId" id="28LtqVNukeMT3sgSSDAeEVUbwHZ" />
+  <component name="ProjectLevelVcsManager" settingsEditedManually="true" />
+  <component name="ProjectViewState">
+    <option name="hideEmptyMiddlePackages" value="true" />
+    <option name="showLibraryContents" value="true" />
+  </component>
+  <component name="PropertiesComponent">{
+  &quot;keyToString&quot;: {
+    &quot;RunOnceActivity.OpenProjectViewOnStart&quot;: &quot;true&quot;,
+    &quot;RunOnceActivity.ShowReadmeOnStart&quot;: &quot;true&quot;
+  }
+}</component>
+  <component name="SpellCheckerSettings" RuntimeDictionaries="0" Folders="0" CustomDictionaries="0" DefaultDictionary="application-level" UseSingleDictionary="true" transferred="true" />
+  <component name="TaskManager">
+    <task active="true" id="Default" summary="Default task">
+      <changelist id="1af2d8e2-435e-4ad7-807c-be3ee3f35fe6" name="Changes" comment="" />
+      <created>1651007886438</created>
+      <option name="number" value="Default" />
+      <option name="presentableId" value="Default" />
+      <updated>1651007886438</updated>
+    </task>
+    <servers />
+  </component>
+</project>

stretch_camera_testrig/config/testrig_marker_info.yaml

@@ -5,12 +5,12 @@ testrig_aruco_marker_info:
     - 0.0
     - 0.009
   - - 0.0
-    - 1.0
+    - -1.0
     - 0.0
     - -0.06418
   - - 0.0
     - 0.0
-    - 1.0
+    - -1.0
     - 0.97551
   - - 0.0
     - 0.0
@@ -22,12 +22,12 @@ testrig_aruco_marker_info:
     - 0.0
     - 0.078
   - - 0.0
-    - 1.0
+    - -1.0
     - 0.0
     - -0.06418
   - - 0.0
     - 0.0
-    - 1.0
+    - -1.0
     - 0.97551
   - - 0.0
     - 0.0
@@ -39,12 +39,12 @@ testrig_aruco_marker_info:
     - 0.0
     - 0.0858
   - - 0.0
-    - 1.0
+    - -1.0
     - 0.0
     - -0.12048
   - - 0.0
     - 0.0
-    - 1.0
+    - -1.0
     - 0.97551
   - - 0.0
     - 0.0
@@ -56,12 +56,12 @@ testrig_aruco_marker_info:
     - 0.0
     - -0.00275
   - - 0.0
-    - 1.0
+    - -1.0
     - 0.0
     - -0.12443
   - - 0.0
     - 0.0
-    - 1.0
+    - -1.0
     - 0.97551
   - - 0.0
     - 0.0
@@ -73,12 +73,12 @@ testrig_aruco_marker_info:
     - 0.0
     - 0.03935
   - - 0.0
-    - 1.0
+    - -1.0
     - 0.0
     - -0.12443
   - - 0.0
     - 0.0
-    - 1.0
+    - -1.0
     - 0.97551
   - - 0.0
     - 0.0

stretch_camera_testrig/data/Testrig_Data_Analyse.ipynb

@@ -2,7 +2,7 @@
  "cells": [
   {
    "cell_type": "code",
-   "execution_count": 36,
+   "execution_count": 4,
    "metadata": {},
    "outputs": [],
    "source": [
@@ -13,16 +13,16 @@
   },
   {
    "cell_type": "code",
-   "execution_count": 37,
+   "execution_count": 5,
    "metadata": {},
    "outputs": [],
    "source": [
-    "filename = 'testrig_collected_data_202204261559.yaml'"
+    "filename = 'testrig_collected_data_202204261813.yaml'"
    ]
   },
   {
    "cell_type": "code",
-   "execution_count": 56,
+   "execution_count": 6,
    "metadata": {},
    "outputs": [],
    "source": [
@@ -58,7 +58,7 @@
   },
   {
    "cell_type": "code",
-   "execution_count": 57,
+   "execution_count": 7,
    "metadata": {},
    "outputs": [],
    "source": [
@@ -67,20 +67,20 @@
   },
   {
    "cell_type": "code",
-   "execution_count": 58,
+   "execution_count": 8,
    "metadata": {},
    "outputs": [
     {
      "data": {
       "text/plain": [
-       "{'base_left_marker_pose': array([ 0.0239215, -0.0345387,  0.9687601]),\n",
-       " 'base_right_marker_pose': array([ 0.09184391, -0.03379975,  0.96848354]),\n",
-       " 'shoulder_marker_pose': array([ 0.09992021, -0.08972844,  0.97250558]),\n",
-       " 'wrist_inside_marker_pose': array([ 0.01281881, -0.09311278,  0.97060522]),\n",
-       " 'wrist_top_marker_pose': array([ 0.05436245, -0.09324136,  0.97268975])}"
+       "{'base_left_marker_pose': array([ 0.0238681 , -0.03427565,  0.96917111]),\n",
+       " 'base_right_marker_pose': array([ 0.09188251, -0.03348409,  0.96873629]),\n",
+       " 'shoulder_marker_pose': array([ 0.09998645, -0.08940415,  0.97319534]),\n",
+       " 'wrist_inside_marker_pose': array([ 0.01273042, -0.09285669,  0.97215891]),\n",
+       " 'wrist_top_marker_pose': array([ 0.05442445, -0.09320034,  0.97401416])}"
       ]
      },
-     "execution_count": 58,
+     "execution_count": 8,
      "metadata": {},
      "output_type": "execute_result"
     }

stretch_camera_testrig/data/results/testrig_results_134322070297_202204261845.yaml

@@ -0,0 +1,68 @@
+Realsense Details:
+  firmware: 05.13.00.50
+  serial: '134322070297'
+  usb: '3.2'
+capture_id: '202204261845'
+lighting_condition:
+  brightness: null
+  temperature: null
+null_frames:
+  base_left_marker_pose: 0
+  base_right_marker_pose: 61
+  shoulder_marker_pose: 131
+  wrist_inside_marker_pose: 0
+  wrist_top_marker_pose: 0
+number_samples: 600
+performance_metrics:
+  angle_rotation_error:
+    base_left_marker_pose:
+      maximum: 0.41294171847747907
+      mean: 0.21592936261674547
+      median: 0.22412447149939774
+      rmse: 0.24310818842633086
+    base_right_marker_pose:
+      maximum: 0.15919860191633364
+      mean: 0.05750833432180123
+      median: 0.05552143849311667
+      rmse: 0.061931342858066916
+    shoulder_marker_pose:
+      maximum: 0.3864074601579912
+      mean: 0.1276920001965476
+      median: 0.12322134360935724
+      rmse: 0.141470133793462
+    wrist_inside_marker_pose:
+      maximum: 0.5790925519286533
+      mean: 0.16825776269524945
+      median: 0.15438178417289702
+      rmse: 0.18893775669030632
+    wrist_top_marker_pose:
+      maximum: 0.40511702679212597
+      mean: 0.14459008904920764
+      median: 0.145501249651101
+      rmse: 0.15889458853774657
+  euclidean_error:
+    base_left_marker_pose:
+      maximum: 0.03401788769999435
+      mean: 0.03335710878766747
+      median: 0.033229176839096174
+      rmse: 0.03335805300035855
+    base_right_marker_pose:
+      maximum: 0.034748772059683325
+      mean: 0.03416716556930868
+      median: 0.03419650881056504
+      rmse: 0.03416792013693636
+    shoulder_marker_pose:
+      maximum: 0.034352006936188964
+      mean: 0.034017375298520065
+      median: 0.03401204867261022
+      rmse: 0.034017477444385574
+    wrist_inside_marker_pose:
+      maximum: 0.03598241881671745
+      mean: 0.035063922182315986
+      median: 0.035045264256581214
+      rmse: 0.03506478866265719
+    wrist_top_marker_pose:
+      maximum: 0.03549062723316463
+      mean: 0.03480551427832518
+      median: 0.03478254826498238
+      rmse: 0.03480618391418952

stretch_camera_testrig/rviz/rviz.rviz

@@ -6,9 +6,10 @@ Panels:
       Expanded:
         - /Global Options1
         - /Status1
+        - /TF1
         - /TF1/Frames1
-      Splitter Ratio: 0.5
-    Tree Height: 728
+      Splitter Ratio: 0.8294117450714111
+    Tree Height: 1088
   - Class: rviz/Selection
     Name: Selection
   - Class: rviz/Tool Properties
@@ -182,7 +183,7 @@ Visualization Manager:
         base_left:
           Value: true
         base_link:
-          Value: true
+          Value: false
         base_right:
           Value: true
         camera_accel_frame:
@@ -196,7 +197,7 @@ Visualization Manager:
         camera_color_frame:
           Value: false
         camera_color_optical_frame:
-          Value: false
+          Value: true
         camera_depth_frame:
           Value: false
         camera_depth_optical_frame:
@@ -217,8 +218,6 @@ Visualization Manager:
           Value: false
         shoulder_top:
           Value: true
-        unknown:
-          Value: false
         wrist_inside:
           Value: true
         wrist_top:
@@ -236,8 +235,6 @@ Visualization Manager:
                 camera_accel_optical_frame:
                   {}
               camera_aligned_depth_to_color_frame:
-                {}
-              camera_color_frame:
                 camera_color_optical_frame:
                   base_left:
                     {}
@@ -245,12 +242,12 @@ Visualization Manager:
                     {}
                   shoulder_top:
                     {}
-                  unknown:
-                    {}
                   wrist_inside:
                     {}
                   wrist_top:
                     {}
+              camera_color_frame:
+                {}
               camera_depth_frame:
                 camera_depth_optical_frame:
                   {}
@@ -293,33 +290,33 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz/Orbit
-      Distance: 0.4894060790538788
+      Distance: 0.5751698017120361
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
         Swap Stereo Eyes: false
         Value: false
       Focal Point:
-        X: 0.27744147181510925
-        Y: -0.002162957563996315
-        Z: 0.0087382011115551
+        X: 0.12805525958538055
+        Y: 0.12409159541130066
+        Z: 0.06391580402851105
       Focal Shape Fixed Size: true
       Focal Shape Size: 0.05000000074505806
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 0.4003978967666626
+      Pitch: 0.28039810061454773
       Target Frame: <Fixed Frame>
       Value: Orbit (rviz)
-      Yaw: 2.0953991413116455
+      Yaw: 2.8453946113586426
     Saved: ~
 Window Geometry:
   Displays:
     collapsed: true
-  Height: 1025
+  Height: 1385
   Hide Left Dock: true
   Hide Right Dock: true
-  QMainWindow State: 000000ff00000000fd00000004000000000000015600000363fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000003d00000363000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000363fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000363000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073d0000003efc0100000002fb0000000800540069006d006501000000000000073d000002eb00fffffffb0000000800540069006d006501000000000000045000000000000000000000073d0000036300000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd0000000400000000000002c3000004cbfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000003d000004cb000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000363fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000363000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000009bd0000003efc0100000002fb0000000800540069006d00650100000000000009bd000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000009bd000004cb00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Selection:
     collapsed: false
   Time:
@@ -328,6 +325,6 @@ Window Geometry:
     collapsed: false
   Views:
     collapsed: true
-  Width: 1853
+  Width: 2493
   X: 67
   Y: 27