[wip] Vidhi hardware experiments

.circleci/config.yml

@@ -458,6 +458,16 @@ jobs:
             cd perception/sandbox/eyehandcal
             pip install -e .
             pytest tests
+  polygrasp:
+    machine:
+      image: ubuntu-2004:202104-01
+    resource_class: gpu.nvidia.small
+    steps:
+      - checkout
+      - run:
+          name: Initialize Git submodules
+          command: |
+            git submodule update --init --recursive ./perception/sandbox/polygrasp/
 
 workflows:
   build:
@@ -474,6 +484,7 @@ workflows:
       - fairotag
       - fairomsg
       - eyehandcal
+      - polygrasp
       - update-docs:
           requires:
             - polymetis-local-ubuntu-20

.gitmodules

@@ -20,3 +20,9 @@
 [submodule "polymetis/polymetis/tests/cpp/fake_clock"]
 	path = polymetis/polymetis/tests/cpp/fake_clock
 	url = https://github.com/korfuri/fake_clock
+[submodule "perception/sandbox/polygrasp/third_party/UnseenObjectClustering"]
+	path = perception/sandbox/polygrasp/third_party/UnseenObjectClustering
+	url = git@github.com:1heart/UnseenObjectClustering.git
+[submodule "perception/sandbox/polygrasp/third_party/graspnet-baseline"]
+	path = perception/sandbox/polygrasp/third_party/graspnet-baseline
+	url = git@github.com:1heart/graspnet-baseline.git

perception/realsense_driver/python/realsense_wrapper/realsense_d435.py

@@ -38,6 +38,16 @@ def __init__(self, height=480, width=640, fps=30, warm_start=60):
         # Warm start camera (realsense automatically adjusts brightness during initial frames)
         for _ in range(warm_start):
             self._get_frames()
+        self.decimation = rs.decimation_filter()
+        self.spatial = rs.spatial_filter()
+        self.temporal = rs.temporal_filter()
+        self.depth_to_disparity = rs.disparity_transform(True)
+        self.disparity_to_depth = rs.disparity_transform(False)
+
+        self.spatial.set_option(rs.option.filter_magnitude, 5)
+        self.spatial.set_option(rs.option.filter_smooth_alpha, 1)
+        self.spatial.set_option(rs.option.filter_smooth_delta, 50)
+        self.spatial.set_option(rs.option.holes_fill, 3)
 
     def _get_frames(self):
         framesets = [pipe.wait_for_frames() for pipe in self.pipes]
@@ -58,20 +68,27 @@ def get_num_cameras(self):
 
     def get_rgbd(self):
         """Returns a numpy array of [n_cams, height, width, RGBD]"""
-        framesets = self._get_frames()
         num_cams = self.get_num_cameras()
 
         rgbd = np.empty([num_cams, self.height, self.width, 4], dtype=np.uint16)
 
-        for i, frameset in enumerate(framesets):
+        for i, frameset in enumerate(self._get_frames()):
             color_frame = frameset.get_color_frame()
             rgbd[i, :, :, :3] = np.asanyarray(color_frame.get_data())
 
             depth_frame = frameset.get_depth_frame()
-            rgbd[i, :, :, 3] = np.asanyarray(depth_frame.get_data())
+            # breakpoint() # apply post-processing filters
+            # print('I was in realsense wrapper!!!!!!!')
+
+            # depth_frame = self.decimation.process(depth_frame)
+            filtered_depth = self.spatial.process(depth_frame)
+
+            rgbd[i, :, :, 3] = np.asanyarray(filtered_depth.get_data())
+            # rgbd[i, :, :, 3] = np.asanyarray(depth_frame.get_data())
 
         return rgbd
 
+    # def visualize_depth(self, )
 
 if __name__ == "__main__":
     cams = RealsenseAPI()

perception/sandbox/eyehandcal/calibration.json

@@ -0,0 +1,78 @@
+[
+    {
+        "num_marker_seen": 40,
+        "stage2_retry": 1,
+        "pixel_error": 0.8557764242470979,
+        "proj_func": "hand_marker_proj_world_camera",
+        "camera_base_ori": [
+            [
+                -0.08659041738075304,
+                0.5633264547579535,
+                -0.8216844923616736
+            ],
+            [
+                0.9924711811719529,
+                -0.022934989100674767,
+                -0.12031184820332479
+            ],
+            [
+                -0.08662017179026467,
+                -0.825916031836643,
+                -0.5570993216601846
+            ]
+        ],
+        "camera_base_ori_rotvec": [
+            -1.631172325657952,
+            -1.6992764571216632,
+            0.9920703670208724
+        ],
+        "camera_base_pos": [
+            1.0515278099246723,
+            -0.29931798637008716,
+            0.3297260234873946
+        ],
+        "p_marker_ee": [
+            0.018598814978811502,
+            0.07479750137090518,
+            0.07041129006325338
+        ]
+    },
+    {
+        "num_marker_seen": 30,
+        "stage2_retry": 1,
+        "pixel_error": 1.2994802274525665,
+        "proj_func": "hand_marker_proj_world_camera",
+        "camera_base_ori": [
+            [
+                0.9999115948309912,
+                0.008265638017755803,
+                0.010415457296892267
+            ],
+            [
+                -0.0012524188160752225,
+                -0.7212821241688373,
+                0.6926402592988654
+            ],
+            [
+                0.013237596823181205,
+                -0.6925920708343763,
+                -0.721208007060167
+            ]
+        ],
+        "camera_base_ori_rotvec": [
+            -2.3763972172278973,
+            -0.004841443829322705,
+            -0.016328440566469732
+        ],
+        "camera_base_pos": [
+            0.5125572254847081,
+            -0.8739401831431185,
+            0.44557712197436244
+        ],
+        "p_marker_ee": [
+            0.018034999783211304,
+            0.07429549411283291,
+            0.06650570163149509
+        ]
+    }
+]

perception/sandbox/eyehandcal/calibration_fre.json

@@ -0,0 +1,117 @@
+[
+    {
+        "num_marker_seen": 95,
+        "stage2_retry": 1,
+        "pixel_error": 0.9547806177666195,
+        "proj_func": "hand_marker_proj_world_camera",
+        "camera_base_ori": [
+            [
+                0.13046836385392596,
+                0.696151099938567,
+                -0.7059402609906897
+            ],
+            [
+                0.9912835170438116,
+                -0.07844863725257158,
+                0.10584328108323836
+            ],
+            [
+                0.018302865090759114,
+                -0.7135961444455386,
+                -0.7003181760899339
+            ]
+        ],
+        "camera_base_ori_rotvec": [
+            -1.837095997517653,
+            -1.623676023758974,
+            0.6616554748410934
+        ],
+        "camera_base_pos": [
+            1.1223414854669889,
+            0.4289414989574941,
+            0.6010171201189213
+        ],
+        "p_marker_ee": [
+            0.03831918500065905,
+            0.019617732009179226,
+            0.00296617146633602
+        ]
+    },
+    {
+        "num_marker_seen": 32,
+        "stage2_retry": 1,
+        "pixel_error": 0.9590942590683659,
+        "proj_func": "hand_marker_proj_world_camera",
+        "camera_base_ori": [
+            [
+                -0.9999816143832455,
+                -0.002550738818823516,
+                -0.005501329562555543
+            ],
+            [
+                0.0020411967856876507,
+                0.7126907627175928,
+                -0.7014753810738449
+            ],
+            [
+                0.005710027246852617,
+                -0.7014737133125458,
+                -0.712672452898488
+            ]
+        ],
+        "camera_base_ori_rotvec": [
+            -0.00043329766126271746,
+            2.9127997443564024,
+            -1.1930214230356864
+        ],
+        "camera_base_pos": [
+            0.5988010446469789,
+            1.1827937514184672,
+            0.5821881700194991
+        ],
+        "p_marker_ee": [
+            0.04152649415234615,
+            0.029126794033413686,
+            -0.0007389444425027823
+        ]
+    },
+    {
+        "num_marker_seen": 98,
+        "stage2_retry": 1,
+        "pixel_error": 1.365541314657748,
+        "proj_func": "hand_marker_proj_world_camera",
+        "camera_base_ori": [
+            [
+                -0.7781255363866066,
+                0.20680564056341014,
+                0.5930869048075627
+            ],
+            [
+                0.2725888363260759,
+                0.9618736506353099,
+                0.022235254077657744
+            ],
+            [
+                -0.5658762903086264,
+                0.17897068822759726,
+                -0.8048313592435926
+            ]
+        ],
+        "camera_base_ori_rotvec": [
+            0.3366391965818879,
+            2.4892420848412318,
+            0.14129033610196823
+        ],
+        "camera_base_pos": [
+            -0.6887764484164224,
+            0.5199716996384507,
+            0.5871127084134348
+        ],
+        "p_marker_ee": [
+            0.03860891277607357,
+            0.0222322033273417,
+            0.005945508211621145
+        ]
+    }
+
+]

perception/sandbox/eyehandcal/calibration_grasp.json

@@ -0,0 +1,84 @@
+[
+    {
+        "num_marker_seen": 68,
+        "stage2_retry": 1,
+        "pixel_error": 1.4255832754110596,
+        "proj_func": "hand_marker_proj_world_camera",
+        "camera_base_ori": [
+            [
+                0.9695585063566714,
+                -0.2369145203855838,
+                -0.06186932019902844
+            ],
+            [
+                -0.1261127146316217,
+                -0.6997408143696652,
+                0.7031773431457553
+            ],
+            [
+                -0.20988541149795112,
+                -0.6739690666015383,
+                -0.7083175921185277
+            ]
+        ],
+        "camera_base_ori_rotvec": [
+            -2.3524115261508154,
+            0.25283786586734175,
+            0.189269233197779
+        ],
+        "camera_base_pos": [
+            0.5440563000671933,
+            -0.8344297084516437,
+            0.4531890109386328
+        ],
+        "p_marker_ee": [
+            0.021262156877639195,
+            0.07641382134614867,
+            0.06698147912227602
+        ]
+    },
+    {
+        "num_marker_seen": 0,
+        "stage2_retry": null,
+        "pixel_error": null,
+        "proj_func": null
+    },
+    {
+        "num_marker_seen": 36,
+        "stage2_retry": 1,
+        "pixel_error": 1.6886170204254602,
+        "proj_func": "hand_marker_proj_world_camera",
+        "camera_base_ori": [
+            [
+                -0.1909542618662141,
+                0.4721818402498436,
+                -0.8605700317890473
+            ],
+            [
+                0.980867631712919,
+                0.05795179156514297,
+                -0.18585015176824402
+            ],
+            [
+                -0.03788349156319442,
+                -0.8795941675526634,
+                -0.4742140249658582
+            ]
+        ],
+        "camera_base_ori_rotvec": [
+            -1.4594638185435622,
+            -1.7307266255908527,
+            1.070147620384316
+        ],
+        "camera_base_pos": [
+            1.0452644317567419,
+            -0.28531817047932034,
+            0.24218804010822173
+        ],
+        "p_marker_ee": [
+            0.02019930033538613,
+            0.07496012579444654,
+            0.0669902240646309
+        ]
+    }
+]